search

ros-industrial / industrial_core

ROS-Industrial core communication packages (http://wiki.ros.org/industrial_core)
156 stars 181 forks source link

Joint trajectory action in Industrial Robot Client does not initialize joint names #181

Closed srsidd closed 7 years ago

srsidd commented 7 years ago

I have multiple ABB robots, and they're name spaced in the URDF. For example the links are robot1/link1, robot1/link2, robot2/link1 and so on. The moveit configuration for simulation uses the joint_trajectory_action node from the industrial_robot_client package. However I'm running into issues because of the custom joint names.

Every time I try to execute I get a warning Controller bot1 failed with error code INVALID_JOINTS.

Doing some digging around I noticed that the source code uses a vector of strings to keep track of the joint names (joint_names_). The function industrial_utils::param::getJointNames verifies that the joints in joint_names_indeed do exist either from the ros parameter server (/controller_joint_names) or from the URDF. However I don't see where joint_names_ is getting the list of joints. It seems like joint_names_ doesn't get initialized anywhere. Is that right? Am I doing something wrong here?

srsidd commented 7 years ago

Ok more did some debugging. Turns out, param_utils calls a method getListParam(), where the joint_names_ gets populated. I don't have an explanation but adding a ros debug statement invoked the function and the joint_names_ method got populated....

I know this to be true because I would get the following warning and information messages on my terminal before adding the debug statement - 

[ WARN] [1507240994.621990377]: Unable to find user-specified joint names in 'controller_joint_names'
[ WARN] [1507240994.622470521]: Unable to find URDF joint names in 'robot_description'
[ INFO] [1507240994.622528270]: Using standard 6-DOF joint names: [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6]

And it turns out that joint_names_ was an empty vector which never got populated. Hence it could not find the joint names. But now I get the following information messages -

[ INFO] [1507242288.871588542]: Adding robot1/joint_1 to list parameter
[ INFO] [1507242288.871669512]: Adding robot1/joint_2 to list parameter
[ INFO] [1507242288.871707971]: Adding robot1/joint_3 to list parameter
[ INFO] [1507242288.871724614]: Adding robot1/joint_4 to list parameter
[ INFO] [1507242288.871741052]: Adding robot1/joint_5 to list parameter
[ INFO] [1507242288.871757794]: Adding robot1/joint_6 to list parameter
[ INFO] [1507242288.871772393]: Adding robot2/joint_1 to list parameter
[ INFO] [1507242288.871787999]: Adding robot2/joint_2 to list parameter
[ INFO] [1507242288.871801869]: Adding robot2/joint_3 to list parameter
[ INFO] [1507242288.871818744]: Adding robot2/joint_4 to list parameter
[ INFO] [1507242288.871832585]: Adding robot2/joint_5 to list parameter
[ INFO] [1507242288.871846936]: Adding robot2/joint_6 to list parameter
[ INFO] [1507242288.872066388]: Found user-specified joint names in '/controller_joint_names': [robot1/joint_1, robot1/joint_2, robot1/joint_3, robot1/joint_4, robot1/joint_5, robot1/joint_6, robot2/joint_1, robot2/joint_2, robot2/joint_3, robot2/joint_4, robot2/joint_5, robot2/joint_6]
gavanderhoorn commented 7 years ago

I'm a little confused as to what your question is now.

joint_trajectory_action is not supposed to initialise any joint names, users are responsible for setting the controller_joint_names parameter to the correct value(s).

https://github.com/ros-industrial/industrial_core/blob/9a0fe17aa129fe1857dccf54043ab534379eb924/industrial_utils/src/param_utils.cpp#L122-L129

The fact that industrial_utils/param_utils::getJointNames(..) (not getListParam()) makes up some default values (see above) if/when it cannot find controller_joint_names is actually undesirable behaviour (and ticketed here: #180).

Afaict setting the controller_joint_names parameter for all your robots (in their respective namespaces) should work.

gavanderhoorn commented 7 years ago

Ah. Perhaps your problem is with having multiple robots and trying to use a single industrial_robot_simulator with them? I'm not sure, but I don't believe that is a supported scenario at this point.

srsidd commented 7 years ago

Yeah, my original question was why the joint_trajectory_action was not recognizing the joints though the controller_joint_names parameter was set. But later I realized it was because industrial_utils/param_utils::getJointNames(..) was not being called. I guess I solved my original issue. But it'd be nice to know if there's a fix for the multiple robots? Or is the right way of doing this is to spawn individual industrial_robot_simulator's for each robot?

srsidd commented 7 years ago

What's even more weird is that I tried removing the debug statement now and it seems work just fine......

gavanderhoorn commented 7 years ago

I think I'm going to need a MWE to reproduce your issue if you'd like us to help you.

srsidd commented 7 years ago

I'm sorry, what's an MWE?

gavanderhoorn commented 7 years ago

From StackOverflow: How to create a Minimal, Complete, and Verifiable example.

srsidd commented 7 years ago

Ah, I wasn't able to find that easily. Thanks. I will look into making an MWE and post a link here.

On a related note, I noticed that the industrial_utils/param_utils::getJointNames(..) looks for a global parameter under controller_joint_names. However I need it to be name spaced by robot, so that the controller recognizes only the joints for that robot. What would be the best way to do this?

srsidd commented 7 years ago

Alright got it. I had to create multiple instances of industrial_robot_simulator::industrial_robot_simulator, and remap my controller_joint_names accordingly. 

<node pkg="industrial_robot_simulator" type="industrial_robot_simulator" name="robot_simulator" output="screen" >
  <remap from="/robot1/feedback_states" to="/feedback_states" />
  <remap from="/robot1/robot_status" to="/robot_status" />
  <remap from="/robot1/joint_path_command" to="/joint_path_command" />
  <remap from="/robot1/joint_states" to="/joint_states" />
  <remap from="/robot1/controller_joint_names" to="controller_joint_names" />
</node>

I also had to do similar remappings for the industrial_robot_client::joint_trajectory_action, and ensure the joint names were specified correctly in all my config files. For eg, I updated my joint_names.yaml file to look like this - 

robot1:
  controller_joint_names: ['robot1/joint_1', 'robot1/joint_2', 'robot1/joint_3', 'robot1/joint_4', 'robot1/joint_5', 'robot1/joint_6']
robot2:
  controller_joint_names: ['robot2/joint_1', 'robot2/joint_2', 'robot2/joint_3', 'robot2/joint_4', 'robot2/joint_5', 'robot2/joint_6']

This worked.

srsidd commented 7 years ago

I will close this issue, since I'm unable to reproduce it.

willcbaker commented 7 years ago

for those who stumble upon this in the future, if you launch the robot simulator in the correct robot namespace, then you should not be remapping the joint states topic to the global /joint_states topic; instead you should be combining your /robot1/joint_states, /robot2/joint_states, etc. into the global using thesource_list parameter of joint_state_publisher. see: http://wiki.ros.org/joint_state_publisher#Parameters