MoveIt和Gazebo的roslaunch文件以及通信
研究roslaunch文件中启动了什么以及相互通信使用的话题和配置
arg/param/rosparam的介绍在这里
- 主题
- robot model
- robot description
- joint state publisher
- use_gui
- robot state publisher
- gazebo
- gazebo empty world
- spawn model
- controller
- moveit
- rviz
- robot model
robot model
robot description
分urdf和xacro两种
"robot_description" textfile="$(find mastering_ros_robot_description_pkg)/urdf/pan_tilt.urdf" />xacro需要调用xacro.py转换一下
<param name="robot_description" command="$(find xacro)/xacro.py $(find mastering_ros_robot_description_pkg)/urdf/seven_dof_arm.xacro" />xacro除了建立机器人几何模型，还可以添加一系列传动接口、控制接口甚至可以添加虚拟摄像机读取gazebo中的机器人图像点云等等，urdf中允许配置传感器话题，一旦urdf被加载到gazebo，就会开始发布话题。
joint state publisher
默认发送关节值到/joint_states这个话题，数据来源是Joint State Interfaces，这个接口会从编码器中读取机器人的关节值，由joint_state_controller来控制发布的速率。remap可以改变话题名
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
<remap from="/joint_states" to="/seven_dof_arm/joint_states"/>
node>如果没有真实机器人或者gazebo，可以在node中添加
<rosparam param="/source_list">[/move_group/fake_controller_joint_states]rosparam>让moveit监听fake_controller发回的关节角
use_gui
设置为true，就可以跳出一个GUI框，关节角运动范围是从URDF中读取到的，拖动进度条可以改变机器人的关节值，让机器人运动。
<param name="use_gui" value="true"/>如果在roslaunch文件中，则对全局有效，上面这句代码也可以放置在节点内，比如
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
<param name="use_gui" value="true"/>
node>那么该参数仅在节点内有效，参数服务器中显示的是/node_name/param_name，本例中是/joint_state_publisher/use_gui
robot state publisher
读取/joint_states的数据然后发布到/tf和/tf_static
"robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" /> gazebo
gazebo empty world
调用gazebo_ros生成一个空的世界模型，如果跑的时候出错了，可能是安装的时候模型不全，去下一个完整的模型包放到主目录的.gazebo目录下models文件夹里就好了
<arg name="paused" default="false"/>
<arg name="use_sim_time" default="true"/>
<arg name="gui" default="true"/>
<arg name="headless" default="false"/>
<arg name="debug" default="false"/>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="debug" value="$(arg debug)" />
<arg name="gui" value="$(arg gui)" />
<arg name="paused" value="$(arg paused)"/>
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
<arg name="headless" value="$(arg headless)"/>
include>spawn model
把模型加载到gazebo，arg中-urdf是说要解析的模型是urdf类型的，-model后面跟的是加载的模型的名字，-param是文件来源，来自参数服务器的robot_description参数
"urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-urdf -model seven_dof_arm -param robot_description"/> controller
要使gazebo机器人能够运动，要在urdf中添加相应的虚拟硬件接口，hardwareInterface除了position还有velocity、effort等。
<xacro:macro name="transmission_block" params="joint_name">
<transmission name="tran1">
<type>transmission_interface/SimpleTransmissiontype>
<joint name="${joint_name}">
<hardwareInterface>PositionJointInterfacehardwareInterface>
joint>
<actuator name="motor1">
<mechanicalReduction>1mechanicalReduction>
actuator>
transmission>
xacro:macro>为了控制机器人，需要添加控制接口
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/seven_dof_armrobotNamespace>
plugin>
gazebo>控制器的参数一般设在yaml文件中，第一句是命名空间，joint state controller控制关节值发布速率，joint position controller是位置控制，同时设置了PID。
seven_dof_arm:
# Publish all joint states -----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
# Position Controllers ---------------------------------------
joint1_position_controller:
type: position_controllers/JointPositionController
joint: shoulder_pan_joint
pid: {p: 100.0, i: 0.01, d: 10.0}
joint2_position_controller:
type: position_controllers/JointPositionController
joint: shoulder_pitch_joint
pid: {p: 100.0, i: 0.01, d: 10.0}
joint3_position_controller:
type: position_controllers/JointPositionController
joint: elbow_roll_joint
pid: {p: 100.0, i: 0.01, d: 10.0}
joint4_position_controller:
type: position_controllers/JointPositionController
joint: elbow_pitch_joint
pid: {p: 100.0, i: 0.01, d: 10.0}
joint5_position_controller:
type: position_controllers/JointPositionController
joint: wrist_roll_joint
pid: {p: 100.0, i: 0.01, d: 10.0}
joint6_position_controller:
type: position_controllers/JointPositionController
joint: wrist_pitch_joint
pid: {p: 100.0, i: 0.01, d: 10.0}
joint7_position_controller:
type: position_controllers/JointPositionController
joint: gripper_roll_joint
pid: {p: 100.0, i: 0.01, d: 10.0}
yaml文件需要先加载到参数服务器才能使用
<rosparam file="$(find seven_dof_arm_gazebo)/config/seven_dof_arm_gazebo_control.yaml" command="load"/>生成控制器才能控制gazebo中机器人
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" ns="/seven_dof_arm" args="joint_state_controller
joint1_position_controller
joint2_position_controller
joint3_position_controller
joint4_position_controller
joint5_position_controller
joint6_position_controller
joint7_position_controller"/>加载控制器以后，rostopic中会出现这个话题，接收命令
/seven_dof_arm/joint1_position_controller/command通过向该话题发布命令，gazebo中的机器人就可以运动了
rostopic pub /seven_dof_arm/joint4_position_controller/command std_msgs/Float64 1.0
moveit
moveit通过FollowJointTrajectoryAction与机器人控制器通信，move_node会初始化一个client，server是跑在机器人上或者Gazebo上的。
setup assistant可以创建moveit配置包
roslaunch moveit_setup_assistant setup_assistant.launchplanning scene monitor负责机器人自身位姿（从/joint_states话题读取）和各种传感器输入
首先要启动planning context
<include file="$(find seven_dof_arm_config)/launch/planning_context.launch">
<arg name="load_robot_description" value="true"/>
include>下面是planning_context.launch，加载机器人和环境
<launch>
<arg name="load_robot_description" default="false"/>
<arg name="robot_description" default="robot_description"/>
<param if="$(arg load_robot_description)" name="$(arg robot_description)" command="$(find xacro)/xacro.py '$(find mastering_ros_robot_description_pkg)/urdf/seven_dof_arm.xacro'"/>
<param name="$(arg robot_description)_semantic" textfile="$(find seven_dof_arm_config)/config/seven_dof_arm.srdf" />
<group ns="$(arg robot_description)_planning">
<rosparam command="load" file="$(find seven_dof_arm_config)/config/joint_limits.yaml"/>
group>
<group ns="$(arg robot_description)_kinematics">
<rosparam command="load" file="$(find seven_dof_arm_config)/config/kinematics.yaml"/>
group>
launch>然后启动move group
<include file="$(find seven_dof_arm_config)/launch/move_group.launch">
<arg name="allow_trajectory_execution" value="true"/>
<arg name="fake_execution" value="true"/>
<arg name="info" value="true"/>
<arg name="debug" value="$(arg debug)"/>
include>move_group.launch文件，默认使用OMPL库进行规划
<launch>
<include file="$(find seven_dof_arm_config)/launch/planning_context.launch" />
<arg name="debug" default="false" />
<arg unless="$(arg debug)" name="launch_prefix" value="" />
<arg if="$(arg debug)" name="launch_prefix"
value="gdb -x $(find seven_dof_arm_config)/launch/gdb_settings.gdb --ex run --args" />
<arg name="info" default="$(arg debug)" />
<arg unless="$(arg info)" name="command_args" value="" />
<arg if="$(arg info)" name="command_args" value="--debug" />
<arg name="allow_trajectory_execution" default="true"/>
<arg name="fake_execution" default="false"/>
<arg name="max_safe_path_cost" default="1"/>
<arg name="jiggle_fraction" default="0.05" />
<arg name="publish_monitored_planning_scene" default="true"/>
<include ns="move_group" file="$(find seven_dof_arm_config)/launch/planning_pipeline.launch.xml">
<arg name="pipeline" value="ompl" />
include>
<include ns="move_group" file="$(find seven_dof_arm_config)/launch/trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
<arg name="moveit_manage_controllers" value="true" />
<arg name="moveit_controller_manager" value="seven_dof_arm" unless="$(arg fake_execution)"/>
<arg name="moveit_controller_manager" value="fake" if="$(arg fake_execution)"/>
include>
<include ns="move_group" file="$(find seven_dof_arm_config)/launch/sensor_manager.launch.xml" if="$(arg allow_trajectory_execution)">
<arg name="moveit_sensor_manager" value="seven_dof_arm" />
include>
<node name="move_group" launch-prefix="$(arg launch_prefix)" pkg="moveit_ros_move_group" type="move_group" respawn="false" output="screen" args="$(arg command_args)">
<env name="DISPLAY" value="$(optenv DISPLAY :0)" />
<param name="allow_trajectory_execution" value="$(arg allow_trajectory_execution)"/>
<param name="max_safe_path_cost" value="$(arg max_safe_path_cost)"/>
<param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
<param name="planning_scene_monitor/publish_planning_scene" value="$(arg publish_monitored_planning_scene)" />
<param name="planning_scene_monitor/publish_geometry_updates" value="$(arg publish_monitored_planning_scene)" />
<param name="planning_scene_monitor/publish_state_updates" value="$(arg publish_monitored_planning_scene)" />
<param name="planning_scene_monitor/publish_transforms_updates" value="$(arg publish_monitored_planning_scene)" />
node>
launch>其中，下面这边是生成控制器
<include ns="move_group" file="$(find seven_dof_arm_config)/launch/trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
<arg name="moveit_manage_controllers" value="true" />
<arg name="moveit_controller_manager" value="seven_dof_arm" unless="$(arg fake_execution)"/>
<arg name="moveit_controller_manager" value="fake" if="$(arg fake_execution)"/>
include>trajectory_execution.launch.xml如下
<launch>
<arg name="moveit_manage_controllers" default="true"/>
<param name="moveit_manage_controllers" value="$(arg moveit_manage_controllers)"/>
<param name="trajectory_execution/allowed_execution_duration_scaling" value="1.2"/>
<param name="trajectory_execution/allowed_goal_duration_margin" value="0.5"/>
<param name="trajectory_execution/allowed_start_tolerance" value="0.01"/>
<arg name="moveit_controller_manager" default="seven_dof_arm" />
<include file="$(find seven_dof_arm_config)/launch/$(arg moveit_controller_manager)_moveit_controller_manager.launch.xml" />
launch>根据fake execution会选择不同的配置文件
fake moveit controller manage如下
<launch>
<param name="moveit_controller_manager" value="moveit_fake_controller_manager/MoveItFakeControllerManager"/>
<rosparam file="$(find seven_dof_arm_config)/config/fake_controllers.yaml"/>
launch>其中fake_controllers.yaml如下，不需要配置控制类型等
controller_list:
- name: fake_arm_controller
joints:
- shoulder_pan_joint
- shoulder_pitch_joint
- elbow_roll_joint
- elbow_pitch_joint
- wrist_roll_joint
- wrist_pitch_joint
- gripper_roll_joint
- name: fake_gripper_controller
joints:
- finger_joint1如果不是fake execution，加载robot moveit controller manager
<launch>
<arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
<param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
<arg name="use_controller_manager" default="true" />
<param name="use_controller_manager" value="$(arg use_controller_manager)" />
<rosparam file="$(find seven_dof_arm_config)/config/controllers.yaml"/>
launch>controllers.yaml如下
controller_manager_ns: controller_manager
controller_list:
- name: seven_dof_arm/seven_dof_arm_joint_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- shoulder_pan_joint
- shoulder_pitch_joint
- elbow_roll_joint
- elbow_pitch_joint
- wrist_roll_joint
- wrist_pitch_joint
- gripper_roll_joint
- name: seven_dof_arm/gripper_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- finger_joint1
- finger_joint2最后启动rviz
<include file="$(find seven_dof_arm_config)/launch/moveit_rviz.launch">
<arg name="config" value="true"/>
<arg name="debug" value="$(arg debug)"/>
include>moveit_rviz.launch的介绍在下面rviz中。
rviz
启动rviz，-d参数是display config file load，加载一些显示参数，required参数不知道是干啥的，回头再研究。
"rviz" pkg="rviz" type="rviz" args="-d $(find mastering_ros_robot_description_pkg)/urdf.rviz" required="true" /> -f可以指定fixed frame
rosrun rviz rviz -f /rgbd_camera_optical_framemoveit_rviz.launch文件
anon是取一个匿名的节点名，config = true则选择自定义的moveit.rviz作为参数设置
<launch>
name="debug" default="false" />
"$(arg debug)" name="launch_prefix" value="" />
if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
name="config" default="false" />
"$(arg config)" name="command_args" value="" />
if="$(arg config)" name="command_args" value="-d $(find seven_dof_arm_config)/launch/moveit.rviz" />
name="$(anon rviz)" launch-prefix="$(arg launch_prefix)" pkg="rviz" type="rviz" respawn="false"
args="$(arg command_args)" output="screen">
"load" file="$(find seven_dof_arm_config)/config/kinematics.yaml"/>
launch>