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;
}
Exemplo n.º 2
0
bool DMPPolicy::initialize(dmp_lib::DMPPtr dmp)
{
    if (!dmp->isInitialized())
    {
        ROS_ERROR("DMP is not initialized.");
        return false;
    }
    dmp_ = dmp;
    return (initialized_ = true);
}