Revision history [back]

I actually found a solution for this after messing around some more with the launch files and doing research on how they work and such. I was able to get a simulation running with 2 independent arms with all the topics and services and nodes correctly duplicated and under the right namespace for each arm. I also had to create a second version of the URDF files just for convenience sake so as to work with this package easier. I gave them a _2 suffix.

Here is the resulting code for the initial launch file, in the open_manipulator_gazebo package:

<launch>
  <!-- These are the arguments you can pass this launch file, for example paused:=true -->
  <arg name="use_robot_name" default="open_manipulator"
       doc="Must match the robotNamespace tag in the gazebo description file"/>

  <arg name="use_robot_name_2" default="open_manipulator_2"
       doc="Must match the robotNamespace tag in the gazebo description file"/>

  <arg name="paused" default="true"/>
  <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 open_manipulator_gazebo)/worlds/empty.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>


  <!-- Load the URDF into the ROS Parameter Server -->
  <param name="robot_description"
   command="$(find xacro)/xacro --inorder '$(find open_manipulator_description)/urdf/open_manipulator.urdf.xacro'"/>


  <!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
     args="-urdf -model open_manipulator -z 0.0 -param robot_description"/>

  <!-- ros_control robotis manipulator launch file -->
  <include file="$(find open_manipulator_gazebo)/launch/open_manipulator_controller.launch">
    <arg name="use_robot_name" value="$(arg use_robot_name)"/>
  </include>

    <!-- Load the URDF into the ROS Parameter Server -->
  <param name="robot_description_2"
   command="$(find xacro)/xacro --inorder '$(find open_manipulator_description)/urdf/open_manipulator_2.urdf.xacro'"/>


  <!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
  <node name="urdf_spawner_2" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
     args="-urdf -model open_manipulator_2 -y 1.0 -param robot_description_2"/>

  <!-- ros_control robotis manipulator launch file -->
  <include file="$(find open_manipulator_gazebo)/launch/open_manipulator_controller.launch">
    <arg name="use_robot_name" value="$(arg use_robot_name_2)"/>
  </include>

</launch>

I changed the open_manipulator_controller.launch file back to the original version, so it is unchanged from what is in the open_manipulator_gazebo package:

<launch>
  <arg name="use_robot_name"         default="open_manipulator"/>

  <!-- Load joint controller configurations from YAML file to parameter server -->
  <rosparam file="$(find open_manipulator_gazebo)/config/open_manipulator_controller.yaml" command="load"
            ns="$(arg use_robot_name)"/>

  <!-- load the controllers -->
  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
  output="screen" ns="$(arg use_robot_name)" args="joint_state_controller
                                               joint1_position
                                               joint2_position
                                               joint3_position
                                               joint4_position
                                               gripper_position
                                               gripper_sub_position"/>

  <!-- Run gripper sub position publisher -->
  <node name="gripper_sub_publisher" pkg="open_manipulator_gazebo" type="gripper_sub_publisher" output="screen" ns="$(arg use_robot_name)"/>

</launch>

As for the code that starts up the move_group controllers and the services that allow for control of the inverse kinematics using moveit, I have the following initial launch file in the open_manipulator_controller package:

<launch>
  <arg name="robot_name"         default="open_manipulator"/>
  <arg name="robot_name_2"         default="open_manipulator_2"/>
  <arg name="robot_description" default="robot_description"/>
  <arg name="robot_description_2" default="robot_description_2"/>
  <arg name="dynamixel_usb_port"     default="/dev/ttyUSB0"/>
  <arg name="dynamixel_baud_rate"    default="1000000"/>

  <arg name="control_period"         default="0.010"/>

  <arg name="use_platform"           default="false"/>

  <arg name="use_moveit"             default="true"/>
  <arg name="planning_group_name"    default="arm"/>
  <arg name="moveit_sample_duration" default="0.050"/>
  <arg name="debug"                  default="false" />

  <group if="$(arg use_moveit)">
    <include file="$(find open_manipulator_controller)/launch/open_manipulator_moveit.launch">
      <arg name="robot_name"      value="$(arg robot_name)"/>
      <arg name="sample_duration" value="$(arg moveit_sample_duration)"/>
      <arg name="robot_description" value="$(arg robot_description)"/>
    </include>
    <include file="$(find open_manipulator_controller)/launch/open_manipulator_moveit.launch">
      <arg name="robot_name"      value="$(arg robot_name_2)"/>
      <arg name="sample_duration" value="$(arg moveit_sample_duration)"/>
      <arg name="robot_description" value="$(arg robot_description_2)"/>
    </include>
  </group>

  <node name="$(arg robot_name)" pkg="open_manipulator_controller" type="open_manipulator_controller" output="screen" args="$(arg dynamixel_usb_port) $(arg dynamixel_baud_rate)">
      <param name="using_platform"       value="$(arg use_platform)"/>
      <param name="using_moveit"         value="$(arg use_moveit)"/>
      <param name="planning_group_name"  value="$(arg planning_group_name)"/>
      <param name="control_period"       value="$(arg control_period)"/>
      <param name="moveit_sample_duration"  value="$(arg moveit_sample_duration)"/>
  </node>

    <node name="$(arg robot_name_2)" pkg="open_manipulator_controller" type="open_manipulator_controller" output="screen" args="$(arg dynamixel_usb_port) $(arg dynamixel_baud_rate)">
      <param name="using_platform"       value="$(arg use_platform)"/>
      <param name="using_moveit"         value="$(arg use_moveit)"/>
      <param name="planning_group_name"  value="$(arg planning_group_name)"/>
      <param name="control_period"       value="$(arg control_period)"/>
      <param name="moveit_sample_duration"  value="$(arg moveit_sample_duration)"/>
  </node>
  <!-- Run Rviz and load the default config to see the state of the move_group node -->
  <!--<include file="$(find open_manipulator_moveit)/launch/moveit_rviz.launch">
    <arg name="config" value="true"/>
    <arg name="debug" value="$(arg debug)"/>
  </include>-->

</launch>

I have commented out the part that starts rviz for myself because I do not need that. This file calls a few launch files which I have modified as well to correctly duplicate the topics, services and nodes and put them under the correct namespaces for each arm.

here is the modified open_manipulator_moveit.launch file in the open_manipulator_controller package:

<launch>
  <arg name="robot_name"/>
  <arg name="robot_description"/>
  <arg name="sample_duration"        default="0.010"/>
  <arg name="debug"                  default="false" />

  <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
  <include file="$(find open_manipulator_moveit)/launch/planning_context.launch">
    <arg name="load_robot_description" value="true"/>
    <arg name="robot_name" value="$(arg robot_name)"/>
    <arg name="robot_description" value="$(arg robot_description)"/>
  </include>

  <!-- We do not have a robot connected, so publish fake joint states -->
  <node name="$(arg robot_name)_joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
    <param name="/use_gui" value="false"/>
    <rosparam param="source_list" subst_value="true">["$(arg robot_name)/joint_states"]</rosparam>
  </node>

  <!-- Given the published joint states, publish tf for the robot links -->
  <node name="$(arg robot_name)_robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />

  <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
  <include file="$(find open_manipulator_moveit)/launch/move_group.launch">
    <arg name="allow_trajectory_execution" value="false"/>
    <arg name="fake_execution" value="false"/>
    <arg name="info" value="false"/>
    <arg name="debug" value="$(arg debug)"/>
    <arg name="sample_duration" value="$(arg sample_duration)"/>
    <arg name="robot_name" value="$(arg robot_name)"/>
    <arg name="robot_description" value="$(arg robot_description)"/>
  </include>

  <!-- Run Rviz and load the default config to see the state of the move_group node -->
  <!--<include file="$(find open_manipulator_moveit)/launch/moveit_rviz.launch">
    <arg name="config" value="true"/>
    <arg name="debug" value="$(arg debug)"/>
  </include>-->

</launch>

Next I slightly modified plannext_context.launch in the open_manipulator_moveit package:

<launch>
  <!-- By default we do not overwrite the URDF. Change the following to true to change the default behavior -->
  <arg name="load_robot_description" default="false"/>
  <arg name="robot_name"/>
  <!-- The name of the parameter under which the URDF is loaded -->
  <arg name="robot_description"/>

  <!-- Load universal robot description format (URDF) -->
  <param if="$(arg load_robot_description)" name="$(arg robot_description)" command="xacro --inorder  '$(find open_manipulator_description)/urdf/$(arg robot_name).urdf.xacro'"/>

  <!-- The semantic description that corresponds to the URDF -->
  <param name="$(arg robot_description)_semantic" textfile="$(find open_manipulator_moveit)/config/$(arg robot_name).srdf" />

  <!-- Load updated joint limits (override information from URDF) -->
  <group ns="$(arg robot_description)_planning">
    <rosparam command="load" file="$(find open_manipulator_moveit)/config/joint_limits.yaml"/>
  </group>

  <!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->
  <group ns="$(arg robot_description)_kinematics">
    <rosparam command="load" file="$(find open_manipulator_moveit)/config/kinematics.yaml"/>
  </group>

</launch>

And finally I slightly modified move_group.launch in the open_manipulator_moveit package:

<launch>

  <arg name="sample_duration" default="0.010"/>
  <arg name="robot_name" />
  <arg name="robot_description"/>

 <include file="$(find open_manipulator_moveit)/launch/planning_context.launch">
    <arg name="load_robot_description" value="false"/>
    <arg name="robot_name" value="$(arg robot_name)"/>
    <arg name="robot_description" value="$(arg robot_description)"/>
  </include>




  <!-- GDB Debug Option -->
  <arg name="debug" default="false" />
  <arg unless="$(arg debug)" name="launch_prefix" value="" />
  <arg     if="$(arg debug)" name="launch_prefix"
       value="gdb -x $(find open_manipulator_moveit)/launch/gdb_settings.gdb --ex run --args" />

  <!-- Verbose Mode Option -->
  <arg name="info" default="$(arg debug)" />
  <arg unless="$(arg info)" name="command_args" value="" />
  <arg     if="$(arg info)" name="command_args" value="--debug" />

  <!-- move_group settings -->
  <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"/>

  <arg name="capabilities" default=""/>
  <arg name="disable_capabilities" default=""/>
  <!-- load these non-default MoveGroup capabilities (space seperated) -->
  <!--
  <arg name="capabilities" value="
                a_package/AwsomeMotionPlanningCapability
                another_package/GraspPlanningPipeline
                " />
  -->

  <!-- inhibit these default MoveGroup capabilities (space seperated) -->
  <!--
  <arg name="disable_capabilities" value="
                move_group/MoveGroupKinematicsService
                move_group/ClearOctomapService
                " />
  -->

  <!-- Planning Functionality -->
  <include ns="$(arg robot_name)/move_group" file="$(find open_manipulator_moveit)/launch/planning_pipeline.launch.xml">
    <arg name="pipeline" value="ompl" />
    <arg name="sample_duration" value="$(arg sample_duration)"/>
  </include>

  <!-- Trajectory Execution Functionality -->
  <include ns="$(arg robot_name)/move_group" file="$(find open_manipulator_moveit)/launch/trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
    <arg name="moveit_manage_controllers" value="true" />
    <arg name="moveit_controller_manager" value="$(arg robot_name)" unless="$(arg fake_execution)"/>
    <arg name="moveit_controller_manager" value="fake" if="$(arg fake_execution)"/>
  </include>

  <!-- Sensors Functionality -->
  <include ns="$(arg robot_name)/move_group" file="$(find open_manipulator_moveit)/launch/sensor_manager.launch.xml" if="$(arg allow_trajectory_execution)">
    <arg name="moveit_sensor_manager" value="$(arg robot_name)" />
  </include>

  <!-- Start the actual move_group node/action server -->
  <node name="$(arg robot_name)_move_group" launch-prefix="$(arg launch_prefix)" pkg="moveit_ros_move_group" type="move_group" respawn="false" output="screen" args="$(arg command_args)">
    <!-- Set the display variable, in case OpenGL code is used internally -->
    <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="capabilities" value="$(arg capabilities)"/>
    <param name="disable_capabilities" value="$(arg disable_capabilities)"/>


    <!-- Publish the planning scene of the physical robot so that rviz plugin can know actual robot -->
    <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>

After all those files have been modified, in two terminals, run the following commands: (assuming you named the initial launch files in each package test.launch) $ roslaunch open_manipulator_gazebo test.launch $ roslaunch open_manipulator_controller test.launch

That should bring up all the necessary topics to control each arm on it's own using inverse kinematics with the moveit package.

If you want two GUI's to come up for each arm, change the open_manipulator_controller_gui.launch file in the open_manipulator_controller_gui package to the following:

<launch>
  <arg name="robot_name"   default="open_manipulator"/>
  <arg name="robot_name_2"   default="open_manipulator_2"/>
  <arg name="end_effector" default="gripper"/>

  <group ns="$(arg robot_name)">
    <node name="control_gui" pkg="open_manipulator_control_gui" type="open_manipulator_control_gui" output="screen">
      <remap from="kinematics_pose" to="$(arg end_effector)/kinematics_pose"/>
    </node>
  </group>
  <group ns="$(arg robot_name_2)">
    <node name="control_gui" pkg="open_manipulator_control_gui" type="open_manipulator_control_gui" output="screen">
      <remap from="kinematics_pose" to="$(arg end_effector)/kinematics_pose"/>
    </node>
  </group>
</launch>

Each GUI will control one of the arms. You can get the namespace of the specific one in the c++ code of their GUI and use it to subscribe to specific topics for each arm based on the namepsace. Following code in qnode.cpp under the open_manipulator_controller_gui package in the init function where the other subscribers are.

std::cout << "namespace is: " << n.getNamespace() << std::endl;
  if(n.getNamespace() == "//open_manipulator")
  {
    unity_control_sub = n.subscribe("/rhs/cmd_velocity", 10, &QNode::unityControlCallback, this);
  }
  else if(n.getNamespace() == "//open_manipulator_2")
  {
    unity_control_sub = n.subscribe("/lhs/cmd_velocity", 10, &QNode::unityLeftControlCallback, this);
  }

That last part was just an extra I thought would be helpful. I hope this answer helps anyone else who was having the same issue as I was.