00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026 #include <ros/ros.h>
00027 #include <urdf/model.h>
00028
00029
00030 #include <std_msgs/String.h>
00031 #include <std_msgs/Float64MultiArray.h>
00032 #include <sensor_msgs/JointState.h>
00033 #include <trajectory_msgs/JointTrajectory.h>
00034 #include <control_msgs/JointTrajectoryControllerState.h>
00035 #include <diagnostic_msgs/DiagnosticArray.h>
00036
00037
00038 #include <std_srvs/Trigger.h>
00039 #include <cob_srvs/SetString.h>
00040
00041
00042 #include <schunk_powercube_chain/PowerCubeCtrl.h>
00043 #include <schunk_powercube_chain/PowerCubeCtrlParams.h>
00044
00045
00051 class PowerCubeChainNode
00052 {
00053 public:
00055 ros::NodeHandle n_;
00056 ros::NodeHandle n_private_;
00057
00059 ros::Publisher topicPub_JointState_;
00060 ros::Publisher topicPub_ControllerState_;
00061 ros::Publisher topicPub_OperationMode_;
00062 ros::Publisher topicPub_Diagnostic_;
00063
00065 ros::Subscriber topicSub_CommandPos_;
00066 ros::Subscriber topicSub_CommandVel_;
00067
00069 ros::ServiceServer srvServer_Init_;
00070 ros::ServiceServer srvServer_SetOperationMode_;
00071 ros::ServiceServer srvServer_Stop_;
00072 ros::ServiceServer srvServer_Recover_;
00073
00075 PowerCubeCtrl *pc_ctrl_;
00076
00078 PowerCubeCtrlParams *pc_params_;
00079
00081 bool initialized_;
00082 bool stopped_;
00083 bool error_;
00084 std::string error_msg_;
00085 ros::Time last_publish_time_;
00086
00088 PowerCubeChainNode()
00089 {
00090 n_private_ = ros::NodeHandle("~");
00091
00092 pc_params_ = new PowerCubeCtrlParams();
00093 pc_ctrl_ = new PowerCubeCtrl(pc_params_);
00094
00096 topicPub_JointState_ = n_.advertise<sensor_msgs::JointState>("joint_states", 1);
00097 topicPub_ControllerState_ = n_.advertise<control_msgs::JointTrajectoryControllerState>("joint_trajectory_controller/state", 1);
00098 topicPub_Diagnostic_ = n_.advertise<diagnostic_msgs::DiagnosticArray>("diagnostics", 1);
00099
00101 topicSub_CommandPos_ = n_.subscribe("joint_group_position_controller/command", 1, &PowerCubeChainNode::topicCallback_CommandPos, this);
00102 topicSub_CommandVel_ = n_.subscribe("joint_group_velocity_controller/command", 1, &PowerCubeChainNode::topicCallback_CommandVel, this);
00103
00105 srvServer_Init_ = n_.advertiseService("driver/init", &PowerCubeChainNode::srvCallback_Init, this);
00106 srvServer_Stop_ = n_.advertiseService("driver/stop", &PowerCubeChainNode::srvCallback_Stop, this);
00107 srvServer_Recover_ = n_.advertiseService("driver/recover", &PowerCubeChainNode::srvCallback_Recover, this);
00108 srvServer_SetOperationMode_ = n_.advertiseService("driver/set_operation_mode", &PowerCubeChainNode::srvCallback_SetOperationMode, this);
00109 topicPub_OperationMode_ = n_.advertise<std_msgs::String>("driver/current_operationmode", 1);
00110
00111 initialized_ = false;
00112 stopped_ = true;
00113 error_ = false;
00114 last_publish_time_ = ros::Time::now();
00115 }
00116
00118 ~PowerCubeChainNode()
00119 {
00120 bool closed = pc_ctrl_->Close();
00121 if (closed)
00122 ROS_INFO("PowerCube Device closed!");
00123 }
00124
00128 void getROSParameters()
00129 {
00131 std::string CanModule;
00132 if (n_private_.hasParam("can_module"))
00133 {
00134 n_private_.getParam("can_module", CanModule);
00135 }
00136 else
00137 {
00138 ROS_ERROR("Parameter can_module not set, shutting down node...");
00139 n_.shutdown();
00140 }
00141
00143 std::string CanDevice;
00144 if (n_private_.hasParam("can_device"))
00145 {
00146 n_private_.getParam("can_device", CanDevice);
00147 }
00148 else
00149 {
00150 ROS_ERROR("Parameter can_device not set, shutting down node...");
00151 n_.shutdown();
00152 }
00153
00155 int CanBaudrate;
00156 if (n_private_.hasParam("can_baudrate"))
00157 {
00158 n_private_.getParam("can_baudrate", CanBaudrate);
00159 }
00160 else
00161 {
00162 ROS_ERROR("Parameter can_baudrate not set, shutting down node...");
00163 n_.shutdown();
00164 }
00165
00167 XmlRpc::XmlRpcValue ModulIDsXmlRpc;
00168 std::vector<int> ModulIDs;
00169 if (n_private_.hasParam("modul_ids"))
00170 {
00171 n_private_.getParam("modul_ids", ModulIDsXmlRpc);
00172 }
00173 else
00174 {
00175 ROS_ERROR("Parameter modul_ids not set, shutting down node...");
00176 n_.shutdown();
00177 }
00178
00180 bool UseMoveVel;
00181 if (n_private_.hasParam("force_use_movevel"))
00182 {
00183 n_private_.getParam("force_use_movevel", UseMoveVel);
00184 ROS_INFO("Parameter force_use_movevel set, using moveVel");
00185 }
00186 else
00187 {
00188 ROS_INFO("Parameter force_use_movevel not set, using moveStep");
00189 UseMoveVel = false;
00190 }
00191 pc_params_->SetUseMoveVel(UseMoveVel);
00192
00194 ModulIDs.resize(ModulIDsXmlRpc.size());
00195 for (int i = 0; i < ModulIDsXmlRpc.size(); i++)
00196 {
00197 ModulIDs[i] = (int)ModulIDsXmlRpc[i];
00198 }
00199
00201 pc_params_->Init(CanModule, CanDevice, CanBaudrate, ModulIDs);
00202
00204 XmlRpc::XmlRpcValue JointNamesXmlRpc;
00205 std::vector<std::string> JointNames;
00206 if (n_private_.hasParam("joint_names"))
00207 {
00208 n_private_.getParam("joint_names", JointNamesXmlRpc);
00209 }
00210 else
00211 {
00212 ROS_ERROR("Parameter joint_names not set, shutting down node...");
00213 n_.shutdown();
00214 }
00215
00217 JointNames.resize(JointNamesXmlRpc.size());
00218 for (int i = 0; i < JointNamesXmlRpc.size(); i++)
00219 {
00220 JointNames[i] = (std::string)JointNamesXmlRpc[i];
00221 }
00222
00224 if ((int)JointNames.size() != pc_params_->GetDOF())
00225 {
00226 ROS_ERROR("Wrong dimensions of parameter joint_names, shutting down node...");
00227 n_.shutdown();
00228 }
00229 pc_params_->SetJointNames(JointNames);
00230
00232 XmlRpc::XmlRpcValue MaxAccelerationsXmlRpc;
00233 std::vector<double> MaxAccelerations;
00234 if (n_private_.hasParam("max_accelerations"))
00235 {
00236 n_private_.getParam("max_accelerations", MaxAccelerationsXmlRpc);
00237 }
00238 else
00239 {
00240 ROS_ERROR("Parameter max_accelerations not set, shutting down node...");
00241 n_.shutdown();
00242 }
00243
00245 MaxAccelerations.resize(MaxAccelerationsXmlRpc.size());
00246 for (int i = 0; i < MaxAccelerationsXmlRpc.size(); i++)
00247 {
00248 MaxAccelerations[i] = (double)MaxAccelerationsXmlRpc[i];
00249 }
00250
00252 if ((int)MaxAccelerations.size() != pc_params_->GetDOF())
00253 {
00254 ROS_ERROR("Wrong dimensions of parameter max_accelerations, shutting down node...");
00255 n_.shutdown();
00256 }
00257 pc_params_->SetMaxAcc(MaxAccelerations);
00258
00260 double Horizon;
00261 if (n_private_.hasParam("horizon"))
00262 {
00263 n_private_.getParam("horizon", Horizon);
00264 }
00265 else
00266 {
00268 Horizon = 0.05;
00269 ROS_WARN("Parameter horizon not available, setting to default value: %f sec", Horizon);
00270 }
00271 pc_ctrl_->setHorizon(Horizon);
00272 }
00273
00277 void getRobotDescriptionParameters()
00278 {
00279 unsigned int DOF = pc_params_->GetDOF();
00280 std::vector<std::string> JointNames = pc_params_->GetJointNames();
00281
00283 std::string xml_string;
00284 if (n_.hasParam("/robot_description"))
00285 {
00286 n_.getParam("/robot_description", xml_string);
00287 }
00288 else
00289 {
00290 ROS_ERROR("Parameter '/robot_description' not set, shutting down node...");
00291 n_.shutdown();
00292 }
00293
00294 if (xml_string.size() == 0)
00295 {
00296 ROS_ERROR("Unable to load robot model from parameter /robot_description'");
00297 n_.shutdown();
00298 }
00299
00301 urdf::Model model;
00302 if (!model.initString(xml_string))
00303 {
00304 ROS_ERROR("Failed to parse urdf file");
00305 n_.shutdown();
00306 }
00307 ROS_DEBUG("Successfully parsed urdf file");
00308
00310 std::vector<double> MaxVelocities(DOF);
00311 for (unsigned int i = 0; i < DOF; i++)
00312 {
00313 MaxVelocities[i] = model.getJoint(JointNames[i].c_str())->limits->velocity;
00314 }
00315
00317 std::vector<double> LowerLimits(DOF);
00318 for (unsigned int i = 0; i < DOF; i++)
00319 {
00320 LowerLimits[i] = model.getJoint(JointNames[i].c_str())->limits->lower;
00321 }
00322
00323
00324 std::vector<double> UpperLimits(DOF);
00325 for (unsigned int i = 0; i < DOF; i++)
00326 {
00327 UpperLimits[i] = model.getJoint(JointNames[i].c_str())->limits->upper;
00328 }
00329
00331 std::vector<double> Offsets(DOF);
00332 for (unsigned int i = 0; i < DOF; i++)
00333 {
00334 if(model.getJoint(JointNames[i].c_str())->calibration == NULL)
00335 Offsets[i] = 0.0;
00336 else
00337 Offsets[i] = model.getJoint(JointNames[i].c_str())->calibration->rising.get()[0];
00338 }
00339
00341 pc_params_->SetMaxVel(MaxVelocities);
00342 pc_params_->SetLowerLimits(LowerLimits);
00343 pc_params_->SetUpperLimits(UpperLimits);
00344 pc_params_->SetOffsets(Offsets);
00345 }
00346
00353 void topicCallback_CommandPos(const std_msgs::Float64MultiArray::ConstPtr& msg)
00354 {
00355 ROS_DEBUG("Received new position command");
00356 if (!initialized_)
00357 {
00358 ROS_WARN("Skipping command: powercubes not initialized");
00359 publishState();
00360 return;
00361 }
00362
00363 if (pc_ctrl_->getPC_Status() != PowerCubeCtrl::PC_CTRL_OK)
00364 {
00365 publishState();
00366 return;
00367 }
00368
00369 PowerCubeCtrl::PC_CTRL_STATUS status;
00370 std::vector<std::string> errorMessages;
00371 pc_ctrl_->getStatus(status, errorMessages);
00372
00374 if (msg->data.size() != pc_params_->GetDOF())
00375 {
00376 ROS_ERROR("Skipping command: Commanded positionss and DOF are not same dimension.");
00377 return;
00378 }
00379
00381 if (!pc_ctrl_->MoveJointSpaceSync(msg->data))
00382 {
00383 error_ = true;
00384 error_msg_ = pc_ctrl_->getErrorMessage();
00385 ROS_ERROR("Skipping command: %s", pc_ctrl_->getErrorMessage().c_str());
00386 return;
00387 }
00388
00389 ROS_DEBUG("Executed position command");
00390
00391 publishState();
00392 }
00393
00400 void topicCallback_CommandVel(const std_msgs::Float64MultiArray::ConstPtr &msg)
00401 {
00402 ROS_DEBUG("Received new velocity command");
00403 if (!initialized_)
00404 {
00405 ROS_WARN("Skipping command: powercubes not initialized");
00406 publishState(false);
00407 return;
00408 }
00409
00410 if (pc_ctrl_->getPC_Status() != PowerCubeCtrl::PC_CTRL_OK)
00411 {
00412 publishState(false);
00413 return;
00414 }
00415
00416 PowerCubeCtrl::PC_CTRL_STATUS status;
00417 std::vector<std::string> errorMessages;
00418 pc_ctrl_->getStatus(status, errorMessages);
00419
00420 unsigned int DOF = pc_params_->GetDOF();
00421
00423 if (msg->data.size() != DOF)
00424 {
00425 ROS_ERROR("Skipping command: Commanded velocities and DOF are not same dimension.");
00426 return;
00427 }
00428
00430 if (!pc_ctrl_->MoveVel(msg->data))
00431 {
00432 error_ = true;
00433 error_msg_ = pc_ctrl_->getErrorMessage();
00434 ROS_ERROR("Skipping command: %s", pc_ctrl_->getErrorMessage().c_str());
00435 return;
00436 }
00437
00438 ROS_DEBUG("Executed velocity command");
00439
00440 publishState(false);
00441 }
00442
00450 bool srvCallback_Init(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
00451 {
00452 if (!initialized_)
00453 {
00454 ROS_INFO("Initializing powercubes...");
00455
00457 if (pc_ctrl_->Init(pc_params_))
00458 {
00459 initialized_ = true;
00460 res.success = true;
00461 ROS_INFO("...initializing powercubes successful");
00462 }
00463
00464 else
00465 {
00466 error_ = true;
00467 error_msg_ = pc_ctrl_->getErrorMessage();
00468 res.success = false;
00469 res.message = pc_ctrl_->getErrorMessage();
00470 ROS_INFO("...initializing powercubes not successful. error: %s", res.message.c_str());
00471 }
00472 }
00473
00474 else
00475 {
00476 res.success = true;
00477 res.message = "powercubes already initialized";
00478 ROS_WARN("...initializing powercubes not successful. error: %s", res.message.c_str());
00479 }
00480
00481 return true;
00482 }
00483
00491 bool srvCallback_Stop(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
00492 {
00493 ROS_INFO("Stopping powercubes...");
00494
00496 if (pc_ctrl_->Stop())
00497 {
00498 res.success = true;
00499 ROS_INFO("...stopping powercubes successful.");
00500 }
00501
00502 else
00503 {
00504 res.success = false;
00505 res.message = pc_ctrl_->getErrorMessage();
00506 ROS_ERROR("...stopping powercubes not successful. error: %s", res.message.c_str());
00507 }
00508 return true;
00509 }
00510
00518 bool srvCallback_Recover(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
00519 {
00520 ROS_INFO("Recovering powercubes...");
00521 if (initialized_)
00522 {
00524 if (pc_ctrl_->Recover())
00525 {
00526 error_ = false;
00527 error_msg_ = "";
00528 res.success = true;
00529 ROS_INFO("...recovering powercubes successful.");
00530 }
00531 else
00532 {
00533 res.success = false;
00534 error_ = true;
00535 error_msg_ = pc_ctrl_->getErrorMessage();
00536 res.message = pc_ctrl_->getErrorMessage();
00537 ROS_ERROR("...recovering powercubes not successful. error: %s", res.message.c_str());
00538 }
00539 }
00540 else
00541 {
00542 res.success = false;
00543 res.message = "powercubes not initialized";
00544 ROS_ERROR("...recovering powercubes not successful. error: %s", res.message.c_str());
00545 }
00546
00547 return true;
00548 }
00549
00557 bool srvCallback_SetOperationMode(cob_srvs::SetString::Request &req, cob_srvs::SetString::Response &res)
00558 {
00559 if (req.data != "velocity")
00560 {
00561 ROS_WARN("Powercube chain currently only supports velocity commands");
00562 res.success = false;
00563 }
00564 else
00565 {
00566 res.success = true;
00567 }
00568 return true;
00569 }
00570
00577 void publishState(bool update = true)
00578 {
00579 if (initialized_)
00580 {
00581 ROS_DEBUG("publish state");
00582
00583 if (update)
00584 {
00585 pc_ctrl_->updateStates();
00586 }
00587
00588 sensor_msgs::JointState joint_state_msg;
00589 joint_state_msg.header.stamp = ros::Time::now();
00590 joint_state_msg.name = pc_params_->GetJointNames();
00591 joint_state_msg.position = pc_ctrl_->getPositions();
00592 joint_state_msg.velocity = pc_ctrl_->getVelocities();
00593 joint_state_msg.effort.resize(pc_params_->GetDOF());
00594
00595 control_msgs::JointTrajectoryControllerState controller_state_msg;
00596 controller_state_msg.header.stamp = joint_state_msg.header.stamp;
00597 controller_state_msg.joint_names = pc_params_->GetJointNames();
00598 controller_state_msg.actual.positions = pc_ctrl_->getPositions();
00599 controller_state_msg.actual.velocities = pc_ctrl_->getVelocities();
00600 controller_state_msg.actual.accelerations = pc_ctrl_->getAccelerations();
00601
00602 std_msgs::String opmode_msg;
00603 opmode_msg.data = "velocity";
00604
00606 topicPub_JointState_.publish(joint_state_msg);
00607 topicPub_ControllerState_.publish(controller_state_msg);
00608 topicPub_OperationMode_.publish(opmode_msg);
00609
00610 last_publish_time_ = joint_state_msg.header.stamp;
00611 }
00612
00613
00614 if (pc_ctrl_->getPC_Status() != PowerCubeCtrl::PC_CTRL_OK)
00615 {
00616 error_ = true;
00617 }
00618
00619
00620 if (pc_ctrl_->getPC_Status() != PowerCubeCtrl::PC_CTRL_OK)
00621 {
00622 error_ = true;
00623 }
00624 else
00625 {
00626 error_ = false;
00627 }
00628
00629
00630 diagnostic_msgs::DiagnosticArray diagnostics;
00631 diagnostics.status.resize(1);
00632
00633
00634 if (error_)
00635 {
00636 diagnostics.status[0].level = 2;
00637 diagnostics.status[0].name = n_.getNamespace();
00638 diagnostics.status[0].message = pc_ctrl_->getErrorMessage();
00639 }
00640 else
00641 {
00642 if (initialized_)
00643 {
00644 diagnostics.status[0].level = 0;
00645 diagnostics.status[0].name = n_.getNamespace();
00646 diagnostics.status[0].message = "powercubechain initialized and running";
00647 }
00648 else
00649 {
00650 diagnostics.status[0].level = 1;
00651 diagnostics.status[0].name = n_.getNamespace();
00652 diagnostics.status[0].message = "powercubechain not initialized";
00653 }
00654 }
00655
00656 topicPub_Diagnostic_.publish(diagnostics);
00657 }
00658 };
00659
00665 int main(int argc, char **argv)
00666 {
00668 ros::init(argc, argv, "powercube_chain");
00669
00670
00671 PowerCubeChainNode pc_node;
00672 pc_node.getROSParameters();
00673 pc_node.getRobotDescriptionParameters();
00674
00676 double frequency;
00677 if (pc_node.n_private_.hasParam("frequency"))
00678 {
00679 pc_node.n_private_.getParam("frequency", frequency);
00680
00681 }
00682 else
00683 {
00684
00685 frequency = 10;
00686 ROS_WARN("Parameter 'frequency' not available, setting to default value: %f Hz", frequency);
00687 }
00688
00689 ros::Duration min_publish_duration;
00690 if (pc_node.n_private_.hasParam("min_publish_duration"))
00691 {
00692 double sec;
00693 pc_node.n_private_.getParam("min_publish_duration", sec);
00694 min_publish_duration.fromSec(sec);
00695 }
00696 else
00697 {
00698 ROS_ERROR("Parameter 'min_publish_time' not available");
00699 return 0;
00700 }
00701
00702 if ((1.0 / min_publish_duration.toSec()) > frequency)
00703 {
00704 ROS_ERROR("min_publish_duration has to be longer then delta_t of controller frequency!");
00705 return 0;
00706 }
00707
00709 ros::Rate loop_rate(frequency);
00710 while (pc_node.n_.ok())
00711 {
00712 if ((ros::Time::now() - pc_node.last_publish_time_) >= min_publish_duration)
00713 {
00714 pc_node.publishState();
00715 }
00716
00718 ros::spinOnce();
00719 loop_rate.sleep();
00720 }
00721
00722 return 0;
00723 }
00724
00725