rviz

Rviz与机器人的控制 §

基本操作 §

运动规划 §

• 任务：给定Task space中的一点作为末端的运动目标位姿，对从初始点开始至目标点结束的中间每个采样点的状态进行规划，包括运动的轨迹、速度、避障等
• 运动规划可以配合Moveit完成

交互界面 §

调取相应的交互界面配置文件 §

• 启动时传入参数-d $(find YOUR_PACAKGE)/launch/moveit.rviz

  <!--The visualization component of MoveIt!-->
  <node name="$(anon rviz)" pkg="rviz" type="rviz" respawn="false"
  args="-d $(find YOUR_PACAKGE)/launch/moveit.rviz" output="screen">
    <rosparam command="load" file="$(find YOUR_ROBOT_moveit_config)/config/kinematics.yaml"/>
  </node>

Rviz交互界面的配置 §

• 在执行结束后停止显示planning的动画

  That is a visualisation setting. Dig into the settings in RViz:

  • MotionPlanning
    • Planned path
      • Loop animation

• 通过鼠标拖动进行交互的目标状态：Planning Request -> Query Goal State

在环境中添加物体 §

• 示例代码（添加圆柱体）

  def add_to_scene(scene, pose, l, r):
      # updated = False
      cylinder_pose = PoseStamped()
      cylinder_pose.header.frame_id = "world"
      # assign cylinder's pose
      cylinder_pose.pose.position = pose.position
      cylinder_pose.pose.orientation = pose.orientation
      cylinder_name = "cylinder"
      # add_cylinder(self, name, pose, height, radius)
      self.scene.add_cylinder(cylinder_name, cylinder_pose, l, r)
   
      # ensuring collision updates are received
      start = rospy.get_time()
      seconds = rospy.get_time()
      timeout = 5
      while (seconds - start < timeout) and not rospy.is_shutdown():
          # attached_objects = self.scene.get_attached_objects([cylinder_name])
          # print("attached_objects: ", end=',')
          # print(attached_objects)
          # is_attached = len(attached_objects.keys()) > 0
   
          is_known = cylinder_name in scene.get_known_object_names()
   
          # if (is_attached) and (is_known):
          #    return True
          if is_known:
              return True
   
          rospy.sleep(0.1)
          seconds = rospy.get_time()
              
      return False

可视化配置 §

路径可视化 §

• 路径可视化
  • 在Displays中选择Add，然后通过显示类型添加（然后再选择对应的topic），或者通过topic添加

标记点Marker与MarkerArray §

单一标记点Marker §

• rviz/DisplayTypes/Marker
• Python例程
  • 注意marker.header.frame_id要在rViz中已经定义（如Global Options->Fixed Frame中默认的world）
  • 在rViz中subscribe对应的topic（如/visualization_maker）
  • scale为模型大小（单位为米）

  #! /usr/bin/env python
   
  import rospy
  from visualization_msgs.msg import Marker
   
  rospy.init_node('rviz_marker')
   
  marker_pub = rospy.Publisher("/visualization_marker", Marker, queue_size = 2)
   
  marker = Marker()
   
  marker.header.frame_id = "world"
  marker.header.stamp = rospy.Time.now()
   
  # set shape, Arrow: 0; Cube: 1 ; Sphere: 2 ; Cylinder: 3
  marker.type = 2
  marker.id = 0
   
  # Set the scale of the marker
  marker.scale.x = 1.0
  marker.scale.y = 1.0
  marker.scale.z = 1.0
   
  # Set the color
  marker.color.r = 0.0
  marker.color.g = 1.0
  marker.color.b = 0.0
  marker.color.a = 1.0
   
  # Set the pose of the marker
  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
   
  while not rospy.is_shutdown():
    marker_pub.publish(marker)
    rospy.rostime.wallsleep(1.0)

多个标记点MarkerArray §

• 若MarkerArray中有多个具有相同ID的标记点，在显示时只显示最后压入标记点
• 常用操作（若初始化markerArray = MarkerArray()）：
  • 在队列尾追加一个标记markerArray.markers.append(marker)
  • 弹出第一个标记markerArray.markers.pop(0)
  • 队列的长度len(markerArray.markers)，追加新标记时可将新的标记的ID设置为marker.id = len(markerArray.markers)+1
• 例程

  import rospy
  from visualization_msgs.msg import Marker, MarkerArray
   
  def spheres2markers(marker_array):
      start_id = len(marker_array.markers)
      for i in range(3):
          marker = Marker()
          marker.header.frame_id = "world"
          marker.header.stamp = rospy.Time.now()
   
          marker.type = 2
          marker.id = start_id+i
   
          marker.scale.x = 0.1
          marker.scale.y = 0.1
          marker.scale.z = 0.1
   
          marker.color.r = 0
          marker.color.g = 1
          marker.color.b = 0
          marker.color.a = 1
   
          marker.pose.position.x = i*0.1
          marker.pose.position.y = i*0.1
          marker.pose.position.z = i*0.1
          marker.pose.orientation.x = 0
          marker.pose.orientation.y = 0
          marker.pose.orientation.z = 0
          marker.pose.orientation.w = 1
   
          marker_array.markers.append(marker)
   
  if __name__ == '__main__':
      rospy.init_node('marker_array_test', anonymous=True)
      rospy.sleep(1)
   
      marker_pub = rospy.Publisher("/hand_l", MarkerArray, queue_size = 10)
      marker_array = MarkerArray()
      
      spheres2markers(marker_array)
      spheres2markers(marker_array)
      
      while not rospy.is_shutdown():
          marker_pub.publish(marker_array)
          rospy.rostime.wallsleep(1.0)

清空显示的所有marker §

• 使用DELETEALL action

from visualization_msgs.msg import Marker, MarkerArray
...
marker_array_msg = MarkerArray
...
marker = Marker()
marker.id = 0
marker.action = Marker.DELETEALL
marker_array_msg.markers.clear()
marker_array_msg.markers.append(marker)
self.marker_array_pub.publish(marker_array_msg)
rospy.sleep(0.2)

故障排除 §

错误提示：Global Status: Error -> Fixed Frame, Unknown frame map §

• 缺少一个作为基准的静态坐标系，解决方案：
  • rosrun tf static_transform_publisher 0 0 0 0 0 0 map base_link 50
  • 如果subscribe了其它topic，在Global Options->Fixed Frame中看有没有可以选择的坐标系

错误提示：读取libmoveit_motion_planning_rviz_plugin.so时找不到库 §

• 错误日志

  [ERROR] [1678130372.698112823]: PluginlibFactory: The plugin for class 'moveit_rviz_plugin/MotionPlanning' failed to load.  Error: Failed to load library /opt/ros/noetic/lib/libmoveit_motion_planning_rviz_plugin.so. Make sure that you are calling the PLUGINLIB_EXPORT_CLASS macro in the library code, and that names are consistent between this macro and your XML. Error string: Could not load library (Poco exception = libmoveit_background_processing.so.1.1.11: cannot open shared object file: No such file or directory)
  

• 解决方案

  sudo apt-get update
  sudo apt-get dist-upgrade