launch 2 independently controlled ur5 arms in gazebo
I am running ROS Kinetic on Ubuntu 16.04. I am able to launch a single ur5 arm in Gazebo and I am also able to control it from a simple ROS node.
My launchonearm.launch file looks like this. I actually just pulled it directly from the ros-industrial/universalrobot repository (https://github.com/ros-industrial/universalrobot/blob/kinetic-devel/ur_gazebo/launch/ur5.launch).
<?xml version="1.0"?>
<launch>
<arg name="limited" default="false"/>
<arg name="paused" default="false"/>
<arg name="gui" default="true"/>
<!-- startup simulated world -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" default="worlds/empty.world"/>
<arg name="paused" value="$(arg paused)"/>
<arg name="gui" value="$(arg gui)"/>
</include>
<!-- send robot urdf to param server -->
<include file="$(find ur_description)/launch/ur5_upload.launch">
<arg name="limited" value="$(arg limited)"/>
</include>
<!-- push robot_description to factory and spawn robot in gazebo -->
<node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot -z 0.1" respawn="false" output="screen" />
<include file="$(find ur_gazebo)/launch/controller_utils.launch"/>
<rosparam file="$(find ur_gazebo)/controller/arm_controller_ur5.yaml" command="load"/>
<node name="arm_controller_spawner" pkg="controller_manager" type="controller_manager" args="spawn arm_controller" respawn="false" output="screen"/>
</launch>
And here is my simple move_arm.cpp file that successfully makes the arm move in Gazebo. It creates a JointTrajectory Message and publishes that message to the "/arm_controller/command" Topic.
#include <ros/ros.h>
#include <trajectory_msgs/JointTrajectory.h>
#include <cstdlib>
using namespace std;
float PI = 3.14159;
int main(int argc, char** argv) {
ros::init(argc, argv, "move_arm");
ros::NodeHandle nh;
ros::Publisher publisher_arm_command = nh.advertise<trajectory_msgs::JointTrajectory>( "/arm_controller/command",
10,
true );
// create arm command message
float joint_velocity = 0.001;
float joint_acceleration = 0.001;
float joint_effort = 1.0;
trajectory_msgs::JointTrajectory message_arm_command;
message_arm_command.joint_names.resize(6);
message_arm_command.joint_names[0] = "shoulder_pan_joint";
message_arm_command.joint_names[1] = "shoulder_lift_joint";
message_arm_command.joint_names[2] = "elbow_joint";
message_arm_command.joint_names[3] = "wrist_1_joint";
message_arm_command.joint_names[4] = "wrist_2_joint";
message_arm_command.joint_names[5] = "wrist_3_joint";
trajectory_msgs::JointTrajectoryPoint point1, point2;
point1.positions.resize(6);
point1.positions[0] = 0.0 * (PI / 180.0);
point1.positions[1] = -90.0 * (PI / 180.0);
point1.positions[2] = 0.0 * (PI / 180.0);
point1.positions[3] = 0.0 * (PI / 180.0);
point1.positions[4] = 0.0 * (PI / 180.0);
point1.positions[5] = 0.0 * (PI / 180.0);
point1.velocities.resize(6);
point1.velocities[0] = joint_velocity;
point1.velocities[1] = joint_velocity;
point1.velocities[2] = joint_velocity;
point1.velocities[3] = joint_velocity;
point1.velocities[4] = joint_velocity;
point1.velocities[5] = joint_velocity;
point1.accelerations.resize(6);
point1.accelerations[0] = joint_acceleration;
point1.accelerations[1] = joint_acceleration;
point1.accelerations[2] = joint_acceleration;
point1.accelerations[3] = joint_acceleration;
point1.accelerations[4] = joint_acceleration;
point1.accelerations[5] = joint_acceleration;
point1.effort.resize(6);
point1.effort[0] = joint_effort;
point1.effort[1] = joint_effort;
point1.effort[2] = joint_effort;
point1.effort[3] = joint_effort;
point1.effort[4] = joint_effort;
point1.effort[5] = joint_effort;
ros::Duration five_seconds(5.0);
point1.time_from_start = five_seconds;
point2.positions.resize(6);
point2.positions[0] = -90.0 * (PI / 180.0);
point2.positions[1] = -90.0 * (PI / 180.0);
point2.positions[2] = 90.0 * (PI / 180.0);
point2.positions[3] = -90.0 * (PI / 180.0);
point2.positions[4] = -90.0 * (PI / 180.0);
point2.positions[5] = 0.0 * (PI / 180.0);
point2.velocities.resize(6);
point2.velocities[0] = joint_velocity;
point2.velocities[1] = joint_velocity;
point2.velocities[2] = joint_velocity;
point2.velocities[3] = joint_velocity;
point2.velocities[4] = joint_velocity;
point2.velocities[5] = joint_velocity;
point2.accelerations.resize(6);
point2.accelerations[0] = joint_acceleration;
point2.accelerations[1] = joint_acceleration;
point2.accelerations[2] = joint_acceleration;
point2.accelerations[3] = joint_acceleration;
point2.accelerations[4] = joint_acceleration;
point2.accelerations[5] = joint_acceleration;
point2.effort.resize(6);
point2.effort[0] = joint_effort;
point2.effort[1] = joint_effort;
point2.effort[2] = joint_effort;
point2.effort[3] = joint_effort;
point2.effort[4] = joint_effort;
point2.effort[5] = joint_effort;
ros::Duration ten_seconds(10.0);
point2.time_from_start = ten_seconds;
message_arm_command.points.resize(2);
message_arm_command.points[0] = point1;
message_arm_command.points[1] = point2;
publisher_arm_command.publish(message_arm_command);
// don't just spin forever. wait for the arm motion to stop.
ros::spin();
return(0);
}
Now I just want to add a second arm to my Gazebo launch file and configure the ros_control topics to be in different namespaces so that I can send control commands to both arms independently. But I can't figure out how to do that.
I tried this launchtwoarms.launch file. I removed the controller spawning portions and it still doesn't work. It actually puts 2 arms into Gazebo, but I get errors saying that the entire list of controller_manager services are already advertised. And I also can't figure out how to launch the 2 separate controllers using two different namespaces.
<?xml version="1.0"?>
<launch>
<arg name="limited" default="false"/>
<arg name="paused" default="false"/>
<arg name="gui" default="true"/>
<!-- startup simulated world -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" default="worlds/empty.world"/>
<arg name="paused" value="$(arg paused)"/>
<arg name="gui" value="$(arg gui)"/>
</include>
<!-- send robot urdf to param server -->
<include file="$(find ur_description)/launch/ur5_upload.launch">
<arg name="limited" value="$(arg limited)"/>
</include>
<!-- first robot arm -->
<node name="spawn_gazebo_model1" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot1 -z 0.1" respawn="false" output="screen" />
<!-- second robot arm -->
<node name="spawn_gazebo_model2" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot2 -z 0.1 -y 1.0" respawn="false" output="screen" />
</launch>
I tried to get rid of the controller_manager service errors by adding ns="robot1" and ns="robot2" to the nodes in the above launch file, but that failed to even put the arms into Gazebo.
I have no idea what to try next. I thought I understood namespaces and Topic remapping, but it seems like the Topic names for ros_controllers may be hard coded or something?
Asked by Kurt Leucht on 2018-02-14 14:06:14 UTC
Comments
Many of the xacro files seem to have a ${prefix} variable before most of the joint names and such, so that may be the answer I'm looking for. But I can't figure out how I'm supposed to configure and start up 2 separate arms with 2 separate controllers without overlap and conflicts.
Asked by Kurt Leucht on 2018-02-16 08:14:13 UTC