Use a copy of the pointer in update() to avoid crash by cancelCB()

[oka1125]

In the issue of #301(Controller crashes due to preempt request), JointTrajectoryController's Action has the problem of crash on preempt request.
This issue was solved by copy of shared pointer.

--> Use a copy of the pointer in update() #327

Now I found the same problem in GripperActionController.

I repeat sending GripperAction frequently, Gazebo crashes by memory access violated.

gzserver: /usr/include/boost/smart_ptr/shared_ptr.hpp:648: typename boost::detail::sp_member_access<T>::type boost::shared_ptr<T>::operator->() const [with T = realtime_tools::RealtimeServerGoalHandle<control_msgs::GripperCommandAction_<std::allocator<void> > >; typename boost::detail::sp_member_access<T>::type = realtime_tools::RealtimeServerGoalHandle<control_msgs::GripperCommandAction_<std::allocator<void> > >*]: Assertion `px != 0' failed.
Aborted (core dumped)
[gazebo-4] process has died [pid 20368, exit code 134, cmd /opt/ros/kinetic/lib/gazebo_ros/gzserver -e ode /home/torobo/catkin_ws/src/torobo_robot/torobo_gazebo/worlds/ground_plane.world __name:=gazebo __log:=/home/torobo/.ros/log/34ac2ae2-e33a-11e8-9547-bc5ff41a95e3/gazebo-4.log].
log file: /home/torobo/.ros/log/34ac2ae2-e33a-11e8-9547-bc5ff41a95e3/gazebo-4*.log

Following is gdb backtrace.

#0  0x00007fff18177317 in gripper_action_controller::GripperActionController<hardware_interface::PositionJointInterface>::checkForSuccess(ros::Time const&, double, double, double) () from /opt/ros/kinetic/lib//libgripper_action_controller.so
#1  0x00007fff181775da in gripper_action_controller::GripperActionController<hardware_interface::PositionJointInterface>::update(ros::Time const&, ros::Duration const&) () from /opt/ros/kinetic/lib//libgripper_action_controller.so
#2  0x00007fff2113cd17 in controller_manager::ControllerManager::update(ros::Time const&, ros::Duration const&, bool) ()
   from /opt/ros/kinetic/lib/libcontroller_manager.so
#3  0x00007fff21413f50 in gazebo_ros_control::GazeboRosControlPlugin::Update() ()
   from /home/oka/catkin_ws/devel/lib/libgazebo_ros_control.so
#4  0x00007fff2143ca69 in boost::_mfi::mf0<void, gazebo_ros_control::GazeboRosControlPlugin>::operator()(gazebo_ros_control::GazeboRosControlPlugin*) const () from /home/oka/catkin_ws/devel/lib/libgazebo_ros_control.so
#5  0x00007fff2143a842 in void boost::_bi::list1<boost::_bi::value<gazebo_ros_control::GazeboRosControlPlugin*> >::operator()<boost::_mfi::mf0<void, gazebo_ros_control::GazeboRosControlPlugin>, boost::_bi::list1<gazebo::common::UpdateInfo const&> >(boost::_bi::type<void>, boost::_mfi::mf0<void, gazebo_ros_control::GazeboRosControlPlugin>&, boost::_bi::list1<gazebo::common::UpdateInfo const&>&, int) () from /home/oka/catkin_ws/devel/lib/libgazebo_ros_control.so
#6  0x00007fff21437c7b in void boost::_bi::bind_t<void, boost::_mfi::mf0<void, gazebo_ros_control::GazeboRosControlPlugin>, boost::_bi::list1<boost::_bi::value<gazebo_ros_control::GazeboRosControlPlugin*> > >::operator()<gazebo::common::UpdateInfo const&>(gazebo::common::UpdateInfo const&) () from /home/oka/catkin_ws/devel/lib/libgazebo_ros_control.so
#7  0x00007fff21434177 in boost::detail::function::void_function_obj_invoker1<boost::_bi::bind_t<void, boost::_mfi::mf0<void, gazebo_ros_control::GazeboRosControlPlugin>, boost::_bi::list1<boost::_bi::value<gazebo_ros_control::GazeboRosControlPlugin*> > >, void, gazebo::common::UpdateInfo const&>::invoke(boost::detail::function::function_buffer&, gazebo::common::UpdateInfo const&) ()
   from /home/oka/catkin_ws/devel/lib/libgazebo_ros_control.so
#8  0x00007ffff62ca367 in boost::function1<void, gazebo::common::UpdateInfo const&>::operator() (a0=..., this=<optimized out>)
    at /usr/include/boost/function/function_template.hpp:773
#9  gazebo::event::EventT<void (gazebo::common::UpdateInfo const&)>::Signal<gazebo::common::UpdateInfo>(gazebo::common::UpdateInfo const&) (_p=..., this=<optimized out>) at /build/gazebo-nhSAPd/gazebo-7.0.0+dfsg/gazebo/common/Event.hh:380
#10 gazebo::event::EventT<void (gazebo::common::UpdateInfo const&)>::operator()<gazebo::common::UpdateInfo>(gazebo::common::UpdateInfo const&) (_p=..., this=<optimized out>) at /build/gazebo-nhSAPd/gazebo-7.0.0+dfsg/gazebo/common/Event.hh:216
#11 gazebo::physics::World::Update (this=this@entry=0x129c510) at /build/gazebo-nhSAPd/gazebo-7.0.0+dfsg/gazebo/physics/World.cc:740
#12 0x00007ffff62d89af in gazebo::physics::World::Step (this=this@entry=0x129c510)
    at /build/gazebo-nhSAPd/gazebo-7.0.0+dfsg/gazebo/physics/World.cc:672
#13 0x00007ffff62d8e25 in gazebo::physics::World::RunLoop (this=0x129c510)
    at /build/gazebo-nhSAPd/gazebo-7.0.0+dfsg/gazebo/physics/World.cc:481
#14 0x00007ffff40d85d5 in ?? () from /usr/lib/x86_64-linux-gnu/libboost_thread.so.1.58.0
#15 0x00007ffff65ed6ba in start_thread (arg=0x7fff337fe700) at pthread_create.c:333
#16 0x00007ffff6bef41d in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:109

When I modify GripperActionController like following, this bug is solved.
So please confirm my pull request for stabilizing gripper_action.

checkForSuccess(const ros::Time& time, double error_position, double current_position, double current_velocity)
{
  RealtimeGoalHandlePtr current_active_goal(rt_active_goal_);

- if(!rt_active_goal_)
+ if(!current_active_goal)
    return;

- if(rt_active_goal_->gh_.getGoalStatus().status != actionlib_msgs::GoalStatus::ACTIVE)
+ if(current_active_goal->gh_.getGoalStatus().status != actionlib_msgs::GoalStatus::ACTIVE)
    return;

  if(fabs(error_position) < goal_tolerance_)
  {
    pre_alloc_result_->effort = computed_command_;
    pre_alloc_result_->position = current_position;
    pre_alloc_result_->reached_goal = true;
    pre_alloc_result_->stalled = false;
-   rt_active_goal_->setSucceeded(pre_alloc_result_);
+   current_active_goal->setSucceeded(pre_alloc_result_);
  }
  else
  {
    if(fabs(current_velocity) > stall_velocity_threshold_)
    {
      last_movement_time_ = time;
    }
    else if( (time - last_movement_time_).toSec() > stall_timeout_)
    {
      pre_alloc_result_->effort = computed_command_;
      pre_alloc_result_->position = current_position;
      pre_alloc_result_->reached_goal = false;
      pre_alloc_result_->stalled = true;
-     rt_active_goal_->setAborted(pre_alloc_result_);
+     current_active_goal->setAborted(pre_alloc_result_);
    }
  }
}

[bmagyar]

Thanks for this. @ipa-mdl I think this looks good, could you confirm please?

[mathias-luedtke]

Looks good to me as well!