zoukankan html css js c++ java

ROS入门(三)——常用功能与总结

ROS入门(三)——常用功能与总结
　　iwehdio的博客园：https://www.cnblogs.com/iwehdio/

1、坐标管理（tf）

用于坐标变化，计算各个坐标系之间的关系。

运行坐标变换例程：

$ roscore
$ sudo apt-get install ros-melodic-turtle-tf
$ roslaunch turtle_tf turtle_tf_demo.launch
$ rosrun turtlesim turtle_teleop_key
$ rosrun tf view_frames

最后一句会生成 tf 框架的 pdf 。

动态查询二者的坐标关系：

$ rosrun tf tf_echo turtle1 turtle2
$ rosrun rviz rviz d rospack find turtle_tf rviz turtle_rviz rviz

实现坐标广播：

创建功能包：

$ catkin_create_pkg learning_tf roscpp rospy tf turtlesim

实现 TF 广播器：

定义 TF 广播器。
订阅第一只海龟的位置信息。
创建坐标变换值。
发布坐标变换。

C++ 实现：

/**
 * 该例程产生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;
};

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()

实现坐标监听：

定义 TF 监听器。
查找坐标变换。
将运动指令发布给第二只海龟。

C++ 实现：

/**
 * 该例程监听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
		{
			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;
};

Python 实现：

#!/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()

C++ 的编译规则：

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})

C++ 命令行：

$ roscore
$ rosrun turtlesim turtlesim_node
$ rosrun learning_tf turtle_tf_broadcaster __name:=turtle1_tf_broadcaster /turtle1
$ rosrun learning_tf turtle_tf_broadcaster __name:=turtle2_tf_broadcaster /turtle2
$ rosrun learning_tf turtle_tf_listener
$ rosrun turtlesim turtle_teleop_key

其中最长的两行是通过重映射，复用了TF广播程序并且修改了节点名。

launch 启动文件：
- XML 文件格式。加入后也需要先编译工程。
- 启动 launch 文件：roslaunch 功能包名 launch文件名。会自动运行 roscore 命令。
- 根标签：<launch>。
- 启动节点：<node>。
  - pkg：节点所在的功能包名称。
  - type：节点的可执行文件名称。
  - name：节点运行时的名称。
  - output 是否输出日志、respawn 意外关闭是否重启、required 是否必须启动、ns 命名空间、args 输入的参数。
- 存储 ROS 参数：<param>。
  - name：参数名。
  - value：参数值。
- 从文件加载参数：<rosparam file=filename command="load" ns="param">。
- launch 文件内部参数：<arg name="arg-name" default="arg-value">。
  - 被调用时形式为 "$(arg arg-name)"。
- 重映射：<remap>。
  - from：原命名。
  - to：映射之后的命名。
- 嵌套：<include>。
  - file：包含其他 launch 文件路径，类似头文件。

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>

小海龟参数配置：

<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>

param 在 node 中时，命名会变成 node节点名 / param名称。都是 name 属性。
rosparam 中的 $(find learning_launch) 表示查找这个功能包并返回绝对路径。
如果是从别的文件中 include 中，可以用名称：加命名空间。也会在参数名前加命名空间名。

使用 tf 小海龟追逐（C++）：

<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>

引入其他文件并重映射：

remap：

<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>

simple：

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

2、可视化工具

Qt 工具箱：
- 日志输出工具：rqt_console。
- 计算图可视化工具： rqt_graph。
- 数据绘图工具： rqt_plot。
- 图像渲染工具：rqt _image_view。
- 所有工具集成：rqt。
Rviz：三维可视化工具。
- 通过 xml 对可视化属性进行配置。
- 启动 rviz：先启动 roscore，再输入rosrun rviz rviz。
Gazebo：三维物理仿真平台。
- 相比 Rviz，可以创建仿真数据，有环境模型库。
- 启动 Gazebo：roslaunch gazebo_ros 环境配置文件。
其他工具包：
- 机器人控制算法：ros_control。
- 即时定位与地图建模：gamapping / hector。
- 机械臂：moveit。

3、ROS编程流程总结（python）

1、话题

在 Python 中 ROS 节点初始化中的节点名、函数名要一致。
需要先import rospy。
发布话题的流程：
1. ROS节点初始化：rospy.init_node('发布者名', anonymous=True)。
2. 创建发布者：rospy.Publisher('话题名', 消息类型, queue_size=10)。
  - 返回一个发布者对象。
  - 消息类型如 Twist 在 geometry_msgs.msg 包下。
3. 循环终止条件：while not rospy.is_shutdown()，即ROS系统开启。
4. 初始化消息类型：
  - 以 Twist 为例，创建消息对象：
```
vel_msg = Twist()		# 首先构造方法创建对象
vel_msg.linear.x = 0.5		# 修改成员变量
vel_msg.angular.z = 0.2
```
5. 发布者发布消息：发布者对象.publish(消息对象)。
6. 按一定间隔发布消息：
```
#设置循环的频率
rate = rospy.Rate(一秒发送的次数) 
# 按照循环频率延时
rate.sleep()
```
7. 在主函数中抛出异常：rospy.ROSInterruptException。
8. 打印日志信息：rospy.loginfo()。
订阅话题的流程：
1. ROS节点初始化：rospy.init_node('订阅者名', anonymous=True)。
2. 创建订阅者：rospy.Subscriber('话题名', 消息类型, 回调函数)。
  - 订阅者接收到话题后会执行回调函数，回调函数传入的是订阅得到的消息：def xxxCallback(msg)。
3. 循环等待回调函数：rospy.spin()。在这之前阻塞。

2、服务

同样的，在 Python 中 ROS 节点初始化中的节点名、函数名要一致。
客户端请求服务的流程：
1. ROS节点初始化：rospy.init_node('客户端名')。
2. 等待发现服务： rospy.wait_for_service('服务名')。
  - 发现服务后的操作都在try..except...中完成。
  - 抛出rospy.ServiceException异常。
3. 创建一个客户端进行连接：rospy.ServiceProxy('服务名', 消息类型)。
  - 返回一个客户端对象。
4. 输入请求参数：客户端对象(请求数据)。
  - 返回一个服务响应response。
5. 请求服务调用：return response。

服务器响应服务的流程：

导入依赖：

import rospy
import thread,time
from geometry_msgs.msg import Twist		# 话题发布消息类型
from std_srvs.srv import Trigger, TriggerResponse
						# 请求消息类型、响应消息类型

ROS节点初始化：rospy.init_node('服务器名')。
创建一个服务，注册回调函数：rospy.Service('服务名', 请求消息类型, 回调函数)。

循环等待回调函数，通过启动多线程：

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

在函数外，全局声明一个话题发布者：rospy.Publisher('话题名', 消息类型, 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!")

3、操作全局参数

操作全局参数流程：
1. ROS节点初始化：rospy.init_node('服务器名')。
2. 读取全局参数：rospy.get_param('参数名')。
3. 设置全局参数：rospy.set_param('参数名', 参数值)。

参考：古月ROS入门21讲：https://www.bilibili.com/video/BV1zt411G7Vn?p=21

iwehdio的博客园：https://www.cnblogs.com/iwehdio/

查看全文

相关阅读:
Echart 动态生成series数据
 转换Excel格式
 .NET接收邮件下载邮件附件——openpop.net
百度Echart 地图
 mobiscroll 日期问题
 jQuery自动完成插件flexselect
HTML5 video 连续播放视频
 team foundation server 工具的使用
 实现主成分分析与白化
 白化（预处理步骤）【转】

原文地址：https://www.cnblogs.com/iwehdio/p/12790315.html