内容为自行测试得来,不一定准确。
ROS多机器人仿真时需要复用launch文件用来启动比如定位和状态发布等功能,复用会存在结点、话题、tf等重名的情况,但为每一个机器人修改特定的文件会很麻烦,特别是如果文件有改动的话。
对于<node>
标签可以使用ns
属性即namespace对进行区分,但是存在两种情况不会受ns
影响:
1、以/
开头的话题,这样的话题是全局名称,无论是在launch文件还是程序代码中都不会受命名空间影响,因此我觉得写话题名称的时候可以尽量不加/
。对于这种情况,可以修改代码中的话题名称删掉开头的/
,又或者额外使用<remap>
标签进行重命名。<remap>
标签经测试如果remap之后的名称不以/
开头的话是会受namespace影响的。服务名称也一样。
2、tf话题,tf话题我测试了lookupTransform()函数并不会考虑namespace(也应该没办法考虑),只会查询所传入的字符串,而且也不会解释~
字符。对于这种情况,可以修改代码,将复用会重名的tf话题通过读取参数的形式读入,并在launch文件中设置不同的tf话题避免冲突,amcl中就是使用这种方式进行设置的;也可以添加一个tf_prefix
参数,为每个tf话题名称添加前缀以区分不同机器人的tf话题,robot_state_publisher中就是使用这种方式进行设置的。
另外还发现rospy.get_param()函数无论是否在最前面添加/
字符都会认为是全局的名称,不会受到ns
的影响,但可以在最前面加~
字符表明是相对的,自动加上namespace(c++里面定义了ros::NodeHandle private_nh("~");
使用private_nh
读取参数就不需要再加~
,或许python里面也有对应写法,但我还没找到)
例如对于robot_state_publisher和gazebo可以如下对建造的模型进行复用:
<!-- 发布机器人模型信息 -->
<param name="/robot1/robot_description" command="$(find xacro)xacro '$(find description)/urdf/robot.urdf.xacro'"/>
<node ns='robot1' name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher">
<param name="tf_prefix" value="robot1" />
</node>
<param name="/robot2/robot_description" command="$(find xacro)xacro '$(find description)/urdf/robot.urdf.xacro'"/>
<node ns='robot2' name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher">
<param name="tf_prefix" value="robot2" />
</node>
<param name="/robot3/robot_description" command="$(find xacro)xacro '$(find description)/urdf/robot.urdf.xacro'"/>
<node ns='robot3' name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher">
<param name="tf_prefix" value="robot3" />
</node>
<!-- 启动 gazebo -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find description)/rviz/gazeboworld.world"/>
</include>
<!-- 在 gazebo 中显示机器人模型 -->
<node ns="robot1" pkg="gazebo_ros" type="spawn_model" name="model" args="-urdf -model /robot1 -param /robot1/robot_description -z 0.1"/>
<node ns="robot2" pkg="gazebo_ros" type="spawn_model" name="model" args="-urdf -model /robot2 -param /robot2/robot_description -x -0.8 -y 0.8 -z 0.1"/>
<node ns="robot3" pkg="gazebo_ros" type="spawn_model" name="model" args="-urdf -model /robot3 -param /robot3/robot_description -x -0.8 -y -0.8 -z 0.1"/>
其中robot_state_publisher会读取名为robot_description
的参数并进行发布,robot_description
参数是从相同的robot.urdf.xacro文件中读取的内容,因此发布的tf话题也会是一样的,从而需要tf_prefix
作为前缀进行修饰以区分。这里即使加了ns
属性也必须要用tf_prefix
进行区分,也说明ns
属性的确不影响tf话题。
添加ns
属性是为了避免<node>
结点重名,robot_state_publisher读取的参数也因此会加上ns
设置的命名空间,即会读取参数/robot1/robot_description
,因此前面设置的<param>
的名称也需要改成相应的名称。
gazebo的spawn_model
用于在gazebo 中显示机器人模型并模拟相关的传感器和控制功能,从程序输出来看,部分功能的namespace是根据ns
属性设置的,也有部分是根据args
属性内-model
对应的名称设置的(gazebo里面发布的tf话题也应该根据namespace进行了修改)。另外args
属性内-param
则需要与前面设置的<param>
名称对应,xyz则是机器人的初始位置。这里不同机器人需要修改的就是这几点。
对于单个机器人一个launch文件内往往会执行多个<node>
,那就需要包含launch文件时实现对整个launch文件中所有结点的ns
的统一修改。但<include>
标签并没有ns
属性,这时可以在所包含的launch文件中设置额外的<arg>
标签用来设置文件中<node>
的ns
。
如下实现了对amcl的复用为每个机器人不冲突地提供定位功能(我这里三个机器人用了同一张地图):
<!-- 总的launch文件-->
<!-- Run Map Server -->
<node name="map_server" pkg="map_server" type="map_server" args="$(find navigation)/maps/gazebomap.yaml"/>
<!-- Run AMCL -->
<include file="$(find navigation)/launch/amcl.launch">
<arg name='robot_name' value='robot1'/><!-- 通过该参数设置ns -->
</include>
<include file="$(find navigation)/launch/amcl.launch">
<arg name='robot_name' value='robot2'/>
</include>
<include file="$(find navigation)/launch/amcl.launch">
<arg name='robot_name' value='robot3'/>
</include>
<!-- 包含的amcl.launch文件-->
<arg name="robot_name" default=""/><!-- default为空,从而不影响单个机器人执行。另外不能通过value设置值,否则被包含之后修改会冲突报错 -->
<arg name="scan_topic" default="front_laser/scan"/><!-- 相对名称,会随命名空间改变 -->
<arg name="map_topic" default="/map"/>
<arg name="map_service" default="/static_map"/><!-- 因为使用的是统一的地图,因此不需要根据机器人名字修改,需要在开始加/作为绝对名称 -->
<arg name="global_frame" default="map"/><!-- globalframe是一致的不需要根据机器人修改,加不加/不影响 -->
<arg name="odom_frame" default="$(arg robot_name)/odom"/><!-- amcl的tf话题可以直接设置,从而可以为不同机器人设置不同话题名称避免冲突 -->
<arg name="base_frame" default="$(arg robot_name)/base_link"/>
<node ns="/$(arg robot_name)" pkg="amcl" type="amcl" name="amcl" output="screen"><!--ns不能为空,在ns最开始加上/,避免单个机器人时出现空值-->
<remap from="scan" to="$(arg scan_topic)"/>
<remap from="map" to="$(arg map_topic)"/>
<remap from="static_map" to="$(arg map_service)"/><!-- 这是服务的名称,也可以remap,原来是相对名称,会随命名空间改变 -->
<param name="odom_frame_id" value="$(arg odom_frame)"/><!-- 通过param的形式设置tf话题名称 -->
<param name="base_frame_id" value="$(arg base_frame)"/>
<param name="global_frame_id" value="$(arg global_frame)"/>
......
</node>
除了用来区分机器人的名字可以通过添加<arg>
进行设置,机器人之间其它不相同的参数也可以类似地进行设置。