/*!
* \brief Executes the service callback for recover.
*
* Recovers the driver after an emergency stop.
* \param req Service request
* \param res Service response
*/
bool srvCallback_Recover( cob_srvs::Trigger::Request &req,
cob_srvs::Trigger::Response &res )
{
if (isInitialized_ == true)
{
ROS_INFO("Recovering powercubes");
// stopping all arm movements
if (PCube_->Stop())
{
ROS_INFO("Recovering powercubes succesfull");
res.success.data = true;
}
else
{
ROS_ERROR("Recovering powercubes not succesfull. error: %s", PCube_->getErrorMessage().c_str());
res.success.data = false;
res.error_message.data = PCube_->getErrorMessage();
}
}
else
{
ROS_ERROR("...powercubes already recovered...");
res.success.data = false;
res.error_message.data = "powercubes already recovered";
}
return true;
}
/*!
* \brief Executes the service callback for stop.
*
* Stops all hardware movements.
* \param req Service request
* \param res Service response
*/
bool srvCallback_Stop(cob_srvs::Trigger::Request &req, cob_srvs::Trigger::Response &res)
{
ROS_INFO("Stopping powercubes...");
// stop powercubes
if (pc_ctrl_->Stop())
{
res.success.data = true;
ROS_INFO("...stopping powercubes successful.");
}
else
{
res.success.data = false;
res.error_message.data = pc_ctrl_->getErrorMessage();
ROS_ERROR("...stopping powercubes not successful. error: %s", res.error_message.data.c_str());
}
return true;
}
bool srvCallback_Stop(cob3_srvs::Stop::Request &req,
cob3_srvs::Stop::Response &res )
{
ROS_INFO("Stopping powercubes");
// stopping all arm movements
if (PCube->Stop())
{
ROS_INFO("Stopping powercubes succesfull");
res.success = 0; // 0 = true, else = false
}
else
{
ROS_ERROR("Stopping powercubes not succesfull. error: %s", PCube->getErrorMessage().c_str());
res.success = 1; // 0 = true, else = false
res.errorMessage.data = PCube->getErrorMessage();
}
return true;
}
/*!
* \brief Executes the service callback for stop.
*
* Stops all hardware movements.
* \param req Service request
* \param res Service response
*/
bool srvCallback_Stop( cob_srvs::Trigger::Request &req,
cob_srvs::Trigger::Response &res )
{
ROS_INFO("Stopping powercubes");
newvel_ = false;
// set current trajectory to be finished
traj_point_nr_ = traj_.points.size();
// stopping all arm movements
if (PCube_->Stop())
{
ROS_INFO("Stopping powercubes succesfull");
res.success.data = true;
}
else
{
ROS_ERROR("Stopping powercubes not succesfull. error: %s", PCube_->getErrorMessage().c_str());
res.success.data = false;
res.error_message.data = PCube_->getErrorMessage();
}
return true;
}