//==================================================
void SetPosition(double x, double y)
//==================================================
{
EpuckPosition2d move(SP);
EpuckPosition2d::DynamicConfiguration getOdometry;
getOdometry = move.UpdateOdometry ();
EpuckInterface::Triple setOdometry;
setOdometry.x = MM2M(x);
setOdometry.y = MM2M(y);
setOdometry.theta = getOdometry.pose.theta;
move.SetOdometry(setOdometry);
}
void DensoRobotHW::read(ros::Time time, ros::Duration period)
{
boost::mutex::scoped_lock lockMode(m_mtxMode);
if(m_eng->get_Mode() == DensoRobotRC8::SLVMODE_NONE) {
HRESULT hr = m_rob->ExecCurJnt(m_joint);
if(FAILED(hr)) {
ROS_ERROR("Failed to get current joint. (%X)", hr);
}
}
for(int i = 0; i < m_robJoints; i++) {
switch(m_type[i]){
case 0: // prismatic
m_pos[i] = MM2M(m_joint[i]);
break;
case 1: // revolute
m_pos[i] = DEG2RAD(m_joint[i]);
break;
case -1: // fixed
default:
m_pos[i] = 0.0;
break;
}
}
}