00001
00060
00061
00062
00063
00064
00065
00066
00067
00068 #include <ros/ros.h>
00069 #include <urdf/model.h>
00070
00071
00072 #include <sensor_msgs/JointState.h>
00073 #include <trajectory_msgs/JointTrajectory.h>
00074 #include <pr2_controllers_msgs/JointTrajectoryControllerState.h>
00075 #include <diagnostic_msgs/DiagnosticArray.h>
00076 #include <brics_actuator/JointPositions.h>
00077 #include <brics_actuator/JointVelocities.h>
00078
00079
00080
00081 #include <cob_srvs/Trigger.h>
00082 #include <cob_srvs/SetOperationMode.h>
00083
00084
00085 #include <schunk_powercube_chain/PowerCubeCtrl.h>
00086 #include <schunk_powercube_chain/PowerCubeCtrlParams.h>
00087
00088
00094 class PowerCubeChainNode
00095 {
00096
00097 public:
00099 ros::NodeHandle n_;
00100
00102 ros::Publisher topicPub_JointState_;
00103 ros::Publisher topicPub_ControllerState_;
00104 ros::Publisher topicPub_OperationMode_;
00105 ros::Publisher topicPub_Diagnostic_;
00106
00108 ros::Subscriber topicSub_CommandPos_;
00109 ros::Subscriber topicSub_CommandVel_;
00110
00112 ros::ServiceServer srvServer_Init_;
00113 ros::ServiceServer srvServer_SetOperationMode_;
00114 ros::ServiceServer srvServer_Stop_;
00115 ros::ServiceServer srvServer_Recover_;
00116
00118 PowerCubeCtrl* pc_ctrl_;
00119
00121 PowerCubeCtrlParams* pc_params_;
00122
00124 bool initialized_;
00125 bool stopped_;
00126 bool error_;
00127 std::string error_msg_;
00128 ros::Time last_publish_time_;
00129
00131 PowerCubeChainNode()
00132 {
00133 n_ = ros::NodeHandle("~");
00134
00135 pc_params_ = new PowerCubeCtrlParams();
00136 pc_ctrl_ = new PowerCubeCtrl(pc_params_);
00137
00139 topicPub_JointState_ = n_.advertise<sensor_msgs::JointState> ("/joint_states", 1);
00140 topicPub_ControllerState_ = n_.advertise<pr2_controllers_msgs::JointTrajectoryControllerState> ("state", 1);
00141 topicPub_OperationMode_ = n_.advertise<std_msgs::String> ("current_operationmode", 1);
00142 topicPub_Diagnostic_ = n_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1);
00143
00145 topicSub_CommandPos_ = n_.subscribe("command_pos", 1, &PowerCubeChainNode::topicCallback_CommandPos, this);
00146 topicSub_CommandVel_ = n_.subscribe("command_vel", 1, &PowerCubeChainNode::topicCallback_CommandVel, this);
00147
00149 srvServer_Init_ = n_.advertiseService("init", &PowerCubeChainNode::srvCallback_Init, this);
00150 srvServer_Stop_ = n_.advertiseService("stop", &PowerCubeChainNode::srvCallback_Stop, this);
00151 srvServer_Recover_ = n_.advertiseService("recover", &PowerCubeChainNode::srvCallback_Recover, this);
00152 srvServer_SetOperationMode_ = n_.advertiseService("set_operation_mode", &PowerCubeChainNode::srvCallback_SetOperationMode, this);
00153
00154 initialized_ = false;
00155 stopped_ = true;
00156 error_ = false;
00157 last_publish_time_ = ros::Time::now();
00158 }
00159
00161 ~PowerCubeChainNode()
00162 {
00163 bool closed = pc_ctrl_->Close();
00164 if (closed)
00165 ROS_INFO("PowerCube Device closed!");
00166 }
00167
00171 void getROSParameters()
00172 {
00173
00175 std::string CanModule;
00176 if (n_.hasParam("can_module"))
00177 {
00178 n_.getParam("can_module", CanModule);
00179 }
00180
00181 else
00182 {
00183 ROS_ERROR("Parameter can_module not set, shutting down node...");
00184 n_.shutdown();
00185 }
00186
00188 std::string CanDevice;
00189 if (n_.hasParam("can_device"))
00190 {
00191 n_.getParam("can_device", CanDevice);
00192 }
00193
00194 else
00195 {
00196 ROS_ERROR("Parameter can_device not set, shutting down node...");
00197 n_.shutdown();
00198 }
00199
00201 int CanBaudrate;
00202 if (n_.hasParam("can_baudrate"))
00203 {
00204 n_.getParam("can_baudrate", CanBaudrate);
00205 }
00206 else
00207 {
00208 ROS_ERROR("Parameter can_baudrate not set, shutting down node...");
00209 n_.shutdown();
00210 }
00211
00213 XmlRpc::XmlRpcValue ModulIDsXmlRpc;
00214 std::vector<int> ModulIDs;
00215 if (n_.hasParam("modul_ids"))
00216 {
00217 n_.getParam("modul_ids", ModulIDsXmlRpc);
00218 }
00219
00220 else
00221 {
00222 ROS_ERROR("Parameter modul_ids not set, shutting down node...");
00223 n_.shutdown();
00224 }
00225
00227 bool UseMoveVel;
00228 if (n_.hasParam("force_use_movevel"))
00229 {
00230 n_.getParam("force_use_movevel", UseMoveVel);
00231 ROS_INFO("Parameter force_use_movevel set, using moveVel");
00232 }
00233 else
00234 {
00235 ROS_INFO("Parameter force_use_movevel not set, using moveStep");
00236 UseMoveVel = false;
00237 }
00238 pc_params_->SetUseMoveVel(UseMoveVel);
00239
00241 ModulIDs.resize(ModulIDsXmlRpc.size());
00242 for (int i = 0; i < ModulIDsXmlRpc.size(); i++)
00243 {
00244 ModulIDs[i] = (int)ModulIDsXmlRpc[i];
00245 }
00246
00248 pc_params_->Init(CanModule, CanDevice, CanBaudrate, ModulIDs);
00249
00251 XmlRpc::XmlRpcValue JointNamesXmlRpc;
00252 std::vector<std::string> JointNames;
00253 if (n_.hasParam("joint_names"))
00254 {
00255 n_.getParam("joint_names", JointNamesXmlRpc);
00256 }
00257
00258 else
00259 {
00260 ROS_ERROR("Parameter joint_names not set, shutting down node...");
00261 n_.shutdown();
00262 }
00263
00265 JointNames.resize(JointNamesXmlRpc.size());
00266 for (int i = 0; i < JointNamesXmlRpc.size(); i++)
00267 {
00268 JointNames[i] = (std::string)JointNamesXmlRpc[i];
00269 }
00270
00272 if ((int)JointNames.size() != pc_params_->GetDOF())
00273 {
00274 ROS_ERROR("Wrong dimensions of parameter joint_names, shutting down node...");
00275 n_.shutdown();
00276 }
00277 pc_params_->SetJointNames(JointNames);
00278
00280 XmlRpc::XmlRpcValue MaxAccelerationsXmlRpc;
00281 std::vector<double> MaxAccelerations;
00282 if (n_.hasParam("max_accelerations"))
00283 {
00284 n_.getParam("max_accelerations", MaxAccelerationsXmlRpc);
00285 }
00286
00287 else
00288 {
00289 ROS_ERROR("Parameter max_accelerations not set, shutting down node...");
00290 n_.shutdown();
00291 }
00292
00294 MaxAccelerations.resize(MaxAccelerationsXmlRpc.size());
00295 for (int i = 0; i < MaxAccelerationsXmlRpc.size(); i++)
00296 {
00297 MaxAccelerations[i] = (double)MaxAccelerationsXmlRpc[i];
00298 }
00299
00301 if ((int)MaxAccelerations.size() != pc_params_->GetDOF())
00302 {
00303 ROS_ERROR("Wrong dimensions of parameter max_accelerations, shutting down node...");
00304 n_.shutdown();
00305 }
00306 pc_params_->SetMaxAcc(MaxAccelerations);
00307
00309 double Horizon;
00310 if (n_.hasParam("horizon"))
00311 {
00312 n_.getParam("horizon", Horizon);
00313 }
00314
00315 else
00316 {
00318 Horizon = 0.05;
00319 ROS_WARN("Parameter horizon not available, setting to default value: %f sec", Horizon);
00320 }
00321 pc_ctrl_->setHorizon(Horizon);
00322 }
00323
00327 void getRobotDescriptionParameters()
00328 {
00329 unsigned int DOF = pc_params_->GetDOF();
00330 std::vector<std::string> JointNames = pc_params_->GetJointNames();
00331
00333 std::string param_name = "robot_description";
00334 std::string full_param_name;
00335 std::string xml_string;
00336 n_.searchParam(param_name, full_param_name);
00337 if (n_.hasParam(full_param_name))
00338 {
00339 n_.getParam(full_param_name.c_str(), xml_string);
00340 }
00341
00342 else
00343 {
00344 ROS_ERROR("Parameter %s not set, shutting down node...", full_param_name.c_str());
00345 n_.shutdown();
00346 }
00347
00348 if (xml_string.size() == 0)
00349 {
00350 ROS_ERROR("Unable to load robot model from parameter %s",full_param_name.c_str());
00351 n_.shutdown();
00352 }
00353 ROS_DEBUG("%s content\n%s", full_param_name.c_str(), xml_string.c_str());
00354
00356 urdf::Model model;
00357 if (!model.initString(xml_string))
00358 {
00359 ROS_ERROR("Failed to parse urdf file");
00360 n_.shutdown();
00361 }
00362 ROS_DEBUG("Successfully parsed urdf file");
00363
00365 std::vector<double> MaxVelocities(DOF);
00366 for (unsigned int i = 0; i < DOF; i++)
00367 {
00368 MaxVelocities[i] = model.getJoint(JointNames[i].c_str())->limits->velocity;
00369 }
00370
00372 std::vector<double> LowerLimits(DOF);
00373 for (unsigned int i = 0; i < DOF; i++)
00374 {
00375 LowerLimits[i] = model.getJoint(JointNames[i].c_str())->limits->lower;
00376 }
00377
00378
00379 std::vector<double> UpperLimits(DOF);
00380 for (unsigned int i = 0; i < DOF; i++)
00381 {
00382 UpperLimits[i] = model.getJoint(JointNames[i].c_str())->limits->upper;
00383 }
00384
00386 std::vector<double> Offsets(DOF);
00387 for (unsigned int i = 0; i < DOF; i++)
00388 {
00389 Offsets[i] = model.getJoint(JointNames[i].c_str())->calibration->rising.get()[0];
00390 }
00391
00393 pc_params_->SetMaxVel(MaxVelocities);
00394 pc_params_->SetLowerLimits(LowerLimits);
00395 pc_params_->SetUpperLimits(UpperLimits);
00396 pc_params_->SetOffsets(Offsets);
00397 }
00398
00405 void topicCallback_CommandPos(const brics_actuator::JointPositions::ConstPtr& msg)
00406 {
00407 ROS_WARN("Received new position command. Skipping command: Position commands currently not implemented");
00408 }
00409
00416 void topicCallback_CommandVel(const brics_actuator::JointVelocities::ConstPtr& msg)
00417 {
00418 ROS_DEBUG("Received new velocity command");
00419 if (!initialized_)
00420 {
00421 ROS_WARN("Skipping command: powercubes not initialized");
00422 publishState(false);
00423 return;
00424 }
00425
00426 if (pc_ctrl_->getPC_Status() != PowerCubeCtrl::PC_CTRL_OK)
00427 {
00428 publishState(false);
00429 return;
00430 }
00431
00432
00433 PowerCubeCtrl::PC_CTRL_STATUS status;
00434 std::vector<std::string> errorMessages;
00435 pc_ctrl_->getStatus(status, errorMessages);
00436
00438
00439 unsigned int DOF = pc_params_->GetDOF();
00440 std::vector<std::string> jointNames = pc_params_->GetJointNames();
00441 std::vector<double> cmd_vel(DOF);
00442 std::string unit = "rad";
00443
00445 if (msg->velocities.size() != DOF)
00446 {
00447 ROS_ERROR("Skipping command: Commanded velocities and DOF are not same dimension.");
00448 return;
00449 }
00450
00452 for (unsigned int i = 0; i < DOF; i++)
00453 {
00455 if (msg->velocities[i].joint_uri != jointNames[i])
00456 {
00457 ROS_ERROR("Skipping command: Received joint name %s doesn't match expected joint name %s for joint %d.",msg->velocities[i].joint_uri.c_str(),jointNames[i].c_str(),i);
00458 return;
00459 }
00460
00462 if (msg->velocities[i].unit != unit)
00463 {
00464 ROS_ERROR("Skipping command: Received unit %s doesn't match expected unit %s.",msg->velocities[i].unit.c_str(),unit.c_str());
00465 return;
00466 }
00467
00469 ROS_DEBUG("Parsing velocity %f for joint %s",msg->velocities[i].value,jointNames[i].c_str());
00470 cmd_vel[i] = msg->velocities[i].value;
00471 }
00472
00474 if (!pc_ctrl_->MoveVel(cmd_vel))
00475 {
00476 error_ = true;
00477 error_msg_ = pc_ctrl_->getErrorMessage();
00478 ROS_ERROR("Skipping command: %s",pc_ctrl_->getErrorMessage().c_str());
00479 return;
00480 }
00481
00482 ROS_DEBUG("Executed velocity command");
00483
00484 publishState(false);
00485 }
00486
00494 bool srvCallback_Init(cob_srvs::Trigger::Request &req, cob_srvs::Trigger::Response &res)
00495 {
00496 if (!initialized_)
00497 {
00498 ROS_INFO("Initializing powercubes...");
00499
00501 if (pc_ctrl_->Init(pc_params_))
00502 {
00503 initialized_ = true;
00504 res.success.data = true;
00505 ROS_INFO("...initializing powercubes successful");
00506 }
00507
00508 else
00509 {
00510 error_ = true;
00511 error_msg_ = pc_ctrl_->getErrorMessage();
00512 res.success.data = false;
00513 res.error_message.data = pc_ctrl_->getErrorMessage();
00514 ROS_INFO("...initializing powercubes not successful. error: %s", res.error_message.data.c_str());
00515 }
00516 }
00517
00518 else
00519 {
00520 res.success.data = true;
00521 res.error_message.data = "powercubes already initialized";
00522 ROS_WARN("...initializing powercubes not successful. error: %s",res.error_message.data.c_str());
00523 }
00524
00525 return true;
00526 }
00527
00535 bool srvCallback_Stop(cob_srvs::Trigger::Request &req, cob_srvs::Trigger::Response &res)
00536 {
00537 ROS_INFO("Stopping powercubes...");
00538
00540 if (pc_ctrl_->Stop())
00541 {
00542 res.success.data = true;
00543 ROS_INFO("...stopping powercubes successful.");
00544 }
00545
00546 else
00547 {
00548 res.success.data = false;
00549 res.error_message.data = pc_ctrl_->getErrorMessage();
00550 ROS_ERROR("...stopping powercubes not successful. error: %s", res.error_message.data.c_str());
00551 }
00552 return true;
00553 }
00554
00562 bool srvCallback_Recover(cob_srvs::Trigger::Request &req, cob_srvs::Trigger::Response &res)
00563 {
00564 ROS_INFO("Recovering powercubes...");
00565 if (initialized_)
00566 {
00568 if (pc_ctrl_->Recover())
00569 {
00570 error_ = false;
00571 error_msg_ = "";
00572 res.success.data = true;
00573 ROS_INFO("...recovering powercubes successful.");
00574 }
00575 else
00576 {
00577 res.success.data = false;
00578 error_ = true;
00579 error_msg_ = pc_ctrl_->getErrorMessage();
00580 res.error_message.data = pc_ctrl_->getErrorMessage();
00581 ROS_ERROR("...recovering powercubes not successful. error: %s", res.error_message.data.c_str());
00582 }
00583 }
00584 else
00585 {
00586 res.success.data = false;
00587 res.error_message.data = "powercubes not initialized";
00588 ROS_ERROR("...recovering powercubes not successful. error: %s",res.error_message.data.c_str());
00589 }
00590
00591 return true;
00592 }
00593
00601 bool srvCallback_SetOperationMode(cob_srvs::SetOperationMode::Request &req, cob_srvs::SetOperationMode::Response &res)
00602 {
00603 if(req.operation_mode.data != "velocity")
00604 {
00605 ROS_WARN("Powercube chain currently only supports velocity commands");
00606 res.success.data = false;
00607 }
00608 else
00609 {
00610 res.success.data = true;
00611 }
00612 return true;
00613 }
00614
00621 void publishState(bool update=true)
00622 {
00623 if (initialized_)
00624 {
00625 ROS_DEBUG("publish state");
00626
00627 if(update)
00628 {pc_ctrl_->updateStates();}
00629
00630 sensor_msgs::JointState joint_state_msg;
00631 joint_state_msg.header.stamp = ros::Time::now();
00632 joint_state_msg.name = pc_params_->GetJointNames();
00633 joint_state_msg.position = pc_ctrl_->getPositions();
00634 joint_state_msg.velocity = pc_ctrl_->getVelocities();
00635 joint_state_msg.effort.resize(pc_params_->GetDOF());
00636
00637 pr2_controllers_msgs::JointTrajectoryControllerState controller_state_msg;
00638 controller_state_msg.header.stamp = joint_state_msg.header.stamp;
00639 controller_state_msg.joint_names = pc_params_->GetJointNames();
00640 controller_state_msg.actual.positions = pc_ctrl_->getPositions();
00641 controller_state_msg.actual.velocities = pc_ctrl_->getVelocities();
00642 controller_state_msg.actual.accelerations = pc_ctrl_->getAccelerations();
00643
00644 std_msgs::String opmode_msg;
00645 opmode_msg.data = "velocity";
00646
00648 topicPub_JointState_.publish(joint_state_msg);
00649 topicPub_ControllerState_.publish(controller_state_msg);
00650 topicPub_OperationMode_.publish(opmode_msg);
00651
00652 last_publish_time_ = joint_state_msg.header.stamp;
00653
00654 }
00655
00656
00657 if (pc_ctrl_->getPC_Status() != PowerCubeCtrl::PC_CTRL_OK) { error_ = true; }
00658
00659
00660 if (pc_ctrl_->getPC_Status() != PowerCubeCtrl::PC_CTRL_OK)
00661 {
00662 error_ = true;
00663 }
00664 else
00665 {
00666 error_ = false;
00667 }
00668
00669
00670 diagnostic_msgs::DiagnosticArray diagnostics;
00671 diagnostics.status.resize(1);
00672
00673
00674 if(error_)
00675 {
00676 diagnostics.status[0].level = 2;
00677 diagnostics.status[0].name = n_.getNamespace();;
00678 diagnostics.status[0].message = pc_ctrl_->getErrorMessage();
00679 }
00680 else
00681 {
00682 if (initialized_)
00683 {
00684 diagnostics.status[0].level = 0;
00685 diagnostics.status[0].name = n_.getNamespace();
00686 diagnostics.status[0].message = "powercubechain initialized and running";
00687 }
00688 else
00689 {
00690 diagnostics.status[0].level = 1;
00691 diagnostics.status[0].name = n_.getNamespace();
00692 diagnostics.status[0].message = "powercubechain not initialized";
00693 }
00694 }
00695
00696 topicPub_Diagnostic_.publish(diagnostics);
00697 }
00698 };
00699
00705 int main(int argc, char** argv)
00706 {
00708 ros::init(argc, argv, "powercube_chain");
00709
00710
00711 PowerCubeChainNode pc_node;
00712 pc_node.getROSParameters();
00713 pc_node.getRobotDescriptionParameters();
00714
00716 double frequency;
00717 if (pc_node.n_.hasParam("frequency"))
00718 {
00719 pc_node.n_.getParam("frequency", frequency);
00720
00721 }
00722
00723 else
00724 {
00725
00726 frequency = 10;
00727 ROS_WARN("Parameter frequency not available, setting to default value: %f Hz", frequency);
00728 }
00729
00730 ros::Duration min_publish_duration;
00731 if (pc_node.n_.hasParam("min_publish_duration"))
00732 {
00733 double sec;
00734 pc_node.n_.getParam("min_publish_duration", sec);
00735 min_publish_duration.fromSec(sec);
00736 }
00737 else
00738 {
00739 ROS_ERROR("Parameter min_publish_time not available");
00740 return 0;
00741 }
00742
00743 if((1.0/min_publish_duration.toSec()) > frequency)
00744 {
00745 ROS_ERROR("min_publish_duration has to be longer then delta_t of controller frequency!");
00746 return 0;
00747 }
00748
00750 ros::Rate loop_rate(frequency);
00751 while (pc_node.n_.ok())
00752 {
00753 if ((ros::Time::now() - pc_node.last_publish_time_) >= min_publish_duration)
00754 {
00755 pc_node.publishState();
00756 }
00757
00759 ros::spinOnce();
00760 loop_rate.sleep();
00761 }
00762
00763 return 0;
00764 }
