Launch

roslaunch可通过SSH 轻松地在本地和远程启动多个ROS 节点，以及在Parameter Server上设置参数。

​ roslaunch命令行用法

roslaunch package_name file.launch
roslaunch -p 1234 package filename.launch # 远程端口
roslaunch my_file.launch arg:=value # 参数传递

​ roslaunch .launch / XML格式

<launch>
  <!-- local machine already has a definition by default.
       This tag overrides the default definition with
       specific ROS_ROOT and ROS_PACKAGE_PATH values -->
  <machine name="local_alt" address="localhost" default="true" ros-root="/u/user/ros/ros/" ros-package-path="/u/user/ros/ros-pkg" />
  <!-- a basic listener node -->
  <node name="listener-1" pkg="rospy_tutorials" type="listener" />
  <!-- pass args to the listener node -->
  <node name="listener-2" pkg="rospy_tutorials" type="listener" args="-foo arg2" />
  <!-- a respawn-able listener node -->
  <node name="listener-3" pkg="rospy_tutorials" type="listener" respawn="true" />
  <!-- start listener node in the 'wg1' namespace -->
  <node ns="wg1" name="listener-wg1" pkg="rospy_tutorials" type="listener" respawn="true" />
  <!-- start a group of nodes in the 'wg2' namespace -->
  <group ns="wg2">
    <!-- remap applies to all future statements in this scope. -->
    <remap from="chatter" to="hello"/>
    <node pkg="rospy_tutorials" type="listener" name="listener" args="--test" respawn="true" />
    <node pkg="rospy_tutorials" type="talker" name="talker">
      <!-- set a private parameter for the node -->
      <param name="talker_1_param" value="a value" />
      <!-- nodes can have their own remap args -->
      <remap from="chatter" to="hello-1"/>
      <!-- you can set environment variables for a node -->
      <env name="ENV_EXAMPLE" value="some value" />
    </node>
  </group>
</launch>

roslaunch / PYTHON API

# 从软件包rqt_gui启动和停止rqt_gui的实例
import roslaunch
package = 'rqt_gui'
executable = 'rqt_gui'
node = roslaunch.core.Node(package, executable)

launch = roslaunch.scriptapi.ROSLaunch()
launch.start()

process = launch.launch(node)
print process.is_alive()
process.stop()

# 具有参数列表的多个启动文件
import roslaunch
uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
roslaunch.configure_logging(uuid)

cli_args1 = ['pkg1', 'file1.launch', 'arg1:=arg1', 'arg2:=arg2']
cli_args2 = ['pkg2', 'file2.launch', 'arg1:=arg1', 'arg2:=arg2']
cli_args3 = ['pkg3', 'file3.launch']
roslaunch_file1 = roslaunch.rlutil.resolve_launch_arguments(cli_args1)
roslaunch_args1 = cli_args1[2:]

roslaunch_file2 = roslaunch.rlutil.resolve_launch_arguments(cli_args2)
roslaunch_args2 = cli_args2[2:]

roslaunch_file3 = roslaunch.rlutil.resolve_launch_arguments(cli_args3)

launch_files = [(roslaunch_file1, roslaunch_args1), (roslaunch_file2, roslaunch_args2), roslaunch_file3]
parent = roslaunch.parent.ROSLaunchParent(uuid, launch_files)
parent.start()

# 从base启动文件“继承”
import roslaunch
roslaunch.configure_logging(uuid)
launch = roslaunch.scriptapi.ROSLaunch()
launch.parent = roslaunch.parent.ROSLaunchParent(uuid, "path/to/base.launch")
launch.start()
# Start another node
node = roslaunch.core.Node(package, executable)
launch.launch(node)
try:
  launch.spin()
finally:
  # After Ctrl+C, stop all nodes from running
  launch.shutdown()

TF2

TF是一个让用户随时间跟踪多个坐标系的功能包，它使用树型数据结构，根据时间缓冲并维护多个坐标系之间的坐标变换关系，可以帮助开发者在任意时间，在坐标系间完成点、向量等坐标的变换。

# 工作区
source /opt/ros/noetic/setup.bash
mkdir -p ~/tutorial_ws/src
cd ~/tutorial_ws
catkin_init_workspace src
catkin_make
source devel/setup.bash

# run demo
# 该demo使用tf2库创建三个坐标系：世界框架，turtle1框架和turtle2框架。
# 使用tf2广播器(broadcaster)发布乌龟坐标帧，并使用tf2侦听器(listener)计算乌龟帧中的差异并移动一只乌龟跟随另一只乌龟。
sudo apt-get install ros-noetic-turtle-tf2 ros-noetic-tf2-tools ros-noetic-tf
roslaunch turtle_tf2 turtle_tf2_demo.launch
rosrun tf2_tools view_frames.py # 通过ROS广播的帧
evince frames.pdf # 绘制帧连接方式的树
rosrun tf tf_echo turtle1 turtle2 # 通过ROS广播的任何两个帧之间的转换
rosrun rviz rviz -d `rospack find turtle_tf2`/rviz/turtle_rviz.rviz # 可视化tf2帧

tf2 broadcaster (Python)

如何将坐标系广播到tf2 : 广播海龟移动时不断变化的坐标系。

# 名为learning_tf2的catkin包将继承tf2，tf2_ros,roscpp,rospy和turtlesim.
catkin_create_pkg learning_tf2 tf2 tf2_ros roscpp rospy turtlesim
roscd learning_tf2
mkdirs nodes

# nodes / turtle_tf2_broadcaster.py
#!/usr/bin/env python  
import rospy
import tf_conversions # Because of transformations
import tf2_ros
import geometry_msgs.msg
import turtlesim.msg
def handle_turtle_pose(msg, turtlename):
    br = tf2_ros.TransformBroadcaster()
    t = geometry_msgs.msg.TransformStamped()

    t.header.stamp = rospy.Time.now() # 时间戳
    t.header.frame_id = "world" # 设置要创建的链接的父框架的名称
    t.child_frame_id = turtlename # 设置要创建的链接的子节点的名称
    # 从框架“world”到框架“turtle”的转换
    t.transform.translation.x = msg.x
    t.transform.translation.y = msg.y
    t.transform.translation.z = 0.0
    q = tf_conversions.transformations.quaternion_from_euler(0, 0, msg.theta)
    t.transform.rotation.x = q[0]
    t.transform.rotation.y = q[1]
    t.transform.rotation.z = q[2]
    t.transform.rotation.w = q[3]

    br.sendTransform(t)

if __name__ == '__main__':
    rospy.init_node('tf2_turtle_broadcaster')
    turtlename = rospy.get_param('turtle1') # 该节点采用一个参数
    rospy.Subscriber('/%s/pose' % turtlename, # 订阅turtle/pose主题
                     turtlesim.msg.Pose,
                     handle_turtle_pose,
                     turtlename)
    rospy.spin()
# chmod +x nodes/turtle_tf2_broadcaster.py

<!--start_demo.launch-->
<launch>
    <node pkg="turtlesim" type="turtlesim_node" name="sim"/>
    <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>

    <node name="turtle1_tf2_broadcaster" pkg="learning_tf2" type="turtle_tf2_broadcaster.py" respawn="false" output="screen" >
    <param name="turtle" type="string" value="turtle1" />
    </node>
    
    <node name="turtle2_tf2_broadcaster" pkg="learning_tf2" type="turtle_tf2_broadcaster.py" respawn="false" output="screen" >
    <param name="turtle" type="string" value="turtle2" /> 
    </node>
    
    <!--tf2 listener-->
	<node pkg="learning_tf2" type="turtle_tf2_listener.py" name="listener" output="screen"/>
</launch>

roslaunch learning_tf2 start_demo.launch
rosrun tf tf_echo /world/turtle1

tf2 listener (Python)

# nodes/turtle_tf2_listener.py
#!/usr/bin/env python  
import rospy

import math
import tf2_ros # tf2_ros包提供了tf2_ros.TransformListener的实现
import geometry_msgs.msg
import turtlesim.srv

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

    tfBuffer = tf2_ros.Buffer()
    listener = tf2_ros.TransformListener(tfBuffer) # 创建侦听器

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

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

    rate = rospy.Rate(10.0)
    while not rospy.is_shutdown():
        try:
            trans = tfBuffer.lookup_transform(turtle_name, 'turtle1', rospy.Time())
        except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
            rate.sleep()
            continue

        msg = geometry_msgs.msg.Twist()

        msg.angular.z = 4 * math.atan2(trans.transform.translation.y, trans.transform.translation.x)
        msg.linear.x = 0.5 * math.sqrt(trans.transform.translation.x ** 2 + trans.transform.translation.y ** 2)

        turtle_vel.publish(msg)

        rate.sleep()

四元数

四元数具有4个分量（x，y，z，w），不绕x / y / z轴旋转的常用单位四元数为（0,0,0,1）。

ROS使用两种四元数数据类型：msg和’tf’

from geometry_msgs.msg import Quaternion
# Create a list of floats, which is compatible with tf
# quaternion methods
quat_tf = [0, 1, 0, 0]
quat_msg = Quaternion(quat_tf[0], quat_tf[1], quat_tf[2], quat_tf[3])

#（绕X轴滚动）/（绕Y轴俯仰）/（绕Z轴偏航），然后转换为四元数
# tf.transformations alternative is not yet available in tf2
from tf.transformations import quaternion_from_euler  
# RPY to convert: 90deg, 0, -90deg
q = quaternion_from_euler(1.5707, 0, -1.5707)
print("The quaternion representation is %s %s %s %s." % (q[0], q[1], q[2], q[3]))
print(q[0], q[1], q[2], -q[3]) # 反转四元数的一种简单方法是对w分量求反
# 在同一帧中从q1到q2的相对旋转qr
# q2=qr*q1,故相对旋转qr=q2*q1_inverse

# 将一个四元数的旋转应用于姿势，只需将姿势的前一个四元数乘以表示所需旋转的四元数即可
q_orig = quaternion_from_euler(0, 0, 0)
q_rot = quaternion_from_euler(pi, 0, 0)
q_new = quaternion_multiply(q_rot, q_orig)
print(q_new)

# 四元数&欧拉角
import sys
import math

def quart2rpy(w,x,y,z):
    angles = {'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0}
    r = math.atan2(2*(w*x+y*z),1-2*(x*x+y*y))
    p = math.asin(2*(w*y-z*x))
    y = math.atan2(2*(w*z+x*y),1-2*(z*z+y*y))
    angles['roll'] = r*180/math.pi
    angles['pitch'] = p*180/math.pi
    angles['yaw'] = y*180/math.pi
    print(angles)
    return [r,p,y]

def rpy2quart(roll,pitch,yaw):
    qx = math.sin(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) - math.cos(roll/2) * math.sin(pitch/2) * math.sin(yaw/2)
    qy = math.cos(roll/2) * math.sin(pitch/2) * math.cos(yaw/2) + math.sin(roll/2) * math.cos(pitch/2) * math.sin(yaw/2)
    qz = math.cos(roll/2) * math.cos(pitch/2) * math.sin(yaw/2) - math.sin(roll/2) * math.sin(pitch/2) * math.cos(yaw/2)
    qw = math.cos(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) + math.sin(roll/2) * math.sin(pitch/2) * math.sin(yaw/2)
    print([qw,qx,qy,qz])
    return [qw,qx,qy,qz]

if __name__ == "__main__":
    rpy2quart(1,0,0)
    q=quart2rpy(0.8775825618903728, 0.479425538604203, 0.0, 0.0)
    rpy2quart(q[0],q[1],q[2])

Rviz

机器人通过ROS发布数据，rviz订阅消息接收数据，然后3D可视化。

Markers

rosrun rviz rviz --help
# 发送四个基本形状（框，球体，圆柱体和箭头）
catkin_create_pkg using_markers roscpp visualization_msgs

//// src/basic_shapes.cpp
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
//原始对象是通过visualization_msgs/Marker消息发送到显示器的。

int main( int argc, char** argv )
{
  ros::init(argc, argv, "basic_shapes");
  ros::NodeHandle n;
  ros::Rate r(1);
  ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 1);

  // Set our initial shape type to be a cube,创建一个整数来跟踪要发布的形状
  uint32_t shape = visualization_msgs::Marker::CUBE;

  while (ros::ok())
  {
    visualization_msgs::Marker marker;
    // Set the frame ID and timestamp.  See the TF tutorials for information on these.
    marker.header.frame_id = "/my_frame";
    marker.header.stamp = ros::Time::now();

    // Set the namespace and id for this marker.  This serves to create a unique ID
    // Any marker sent with the same namespace and id will overwrite the old one
    // 名称空间（ns）和id用于为此标记创建唯一的名称
    marker.ns = "basic_shapes";
    marker.id = 0;

    // Set the marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
    marker.type = shape;

    // Set the marker action.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
    marker.action = visualization_msgs::Marker::ADD;

    // Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header
    // xyz位置 & xyzw方向
    marker.pose.position.x = 0;
    marker.pose.position.y = 0;
    marker.pose.position.z = 0;
    marker.pose.orientation.x = 0.0;
    marker.pose.orientation.y = 0.0;
    marker.pose.orientation.z = 0.0;
    marker.pose.orientation.w = 1.0;

    // Set the scale of the marker -- 1x1x1 here means 1m on a side
    // 比例 1：1m
    marker.scale.x = 1.0;
    marker.scale.y = 1.0;
    marker.scale.z = 1.0;

    // Set the color -- be sure to set alpha to something non-zero!
    marker.color.r = 0.0f;
    marker.color.g = 1.0f;
    marker.color.b = 0.0f;
    marker.color.a = 1.0; //alpha（a）值0表示完全透明（不可见），而1则完全不透明。

    marker.lifetime = ros::Duration(); //标记的持续时间/生命周期

    // Publish the marker
    while (marker_pub.getNumSubscribers() < 1)
    {
      if (!ros::ok())
      {
        return 0;
      }
      ROS_WARN_ONCE("Please create a subscriber to the marker");
      sleep(1);
    }
    marker_pub.publish(marker);

    // Cycle between different shapes
    // CUBE立方体 SPHERE球 ARROW箭头 CYLINDER圆柱
    switch (shape)
    {
    case visualization_msgs::Marker::CUBE:
      shape = visualization_msgs::Marker::SPHERE;
      break;
    case visualization_msgs::Marker::SPHERE:
      shape = visualization_msgs::Marker::ARROW;
      break;
    case visualization_msgs::Marker::ARROW:
      shape = visualization_msgs::Marker::CYLINDER;
      break;
    case visualization_msgs::Marker::CYLINDER:
      shape = visualization_msgs::Marker::CUBE;
      break;
    }

    r.sleep();
  }
}

add_executable(basic_shapes src/basic_shapes.cpp)
target_link_libraries(basic_shapes ${catkin_LIBRARIES})
# catkin_make

rosmake rviz
rosrun using_markers basic_shapes
rosrun rviz rviz

//产生一个旋转的螺旋线
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>

#include <cmath>

int main( int argc, char** argv )
{
  ros::init(argc, argv, "points_and_lines");
  ros::NodeHandle n;
  ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 10);
  ros::Rate r(30);

  float f = 0.0;
  while (ros::ok())
  {

    visualization_msgs::Marker points, line_strip, line_list;
    points.header.frame_id = line_strip.header.frame_id = line_list.header.frame_id = "/my_frame";
    points.header.stamp = line_strip.header.stamp = line_list.header.stamp = ros::Time::now();
    points.ns = line_strip.ns = line_list.ns = "points_and_lines";
    points.action = line_strip.action = line_list.action = visualization_msgs::Marker::ADD;
    points.pose.orientation.w = line_strip.pose.orientation.w = line_list.pose.orientation.w = 1.0;

    points.id = 0;
    line_strip.id = 1;
    line_list.id = 2;

    points.type = visualization_msgs::Marker::POINTS;
    line_strip.type = visualization_msgs::Marker::LINE_STRIP;
    line_list.type = visualization_msgs::Marker::LINE_LIST;

    // POINTS markers use x and y scale for width/height respectively
    points.scale.x = 0.2;
    points.scale.y = 0.2;

    // LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width
    line_strip.scale.x = 0.1;
    line_list.scale.x = 0.1;

    // Points are green
    points.color.g = 1.0f;
    points.color.a = 1.0;

    // Line strip is blue
    line_strip.color.b = 1.0;
    line_strip.color.a = 1.0;

    // Line list is red
    line_list.color.r = 1.0;
    line_list.color.a = 1.0

    // Create the vertices for the points and lines
    for (uint32_t i = 0; i < 100; ++i)
    {
      float y = 5 * sin(f + i / 100.0f * 2 * M_PI);
      float z = 5 * cos(f + i / 100.0f * 2 * M_PI);

      geometry_msgs::Point p;
      p.x = (int32_t)i - 50;
      p.y = y;
      p.z = z;

      points.points.push_back(p);
      line_strip.points.push_back(p);

      // The line list needs two points for each line
      line_list.points.push_back(p);
      p.z += 1.0;
      line_list.points.push_back(p);
    }

    marker_pub.publish(points);
    marker_pub.publish(line_strip);
    marker_pub.publish(line_list);

    r.sleep();
    f += 0.04;
  }
}

Interactive Markers

urdf

URDF（Unified Robot Description Format，统一机器人描述格式）是ROS中一个非常重要的机器人模型描述格式。

apt-get install urdf_tutorial
roslaunch urdf_tutorial display.launch model:=urdf/01-myfirst.urdf
roslaunch urdf_tutorial display.launch model:='$(find urdf_tutorial)/urdf/01-myfirst.urdf'

gazebo

gazebo是一款功能强大的三维物理仿真平台，其中的机器人模型与rviz使用的模型相同，但是需要在模型中加入机器人和周围环境的物理属性，例如质量、摩擦系数、弹性系数等。机器人的传感器信息也可以通过插件的形式加入仿真环境，以可视化的方式进行显示。

gazebo
gazebo worlds/pioneer2dx.world
# gzserver可执行文件运行物理更新循环和传感器数据生成。这是gazebo的核心，可以独立于图形界面使用。
gzserver
# gzclient可执行文件运行QT基于用户界面。
gzclient