JointGroupPositionController: Set current position as target in starting()

[wxmerkt]

Currently, the target position upon starting is all zeros. This is not great if a command is not issued immediately (and even then). To be safer, set the goal position to the current sensed position and also reset the PID internal state upon starting.

(This PR is on top of #503)

[matthew-reynolds]

This change has no dependence on #503, could you please trim the branch down to just the relevant commit to make it easier to review and merge.

---

LGTM, especially because this is already what's done in most other position controllers. Set cmd=pos, reset PID.

[matthew-reynolds]

starting() is in the RT domain, so rather than calling writeFromNonRT(), you should write to the command buffer directly. (The realtime buffer lacks a proper writeFromRT method since it's really intended for writing from a single non-RT thread, reading from a single RT thead. I haven't worked through the accesses and how safe this aooriach is, but from these examples by people much smarter than me, it seems this is a safe solution)

Examples:

ros_controllers/position_controllers/src/joint_group_position_controller.cpp

Lines 42 to 50 in 1fd8d25

	void forward_command_controller::ForwardJointGroupCommandController<T>::starting(const ros::Time& time)
	{
	// Start controller with current joint positions
	std::vector<double> & commands = *commands_buffer_.readFromRT();
	for(unsigned int i=0; i<joints_.size(); i++)
	{
	commands[i]=joints_[i].getPosition();
	}
	}

ros_controllers/velocity_controllers/src/joint_group_velocity_controller.cpp

Lines 42 to 46 in 1fd8d25

	void forward_command_controller::ForwardJointGroupCommandController<T>::starting(const ros::Time& time)
	{
	// Start controller with 0.0 velocities
	commands_buffer_.readFromRT()->assign(n_joints_, 0.0);
	}

[matthew-reynolds]

Actually, looking at this again, I think the right solution is to just use initRT(). In practical senses, it'll be pretty much the same thing, but with less overhead and also less hackey.

I think those examples I linked should probably be updated to initRT() as well. I'll look into that (#511).

[wxmerkt]

Thank you for the review and feedback @matthew-reynolds. I updated the PR to use initRT and split it from the changes in #503.

[matthew-reynolds]

This looks great to me, ping @bmagyar for another set of eyes and potential merge

[wxmerkt]

@bmagyar are we good to merge this?

[bmagyar]

Much appreciated guys, thank you!

[bmagyar]

I've rebased this on the latest upstream which should have the CI fixed, merging when all turns green

[bmagyar]

I still consider a diff_drive_controller failure green ;)

[bmagyar]

Thanks for the fix @wxmerkt , merging