00001
00025 #include <string.h>
00026 #include <sstream>
00027 #include "denso_robot_control/denso_robot_hw.h"
00028
00029 #define RAD2DEG(x) ((x) * 180.0 / M_PI)
00030 #define DEG2RAD(x) ((x) / 180.0 * M_PI)
00031
00032 #define M2MM(x) ((x) * 1000.0)
00033 #define MM2M(x) ((x) / 1000.0)
00034
00035 namespace denso_robot_control
00036 {
00037
00038 DensoRobotHW::DensoRobotHW()
00039 {
00040 memset(m_cmd, 0, sizeof(m_cmd));
00041 memset(m_pos, 0, sizeof(m_pos));
00042 memset(m_vel, 0, sizeof(m_vel));
00043 memset(m_eff, 0, sizeof(m_eff));
00044 memset(m_type, 0, sizeof(m_type));
00045 m_joint.resize(JOINT_MAX);
00046
00047 m_eng = boost::make_shared<DensoRobotCore>();
00048 m_ctrl.reset();
00049 m_rob.reset();
00050 m_varErr.reset();
00051
00052 m_robName = "";
00053 m_robJoints = 0;
00054 m_sendfmt = DensoRobotRC8::SENDFMT_MINIIO
00055 | DensoRobotRC8::SENDFMT_HANDIO;
00056 m_recvfmt = DensoRobotRC8::RECVFMT_POSE_PJ
00057 | DensoRobotRC8::RECVFMT_MINIIO
00058 | DensoRobotRC8::RECVFMT_HANDIO;
00059 }
00060
00061 DensoRobotHW::~DensoRobotHW()
00062 {
00063
00064 }
00065
00066 HRESULT DensoRobotHW::Initialize()
00067 {
00068 ros::NodeHandle nh;
00069
00070 if (!nh.getParam("robot_name", m_robName)) {
00071 ROS_WARN("Failed to get robot_name parameter.");
00072 }
00073
00074 if (!nh.getParam("robot_joints", m_robJoints)) {
00075 ROS_WARN("Failed to get robot_joints parameter.");
00076 }
00077
00078 for (int i = 0; i < m_robJoints; i++) {
00079 std::stringstream ss;
00080 ss << "joint_" << i+1;
00081
00082 if (!nh.getParam(ss.str(), m_type[i])) {
00083 ROS_WARN("Failed to get joint_%d parameter.", i+1);
00084 ROS_WARN("It was assumed revolute type.");
00085 m_type[i] = 1;
00086 }
00087
00088 hardware_interface::JointStateHandle state_handle(ss.str(),
00089 &m_pos[i], &m_vel[i], &m_eff[i]);
00090 m_JntStInterface.registerHandle(state_handle);
00091
00092 hardware_interface::JointHandle pos_handle(
00093 m_JntStInterface.getHandle(ss.str()), &m_cmd[i]);
00094 m_PosJntInterface.registerHandle(pos_handle);
00095 }
00096
00097 int armGroup = 0;
00098 if (!nh.getParam("arm_group", armGroup)) {
00099 ROS_INFO("Use arm group 0.");
00100 armGroup = 0;
00101 }
00102
00103 int format = 0;
00104 if(nh.getParam("send_format", format)) {
00105 m_sendfmt = format;
00106 }
00107
00108 format = 0;
00109 if(nh.getParam("recv_format", format)) {
00110 m_recvfmt = format;
00111 }
00112 switch(m_recvfmt & DensoRobotRC8::RECVFMT_POSE) {
00113 case DensoRobotRC8::RECVFMT_POSE_J:
00114 case DensoRobotRC8::RECVFMT_POSE_PJ:
00115 case DensoRobotRC8::RECVFMT_POSE_TJ:
00116 break;
00117 default:
00118 ROS_WARN("Recieve format has to contain joint.");
00119 m_recvfmt = ((m_recvfmt & ~DensoRobotRC8::RECVFMT_POSE)
00120 | DensoRobotRC8::RECVFMT_POSE_J);
00121 break;
00122 }
00123
00124 registerInterface(&m_JntStInterface);
00125 registerInterface(&m_PosJntInterface);
00126
00127 HRESULT hr = m_eng->Initialize();
00128 if(FAILED(hr)) {
00129 ROS_ERROR("Failed to connect real controller. (%X)", hr);
00130 return hr;
00131 }
00132
00133 m_ctrl = m_eng->get_Controller();
00134
00135 DensoRobot_Ptr pRob;
00136 hr = m_ctrl->get_Robot(DensoBase::SRV_ACT, &pRob);
00137 if(FAILED(hr)) {
00138 ROS_ERROR("Failed to connect real robot. (%X)", hr);
00139 return hr;
00140 }
00141
00142 m_rob = boost::dynamic_pointer_cast<DensoRobotRC8>(pRob);
00143
00144 hr = CheckRobotType();
00145 if(FAILED(hr)) {
00146 ROS_ERROR("Invalid robot type.");
00147 return hr;
00148 }
00149
00150 m_rob->ChangeArmGroup(armGroup);
00151
00152 hr = m_rob->ExecCurJnt(m_joint);
00153 if(FAILED(hr)) {
00154 ROS_ERROR("Failed to get current joint. (%X)", hr);
00155 return hr;
00156 }
00157
00158 hr = m_ctrl->AddVariable("@ERROR_CODE");
00159 if(SUCCEEDED(hr)) {
00160 hr = m_ctrl->get_Variable("@ERROR_CODE", &m_varErr);
00161 }
00162 if(FAILED(hr)) {
00163 ROS_ERROR("Failed to get @ERROR_CODE object. (%X)", hr);
00164 return hr;
00165 }
00166
00167 {
00168
00169 VARIANT_Ptr vntVal(new VARIANT());
00170 vntVal->vt = VT_I4; vntVal->lVal = 0L;
00171 hr = m_varErr->ExecPutValue(vntVal);
00172 if(FAILED(hr)) {
00173 ROS_ERROR("Failed to clear error. (%X)", hr);
00174 return hr;
00175 }
00176 }
00177
00178 hr = m_rob->AddVariable("@SERVO_ON");
00179 if(SUCCEEDED(hr)) {
00180 DensoVariable_Ptr pVar;
00181 hr = m_rob->get_Variable("@SERVO_ON", &pVar);
00182 if(SUCCEEDED(hr)) {
00183 VARIANT_Ptr vntVal(new VARIANT());
00184 vntVal->vt = VT_BOOL;
00185 vntVal->boolVal = VARIANT_TRUE;
00186 hr = pVar->ExecPutValue(vntVal);
00187 }
00188 }
00189 if(FAILED(hr)) {
00190 ROS_ERROR("Failed to motor on. (%X)", hr);
00191 return hr;
00192 }
00193
00194 m_rob->put_SendFormat(m_sendfmt);
00195 m_sendfmt = m_rob->get_SendFormat();
00196
00197 m_rob->put_RecvFormat(m_recvfmt);
00198 m_recvfmt = m_rob->get_RecvFormat();
00199
00200 m_subChangeMode = nh.subscribe<Int32>(
00201 "ChangeMode", 1, &DensoRobotHW::Callback_ChangeMode, this);
00202 m_pubCurMode = nh.advertise<Int32>("CurMode", 1);
00203
00204 hr = ChangeModeWithClearError(DensoRobotRC8::SLVMODE_SYNC_WAIT
00205 | DensoRobotRC8::SLVMODE_POSE_J);
00206 if(FAILED(hr)) {
00207 ROS_ERROR("Failed to change to slave mode. (%X)", hr);
00208 return hr;
00209 }
00210
00211 return S_OK;
00212 }
00213
00214 HRESULT DensoRobotHW::ChangeModeWithClearError(int mode)
00215 {
00216 HRESULT hr = m_eng->ChangeMode(mode, mode == DensoRobotRC8::SLVMODE_NONE);
00217 if(FAILED(hr)) {
00218
00219 VARIANT_Ptr vntVal(new VARIANT());
00220 vntVal->vt = VT_I4; vntVal->lVal = 0L;
00221 m_varErr->ExecPutValue(vntVal);
00222 }
00223
00224 Int32 msg;
00225 msg.data = m_eng->get_Mode();
00226 m_pubCurMode.publish(msg);
00227
00228 if(msg.data == DensoRobotRC8::SLVMODE_NONE) {
00229 m_subMiniIO.shutdown();
00230 m_subHandIO.shutdown();
00231 m_subSendUserIO.shutdown();
00232 m_subRecvUserIO.shutdown();
00233 m_pubMiniIO.shutdown();
00234 m_pubHandIO.shutdown();
00235 m_pubRecvUserIO.shutdown();
00236 m_pubCurrent.shutdown();
00237 }
00238 else
00239 {
00240 ros::NodeHandle nh;
00241
00242 if(m_sendfmt & DensoRobotRC8::SENDFMT_HANDIO)
00243 {
00244 m_subHandIO = nh.subscribe<Int32>(
00245 "Write_HandIO", 1, &DensoRobotHW::Callback_HandIO, this);
00246 }
00247 if(m_sendfmt & DensoRobotRC8::SENDFMT_MINIIO)
00248 {
00249 m_subMiniIO = nh.subscribe<Int32>(
00250 "Write_MiniIO", 1, &DensoRobotHW::Callback_MiniIO, this);
00251 }
00252 if(m_sendfmt & DensoRobotRC8::SENDFMT_USERIO)
00253 {
00254 m_subSendUserIO = nh.subscribe<UserIO>(
00255 "Write_SendUserIO", 1, &DensoRobotHW::Callback_SendUserIO, this);
00256 }
00257 if(m_recvfmt & DensoRobotRC8::RECVFMT_HANDIO)
00258 {
00259 m_pubHandIO = nh.advertise<Int32>("Read_HandIO", 1);
00260 }
00261 if(m_recvfmt & DensoRobotRC8::RECVFMT_CURRENT)
00262 {
00263 m_pubCurrent = nh.advertise<Float64MultiArray>("Read_Current", 1);
00264 }
00265 if(m_recvfmt & DensoRobotRC8::RECVFMT_MINIIO)
00266 {
00267 m_pubMiniIO = nh.advertise<Int32>("Read_MiniIO", 1);
00268 }
00269 if(m_recvfmt & DensoRobotRC8::RECVFMT_USERIO)
00270 {
00271 m_subRecvUserIO = nh.subscribe<UserIO>(
00272 "Write_RecvUserIO", 1, &DensoRobotHW::Callback_RecvUserIO, this);
00273 m_pubRecvUserIO = nh.advertise<UserIO>("Read_RecvUserIO", 1);
00274 }
00275 }
00276
00277 return hr;
00278 }
00279
00280 void DensoRobotHW::Callback_ChangeMode(const Int32::ConstPtr& msg)
00281 {
00282 boost::mutex::scoped_lock lockMode(m_mtxMode);
00283
00284 ROS_INFO("Change to mode %d.", msg->data);
00285 HRESULT hr = ChangeModeWithClearError(msg->data);
00286 if(FAILED(hr)) {
00287 ROS_ERROR("Failed to change mode. (%X)", hr);
00288 }
00289 }
00290
00291 HRESULT DensoRobotHW::CheckRobotType()
00292 {
00293 DensoVariable_Ptr pVar;
00294 VARIANT_Ptr vntVal(new VARIANT());
00295 std::string strTypeName = "@TYPE_NAME";
00296
00297 HRESULT hr = m_rob->AddVariable(strTypeName);
00298 if(SUCCEEDED(hr)) {
00299 m_rob->get_Variable(strTypeName, &pVar);
00300 hr = pVar->ExecGetValue(vntVal);
00301 if(SUCCEEDED(hr)) {
00302 strTypeName = DensoBase::ConvertBSTRToString(vntVal->bstrVal);
00303 if(strncmp(m_robName.c_str(), strTypeName.c_str(),
00304 (m_robName.length() < strTypeName.length())
00305 ? m_robName.length() : strTypeName.length()))
00306 {
00307 hr = E_FAIL;
00308 }
00309 }
00310 }
00311
00312 return hr;
00313 }
00314
00315 void DensoRobotHW::read(ros::Time time, ros::Duration period)
00316 {
00317 boost::mutex::scoped_lock lockMode(m_mtxMode);
00318
00319 if(m_eng->get_Mode() == DensoRobotRC8::SLVMODE_NONE) {
00320 HRESULT hr = m_rob->ExecCurJnt(m_joint);
00321 if(FAILED(hr)) {
00322 ROS_ERROR("Failed to get current joint. (%X)", hr);
00323 }
00324 }
00325
00326 for(int i = 0; i < m_robJoints; i++) {
00327 switch(m_type[i]){
00328 case 0:
00329 m_pos[i] = MM2M(m_joint[i]);
00330 break;
00331 case 1:
00332 m_pos[i] = DEG2RAD(m_joint[i]);
00333 break;
00334 case -1:
00335 default:
00336 m_pos[i] = 0.0;
00337 break;
00338 }
00339 }
00340 }
00341
00342 void DensoRobotHW::write(ros::Time time, ros::Duration period)
00343 {
00344 boost::mutex::scoped_lock lockMode(m_mtxMode);
00345
00346 if(m_eng->get_Mode() != DensoRobotRC8::SLVMODE_NONE) {
00347 std::vector<double> pose;
00348 pose.resize(JOINT_MAX);
00349
00350 for(int i = 0; i < m_robJoints; i++) {
00351 switch(m_type[i]){
00352 case 0:
00353 pose[i] = M2MM(m_cmd[i]);
00354 break;
00355 case 1:
00356 pose[i] = RAD2DEG(m_cmd[i]);
00357 break;
00358 case -1:
00359 default:
00360 pose[i] = 0.0;
00361 break;
00362 }
00363 }
00364
00365 HRESULT hr = m_rob->ExecSlaveMove(pose, m_joint);
00366 if(SUCCEEDED(hr)) {
00367 if(m_recvfmt & DensoRobotRC8::RECVFMT_HANDIO)
00368 {
00369 Int32 msg;
00370 msg.data = m_rob->get_HandIO();
00371 m_pubHandIO.publish(msg);
00372 }
00373 if(m_recvfmt & DensoRobotRC8::RECVFMT_CURRENT)
00374 {
00375 Float64MultiArray msg;
00376 m_rob->get_Current(msg.data);
00377 m_pubCurrent.publish(msg);
00378 }
00379 if(m_recvfmt & DensoRobotRC8::RECVFMT_MINIIO)
00380 {
00381 Int32 msg;
00382 msg.data = m_rob->get_MiniIO();
00383 m_pubMiniIO.publish(msg);
00384 }
00385 if(m_recvfmt & DensoRobotRC8::RECVFMT_USERIO)
00386 {
00387 UserIO msg;
00388 m_rob->get_RecvUserIO(msg);
00389 m_pubRecvUserIO.publish(msg);
00390 }
00391 }
00392 else if(FAILED(hr) && (hr != E_BUF_FULL)) {
00393 ROS_ERROR("Failed to write. (%X)", hr);
00394
00395 VARIANT_Ptr vntVal(new VARIANT());
00396 hr = m_varErr->ExecGetValue(vntVal);
00397 if(FAILED(hr) || (vntVal->lVal != 0)) {
00398 ROS_ERROR("Automatically change to normal mode.");
00399 ChangeModeWithClearError(DensoRobotRC8::SLVMODE_NONE);
00400 }
00401 }
00402 }
00403 }
00404
00405 void DensoRobotHW::Callback_MiniIO(const Int32::ConstPtr& msg)
00406 {
00407 m_rob->put_MiniIO(msg->data);
00408 }
00409
00410 void DensoRobotHW::Callback_HandIO(const Int32::ConstPtr& msg)
00411 {
00412 m_rob->put_HandIO(msg->data);
00413 }
00414
00415 void DensoRobotHW::Callback_SendUserIO(const UserIO::ConstPtr& msg)
00416 {
00417 m_rob->put_SendUserIO(*msg.get());
00418 }
00419
00420 void DensoRobotHW::Callback_RecvUserIO(const UserIO::ConstPtr& msg)
00421 {
00422 m_rob->put_RecvUserIO(*msg.get());
00423 }
00424
00425 }