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
merged 12 commits into from Jun 5, 2016

Conversation

Projects
None yet
3 participants
@shaun-edwards
Member

shaun-edwards 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.

Show comment
Hide comment
@shaun-edwards

shaun-edwards May 8, 2016

Member

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

Member

shaun-edwards commented May 8, 2016

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

Show outdated Hide outdated ...an_driver/include/motoman_driver/industrial_robot_client/motoman_utils.h
*
* \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.

@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.

@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.

Show outdated Hide outdated motoman_driver/src/industrial_robot_client/motoman_utils.cpp
namespace motoman_utils
{
bool getJointGroups(std::string topic_param, std::map<int, RobotGroup> robot_groups)

This comment has been minimized.

@gavanderhoorn

gavanderhoorn May 11, 2016

Member

See earlier comment about robot_groups needing to be a reference.

@gavanderhoorn

gavanderhoorn May 11, 2016

Member

See earlier comment about robot_groups needing to be a reference.

@gavanderhoorn

This comment has been minimized.

Show comment
Hide comment
@gavanderhoorn

gavanderhoorn May 11, 2016

Member

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?

Member

gavanderhoorn 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.

Show comment
Hide comment
@jettan

jettan 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.

jettan 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.

Show comment
Hide comment
@gavanderhoorn

gavanderhoorn May 17, 2016

Member

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?

Member

gavanderhoorn 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.

Show comment
Hide comment
@jettan

jettan 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.

jettan 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.

Show comment
Hide comment
@gavanderhoorn

gavanderhoorn May 17, 2016

Member

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

Member

gavanderhoorn 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.

Show comment
Hide comment
@gavanderhoorn

gavanderhoorn May 17, 2016

Member

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

Member

gavanderhoorn commented May 17, 2016

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

@shaun-edwards

This comment has been minimized.

Show comment
Hide comment
@shaun-edwards

shaun-edwards May 21, 2016

Member

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

Member

shaun-edwards 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.

Show comment
Hide comment
@jettan

jettan 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.

jettan 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.

Show comment
Hide comment
@gavanderhoorn

gavanderhoorn May 21, 2016

Member

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.

Member

gavanderhoorn 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.

Show comment
Hide comment
@shaun-edwards

shaun-edwards Jun 5, 2016

Member

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

Member

shaun-edwards 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.

Show comment
Hide comment
@shaun-edwards

shaun-edwards Jun 5, 2016

Member

Ok to merge?

Member

shaun-edwards commented Jun 5, 2016

Ok to merge?

@jettan

This comment has been minimized.

Show comment
Hide comment
@jettan

jettan Jun 5, 2016

Ok for me

jettan commented Jun 5, 2016

Ok for me

@shaun-edwards

This comment has been minimized.

Show comment
Hide comment
@shaun-edwards

shaun-edwards Jun 5, 2016

Member

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!

Member

shaun-edwards 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