/*!
* \brief Executes the service callback for init.
*
* Connects to the hardware and initialized it.
* \param req Service request
* \param res Service response
*/
bool srvCallback_Init(cob_srvs::Trigger::Request &req, cob_srvs::Trigger::Response &res)
{
if (!initialized_)
{
ROS_INFO("Initializing powercubes...");
/// initialize powercubes
if (pc_ctrl_->Init(pc_params_))
{
initialized_ = true;
res.success.data = true;
ROS_INFO("...initializing powercubes successful");
}
else
{
error_ = true;
error_msg_ = pc_ctrl_->getErrorMessage();
res.success.data = false;
res.error_message.data = pc_ctrl_->getErrorMessage();
ROS_INFO("...initializing powercubes not successful. error: %s", res.error_message.data.c_str());
}
}
else
{
res.success.data = true;
res.error_message.data = "powercubes already initialized";
ROS_WARN("...initializing powercubes not successful. error: %s",res.error_message.data.c_str());
}
return true;
}
/*!
* \brief Executes the service callback for init.
*
* Connects to the hardware and initialized it.
* \param req Service request
* \param res Service response
*/
bool srvCallback_Init( cob_srvs::Trigger::Request &req,
cob_srvs::Trigger::Response &res )
{
if (isInitialized_ == false)
{
ROS_INFO("...initializing powercubes...");
// init powercubes
if (PCube_->Init(PCubeParams_))
{
ROS_INFO("Initializing succesfull");
isInitialized_ = true;
res.success.data = true;
res.error_message.data = "success";
}
else
{
ROS_ERROR("Initializing powercubes not succesfull. error: %s", PCube_->getErrorMessage().c_str());
res.success.data = false;
res.error_message.data = PCube_->getErrorMessage();
}
}
else
{
ROS_ERROR("...powercubes already initialized...");
res.success.data = false;
res.error_message.data = "powercubes already initialized";
}
return true;
}
// service callback functions
// function will be called when a service is querried
bool srvCallback_Init(cob3_srvs::Init::Request &req,
cob3_srvs::Init::Response &res )
{
ROS_INFO("Initializing powercubes");
// init powercubes
//TODO: make iniFilepath as an argument
//TODO: read iniFile into ros prarameters
if (PCube->Init("include/iniFile.txt"))
{
ROS_INFO("Initializing succesfull");
res.success = 0; // 0 = true, else = false
}
else
{
ROS_ERROR("Initializing powercubes not succesfull. error: %s", PCube->getErrorMessage().c_str());
res.success = 1; // 0 = true, else = false
res.errorMessage.data = PCube->getErrorMessage();
return true;
}
// homing powercubes
if (PCube->doHoming())
{
ROS_INFO("Homing powercubes succesfull");
res.success = 0; // 0 = true, else = false
}
else
{
ROS_ERROR("Homing powercubes not succesfull. error: %s", PCube->getErrorMessage().c_str());
res.success = 1; // 0 = true, else = false
res.errorMessage.data = PCube->getErrorMessage();
return true;
}
return true;
}
