point-streaming: further development based on PR #88 - #215
Conversation
original author: Brian O'Neil
…al#88 * Addressed comments from @shaun-edwards: - Comment 1: Made JointTrajectoryInterface::jointCommandCB() pure virtual so that JointTrajectoryInterface::jointCommandCB() implementation can be deleted. - Comment 2: Message was changed to ROS_DEBUG * Some other minor changes (e.g., commenting)
|
Thanks @tdl-tbslab for getting this up-to-date again. I'll see if I can test this out sometime soon. @ted-miller: are there any things in here that catch your eye? |
|
@alemme, @andreaskoepf any comments? This sounds similar to what you described on ROS Discourse, sans the trapezoidal velocity generator (and possibly some other things you didn't mention). |
|
@gavanderhoorn The approach of this PR and other point-streaming impls seems to be a dynamic online trajectory generation. The main difference to our TVP controller which runs inside the realtime OS of the controller is that we operate setpoint based and the setpoint can be changed in every cycle .. e.g. it does not continue the move to a previous target but immediately reacts upon the next target, which allows us especially to minimize latency, see my joystick video. |
|
This has been suggested by multiple people. I'm inclined to accept it as is. Any thoughts @gavanderhoorn, @ted-miller, @alemme, @andreaskoepf? I'll wait a couple of days, otherwise, I'll merge. |
There was a problem hiding this comment.
BUSY can occur quite frequently. Changing this to a throttled log output might be good. Or perhaps even a debug.
There was a problem hiding this comment.
Does select(..) also check that all required joints are present in the incoming msg?
If not, such a check would probably need to be added.
There was a problem hiding this comment.
The function select(.) internally checks whether any element of msg->joint_names is empty or whether a matching "ROS element" can be found for the name of each joint in the message. If either of those two conditions are false, the function returns false. So I think that would be the check for all required joints being present.
There was a problem hiding this comment.
The function
select(.)internally checks whether any element ofmsg->joint_namesis empty or whether a matching "ROS element" can be found for the name of each joint in the message. If either of those two conditions are false, the function returnsfalse. So I think that would be the check for all required joints being present.
I agree with you on this. I think checking the return on select is adequate.
There was a problem hiding this comment.
@shaun-edwards: does this need to be added twice? Once here and once in the industrial_robot_client extension part of this package?
There was a problem hiding this comment.
Should the comment be removed?
There was a problem hiding this comment.
Suggestion: rename these to include "of what" they are the "time of last". Right now these variable names say nothing about what this is used for, and without the context of them being used by the new point streaming code, it's rather difficult to understand what they are doing here.
There are also no comments associated with these lines.
There was a problem hiding this comment.
is this missing a trailing underscore?
Additional suggestion: what is this a sequence number of / for?
|
I've added a few more comments (apologies, I should'v done a review instead). Overall question: does this support multi-group systems? |
|
Thanks for all the revisions @gavanderhoorn. I will revise the code accordingly and add a commit. As for multi-group support; I don't know. Am not too familiar with how the driver handles multiple manipulators. But @jim-rees comments the following in PR #88: "I've had a report that this does not work with dual arm robots, because they require motoman_msgs/DynamicJointTrajectory messages which this doesn't implement." |
|
Thanks for iterating on this @tdl-tbslab. Much appreciated. re: multi-group: yes, the DynamicJointPoint would be needed. It should not be too hard to add, but we might not want to complicate things in this PR. |
|
I am assuming that multi-group refers to multi-arm setups such as the SDA5. Unfortunately, we do not have such a dual arm setup in our lab, so I would probably not be able to implement that feature. |
not necessarily. Work cells with a single arm on a linear track, or an arm with a 2-axis positioner can also be setup as multi-group systems. The SDA is just a clever way to use this to create a 'humanoid-like' torso + arms setup. But as I wrote above: let's not complicate this PR. |
Addressed most of @gavanderhoorn's comments, except for comments 3 and 4.
|
@gavanderhoorn, is this good to go? |
|
ping @gavanderhoorn |
|
All, Thoughts on trying to get this tested, updated with latest |
| // variables for point streaming | ||
| double time_ptstreaming_last_point_; // time at which the last point was received | ||
| double dt_ptstreaming_points_; // elapsed time between two received points | ||
| static const double ptstreaming_timeout_ = 3.0; // seconds |
There was a problem hiding this comment.
Should we make this a parameter that defaults to 3.0 seconds?
There was a problem hiding this comment.
We can but I guess it is already covered by "Force a transition to the IDLE state if we get a message with no-points" right?
| this->state_ = TransferStates::POINT_STREAMING; | ||
| this->current_point_ = 0; | ||
| this->ptstreaming_seq_count_ = 0; | ||
| this->streaming_start_ = ros::Time::now(); |
There was a problem hiding this comment.
Reset / clear the queue to prevent sending old trajectory points:
this->ptstreaming_queue_ = std::queue<SimpleMessage>();
| if (msg->points.empty()) | ||
| { | ||
| ROS_INFO("Empty point received, cancelling current trajectory"); | ||
| return; |
There was a problem hiding this comment.
Force a transition to the IDLE state if we get a message with no-points, this functionality will allow users publishing to the joint_command topic to "instantly" stop POINT_STREAMING without waiting for timeout.
this->mutex_.lock();
trajectoryStop();
this->mutex_.unlock();There was a problem hiding this comment.
This would be a good addition I think.
|
Hi @dambrosio, thanks for the reviews. I'll have a look at them later tonight and might have time to take care of this on Wednesday including the |
Sounds good. I will be testing this on a GP7 this afternoon and will let you know how things go. |
Quick update: I tried these changes yesterday and ran into issues with MotoRos reply messages in response to sending a single trajectory point. Here is the ROS console output The I was thinking of updating the POINT_STREAMING mode to process two trajectory points in the Update: Seems like the original PR was tested using this node, I'll try to run with it and see if I get similar errors to what I described above. Update 2/14/19: Yesterday I managed to get point streaming to work on our GP7 manipulator. The jogger node used two very important (yet masked) steps in getting the streaming points successfully propagated through MotoROS.
Item 2 is extremely important. I believe the start point should have a After minimal testing it was found that point streaming can successfully operate around 50 Hz max. If the user is not careful and sends @tdl-ua I have added checks to help the user follow these requirements when using point streaming mode, I will push to my forked branch and give you a link. If you would like to use any changes I have made please feel free. |
| this->sub_cur_pos_ = this->node_.subscribe( | ||
| "joint_states", 1, &JointTrajectoryInterface::jointStateCB, this); | ||
|
|
||
| this->sub_joint_command_ = this->node_.subscribe("joint_command", 0, &JointTrajectoryInterface::jointCommandCB, this); |
There was a problem hiding this comment.
We should just double check that an infinite queue size makes sense for point streaming. Should we drop commands if the streaming node can cycle fast enough, or should we continue to fill up the command queue?
| //If current state is idle, set to POINT_STREAMING | ||
| if (TransferStates::IDLE == state) | ||
| { | ||
| this->mutex_.lock(); |
There was a problem hiding this comment.
I think we could get in an undesired state transition if we get a new msg with empty points and we are we are currently in IDLE. Maybe we should check if the msg->points.empty() and if true report a warning, and return.
There was a problem hiding this comment.
This should be handled above on line 185.
@dambrosio I've added you as a collaborator to the tbs-ualberta fork now so that you can push commits to the PR directly as I currently don't have time to work on this. Feel free to merge point-streaming with |
I'll be able to take a look at all of this next week. I will merge and add any of my changes for review. If I have time I will try and provide a mechanism you can use to do tool tip velocity control using the new POINT_STREAM state. |
I've developed something in the past that does essentially velocity control in cartesian space (basically end-effector teleoperation): https://github.com/tbs-ualberta/manipulator_pose_following where I think I've addressed some of the issues you mention above. The package is a work in progress as obtaining the IK still needs to be improved and the code needs to be made more manipulator-agnostic but it is working. |
…_STREAMING state, removed updates to local data that didn't correspond to POINT_STREAMING state
|
@tdl-ua quick FYI, this branch should be good for testing. |
@dambrosio I'll see if I can test it tonight. |
|
@dambrosio I've finally had a chance to test your commits on our SIA5. I tried to jog the EEF in cartesian space and did not run into any issues. I saw that you added checks on streaming queue bounds and |
|
Please see my comments about latency and jitter at #252 . |
|
Hi, I have an HC10DTP + YRC1000micro controller. Will this work? What adjustments do I need to make in order to enable point streaming with my controller? |
|
At Rutgers we have a sda10f robot (one of the dual arm manipulators) and I maintain a custom fork of this repo, mostly containing custom configurations for our experimental setup. Based on this pull request, however, I was able to recently implement the point streaming functionality. I don't think my code modifications are production ready by any means but I wanted to advertise them here for anyone who was having trouble getting point streaming working for their system. I would also love to help merge this functionality upstream but I'm not sure how to test code compatibility with other controllers and robot systems. |
8 participants
I tried to address some of the comments made by @shaun-edwards in PR #88 (created by @jim-rees):
JointTrajectoryInterface::jointCommandCB()pure virtual so thatJointTrajectoryInterface::jointCommandCB()implementation can be deletedROS_DEBUGcase TransferStates::STREAMING. I am assuming that it was added to be consistent withcase TransferStates::STREAMING. Therefore, no change made.As to why the state
TransferStates::POINT_STREAMINGcomes up multiple times; I have no idea. Was wondering the same thing. Obviously, there must be a good reason for having a classMotomanJointTrajectoryStreamerthat extendsJointTrajectoryStreamerbut I don't really understand it at this point.I have tested the code on an sia5 manipulator and found that commanded constant-velocity motions are carried out very smoothly. I also teleoperated the manipulator using a 6 DOF haptic interface with this point-streaming implementation and also experienced very smooth end-effector motion.