Multi-robot controller-manager for different robot_descripion?
I am simulating a multi-robot system. I have a different name for the robot descriptions for different robots. When I tried to start the controller manager.
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="camera_position_controller joint_state_controller">
<remap from="robot_description" to="robot1_description" />
</node>
It gives the error:
[INFO] [WallTime: 1442100763.719533] [0.533000] Loading controller: camera_position_controller
[ERROR] [1442100763.811078289, 0.621000000]: Could not find parameter robot_description on parameter server
[ERROR] [1442100763.811171897, 0.621000000]: Failed to parse urdf file
[ERROR] [1442100763.811233707, 0.621000000]: Failed to initialize the controller
[ERROR] [1442100763.811282445, 0.621000000]: Initializing controller 'camera_position_controller' failed
The .yaml file for controller params.
robot1:
# Publish all joint states -----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 10
# Position Controllers ---------------------------------------
camera_position_controller:
type: effort_controllers/JointPositionController
joint: camera_joint
pid: {p: 100.0, i: 0.01, d: 10.0}
Kindly help.
Update 1:
Here is the complete launch file.
<launch>
<!-- these are the arguments you can pass this launch file, for example
paused:=true -->
<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" />
<!-- We resume the logic in empty_world.launch, changing only the name of
the world to be launched -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find mrp_p3dx_gazebo)/worlds/mrp_p3dx.world" />
<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>
<!-- BEGIN ROBOT 1-->
<group ns="robot1">
<!-- <param name="tf_prefix" value="robot1" /> -->
<!-- Load joint controller configurations from YAML file to parameter server -->
<rosparam file="$(find mrp_p3dx_control)/config/mrp_p3dx.yaml" command="load"
ns="/robot1" />
<param name="robot_description"
command="$(find xacro)/xacro.py '$(find mrp_p3dx_description)/urdf/robot1.xacro'" />
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model"
respawn="false" output="screen" args="-urdf -x 5.5 -y 8 -z 0 -model robot1 -param robot_description" />
<node pkg="robot_state_publisher" type="state_publisher" name="robot_state_publisher">
<!-- <remap from="robot_description" to="robot_description" /> -->
<param name="publish_frequency" type="double" value="30.0"/>
<param name="tf_prefix" type="string" value="robot1"/>
</node>
<!-- load the controllers -->
<node name="controller_spawner" pkg="controller_manager" type="spawner"
respawn="false" ns="/robot1" output="screen" args="camera_position_controller joint_state_controller">
</node>
<param name="publish_frequency" type="double" value="30.0"/>
</group>
<!-- BEGIN ROBOT 2-->
<group ns="robot2">
<!-- <param name="tf_prefix" value="robot2" /> -->
<param name="robot_description"
command="$(find xacro)/xacro.py '$(find mrp_p3dx_description)/urdf/robot2.xacro'" />
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model"
respawn="false" output="screen" args="-urdf -x 5 -y 5 -z 0 -model robot2
-param robot_description" />
<node pkg="robot_state_publisher" type="state_publisher" name="robot_state_publisher">
<!-- <remap from="robot_description" to="robot_description" /> -->
<param name="publish_frequency" type="double" value="30.0"/>
<param name="tf_prefix" type="string" value="robot2"/>
</node>
<param name="publish_frequency" type="double" value="30.0"/>
</group>
<node pkg="tf" type="static_transform_publisher" name="base_to_odom_r1"
args="0.0 -0.0 0.0 0.0 0.0 0.0 map robot1/odom 200" />
<node pkg="tf" type="static_transform_publisher" name="base_to_odom_r2"
args="0.0 0.0 0.0 0.0 0.0 0.0 map robot2/odom 200" />
</launch>
The changes in the .yaml file:
#robot1: ==> I commented out just the first line since I'm loading it from inside group.
The gazeboroscontrol plugin in robot1.gazebo file:
<!-- ros_control plugin -->
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotParam>/robot1/robot_description</robotParam>
<robotNamespace>/robot1</robotNamespace>
</plugin>
</gazebo>
The output:
started roslaunch server http://venky-N550JV:56204/
SUMMARY
========
PARAMETERS
* /robot1/camera_position_controller/joint: camera_joint
* /robot1/camera_position_controller/pid/d: 10.0
* /robot1/camera_position_controller/pid/i: 0.01
* /robot1/camera_position_controller/pid/p: 100.0
* /robot1/camera_position_controller/type: effort_controller...
* /robot1/joint_state_controller/publish_rate: 10
* /robot1/joint_state_controller/type: joint_state_contr...
* /robot1/publish_frequency: 30.0
* /robot1/robot_description: <?xml version="1....
* /robot1/robot_state_publisher/publish_frequency: 30.0
* /robot1/robot_state_publisher/tf_prefix: robot1
* /robot2/publish_frequency: 30.0
* /robot2/robot_description: <?xml version="1....
* /robot2/robot_state_publisher/publish_frequency: 30.0
* /robot2/robot_state_publisher/tf_prefix: robot2
* /rosdistro: indigo
* /rosversion: 1.11.9
* /use_sim_time: True
NODES
/robot2/
robot_state_publisher (robot_state_publisher/state_publisher)
urdf_spawner (gazebo_ros/spawn_model)
/robot1/
controller_spawner (controller_manager/spawner)
robot_state_publisher (robot_state_publisher/state_publisher)
urdf_spawner (gazebo_ros/spawn_model)
/
base_to_odom_r1 (tf/static_transform_publisher)
base_to_odom_r2 (tf/static_transform_publisher)
gazebo (gazebo_ros/gzserver)
gazebo_gui (gazebo_ros/gzclient)
...
process[robot1/robot_state_publisher-5]: started with pid [28714]
[ INFO] [1442304953.995199055]: Finished loading Gazebo ROS API Plugin.
Msg Waiting for master
[ INFO] [1442304953.996356337]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
process[robot1/controller_spawner-6]: started with pid [28780]
process[robot2/urdf_spawner-7]: started with pid [28789]
spawn_model script started
process[robot2/robot_state_publisher-8]: started with pid [28852]
[INFO] [WallTime: 1442304954.326212] [0.000000] Loading model xml from ros parameter
[INFO] [WallTime: 1442304954.332192] [0.000000] Waiting for service /gazebo/spawn_urdf_model
[INFO] [WallTime: 1442304954.334574] [0.000000] Calling service /gazebo/spawn_urdf_model
process[base_to_odom_r1-9]: started with pid [28905]
[INFO] [WallTime: 1442304954.467328] [0.000000] Controller Spawner: Waiting for service controller_manager/load_controller
spawn_model script started
process[base_to_odom_r2-10]: started with pid [28951]
[ INFO] [1442304954.524209209, 0.022000000]: waitForService: Service [/gazebo/set_physics_properties] is now available.
Warning [ModelDatabase.cc:205] Unable to connect to model database using [http://gazebosim.org/models//database.config]. Only locally installed models will be available.
[INFO] [WallTime: 1442304954.660209] [0.000000] Loading model xml from ros parameter
[INFO] [WallTime: 1442304954.666466] [0.000000] Waiting for service /gazebo/spawn_urdf_model
[INFO] [WallTime: 1442304954.669448] [0.159000] Calling service /gazebo/spawn_urdf_model
[ INFO] [1442304955.209184214, 0.396000000]: Laser Plugin (robotNamespace = /robot1/), Info: Using the 'robotNamespace' param: '/robot1/'
[ INFO] [1442304955.209280840, 0.396000000]: Starting Laser Plugin (ns = /robot1/)!
[ INFO] [1442304955.210238902, 0.396000000]: Laser Plugin (ns = /robot1/) <tf_prefix_>, set to "/robot1"
[ INFO] [1442304955.423941348, 0.396000000]: Camera Plugin (robotNamespace = /robot1/), Info: Using the 'robotNamespace' param: '/robot1/'
[INFO] [WallTime: 1442304955.425476] [0.396000] Spawn status: SpawnModel: Successfully spawned model
[ INFO] [1442304955.434166669, 0.396000000]: Loading gazebo_ros_control plugin
[ INFO] [1442304955.435372831, 0.396000000]: Starting gazebo_ros_control plugin in namespace: /robot1/
[ INFO] [1442304955.438227455, 0.396000000]: gazebo_ros_control plugin is waiting for model URDF in parameter [/robot1/robot_description] on the ROS param server.
[ INFO] [1442304955.441720945, 0.396000000]: Physics dynamic reconfigure ready.
[ INFO] [1442304955.550622485, 0.396000000]: Camera Plugin (ns = /robot1/) <tf_prefix_>, set to "/robot1"
[robot1/urdf_spawner-4] process has finished cleanly
[ INFO] [1442304955.784116409, 0.396000000]: Loaded gazebo_ros_control.
[ INFO] [1442304955.787507193, 0.396000000]: Starting plugin DiffDrive(ns = /robot1//)!
[ WARN] [1442304955.787602492, 0.396000000]: DiffDrive(ns = /robot1//): missing <rosDebugLevel> default is na
....
[ INFO] [1442304955.798759788, 0.396000000]: GazeboRosJointStatePublisher is going to publish joint: base_swivel_joint
[ INFO] [1442304955.798853127, 0.396000000]: GazeboRosJointStatePublisher is going to publish joint: swivel_hubcap_joint
[ INFO] [1442304955.798885306, 0.396000000]: GazeboRosJointStatePublisher is going to publish joint: base_left_wheel_joint
[ INFO] [1442304955.798916901, 0.396000000]: GazeboRosJointStatePublisher is going to publish joint: base_right_wheel_joint
[ INFO] [1442304955.798949253, 0.396000000]: GazeboRosJointStatePublisher is going to publish joint: camera_joint
[ INFO] [1442304955.798982328, 0.396000000]: Starting GazeboRosJointStatePublisher Plugin (ns = robot1/)!, parent name: robot1
[INFO] [WallTime: 1442304955.976143] [0.567000] Controller Spawner: Waiting for service controller_manager/switch_controller
[INFO] [WallTime: 1442304955.977546] [0.568000] Controller Spawner: Waiting for service controller_manager/unload_controller
[INFO] [WallTime: 1442304955.978880] [0.570000] Loading controller: camera_position_controller
[ERROR] [1442304956.057553017, 0.591000000]: Could not find parameter robot_description on parameter server
[ERROR] [1442304956.057623613, 0.591000000]: Failed to parse urdf file
[ERROR] [1442304956.057663589, 0.591000000]: Failed to initialize the controller
[ERROR] [1442304956.057695768, 0.591000000]: Initializing controller 'camera_position_controller' failed
[ INFO] [1442304956.118390925, 0.591000000]: Camera Plugin (robotNamespace = /robot2/), Info: Using the 'robotNamespace' param: '/robot2/'
[ INFO] [1442304956.175312690, 0.591000000]: Camera Plugin (ns = /robot2/) <tf_prefix_>, set to "/robot2"
[ INFO] [1442304956.412929189, 0.591000000]: Laser Plugin (robotNamespace = /robot2/), Info: Using the 'robotNamespace' param: '/robot2/'
[ INFO] [1442304956.413152759, 0.591000000]: Starting Laser Plugin (ns = /robot2/)!
[ INFO] [1442304956.414221308, 0.591000000]: Laser Plugin (ns = /robot2/) <tf_prefix_>, set to "/robot2"
[INFO] [WallTime: 1442304956.422364] [0.591000] Spawn status: SpawnModel: Successfully spawned model
[ INFO] [1442304956.422632960, 0.591000000]: Loading gazebo_ros_control plugin
[ INFO] [1442304956.422678000, 0.591000000]: Starting gazebo_ros_control plugin in namespace: /robot2/
[ INFO] [1442304956.656083017, 0.591000000]: Loaded gazebo_ros_control.
[ INFO] [1442304956.656261395, 0.591000000]: Starting plugin DiffDrive(ns = /robot2//)!
[ WARN] [1442304956.656283005, 0.591000000]: DiffDrive(ns = /robot2//): missing <rosDebugLevel> default is na
...
[ INFO] [1442304956.663448843, 0.591000000]: GazeboRosJointStatePublisher is going to publish joint: base_swivel_joint
[ INFO] [1442304956.663547451, 0.591000000]: GazeboRosJointStatePublisher is going to publish joint: swivel_hubcap_joint
[ INFO] [1442304956.663602395, 0.591000000]: GazeboRosJointStatePublisher is going to publish joint: base_left_wheel_joint
[ INFO] [1442304956.663650805, 0.591000000]: GazeboRosJointStatePublisher is going to publish joint: base_right_wheel_joint
[ INFO] [1442304956.663699109, 0.591000000]: Starting GazeboRosJointStatePublisher Plugin (ns = robot2/)!, parent name: robot2
[ERROR] [WallTime: 1442304957.061887] [0.985000] Failed to load camera_position_controller
[INFO] [WallTime: 1442304957.062539] [0.985000] Loading controller: joint_state_controller
[INFO] [WallTime: 1442304957.137361] [1.058000] Controller Spawner: Loaded controllers: joint_state_controller
[INFO] [WallTime: 1442304957.139226] [1.059000] Started controllers: joint_state_controller
[robot2/urdf_spawner-7] process has finished cleanly
Asked by webvenky on 2015-09-13 23:34:20 UTC
Answers
In my multi-robot environment I use groups/Namespaces to avoid such problems
This means all nodes,params,topics and services of the robot are started in
In this way you can easily distinguish between the different robots...
try putting a
<group ns="<namespace-name>">
... everything you would like to launch for one robot instance...
</group>
But beware you will also have to take care of the TF-prefixes for somenodes if your robots have identical link names(which is the case for most robots...)
Asked by evilBiber on 2015-09-14 02:16:27 UTC
Comments
Thanks. I did exactly what you said. But now I'm facing the exact problem that's stated in this question: http://answers.ros.org/question/198929/running-controller_manager-spawner-with-mybotrobot_description/
Asked by webvenky on 2015-09-14 13:00:20 UTC
It might be helpful to show the complete launchfile... Where is the parameter robot_description on the paramterer server?
Asked by evilBiber on 2015-09-15 02:06:47 UTC
@evilBiber I have updated the question with the complete launchfile. The robot_description resides in the corresponding robot namespace.. for example, /robot1/robot_description
Asked by webvenky on 2015-09-15 03:32:16 UTC
Did you find any solution to this? I got the same problem.
Asked by voliro on 2017-02-27 14:14:26 UTC
Comments
can you try adding
-robot_namespace robot_1
to the args of your spawn_model call?(I am not sure if that helps)Asked by evilBiber on 2015-09-16 02:17:04 UTC
Tried. It didn't work. The problem still remains.
Asked by webvenky on 2015-09-16 03:51:05 UTC