当前位置:   article > 正文

ros入门21讲_ros发布cmd

ros发布cmd

安装编译/解析器

sudo apt-get install g++
sudo apt-get install python

ROS核心概念

节点(Node) --执行单元
*执行具体任务的进程,独立运行的可执行文件;
*不同节点可使用不同的编程语言,可分布运行在不同的主机;
*节点在系统中的名称必须是唯一的.

节点管理器(ROS Master) --控制中心
*为节点提供命名和注册服务;
*跟踪和记录话题/服务通信,辅助节点互相查找,建立连接;
*提供参数服务器,节点使用此服务器存储和检索时的参数.

话题(Topic) --异步通信机制
*节点间用来传输数据的重要总线;
*使用发布/订阅模型,数据由发布者传输到订阅者,同一个话题的订阅者或发布者可以不唯一.

消息(Mwssage) --话题数据
*具有一定的类型和数据结构,包括ROS提供的标准类型和用户自定义类型;
*使用编程语言无关的.msg文件,编译过程中生成对应的代码文件.

服务(Service) --同步通信机制
*使用客户端/服务器(C/S)模型,客户端发送请求数据,服务器完成处理后返回问答数据;
*使用编程语言无关的.srv文件定义请求和应答数据结构,编译过程中生成对应的代码文件.

参数(Parameter) --全局共享字典
*可通过网络访问的共享,多变量字典;
*节点使用此服务器来储存和检索运行时的参数;
*适合存储静态,非二进制的配置参数,不适合存储动态配置的数据.

功能包(Package)
*ROS软件中的基本单元,包含节点源码,配置文件,数据定义等

功能包清单(Package manifest)
*记录功能包的基本信息,包含作者信息,许可信息,依赖选项.编译标志等

元功能包(Meta Packages)
*组织多个用于同一目的的功能包

创建工作空间

mkdir -p ~/catkin_ws/src

cd ~/catkin_ws/src

catkin_init_workspace

编译工作空间

cd ~/catkin_ws/

catkin_make

设置环境变量

source devel/setup.bash

检查环境变量

echo $ROS_PACKAGE_PATH

建立install空间:catkin_make install

创建功能包

cd ~/catkin_ws/src

catkin_create_pkg test_pkg std_msgs rospy roscpp

编译功能包

cd ~/catkin_ws

catkin_make

source ~/catkin_ws/devel/setup.bash

同一个工作空间下,不允许存在同名功能包

不同工作空间下,允许存在同名功能包



查看节点列表:rosnode list
发布话题消息:rostopic pub -r 10 /话题名
发布服务请求:rosservice call /服务文件 “变量:val”
话题记录: rosbag record -a -O fileName
话题复现: rosbag play fileName

发布者Publisher的编程实现

创建功能包

cd ~/catkin_ws/src
catkin_create_pkg learning_topic roscpp rospy std_msgs geometry_msgs turtlesim

创建发布者代码(C++)
如何实现一个发布者
*初始化ROS节点
*向ROS Master注册节点信息,包括发布的话题名和话题中的消息类型
*创建消息数据
*按照一定频率循环发布消息

//创建cpp文件
cd learning_topic/src/
touch velocity_publisher.cpp

/**
 * 该例程将发布turtle1/cmd_vel话题,消息类型geometry_msgs::Twist
 */注释不要复制进去
 
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>

int main(int argc, char **argv)
{
    // ROS节点初始化
    ros::init(argc, argv, "velocity_publisher");

    // 创建节点句柄
    ros::NodeHandle n;

    // 创建一个Publisher,发布名为/turtle1/cmd_vel的topic,消息类型为geometry_msgs::Twist,队列长度10
    ros::Publisher turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);

    // 设置循环的频率
    ros::Rate loop_rate(10);

    int count = 0;
    while (ros::ok())
    {
        // 初始化geometry_msgs::Twist类型的消息
        geometry_msgs::Twist vel_msg;
        vel_msg.linear.x = 0.5;
        vel_msg.angular.z = 0.2;

        // 发布消息
        turtle_vel_pub.publish(vel_msg);
        ROS_INFO("Publsh turtle velocity command[%0.2f m/s, %0.2f rad/s]",
                vel_msg.linear.x, vel_msg.angular.z);

        // 按照循环频率延时
        loop_rate.sleep();
    }

    return 0;
}

配置发布者代码编译规则

执行代码需要配置CMakeList.txt中的编译规则

设置需要编程的代码和生成可执行文件

设置链接库

//添加到# Install ##正上方

add_executable(velocity_publisher src/velocity_publisher.cpp)

target_link_libraries(velocity_publisher ${catkin_LIBRARIES})

编译并运行发布者

//编译在终端运行
cd ~/catkin_ws
catkin_make
//打开home文件夹,ctrl+H显示隐藏文件,找到.bashrc文件
//打开.bashrc,并在文件的最后添加以下这段话
//source /home/catkin_ws/devel/setup.bash
//再重启终端即可。这样每次编译(catkin_make)之后就不需要再写source devel/setup.bash了
source devel/setup.bash
roscore
//新开一个终端,开启turtlesim
rosrun turtlesim turtlesim_node
//新开一个终端,运行发布者cpp程序
rosrun learning_topic velocity_publisher
//编译cpp生成的文件在home/catkin_ws/devel/lib/learning_topic下面

备注
错误:[rospack] Error: package ‘learning_service’ not found
解决方案:
手动配置环境变量,具体操作如下
//打开home文件夹,ctrl+H显示隐藏文件,找到.bashrc文件
//打开.bashrc,并在文件的最后添加以下这段话
//source /home/tfb/catkin_ws/devel/setup.bash
//再重启终端即可。这样每次编译(catkin_make)之后就不需要再写source devel/setup.bash了

Python语言实现

#为了与cpp文件存放的地方——src文件夹区分。在/home/catkin_ws/src/learning_topic/下新建一个scripts文件夹,用来存放py文件
#创建py文件
touch velocity_publisher.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程将发布turtle1/cmd_vel话题,消息类型geometry_msgs::Twist

import rospy
from geometry_msgs.msg import Twist

def velocity_publisher():
    # ROS节点初始化
    rospy.init_node('velocity_publisher', anonymous=True)

    # 创建一个Publisher,发布名为/turtle1/cmd_vel的topic,消息类型为geometry_msgs::Twist,队列长度10
    turtle_vel_pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)

    #设置循环的频率
    rate = rospy.Rate(10)

    while not rospy.is_shutdown():
        # 初始化geometry_msgs::Twist类型的消息
        vel_msg = Twist()
        vel_msg.linear.x = 0.5
        vel_msg.angular.z = 0.2

        # 发布消息
        turtle_vel_pub.publish(vel_msg)
        rospy.loginfo("Publsh turtle velocity command[%0.2f m/s, %0.2f rad/s]",
                vel_msg.linear.x, vel_msg.angular.z)

        # 按照循环频率延时
        rate.sleep()

if __name__ == '__main__':
    try:
        velocity_publisher()
    except rospy.ROSInterruptException:
        pass

在ros下运行python文件一定要注意待执行的python文件有可执行权限
#python不要配置代码编译规则
#其余步骤参考上述cpp文件流程

备注:
错误:import-im6.q16: not authorized `rospy’ @ error/constitute.c/WriteImage/1037.
解决方案:
在py文件前面添加#!/usr/bin/env python
#!/usr/bin/env python这种用法是为了防止操作系统用户没有将python装在默认的/usr/bin路径里。 当系统看到这一行的时候,首先会到env设置(环境变量)里查找python的安装路径,再调用对应路径下的解释器程序完成操作。

记得将py文件或cpp文件设置为程序执行文件

订阅者Subsrciber的编程实现

创建订阅者代码(C++)
如何实现一个订阅者
*初始化ROS节点;
*订阅需要的话题;
*循环等待话题消息,接受到消息后进入回调函数;
*在回调函数中完成消息处理.

//创建cpp文件
cd learning_topic/src/
touch pose_subscriber.cpp

/**
 * 该例程将订阅/turtle1/pose话题,消息类型turtlesim::Pose
 */
 
#include <ros/ros.h>
#include "turtlesim/Pose.h"

// 接收到订阅的消息后,会进入消息回调函数
void poseCallback(const turtlesim::Pose::ConstPtr& msg)
{
    // 将接收到的消息打印出来
    ROS_INFO("Turtle pose: x:%0.6f, y:%0.6f", msg->x, msg->y);
}

int main(int argc, char **argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "pose_subscriber");

    // 创建节点句柄
    ros::NodeHandle n;

    // 创建一个Subscriber,订阅名为/turtle1/pose的topic,注册回调函数poseCallback
    ros::Subscriber pose_sub = n.subscribe("/turtle1/pose", 10, poseCallback);

    // 循环等待回调函数
    ros::spin();

    return 0;
}

编译并运行订阅者

//配置CMakeLists.txt
add_executable(pose_subscriber src/pose_subscriber.cpp)
target_link_libraries(pose_subscriber ${catkin_LIBRARIES})
//编译并运行
cd ~/catkin_ws
catkin_make
source devel/setup.bash
roscore
//新开一个终端,开启turtlesim
rosrun turtlesim turtlesim_node
//新开一个终端,运行发布者cpp程序
rosrun learning_topic pose_subscriber

Python语言实现

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程将订阅/turtle1/pose话题,消息类型turtlesim::Pose

import rospy
from turtlesim.msg import Pose

def poseCallback(msg):
    rospy.loginfo("Turtle pose: x:%0.6f, y:%0.6f", msg.x, msg.y)

def pose_subscriber():
    # ROS节点初始化
    rospy.init_node('pose_subscriber', anonymous=True)

    # 创建一个Subscriber,订阅名为/turtle1/pose的topic,注册回调函数poseCallback
    rospy.Subscriber("/turtle1/pose", Pose, poseCallback)

    # 循环等待回调函数
    rospy.spin()

if __name__ == '__main__':
    pose_subscriber()

话题消息的定义和使用

Person.msg

//在文件夹learning_topic中新建一个命名为msg的文件夹,存放消息类文件,方便管理
//创建.msg文件 touch Person.msg
//打开该文件,输入以下信息后保存

touch Person.msg

//打开该文件,输入以下信息后保存

string name

uint8 sex

uint8 age

uint8 unknown = 0

uint8 male = 1

uint8 female = 2

//这里写的并不是cpp,也不是python。但他在编译过程中会由ros自动编译成cpp或python

      

 

//打开文件夹learning_topic,打开package.xml
//gedit package.xml
//保存到最下方/<build_depend>类中
<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>
//<build_depend>用于添加编译依赖,这里添加message_generation功能包
//<exec_depend<用于添加执行依赖,这里添加message_runtime功能包

在CMakeLists.txt添加编译选项

//打开文件夹learning_topic,打开CMakeLists.txt。
//在最上方的find_package中添加以下语句

find_package(.... message_generation)

//找到Declare ROS dynamic reconfigure parameters的位置,在上方添加以下语句

add_message_files(FILES Person.msg)

generate_messages(DEPENDENCIES std_msgs)

//第一句,告诉编译器Person.msg是我们定义的消息接口
//第二句,告诉编译器在编译Person.msg文件的时候,需要依赖ROS哪些已有的库或者包。

//找到catkin specific configuration(即Build上方),添加以下语句

catkin_package(......

CATKIN_DEPENDS geometry_msgs roscpp rospy std_msgs turtlesim后面添加message_runtime

)

//CATKIN_DEPENDS前面的#去掉

编译生成语言相关文件

//回到工作空间的根目录catkin_ws下,打开命令行,尝试进行编译。输入指令
catkin_make

c++实现

创建publisher和subscriber

person_publisher.cpp

/**
 * 该例程将发布/person_info话题,自定义消息类型learning_topic::Person
 */
 
#include <ros/ros.h>
#include "learning_topic/Person.h"

int main(int argc, char **argv)
{
    // ROS节点初始化
    ros::init(argc, argv, "person_publisher");

    // 创建节点句柄
    ros::NodeHandle n;

    // 创建一个Publisher,发布名为/person_info的topic,消息类型为learning_topic::Person,队列长度10
    ros::Publisher person_info_pub = n.advertise<learning_topic::Person>("/person_info", 10);

    // 设置循环的频率
    ros::Rate loop_rate(1);

    int count = 0;
    while (ros::ok())
    {
        // 初始化learning_topic::Person类型的消息
        learning_topic::Person person_msg;
        person_msg.name = "Tom";
        person_msg.age  = 18;
        person_msg.sex  = learning_topic::Person::male;

        // 发布消息
        person_info_pub.publish(person_msg);

           ROS_INFO("Publish Person Info: name:%s  age:%d  sex:%d",
                  person_msg.name.c_str(), person_msg.age, person_msg.sex);

        // 按照循环频率延时
        loop_rate.sleep();
    }

    return 0;
}

person_subscriber.cpp

/**
 * 该例程将订阅/person_info话题,自定义消息类型learning_topic::Person
 */
 
#include <ros/ros.h>
#include "learning_topic/Person.h"

// 接收到订阅的消息后,会进入消息回调函数
void personInfoCallback(const learning_topic::Person::ConstPtr& msg)
{
    // 将接收到的消息打印出来
    ROS_INFO("Subcribe Person Info: name:%s  age:%d  sex:%d",
             msg->name.c_str(), msg->age, msg->sex);
}

int main(int argc, char **argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "person_subscriber");

    // 创建节点句柄
    ros::NodeHandle n;

    // 创建一个Subscriber,订阅名为/person_info的topic,注册回调函数personInfoCallback
    ros::Subscriber person_info_sub = n.subscribe("/person_info", 10, personInfoCallback);

    // 循环等待回调函数
    ros::spin();

    return 0;
}

//这里相比之前配置CMakeLists.txt,多了add_dependencies的语句目的是让可执行文件(前两句做的工作)和动态生成的文件产生依赖关系
//位置依旧,找到Install位置的上方,添加以下语句,保存.

add_executable(person_publisher src/person_publisher.cpp)

target_link_libraries(person_publisher ${catkin_LIBRARIES})

add_dependencies(person_publisher ${PROJECT_NAME}_generate_messages_cpp)

add_executable(person_subscriber src/person_subscriber.cpp)

target_link_libraries(person_subscriber ${catkin_LIBRARIES})

add_dependencies(person_subscriber ${PROJECT_NAME}_generate_messages_cpp)

编译并运行publisher和subscriber

cd ~/catkin_ws
catkin_make
source devel/setup.bash
roscore
//新开一个终端,运行subscriber
rosrun learning_topic person_subscriber
//新开一个终端,运行publisher
rosrun learning_topic person_publisher
//ROS Master是帮助节点的创建和链接的。节点一旦创建和链接后,就不受ROS Master影响

publisher和subscriber(python)

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程将发布/person_info话题,自定义消息类型learning_topic::Person

import rospy
from learning_topic.msg import Person

def velocity_publisher():
    # ROS节点初始化
    rospy.init_node('person_publisher', anonymous=True)

    # 创建一个Publisher,发布名为/person_info的topic,消息类型为learning_topic::Person,队列长度10
    person_info_pub = rospy.Publisher('/person_info', Person, queue_size=10)

    #设置循环的频率
    rate = rospy.Rate(10)

    while not rospy.is_shutdown():
        # 初始化learning_topic::Person类型的消息
        person_msg = Person()
        person_msg.name = "Tom";
        person_msg.age  = 18;
        person_msg.sex  = Person.male;

        # 发布消息
        person_info_pub.publish(person_msg)
        rospy.loginfo("Publsh person message[%s, %d, %d]",
                person_msg.name, person_msg.age, person_msg.sex)

        # 按照循环频率延时
        rate.sleep()

if __name__ == '__main__':
    try:
        velocity_publisher()
    except rospy.ROSInterruptException:
        pass

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程将订阅/person_info话题,自定义消息类型learning_topic::Person

import rospy
from learning_topic.msg import Person

def personInfoCallback(msg):
    rospy.loginfo("Subcribe Person Info: name:%s  age:%d  sex:%d",
             msg.name, msg.age, msg.sex)

def person_subscriber():
    # ROS节点初始化
    rospy.init_node('person_subscriber', anonymous=True)

    # 创建一个Subscriber,订阅名为/person_info的topic,注册回调函数personInfoCallback
    rospy.Subscriber("/person_info", Person, personInfoCallback)

    # 循环等待回调函数
    rospy.spin()

if __name__ == '__main__':
    person_subscriber()

客户端Client的编程实现

创建功能包

cd ~/catkin_ws/src
catkin_create_pkg learning_service roscpp rospy std_msgs geometry_msgs turtlesim

创建客户端代码(C++)
如何实现一个客户端
*初始化ROS客户端;
*创建一个Client实例;
*发布服务请求数据;
*等待Server处理之后的应答结果

touch turtle_spwan.cpp

/**
 * 该例程将请求/spawn服务,服务数据类型turtlesim::Spawn
 */

#include <ros/ros.h>
#include <turtlesim/Spawn.h>

int main(int argc, char** argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "turtle_spawn");

    // 创建节点句柄
    ros::NodeHandle node;

    // 发现/spawn服务后,创建一个服务客户端,连接名为/spawn的service
    ros::service::waitForService("/spawn");//阻塞型函数
    ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");

    // 初始化turtlesim::Spawn的请求数据
    turtlesim::Spawn srv;
    srv.request.x = 2.0;
    srv.request.y = 2.0;
    srv.request.name = "turtle2";

    // 请求服务调用
    ROS_INFO("Call service to spwan turtle[x:%0.6f, y:%0.6f, name:%s]",
             srv.request.x, srv.request.y, srv.request.name.c_str());

    add_turtle.call(srv); //阻塞型函数

    // 显示服务调用结果
    ROS_INFO("Spwan turtle successfully [name:%s]", srv.response.name.c_str());

    return 0;
};

//配置CMakeLists.txt

add_executable(turtle_spawn src/turtle_spwan.cpp)

target_link_libraries(turtle_spawn ${catkin_LIBRARIES})

编译并运行客户端

//编译并运行
cd ~/catkin_ws
catkin_make
source devel/setup.bash
roscore
//新开一个终端,开启turtlesim
rosrun turtlesim turtlesim_node
//新开一个终端,运行发布者cpp程序
rosrun learning_service turtle_spawn

创建客户端代码(python)

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程将请求/spawn服务,服务数据类型turtlesim::Spawn

import sys
import rospy
from turtlesim.srv import Spawn

def turtle_spawn():
    # ROS节点初始化
    rospy.init_node('turtle_spawn')

    # 发现/spawn服务后,创建一个服务客户端,连接名为/spawn的service
    rospy.wait_for_service('/spawn')
    try:
        add_turtle = rospy.ServiceProxy('/spawn', Spawn)

        # 请求服务调用,输入请求数据
        response = add_turtle(2.0, 2.0, 0.0, "turtle2")
        return response.name
    except rospy.ServiceException, e:
        print "Service call failed: %s"%e

if __name__ == "__main__":
    #服务调用并显示调用结果
    print "Spwan turtle successfully [name:%s]" %(turtle_spawn())

服务端Server的编程实现

创建服务器代码(C++)

如何实现一个服务器
*初始化ROS节点;
*创建Server实例;
*循环等待服务请求,进入回调函数;
*在回调函数中完成服务功能的处理,并反馈应答数据

touch turtle_command_server.cpp

/**
 * 该例程将执行/turtle_command服务,服务数据类型std_srvs/Trigger
 */
 
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <std_srvs/Trigger.h>

ros::Publisher turtle_vel_pub;
bool pubCommand = false;

// service回调函数,输入参数req,输出参数res
bool commandCallback(std_srvs::Trigger::Request  &req,
                     std_srvs::Trigger::Response &res)
{
    pubCommand = !pubCommand;

    // 显示请求数据
    ROS_INFO("Publish turtle velocity command [%s]", pubCommand==true?"Yes":"No");

    // 设置反馈数据
    res.success = true;
    res.message = "Change turtle command state!";

    return true;
}

int main(int argc, char **argv)
{
    // ROS节点初始化
    ros::init(argc, argv, "turtle_command_server");

    // 创建节点句柄
    ros::NodeHandle n;

    // 创建一个名为/turtle_command的server,注册回调函数commandCallback
    ros::ServiceServer command_service = n.advertiseService("/turtle_command", commandCallback);

    // 创建一个Publisher,发布名为/turtle1/cmd_vel的topic,消息类型为geometry_msgs::Twist,队列长度10
    turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);

    // 循环等待回调函数
    ROS_INFO("Ready to receive turtle command.");

    // 设置循环的频率
    ros::Rate loop_rate(10);

    while(ros::ok())
    {
        // 查看一次回调函数队列
        ros::spinOnce();
        
        // 如果标志为true,则发布速度指令
        if(pubCommand)
        {
            geometry_msgs::Twist vel_msg;
            vel_msg.linear.x = 0.5;
            vel_msg.angular.z = 0.2;
            turtle_vel_pub.publish(vel_msg);
        }

        //按照循环频率延时
        loop_rate.sleep();
    }

    return 0;
}

//配置CMakeLists.txt

add_executable(turtle_command_server src/turtle_command_server.cpp)

target_link_libraries(turtle_command_server ${catkin_LIBRARIES})

编译并运行服务器

//编译并运行
cd ~/catkin_ws
catkin_make
source devel/setup.bash
roscore
//新开一个终端,开启turtlesim
rosrun turtlesim turtlesim_node
//新开一个终端,运行server
rosrun learning_service turtle_command_server
//新开一个终端,发送请求
rosservice call /turtle_command "{}"

创建服务器代码(Python)

注意,ros在Python中没有spinonce方法,可通过多线程来实现

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程将执行/turtle_command服务,服务数据类型std_srvs/Trigger

import rospy
import thread,time
from geometry_msgs.msg import Twist
from std_srvs.srv import Trigger, TriggerResponse

pubCommand = False;
turtle_vel_pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)

def command_thread():    
    while True:
        if pubCommand:
            vel_msg = Twist()
            vel_msg.linear.x = 0.5
            vel_msg.angular.z = 0.2
            turtle_vel_pub.publish(vel_msg)
            
        time.sleep(0.1)

def commandCallback(req):
    global pubCommand
    pubCommand = bool(1-pubCommand)

    # 显示请求数据
    rospy.loginfo("Publish turtle velocity command![%d]", pubCommand)

    # 反馈数据
    return TriggerResponse(1, "Change turtle command state!")

def turtle_command_server():
    # ROS节点初始化
    rospy.init_node('turtle_command_server')

    # 创建一个名为/turtle_command的server,注册回调函数commandCallback
    s = rospy.Service('/turtle_command', Trigger, commandCallback)

    # 循环等待回调函数
    print "Ready to receive turtle command."

    thread.start_new_thread(command_thread, ())
    rospy.spin()

if __name__ == "__main__":
    turtle_command_server()

服务数据的定义与使用

//在文件夹learning_service中新建一个命名为srv的文件夹,存放消息类文件,方便管理
//创建.srv文件 touch Person.srv

touch Person.srv

string name

uint8 age

uint8 sex

uint8 unknown = 0

uint8 male = 1

uint8 female = 2

---

string result

在package.xml中添加功能包依赖

/打开文件夹learning_service,打开package.xml
//gedit package.xml
//保存到最下方/<build_depend>类中
<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>
//<build_depend>用于添加编译依赖,这里添加message_generation功能包
//<exec_depend<用于添加执行依赖,这里添加message_runtime功能包

在CMakeLists.txt添加编译选项

/打开文件夹learning_service,打开CMakeLists.txt。
//在最上方的find_package中添加以下语句

find_package(..... message_generation)

//找到Declare ROS dynamic reconfigure parameters的位置,在上方添加以下语句

add_service_files(FILES Person.srv)

generate_messages(DEPENDENCIES std_msgs)

//第一句,告诉编译器Person.msg是我们定义的消息接口
//第二句,告诉编译器在编译Person.msg文件的时候,需要依赖ROS哪些已有的库或者包。

catkin_package(...... message_runtime)

编译生成语言相关文件

//回到工作空间的根目录catkin_ws下,打开命令行,尝试进行编译。输入指令
catkin_make

创建服务器代码(C++)

如何实现一个服务器
*初始化ROS节点;
*创建Server实例;
*循环等待服务请求,进入回调函数;
*在回调函数中完成服务功能的处理,并反馈应答数据

touch person_client.cpp

touch person_server.cpp

//客户端
/**
 * 该例程将请求/show_person服务,服务数据类型learning_service::Person
 */

#include <ros/ros.h>
#include "learning_service/Person.h"

int main(int argc, char** argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "person_client");

    // 创建节点句柄
    ros::NodeHandle node;

    // 发现/spawn服务后,创建一个服务客户端,连接名为/spawn的service
    ros::service::waitForService("/show_person");
    ros::ServiceClient person_client = node.serviceClient<learning_service::Person>("/show_person");

    // 初始化learning_service::Person的请求数据
    learning_service::Person srv; //注意要跟srv的文件名相同
    srv.request.name = "Tom";
    srv.request.age  = 20;
    srv.request.sex  = learning_service::Person::Request::male;

    // 请求服务调用
    ROS_INFO("Call service to show person[name:%s, age:%d, sex:%d]",
             srv.request.name.c_str(), srv.request.age, srv.request.sex);

    person_client.call(srv);

    // 显示服务调用结果
    ROS_INFO("Show person result : %s", srv.response.result.c_str());

    return 0;
};

//服务端

/**
 * 该例程将执行/show_person服务,服务数据类型learning_service::Person
 */
 
#include <ros/ros.h>
#include "learning_service/Person.h"

// service回调函数,输入参数req,输出参数res
bool personCallback(learning_service::Person::Request  &req,
                     learning_service::Person::Response &res)
{
    // 显示请求数据
    ROS_INFO("Person: name:%s  age:%d  sex:%d", req.name.c_str(), req.age, req.sex);

    // 设置反馈数据
    res.result = "OK";

    return true;
}

int main(int argc, char **argv)
{
    // ROS节点初始化
    ros::init(argc, argv, "person_server");

    // 创建节点句柄
    ros::NodeHandle n;

    // 创建一个名为/show_person的server,注册回调函数personCallback
    ros::ServiceServer person_service = n.advertiseService("/show_person", personCallback);

    // 循环等待回调函数
    ROS_INFO("Ready to show person informtion.");
    ros::spin();

    return 0;
}

配置服务器/客户端代码编译规则

如何配置CMakeLists.txt中的编译规则
*设置需要编译的代码和生成的可执行文件;
*设置链接库;
*添加依赖项.

//这里相比之前配置CMakeLists.txt,多了add_dependencies的语句目的是让可执行文件(前两句做的工作)和动态生成的文件产生依赖关系
//位置依旧,找到Install位置的上方,添加以下语句,保存.
add_executable(person_server src/person_server.cpp)
target_link_libraries(person_server ${catkin_LIBRARIES})
add_dependencies(person_server ${PROJECT_NAME}_gencpp)

add_executable(person_client src/person_client.cpp)
target_link_libraries(person_client ${catkin_LIBRARIES})
add_dependencies(person_client ${PROJECT_NAME}_gencpp)

编译并运行发布者和订阅者

//编译并运行
cd ~/catkin_ws
catkin_make
source devel/setup.bash
roscore
//新开一个终端,运行server
rosrun learning_service person_server
//新开一个终端,运行client
rosrun learning_service person_client

创建客户端和服务端代码(Python)

#客户端

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程将请求/show_person服务,服务数据类型learning_service::Person

import sys
import rospy
from learning_service.srv import Person, PersonRequest

def person_client():
    # ROS节点初始化
    rospy.init_node('person_client')

    # 发现/spawn服务后,创建一个服务客户端,连接名为/spawn的service
    rospy.wait_for_service('/show_person')
    try:
        person_client = rospy.ServiceProxy('/show_person', Person)

        # 请求服务调用,输入请求数据
        response = person_client("Tom", 20, PersonRequest.male)
        return response.result
    except rospy.ServiceException, e:
        print "Service call failed: %s"%e

if __name__ == "__main__":
    #服务调用并显示调用结果
    print "Show person result : %s" %(person_client())

#服务端

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程将执行/show_person服务,服务数据类型learning_service::Person

import rospy
from learning_service.srv import Person, PersonResponse

def personCallback(req):
    # 显示请求数据
    rospy.loginfo("Person: name:%s  age:%d  sex:%d", req.name, req.age, req.sex)

    # 反馈数据
    return PersonResponse("OK")

def person_server():
    # ROS节点初始化
    rospy.init_node('person_server')

    # 创建一个名为/show_person的server,注册回调函数personCallback
    s = rospy.Service('/show_person', Person, personCallback)

    # 循环等待回调函数
    print "Ready to show person informtion."
    rospy.spin()

if __name__ == "__main__":
    person_server()

参数的使用与编程方法

创建功能包

cd ~/catkin_ws/src
catkin_create_pkg learning_parameter roscpp rospy std_srvs

参数命令行使用

rospram
*列出当前所有参数

rosparam list

*显示某个参数值

rosparam get param_key

*设置某个参数值

rosparam set param_key param_value

*保存参数到文件

rosparam dump file_name

*从文件读取参数

rosparam load file_name

*删除参数

rosparam delete param_key

编程方法(C++)

如何获取/配置参数
*初始化ROS节点;
*get函数获取参数;
*set函数设置参数.

touch parameter_config.cpp

/**
 * 该例程设置/读取海龟例程中的参数
 */
#include <string>
#include <ros/ros.h>
#include <std_srvs/Empty.h>

int main(int argc, char **argv)
{
    int red, green, blue;

    // ROS节点初始化
    ros::init(argc, argv, "parameter_config");

    // 创建节点句柄
    ros::NodeHandle node;

    // 读取背景颜色参数
    ros::param::get("/background_r", red);
    ros::param::get("/background_g", green);
    ros::param::get("/background_b", blue);

    ROS_INFO("Get Backgroud Color[%d, %d, %d]", red, green, blue);

    // 设置背景颜色参数
    ros::param::set("/background_r", 255);
    ros::param::set("/background_g", 255);
    ros::param::set("/background_b", 255);

    ROS_INFO("Set Backgroud Color[255, 255, 255]");

    // 读取背景颜色参数
    ros::param::get("/background_r", red);
    ros::param::get("/background_g", green);
    ros::param::get("/background_b", blue);

    ROS_INFO("Re-get Backgroud Color[%d, %d, %d]", red, green, blue);

    // 调用服务,刷新背景颜色
    ros::service::waitForService("/clear");
    ros::ServiceClient clear_background = node.serviceClient<std_srvs::Empty>("/clear");
    std_srvs::Empty srv;
    clear_background.call(srv);
    
    sleep(1);

    return 0;
}

add_executable(parameter_config src/parameter_config.cpp)

target_link_libraries(parameter_config ${catkin_LIBRARIES})

编译并运行发布者

//编译并运行
cd ~/catkin_ws
catkin_make
source devel/setup.bash
roscore
//新开一个终端,开启turtlesim
rosrun turtlesim turtlesim_node
//新开一个终端,运行发布者
rosrun learning_parameter parameter_config

编程方法(Python)

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程设置/读取海龟例程中的参数

import sys
import rospy
from std_srvs.srv import Empty

def parameter_config():
    # ROS节点初始化
    rospy.init_node('parameter_config', anonymous=True)

    # 读取背景颜色参数
    red   = rospy.get_param('/background_r')
    green = rospy.get_param('/background_g')
    blue  = rospy.get_param('/background_b')

    rospy.loginfo("Get Backgroud Color[%d, %d, %d]", red, green, blue)

    # 设置背景颜色参数
    rospy.set_param("/background_r", 255);
    rospy.set_param("/background_g", 255);
    rospy.set_param("/background_b", 255);

    rospy.loginfo("Set Backgroud Color[255, 255, 255]");

    # 读取背景颜色参数
    red   = rospy.get_param('/background_r')
    green = rospy.get_param('/background_g')
    blue  = rospy.get_param('/background_b')

    rospy.loginfo("Get Backgroud Color[%d, %d, %d]", red, green, blue)

    # 发现/spawn服务后,创建一个服务客户端,连接名为/spawn的service
    rospy.wait_for_service('/clear')
    try:
        clear_background = rospy.ServiceProxy('/clear', Empty)

        # 请求服务调用,输入请求数据
        response = clear_background()
        return response
    except rospy.ServiceException, e:
        print "Service call failed: %s"%e

if __name__ == "__main__":
    parameter_config()

备注
错误:
KeyError: ‘/background_r’
解决方案:
改为
/turtlesim/background_r
其余一样

TF工具包能干什么?
*五秒钟之前,机器人头部坐标系相对于全局坐标系的关系是什么样的?
*机器人夹取的物体想对于机器人中心坐标系的位置在哪里?
*机器人中心坐标系相对于全局坐标系的位置在哪里?

TF坐标变换如何实现?
*广播TF变换
*监听TF变换

小海龟跟随实验

#安装功能包
sudo apt-get install ros-noetic-turtle-tf
#启动launch文件
roslaunch turtle_tf turtle_tf_demo.launch
#运行海龟键盘控制节点
rosrun turtlesim turtle_teleop_key
#可视化框架
rosrun tf view_frames
#查询位置关系
rosrun tf_echo turtle1 turtle2
#三维可视化位置关系
rosrun rviz rviz -d `rospack find turtle_tf` /rviz/turtle_rviz.rviz

tf坐标系广播与监听的编程实现

创建功能包

cd ~/catkin_ws/src
catkin_create_pkg learning_tf roscpp rospy tf turtlesim

c++

如何实现一个tf广播站
*定义TF广播站(TransformBroadcaster)
*创建坐标变换值;
*发布坐标变换(sendTranform)(广播器的编程)

)/**
 * 该例程产生tf数据,并计算、发布turtle2的速度指令
 */

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>

std::string turtle_name;

void poseCallback(const turtlesim::PoseConstPtr& msg)
{
    // 创建tf的广播器
    static tf::TransformBroadcaster br;

    // 初始化tf数据
    tf::Transform transform;
    transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
    tf::Quaternion q;
    q.setRPY(0, 0, msg->theta);
    transform.setRotation(q);

    // 广播world与海龟坐标系之间的tf数据
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}

int main(int argc, char** argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "my_tf_broadcaster");

    // 输入参数作为海龟的名字
    if (argc != 2)
    {
        ROS_ERROR("need turtle name as argument");
        return -1;
    }

    turtle_name = argv[1];

    // 订阅海龟的位姿话题
    ros::NodeHandle node;
    ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);

    // 循环等待回调函数
    ros::spin();

    return 0;
};

如何实现一个TF监听器
*定义TF监听器(TransformListener);
*查找坐标变换(waitForTransform,lookupTransform)(监听器的编写)

/**
 * 该例程监听tf数据,并计算、发布turtle2的速度指令
 */

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>

int main(int argc, char** argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "my_tf_listener");

    // 创建节点句柄
    ros::NodeHandle node;

    // 请求产生turtle2
    ros::service::waitForService("/spawn");
    ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");
    turtlesim::Spawn srv;
    add_turtle.call(srv);

    // 创建发布turtle2速度控制指令的发布者
    ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 10);

    // 创建tf的监听器
    tf::TransformListener listener;

    ros::Rate rate(10.0);
    while (node.ok())
    {
        // 获取turtle1与turtle2坐标系之间的tf数据
        tf::StampedTransform transform;
        try
        {
            //查询是否有这两个坐标系,查询当前时间,如果超过3s则报错
            listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));

            listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
        }
        catch (tf::TransformException &ex)
        {
            ROS_ERROR("%s",ex.what());
            ros::Duration(1.0).sleep();
            continue;
        }

        // 根据turtle1与turtle2坐标系之间的位置关系,发布turtle2的速度控制指令
        geometry_msgs::Twist vel_msg;
        vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),
                                        transform.getOrigin().x());
        vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
                                      pow(transform.getOrigin().y(), 2));
        turtle_vel.publish(vel_msg);

        rate.sleep();
    }
    return 0;
};

配置tf广播站与监听器代码编译规则

add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)

target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES})

add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)

target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES})

//编译并运行
cd ~/catkin_ws
catkin_make
source devel/setup.bash
roscore
//新开一个终端,开启turtlesim
rosrun turtlesim turtlesim_node
//新开一个终端,发布turtle1与world位置关系
rosrun learning_tf turtle_tf_broadcaster__name:=turtle1_tf_broadcaster /turtle1
//新开一个终端,发布turtle2与world位置关系
rosrun learning_tf turtle_tf_broadcaster__name:=turtle2_tf_broadcaster /turtle
//新开一个终端,运行监听器
rosrun learning_tf turtle_tf_listener
//新开一个终端,键盘控制器
rosrun learning turtle_teleop_key

创建tf广播器与监听器代码(Python)

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程将请求/show_person服务,服务数据类型learning_service::Person

import roslib
roslib.load_manifest('learning_tf')
import rospy

import tf
import turtlesim.msg

def handle_turtle_pose(msg, turtlename):
    br = tf.TransformBroadcaster()
    br.sendTransform((msg.x, msg.y, 0),
                     tf.transformations.quaternion_from_euler(0, 0, msg.theta),
                     rospy.Time.now(),
                     turtlename,
                     "world")

if __name__ == '__main__':
    rospy.init_node('turtle_tf_broadcaster')
    turtlename = rospy.get_param('~turtle')
    rospy.Subscriber('/%s/pose' % turtlename,
                     turtlesim.msg.Pose,
                     handle_turtle_pose,
                     turtlename)
    rospy.spin()

监听器编写

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# 该例程将请求/show_person服务,服务数据类型learning_service::Person

import roslib
roslib.load_manifest('learning_tf')
import rospy
import math
import tf
import geometry_msgs.msg
import turtlesim.srv

if __name__ == '__main__':
    rospy.init_node('turtle_tf_listener')

    listener = tf.TransformListener()

    rospy.wait_for_service('spawn')
    spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
    spawner(4, 2, 0, 'turtle2')

    turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist,queue_size=1)

    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
        try:
            (trans,rot) = listener.lookupTransform('/turtle2', '/turtle1', rospy.Time(0))
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            continue

        angular = 4 * math.atan2(trans[1], trans[0])
        linear = 0.5 * math.sqrt(trans[0] ** 2 + trans[1] ** 2)
        cmd = geometry_msgs.msg.Twist()
        cmd.linear.x = linear
        cmd.angular.z = angular
        turtle_vel.publish(cmd)

        rate.sleep()

launch启动文件的使用方法

Launch文件 :通过XML文件实现多节点的配置和启动(可自动启动ROS Master)
Launch文件语法:

参数设置

重映射

注意,映射完后原资源就不复存在了

嵌套

更多标签可参见:https://wiki.ros.org/roslaunch/XML

Launc示例
simple.launch

<launch>
    <node pkg="learning_topic" type="person_subscriber" name="talker" output="screen" />
    <node pkg="learning_topic" type="person_publisher" name="listener" output="screen" />
</launch>

turtlesim_parameter_config.launch

<launch>

    <param name="/turtle_number"   value="2"/>

    <node pkg="turtlesim" type="turtlesim_node" name="turtlesim_node">
        <param name="turtle_name1"   value="Tom"/>
        <param name="turtle_name2"   value="Jerry"/>

        <rosparam file="$(find learning_launch)/config/param.yaml" command="load"/>
    </node>

    <node pkg="turtlesim" type="turtle_teleop_key" name="turtle_teleop_key" output="screen"/>

</launch>

start_tf_demo_c++.launch

 <launch>

    <!-- Turtlesim Node-->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>

    <node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle1" name="turtle1_tf_broadcaster" />
    <node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle2" name="turtle2_tf_broadcaster" />

    <node pkg="learning_tf" type="turtle_tf_listener" name="listener" />

  </launch>

start_tf_demo_py.launch

<launch>

    <!-- Turtlesim Node-->
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>

    <node name="turtle1_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py">
      <param name="turtle" type="string" value="turtle1" />
    </node>
    <node name="turtle2_tf_broadcaster" pkg="learning_tf" type="turtle_tf_broadcaster.py">
      <param name="turtle" type="string" value="turtle2" />
    </node>

    <node pkg="learning_tf" type="turtle_tf_listener.py" name="listener" />

</launch>

turtlesim_remap.launch

<launch>

    <include file="$(find learning_launch)/launch/simple.launch" />

    <node pkg="turtlesim" type="turtlesim_node" name="turtlesim_node">
        <remap from="/turtle1/cmd_vel" to="/cmd_vel"/>
    </node>

</launch>

创建功能包

cd ~/catkin_ws/src
catkin_create_pkg learning_launch
cd learning_launch/
//用于存放launch文件
mkdir launch

编译并运行launch文件

cd ~/catkin_ws
catkin_make
roslaunch learning_launch simple.launch

常用可视化工具的使用

Qt工具箱

Rviz

Rviz是一款三维可视化工具,可以很好的兼容基于ROS软件框架的机器人平台.
*在rviz中,可以使用可扩展标记语言XML对机器人,周围事物等任何实物进行尺寸,质量,位置,材质,关节等属性的描述,并且在界面中呈现出来.
*同时,rviz还可以通过图形化的方式,实时显示机器人传感器的信息,机器人的运动状态,周围环境的变化等信息.
*总而言之,rviz通过机器人模型参数,机器人发布的传感信息等数据,为用户进行所有可检测信息的图形化显示.用户和开发者也可以在rviz的控制界面下,通过按钮,滑动条,数值等方式.控制机器人的行为.

启动

roscore
rosrun rviz rviz

Gazebo

Gazebo是一款功能强大的三维物理仿真平台
*具备强大的物理引擎
*高质量的图形渲染
*方便的编程与图形接口
*开源免费
其典型应用场景包括
*测试机器人算法
*机器人的设计
*现实情景下的回溯测试

启动

roslaunch gazebo_ros

进阶攻略

声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/Cpp五条/article/detail/674744
推荐阅读
相关标签
  

闽ICP备14008679号