Goals aren't cancelled when the trajectory controller is stopped/started (ros ticket #4429)

[ahendrix]

No description provided.

[calderpg]

We implemented a trajectory controller interface for a different robot based on the one for the PR2, and we addressed this problem by adding a shutdown function to the joint trajectory action server class:

    void shutdown(int signum)
    {
        if (signum == SIGINT)
        {
            // Stops the controller.
            ROS_INFO("Starting safe shutdown...");
            trajectory_msgs::JointTrajectory empty;
            empty.joint_names = joint_names_;
            pub_controller_command_.publish(empty);
            if (has_active_goal_)
            {
                // Marks the current goal as canceled.
                active_goal_.setCanceled();
                has_active_goal_ = false;
            }
            ROS_INFO("Shutting down!");
        }
    }

and a simple signal handler outside it to call the shutdown function:

JointTrajectoryExecuter* g_jte;

void shutdown(int signum)
{
    if (g_jte != NULL)
    {
        g_jte->shutdown(signum);
    }
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "joint_trajectory_action_node", ros::init_options::NoSigintHandler);
  ros::NodeHandle node;
  // Register a signal handler to safely shutdown the node
  signal(SIGINT, shutdown);
  g_jte = new JointTrajectoryExecuter(node);
  ros::spin();
  // Make the compiler happy
  return 0;
}