message: services for stopping motors
diff: @@ -117,6 +117,8 @@ private:
  ros::ServiceServer srvServer_Stop_;
  ros::ServiceServer srvServer_Recover_;
  ros::ServiceServer srvServer_SetOperationMode_;
  + ros::ServiceServer srvServer_EmergencyStop_;
  + ros::ServiceServer srvServer_Disconnect_;
  // actionlib server
  actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> as_;
  @@ -210,6 +212,8 @@ public:
  srvServer_Recover_ = nh_.advertiseService("recover", &SdhNode::srvCallback_Init, this); // HACK: There is no recover implemented yet, so we execute a init
  srvServer_SetOperationMode_ = nh_.advertiseService("set_operation_mode", &SdhNode::srvCallback_SetOperationMode,
  this);
  + srvServer_EmergencyStop_ = nh_.advertiseService("emergency_stop", &SdhNode::srvCallback_EmergencyStop, this);
  + srvServer_Disconnect_ = nh_.advertiseService("disconnect", &SdhNode::srvCallback_Disconnect, this);
  subSetVelocitiesRaw_ = nh_.subscribe("joint_group_velocity_controller/command", 1,
  &SdhNode::topicCallback_setVelocitiesRaw, this);
  @@ -609,6 +613,54 @@ public:
  return true;
  }
  + /*!
  + * \brief Executes the service callback for emergency_stop.
  + *
  + * Performs an emergency stop.
  + * \param req Service request
  + * \param res Service response
  + */
  + bool srvCallback_EmergencyStop(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) {
  + try {
  + sdh_->EmergencyStop();
  + sdh_->SetAxisEnable(sdh_->All, 0.0);
  + sdh_->SetAxisMotorCurrent(sdh_->All, 0.0);
  + }
  + catch(const SDH::cSDHLibraryException* e) {
  + ROS_ERROR("An exception was caught: %s", e->what());
  + res.success = false;
  + res.message = e->what();
  + return false;
  + }
  +
  + res.success = true;
  + res.message = "EMERGENCY stop";
  + return true;
  + }
  +
  + /*!
  + * \brief Executes the service callback for disconnect.
  + *
  + * Disconnect from SDH and disable motors to prevent overheating.
  + * \param req Service request
  + * \param res Service response
  + */
  + bool srvCallback_Disconnect(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) {
  + try {
  + sdh_->Close();
  + }
  + catch(const SDH::cSDHErrorCommunication* e) {
  + ROS_ERROR("An exception was caught: %s", e->what());
  + res.success = false;
  + res.message = e->what();
  + return false;
  + }
  +
  + ROS_INFO("Disconnected from sdh");
  + res.success = true;
  + return true;
  + }
  +
  /*!
  * \brief Main routine to update sdh.
  *
  