bool DynamicMovementPrimitiveControllerClient::sendCommand(const dmp_lib::DMPPtr& dmp,
const std::string& controller_name,
bool wait_for_success)
{
ROS_ASSERT(initialized_);
ROS_ASSERT(dmp->isInitialized());
if(dmp->getVersionString() == dynamic_movement_primitive::DMPUtilitiesMsg::ICRA2009)
{
ICRA2009DynamicMovementPrimitive::DMPMsg dmp_msg;
ICRA2009DynamicMovementPrimitive::writeToMessage(boost::dynamic_pointer_cast<dmp_lib::ICRA2009DMP>(dmp), dmp_msg);
return sendCommand(dmp_msg, controller_name, wait_for_success);
}
else if(dmp->getVersionString() == dynamic_movement_primitive::DMPUtilitiesMsg::NC2010)
{
NC2010DynamicMovementPrimitive::DMPMsg dmp_msg;
NC2010DynamicMovementPrimitive::writeToMessage(boost::dynamic_pointer_cast<dmp_lib::NC2010DMP>(dmp), dmp_msg);
return sendCommand(dmp_msg, controller_name, wait_for_success);
}
else
{
ROS_ERROR("Could not send DMP with of version >%s<.", dmp->getVersionString().c_str());
return false;
}
return true;
}
bool DMPPolicy::initialize(dmp_lib::DMPPtr dmp)
{
if (!dmp->isInitialized())
{
ROS_ERROR("DMP is not initialized.");
return false;
}
dmp_ = dmp;
return (initialized_ = true);
}