ros_controllers: Controller crashes due to preempt request

Hello there,

I find an intermittent error when controlling a robot hand. The controller crashes and I can no longer control the hand.

Here is the ROS launch terminal error:

[ INFO] [1508861516.717055713]: Stopping execution due to preempt request
[ INFO] [1508861516.717101655]: Cancelling execution for H1_trajectory_controller
[ INFO] [1508861516.717143522]: Stopped trajectory execution.
[robot_hw-7] process has died [pid 342, exit code -11, cmd ethercat_grant /home/user/projects/shadow_robot/base_deps/devel/lib/ros_control_robot/ros_control_robot __name:=robot_hw __log:=/home/user/.ros/log/7c234788-b8d5-11e7-8bcd-1c1b0d6eaf82/robot_hw-7.log].
log file: /home/user/.ros/log/7c234788-b8d5-11e7-8bcd-1c1b0d6eaf82/robot_hw-7*.log

If you look here you can see action feedback. Basically, I think it is crashing because when a new goal arrives and the previous one hasn’t finished yet. Hence it is “preempted”.

There is a check for the pointer to be valid here. As a thread calls it, it may be null as it is occupied by another thread.

I wonder what what be a decent fix for this. Perhaps some mutual exclusion is required?

gdb output trace:
bt
#0  0x00007ffff556f428 in __GI_raise (sig=sig@entry=6) at ../sysdeps/unix/sysv/linux/raise.c:54
#1  0x00007ffff557102a in __GI_abort () at abort.c:89
#2  0x00007ffff5567bd7 in __assert_fail_base (fmt=<optimized out>,
   assertion=assertion@entry=0x7fffe44b95c4 "px != 0",
   file=file@entry=0x7fffe44b9598 "/usr/include/boost/smart_ptr/shared_ptr.hpp", line=line@entry=648,
   function=function@entry=0x7fffe44c0e80 <boost::shared_ptr<realtime_tools::RealtimeServerGoalHandle<control_msgs::FollowJointTrajectoryAction_<std::allocator<void> > > >::operator->() const::__PRETTY_FUNCTION__> "typename boost::detail::sp_member_access<T>::type boost::shared_ptr<T>::operator->() const [with T = realtime_tools::RealtimeServerGoalHandle<control_msgs::FollowJointTrajectoryAction_<std::allocator<"...) at assert.c:92
#3  0x00007ffff5567c82 in __GI___assert_fail (assertion=0x7fffe44b95c4 "px != 0",
   file=0x7fffe44b9598 "/usr/include/boost/smart_ptr/shared_ptr.hpp", line=648,
   function=0x7fffe44c0e80 <boost::shared_ptr<realtime_tools::RealtimeServerGoalHandle<control_msgs::FollowJointTrajectoryAction_<std::allocator<void> > > >::operator->() const::__PRETTY_FUNCTION__> "typename boost::detail::sp_member_access<T>::type boost::shared_ptr<T>::operator->() const [with T = realtime_tools::RealtimeServerGoalHandle<control_msgs::FollowJointTrajectoryAction_<std::allocator<"...) at assert.c:101
#4  0x00007fffe4445667 in boost::shared_ptr<realtime_tools::RealtimeServerGoalHandle<control_msgs::FollowJointTrajectoryAction_<std::allocator<void> > > >::operator-> (this=0x70bda0)
   at /usr/include/boost/smart_ptr/shared_ptr.hpp:648
#5  0x00007fffe4436869 in joint_trajectory_controller::JointTrajectoryController<trajectory_interface::QuinticSplineSegment<double>, hardware_interface::EffortJointInterface>::update (this=0x70bcc0, time=..., period=...)
   at /home/user/projects/shadow_robot/base_deps/src/ros_controllers/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h:481
#6  0x00007ffff7760f67 in controller_manager::ControllerManager::update(ros::Time const&, ros::Duration const&, bool) () from /opt/ros/kinetic/lib/libcontroller_manager.so
#7  0x0000000000488f54 in controlLoop ()
   at /home/user/projects/shadow_robot/base_deps/src/ros_control_robot/src/main.cpp:323
#8  0x00007ffff66e76ba in start_thread (arg=0x7fffee34e700) at pthread_create.c:333
#9  0x00007ffff56413dd in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:109
(gdb)