Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Removed version0 parameters (as much as possible) #89

Merged

Conversation

Projects
None yet
3 participants
@shaun-edwards
Copy link
Member

commented May 8, 2016

The version0 parameter is no longer required for joint streaming/state nodes. The nodes determine whether a multi-group message should be handled based on whether groups are specified via parameter.

The joint_trajectory_action did not change behavior based on the version0 parameter as I expected. Instead, different nodes are started when multiple group control is required. I created a new launch robot_multigroup_interface_streaming_<controller>.launch to hide this detail from the user.

If/when this PR is accepted, the multi-group tutorial should be updated.

@shaun-edwards

This comment has been minimized.

Copy link
Member Author

commented May 8, 2016

@ipa-mirb, can someone at IPA test this PR your sda10f? The controller side should stay as is.

*
* \return true on success, false if parameter not found
*/
bool getJointGroups(std::string topic_param, std::map<int, RobotGroup> robot_groups);

This comment has been minimized.

Copy link
@gavanderhoorn

gavanderhoorn May 11, 2016

Member

I think robot_groups should be a reference, as it's now passed by value. This causes callers to never get their robot_groups variables updated by this method, resulting in zero groups everywhere.

namespace motoman_utils
{

bool getJointGroups(std::string topic_param, std::map<int, RobotGroup> robot_groups)

This comment has been minimized.

Copy link
@gavanderhoorn

gavanderhoorn May 11, 2016

Member

See earlier comment about robot_groups needing to be a reference.

@gavanderhoorn

This comment has been minimized.

Copy link
Member

commented May 11, 2016

I quickly ran into an ASSERTION FAILED error after fixing the issue with robot_groups not getting updated:

[1462976142.485998012] [ INFO] [/joint_state] [ros.simple_message]: Added message handler for message type: 0
[1462976142.488148355] [ INFO] [/joint_state] [ros.motoman_driver]: Robot state connecting to IP address: '192.168.0.50:50241'
[1462976142.489689403] [ INFO] [/joint_state] [ros.motoman_driver]: Loading topic list
[1462976142.489841587] [ INFO] [/joint_state] [ros.motoman_driver]: Found 2 topics
[1462976142.490004666] [ INFO] [/joint_state] [ros.motoman_driver]: Topic(state_value): [group:1,joints:{joint_s,joint_l,joint_e,joint_u,joint_r,joint_b,joint_t},name:sia20f_r1_controller,ns:sia20f]
[1462976142.490151612] [ INFO] [/joint_state] [ros.motoman_driver]: Topic(state_value): [group:2,joints:{track_joint_b1},name:sia20f_b1_controller,ns:sia20f]
[1462976142.490297301] [ INFO] [/joint_state] [ros.motoman_driver]: Loading group: [group:1,joints:{joint_s,joint_l,joint_e,joint_u,joint_r,joint_b,joint_t},name:sia20f_r1_controller,ns:sia20f]
[1462976142.490493765] [ INFO] [/joint_state] [ros.motoman_driver]: Loading group: [group:2,joints:{track_joint_b1},name:sia20f_b1_controller,ns:sia20f]
[1462976142.490649860] [ INFO] [/joint_state] [ros.motoman_driver]: Loaded 2 groups
[1462976142.490832425] [ INFO] [/joint_state] [ros.motoman_driver]:  Initializing robot state 2 groups
[1462976142.490976997] [ INFO] [/joint_state] [ros.simple_message]: Initializing message manager with default comms fault handler
[1462976142.491111790] [ INFO] [/joint_state] [ros.simple_message]: Default communications fault handler successfully initialized
[1462976142.491210127] [ INFO] [/joint_state] [ros.simple_message]: Initializing message manager
[1462976142.491308324] [ INFO] [/joint_state] [ros.simple_message]: Added message handler for message type: 1
[1462976142.495108597] [ INFO] [/joint_state] [ros.simple_message]: Added message handler for message type: 10
[1462976142.496061721] [ INFO] [/joint_state] [ros.simple_message]: Added message handler for message type: 15
[1462976142.496201823] [ INFO] [/joint_state] [ros.motoman_driver]: Creating joint_feedback_ex_relay_handler with 2 groups
[1462976142.499896986] [ INFO] [/joint_state] [ros.simple_message]: Added message handler for message type: 2017
process[joint_trajectory_action-4]: started with pid [16542]
[1462976142.501522123] [ INFO] [/joint_state] [ros.simple_message]: Connected to server
[1462976142.501671373] [ INFO] [/joint_state] [ros.motoman_driver]: Successfully initialized robot state interface
[1462976142.501862808] [ INFO] [/joint_state] [ros.simple_message]: Entering message manager spin loop
[1462976142.520808444] [ INFO] [/motion_streaming_interface] [ros.motoman_driver]: Joint Trajectory Interface connecting to IP address: '192.168.0.50:50240'
[1462976142.522451320] [ INFO] [/motion_streaming_interface] [ros.motoman_driver]: Loading topic list
[1462976142.522500838] [ INFO] [/motion_streaming_interface] [ros.motoman_driver]: Found 2 topics
[1462976142.522737181] [ INFO] [/motion_streaming_interface] [ros.motoman_driver]: Topic(state_value): [group:1,joints:{joint_s,joint_l,joint_e,joint_u,joint_r,joint_b,joint_t},name:sia20f_r1_controller,ns:sia20f]
[1462976142.522785092] [ INFO] [/motion_streaming_interface] [ros.motoman_driver]: Topic(state_value): [group:2,joints:{track_joint_b1},name:sia20f_b1_controller,ns:sia20f]
[1462976142.522853257] [ INFO] [/motion_streaming_interface] [ros.motoman_driver]: Loading group: [group:1,joints:{joint_s,joint_l,joint_e,joint_u,joint_r,joint_b,joint_t},name:sia20f_r1_controller,ns:sia20f]
[1462976142.522982882] [ INFO] [/motion_streaming_interface] [ros.motoman_driver]: Loading group: [group:2,joints:{track_joint_b1},name:sia20f_b1_controller,ns:sia20f]
[1462976142.523054260] [ INFO] [/motion_streaming_interface] [ros.motoman_driver]: Loaded 2 groups
[1462976142.523121866] [ INFO] [/motion_streaming_interface] [ros.motoman_driver]: MotomanJointTrajectoryStreamer: init
[1462976142.523180882] [ INFO] [/motion_streaming_interface] [ros.motoman_driver]: JointTrajectoryStreamer: init
[1462976142.523897384] [ INFO] [/motion_streaming_interface] [ros.simple_message]: Connected to server
[1462976142.524638470] [ WARN] [/motion_streaming_interface] [ros.motoman_driver]: Unable to read velocity limits from 'robot_description' param.  Velocity validation disabled.
[1462976142.526337708] [FATAL] [/joint_state] [ros.motoman_driver]: ASSERTION FAILED
    file = /opt/ros/indigo/include/ros/publisher.h
    line = 102
    cond = false
    message = 
[1462976142.526441073] [FATAL] [/joint_state] [ros.motoman_driver]: Call to publish() on an invalid Publisher
[1462976142.526546604] [FATAL] [/joint_state] [ros.motoman_driver]: 

I think I tracked it down to these lines (from here):

this->pub_controls_[robot_id].publish(*control_state);
this->pub_states_[robot_id].publish(*sensor_state);

Commenting out these lines lets the node continue, but I don't get any messages out of it (which makes sense, since I disabled publishing).

But I can't really understand why it's crashing there. Afaict, this PR doesn't change anything connected to these lines (the publishers in pub_controls_ are initialised in bool JointRelayHandler::init(SmplMsgConnection* connection, int msg_type, std::map<int, RobotGroup> &robot_groups)), but the assertion doesn't occur when I run indigo-devel.

I haven't spent too much time trying to figure out what is going on, so any ideas?

@jettan

This comment has been minimized.

Copy link

commented May 17, 2016

@gavanderhoorn:
I have tracked down the problem and it seems to be caused by the group variable in https://github.com/shaun-edwards/motoman/blob/version0_auto/motoman_sda10f_support/config/sda10f_motion_interface.yaml

The numbering of these should start with 0, so when the joint states of the last group needs to be published, it tries to index a publisher which does not exist.

Once this problem is resolved, the next problem (which is also present in indigo-devel master) occurs:

[FATAL] [1463490740.608713916]: ASSERTION FAILED
    file = /home/delft/dev/src/motoman/motoman_driver/src/industrial_robot_client/joint_relay_handler.cpp
    line = 322
    cond = all_joint_state.positions.size() == all_joint_names.size()

This can be surpressed by commenting line 89 of joint_feedback_ex_relay_handler, but causes the /joint_state topic to be empty, as expected.

@gavanderhoorn

This comment has been minimized.

Copy link
Member

commented May 17, 2016

The numbering of these should start with 0, so when the joint states of the last group needs to be published, it tries to index a publisher which does not exist.

ah. So it uses the group key, not the index of the group descriptor in the topic_list list?

@jettan

This comment has been minimized.

Copy link

commented May 17, 2016

That seems like it is the case.

As for the other problem, a workaround/fix has been implemented here:
jettan@5e6a374

We should probably put that into another PR with the complete description of what is happening.

@gavanderhoorn

This comment has been minimized.

Copy link
Member

commented May 17, 2016

@shaun-edwards: the assertion I reported is not related to this PR, and we should probably move the discussion to a separate issue.

@gavanderhoorn

This comment has been minimized.

Copy link
Member

commented May 17, 2016

@jettan, @shaun-edwards: I've created #91 for the issue observed by @jettan.

@shaun-edwards

This comment has been minimized.

Copy link
Member Author

commented May 21, 2016

@gavanderhoorn, @jettan once I fix the pass-by-reference (newbie) mistake ;), can this be merged or should it wait on #91?

@jettan

This comment has been minimized.

Copy link

commented May 21, 2016

imo it can be merged after the pass by reference fix
The out of bounds publishers are caused by the motion_interface file in some config folder.

@gavanderhoorn

This comment has been minimized.

Copy link
Member

commented May 21, 2016

Yes, this can be merged. Afaict this is actually a pretty nice clean-up, although without unit-tests we can't know for sure there aren't any regressions introduced by this.

I only have access to a single system, and it's not a dual-arm, so I can't say anything about the changes in this PR when used with a dual-arm setup.

@shaun-edwards

This comment has been minimized.

Copy link
Member Author

commented Jun 5, 2016

@gavanderhoorn, corrected my issue and change has been tested on a second multi-group setup (not dual arm though)

@shaun-edwards

This comment has been minimized.

Copy link
Member Author

commented Jun 5, 2016

Ok to merge?

@jettan

This comment has been minimized.

Copy link

commented Jun 5, 2016

Ok for me

@shaun-edwards

This comment has been minimized.

Copy link
Member Author

commented Jun 5, 2016

Thanks @jettan, merging. Thanks for all the review help. In combination with a few more PR's, we should have an indigo release for motoman FINALLY!

@shaun-edwards shaun-edwards merged commit d17f65f into ros-industrial:indigo-devel Jun 5, 2016

1 check failed

continuous-integration/travis-ci/pr The Travis CI build failed
Details
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.