C++ (Cpp) StrSequence Examples

Example #1

File: HrpsysJointTrajectoryBridge.cpp Project: hyaguchijsk/rtmros_common

HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::jointTrajectoryActionObj(HrpsysJointTrajectoryBridge *ptr,
                                                                                std::string &cname, std::string &gname,
                                                                                std::vector<std::string> &jlist)
{
  parent = ptr;
  controller_name = cname;
  groupname = gname;
  joint_list = jlist;

  joint_trajectory_server.reset(
      new actionlib::SimpleActionServer<pr2_controllers_msgs::JointTrajectoryAction>(
          parent->nh, controller_name + "/joint_trajectory_action", false));
  follow_joint_trajectory_server.reset(
      new actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction>(
          parent->nh, controller_name + "/follow_joint_trajectory_action", false));

  joint_trajectory_server->registerGoalCallback(
      boost::bind(&HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onJointTrajectoryActionGoal, this));
  joint_trajectory_server->registerPreemptCallback(
      boost::bind(&HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onJointTrajectoryActionPreempt, this));
  follow_joint_trajectory_server->registerGoalCallback(
      boost::bind(&HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onFollowJointTrajectoryActionGoal, this));
  follow_joint_trajectory_server->registerPreemptCallback(
      boost::bind(&HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onFollowJointTrajectoryActionPreempt, this));

  joint_controller_state_pub = parent->nh.advertise<pr2_controllers_msgs::JointTrajectoryControllerState>(
      controller_name + "/state", 1);

  if (groupname.length() > 0)
  {
    OpenHRP::SequencePlayerService::StrSequence jnames;
    jnames.length(joint_list.size());
    for (size_t i = 0; i < joint_list.size(); i++)
    {
      jnames[i] = joint_list[i].c_str();
    }
    try
    {
      parent->m_service0->addJointGroup(groupname.c_str(), jnames);
    }
    catch (CORBA::SystemException& ex)
    {
      std::cerr << "[HrpsysJointTrajectoryBridge] addJointGroup(" << groupname << "), CORBA::SystemException "
          << ex._name() << std::endl;
      sleep(1);
    }
    catch (...)
    {
      std::cerr << "[HrpsysJointTrajectoryBridge] addJointGroup(" << groupname << "), failed to addJointGroup["
          << groupname.c_str() << "]" << std::endl;
      ;
      sleep(1);
    }
  }

  interpolationp = false;

  joint_trajectory_server->start();
  follow_joint_trajectory_server->start();
}

Example #2

File: HrpsysJointTrajectoryBridge.cpp Project: hyaguchijsk/rtmros_common

void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::restart()
{
  parent->m_service0->removeJointGroup(groupname.c_str());
  sleep(0.1);
  if (groupname.length() > 0)
  {
    OpenHRP::SequencePlayerService::StrSequence jnames;
    jnames.length(joint_list.size());
    for (size_t i = 0; i < joint_list.size(); i++)
    {
      jnames[i] = joint_list[i].c_str();
    }
    try
    {
      parent->m_service0->addJointGroup(groupname.c_str(), jnames);
    }
    catch (CORBA::SystemException& ex)
    {
      std::cerr << "[HrpsysJointTrajectoryBridge] addJointGroup(" << groupname << "), CORBA::SystemException "
          << ex._name() << std::endl;
      sleep(1);
    }
    catch (...)
    {
      std::cerr << "[HrpsysJointTrajectoryBridge] addJointGroup(" << groupname << "), failed to addJointGroup["
          << groupname.c_str() << "]" << std::endl;
      ;
    }
  }
}

Example #3

File: SequencePlayer.cpp Project: thomas-moulard/hrpsys-base-deb

bool SequencePlayer::addJointGroup(const char *gname, const OpenHRP::SequencePlayerService::StrSequence& jnames)
{
    Guard guard(m_mutex);
    std::vector<int> indices;
    for (size_t i=0; i<jnames.length(); i++){
        hrp::Link *l = m_robot->link(std::string(jnames[i]));
        if (l){
            indices.push_back(l->jointId);
        }else{
            return false;
        }
    }
    return m_seq->addJointGroup(gname, indices);
}

Example #4

File: SequencePlayer.cpp Project: 130s/hrpsys-base

bool SequencePlayer::addJointGroup(const char *gname, const OpenHRP::SequencePlayerService::StrSequence& jnames)
{
    std::cerr << "[addJointGroup] group name = " << gname << std::endl;
    if ( m_debugLevel > 0 ) {
        std::cerr << __PRETTY_FUNCTION__ << std::endl;
    }
    if (!waitInterpolationOfGroup(gname)) return false;

    Guard guard(m_mutex);
    std::vector<int> indices;
    for (size_t i=0; i<jnames.length(); i++){
        hrp::Link *l = m_robot->link(std::string(jnames[i]));
        if (l){
            indices.push_back(l->jointId);
        }else{
            std::cerr << "[addJointGroup] link name " << jnames[i] << "is not found" << std::endl;
            return false;
        }
    }
    return m_seq->addJointGroup(gname, indices);
}