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();
}
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;
;
}
}
}
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);
}
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);
}