00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 #include <unistd.h>
00022 #include <map>
00023 #include <string>
00024 #include <vector>
00025
00026
00027 #include <ros/ros.h>
00028 #include <urdf/model.h>
00029 #include <actionlib/server/simple_action_server.h>
00030
00031
00032 #include <std_msgs/Float64MultiArray.h>
00033 #include <trajectory_msgs/JointTrajectory.h>
00034 #include <sensor_msgs/JointState.h>
00035 #include <control_msgs/FollowJointTrajectoryAction.h>
00036 #include <control_msgs/JointTrajectoryControllerState.h>
00037 #include <schunk_sdh/TactileSensor.h>
00038 #include <schunk_sdh/TactileMatrix.h>
00039 #include <schunk_sdh/TemperatureArray.h>
00040 #include <schunk_sdh/PressureArrayList.h>
00041
00042
00043 #include <std_srvs/Trigger.h>
00044 #include <cob_srvs/SetString.h>
00045
00046
00047 #include <diagnostic_msgs/DiagnosticArray.h>
00048
00049
00050 #include <schunk_sdh/sdh.h>
00051 #include <schunk_sdh/dsa.h>
00052
00058 class SdhNode
00059 {
00060 public:
00062 ros::NodeHandle nh_;
00063 private:
00064
00065 ros::Publisher topicPub_JointState_;
00066 ros::Publisher topicPub_ControllerState_;
00067 ros::Publisher topicPub_TactileSensor_;
00068 ros::Publisher topicPub_Diagnostics_;
00069 ros::Publisher topicPub_Temperature_;
00070 ros::Publisher topicPub_Pressure_;
00071
00072
00073 ros::Subscriber subSetVelocitiesRaw_;
00074
00075
00076 ros::ServiceServer srvServer_Init_;
00077 ros::ServiceServer srvServer_Stop_;
00078 ros::ServiceServer srvServer_Recover_;
00079 ros::ServiceServer srvServer_SetOperationMode_;
00080 ros::ServiceServer srvServer_EmergencyStop_;
00081 ros::ServiceServer srvServer_Disconnect_;
00082 ros::ServiceServer srvServer_MotorOn_;
00083 ros::ServiceServer srvServer_MotorOff_;
00084
00085
00086 actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> as_;
00087 std::string action_name_;
00088
00089
00090
00091
00092
00093 SDH::cSDH *sdh_;
00094 SDH::cDSA *dsa_;
00095 std::vector<SDH::cSDH::eAxisState> state_;
00096
00097 std::string sdhdevicetype_;
00098 std::string sdhdevicestring_;
00099 int sdhdevicenum_;
00100 std::string dsadevicestring_;
00101 int dsa_dbg_level_;
00102 double dsa_sensitivity_;
00103 double dsa_calib_pressure_;
00104 double dsa_calib_voltage_;
00105 int dsadevicenum_;
00106 int baudrate_, id_read_, id_write_;
00107 double timeout_;
00108
00109 bool isInitialized_;
00110 bool isDSAInitialized_;
00111 bool isError_;
00112 int DOF_;
00113 double pi_;
00114
00115 trajectory_msgs::JointTrajectory traj_;
00116
00117 std::vector<std::string> joint_names_;
00118 std::vector<int> axes_;
00119 std::vector<double> targetAngles_;
00120 std::vector<double> velocities_;
00121 bool hasNewGoal_;
00122 std::string operationMode_;
00123
00124 static const std::vector<std::string> temperature_names_;
00125 static const std::vector<std::string> finger_names_;
00126
00127 public:
00133 SdhNode(std::string name) :
00134 as_(nh_, name, boost::bind(&SdhNode::executeCB, this, _1), true), action_name_(name)
00135 {
00136 pi_ = 3.1415926;
00137
00138 nh_ = ros::NodeHandle("~");
00139 isError_ = false;
00140
00141 topicPub_Diagnostics_ = nh_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1);
00142 }
00143
00147 ~SdhNode()
00148 {
00149 if (isDSAInitialized_)
00150 dsa_->Close();
00151 if (isInitialized_)
00152 sdh_->Close();
00153 delete sdh_;
00154 }
00155
00159 bool init()
00160 {
00161
00162 isInitialized_ = false;
00163 isDSAInitialized_ = false;
00164 hasNewGoal_ = false;
00165
00166
00167 topicPub_JointState_ = nh_.advertise<sensor_msgs::JointState>("joint_states", 1);
00168 topicPub_ControllerState_ = nh_.advertise<control_msgs::JointTrajectoryControllerState>(
00169 "joint_trajectory_controller/state", 1);
00170 topicPub_TactileSensor_ = nh_.advertise<schunk_sdh::TactileSensor>("tactile_data", 1);
00171 topicPub_Temperature_ = nh_.advertise<schunk_sdh::TemperatureArray>("temperature", 1);
00172 topicPub_Pressure_ = nh_.advertise<schunk_sdh::PressureArrayList>("pressure", 1);
00173
00174
00175 sdh_ = new SDH::cSDH(false, false, 0);
00176
00177
00178 srvServer_Init_ = nh_.advertiseService("init", &SdhNode::srvCallback_Init, this);
00179 srvServer_Stop_ = nh_.advertiseService("stop", &SdhNode::srvCallback_Stop, this);
00180 srvServer_Recover_ = nh_.advertiseService("recover", &SdhNode::srvCallback_Init, this);
00181 srvServer_SetOperationMode_ = nh_.advertiseService("set_operation_mode", &SdhNode::srvCallback_SetOperationMode,
00182 this);
00183 srvServer_EmergencyStop_ = nh_.advertiseService("emergency_stop", &SdhNode::srvCallback_EmergencyStop, this);
00184 srvServer_Disconnect_ = nh_.advertiseService("shutdown", &SdhNode::srvCallback_Disconnect, this);
00185
00186 srvServer_MotorOn_ = nh_.advertiseService("motor_on", &SdhNode::srvCallback_MotorPowerOn, this);
00187 srvServer_MotorOff_ = nh_.advertiseService("motor_off", &SdhNode::srvCallback_MotorPowerOff, this);
00188
00189 subSetVelocitiesRaw_ = nh_.subscribe("joint_group_velocity_controller/command", 1,
00190 &SdhNode::topicCallback_setVelocitiesRaw, this);
00191
00192
00193 nh_.param("sdhdevicetype", sdhdevicetype_, std::string("PCAN"));
00194 nh_.param("sdhdevicestring", sdhdevicestring_, std::string("/dev/pcan0"));
00195 nh_.param("sdhdevicenum", sdhdevicenum_, 0);
00196
00197 nh_.param("dsadevicestring", dsadevicestring_, std::string(""));
00198 nh_.param("dsadevicenum", dsadevicenum_, 0);
00199 nh_.param("dsa_dbg_level", dsa_dbg_level_, 0);
00200 nh_.param("dsa_sensitivity", dsa_sensitivity_, 0.5);
00201 nh_.param("dsa_calib_pressure", dsa_calib_pressure_, 0.000473);
00202 nh_.param("dsa_calib_voltage", dsa_calib_voltage_, 592.1);
00203
00204 nh_.param("baudrate", baudrate_, 1000000);
00205 nh_.param("timeout", timeout_, static_cast<double>(0.04));
00206 nh_.param("id_read", id_read_, 43);
00207 nh_.param("id_write", id_write_, 42);
00208
00209
00210 ROS_INFO("getting joint_names from parameter server");
00211 XmlRpc::XmlRpcValue joint_names_param;
00212 if (nh_.hasParam("joint_names"))
00213 {
00214 nh_.getParam("joint_names", joint_names_param);
00215 }
00216 else
00217 {
00218 ROS_ERROR("Parameter joint_names not set, shutting down node...");
00219 nh_.shutdown();
00220 return false;
00221 }
00222 DOF_ = joint_names_param.size();
00223 joint_names_.resize(DOF_);
00224 for (int i = 0; i < DOF_; i++)
00225 {
00226 joint_names_[i] = (std::string)joint_names_param[i];
00227 }
00228 std::cout << "joint_names = " << joint_names_param << std::endl;
00229
00230
00231 axes_.resize(DOF_);
00232 velocities_.resize(DOF_);
00233 for (int i = 0; i < DOF_; i++)
00234 {
00235 axes_[i] = i;
00236 }
00237 ROS_INFO("DOF = %d", DOF_);
00238
00239 state_.resize(axes_.size());
00240
00241 nh_.param("OperationMode", operationMode_, std::string("position"));
00242 return true;
00243 }
00249 bool switchOperationMode(const std::string &mode)
00250 {
00251 hasNewGoal_ = false;
00252 sdh_->Stop();
00253
00254 try
00255 {
00256 if (mode == "position")
00257 {
00258 sdh_->SetController(SDH::cSDH::eCT_POSE);
00259 }
00260 else if (mode == "velocity")
00261 {
00262 sdh_->SetController(SDH::cSDH::eCT_VELOCITY);
00263 }
00264 else
00265 {
00266 ROS_ERROR_STREAM("Operation mode '" << mode << "' not supported");
00267 return false;
00268 }
00269 sdh_->SetAxisEnable(sdh_->All, 1.0);
00270 }
00271 catch (SDH::cSDHLibraryException* e)
00272 {
00273 ROS_ERROR("An exception was caught: %s", e->what());
00274 delete e;
00275 return false;
00276 }
00277
00278 operationMode_ = mode;
00279 return true;
00280 }
00281
00288 void executeCB(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal)
00289 {
00290 ROS_INFO("sdh: executeCB");
00291 if (operationMode_ != "position")
00292 {
00293 ROS_ERROR("%s: Rejected, sdh not in position mode", action_name_.c_str());
00294 as_.setAborted();
00295 return;
00296 }
00297 if (!isInitialized_)
00298 {
00299 ROS_ERROR("%s: Rejected, sdh not initialized", action_name_.c_str());
00300 as_.setAborted();
00301 return;
00302 }
00303
00304 if (goal->trajectory.points.empty() || goal->trajectory.points[0].positions.size() != size_t(DOF_))
00305 {
00306 ROS_ERROR("%s: Rejected, malformed FollowJointTrajectoryGoal", action_name_.c_str());
00307 as_.setAborted();
00308 return;
00309 }
00310 while (hasNewGoal_ == true)
00311 usleep(10000);
00312
00313 std::map<std::string, int> dict;
00314 for (int idx = 0; idx < goal->trajectory.joint_names.size(); idx++)
00315 {
00316 dict[goal->trajectory.joint_names[idx]] = idx;
00317 }
00318
00319 targetAngles_.resize(DOF_);
00320 targetAngles_[0] = goal->trajectory.points[0].positions[dict["sdh_knuckle_joint"]] * 180.0 / pi_;
00321 targetAngles_[1] = goal->trajectory.points[0].positions[dict["sdh_finger_22_joint"]] * 180.0 / pi_;
00322 targetAngles_[2] = goal->trajectory.points[0].positions[dict["sdh_finger_23_joint"]] * 180.0 / pi_;
00323 targetAngles_[3] = goal->trajectory.points[0].positions[dict["sdh_thumb_2_joint"]] * 180.0 / pi_;
00324 targetAngles_[4] = goal->trajectory.points[0].positions[dict["sdh_thumb_3_joint"]] * 180.0 / pi_;
00325 targetAngles_[5] = goal->trajectory.points[0].positions[dict["sdh_finger_12_joint"]] * 180.0 / pi_;
00326 targetAngles_[6] = goal->trajectory.points[0].positions[dict["sdh_finger_13_joint"]] * 180.0 / pi_;
00327 ROS_INFO(
00328 "received position goal: [['sdh_knuckle_joint', 'sdh_thumb_2_joint', 'sdh_thumb_3_joint', 'sdh_finger_12_joint', 'sdh_finger_13_joint', 'sdh_finger_22_joint', 'sdh_finger_23_joint']] = [%f,%f,%f,%f,%f,%f,%f]",
00329 goal->trajectory.points[0].positions[dict["sdh_knuckle_joint"]],
00330 goal->trajectory.points[0].positions[dict["sdh_thumb_2_joint"]],
00331 goal->trajectory.points[0].positions[dict["sdh_thumb_3_joint"]],
00332 goal->trajectory.points[0].positions[dict["sdh_finger_12_joint"]],
00333 goal->trajectory.points[0].positions[dict["sdh_finger_13_joint"]],
00334 goal->trajectory.points[0].positions[dict["sdh_finger_22_joint"]],
00335 goal->trajectory.points[0].positions[dict["sdh_finger_23_joint"]]);
00336
00337 hasNewGoal_ = true;
00338
00339 usleep(500000);
00340
00341 bool finished = false;
00342 while (finished == false)
00343 {
00344 if (as_.isNewGoalAvailable())
00345 {
00346 ROS_WARN("%s: Aborted", action_name_.c_str());
00347 as_.setAborted();
00348 return;
00349 }
00350 for (unsigned int i = 0; i < state_.size(); i++)
00351 {
00352 ROS_DEBUG("state[%d] = %d", i, state_[i]);
00353 if (state_[i] == 0)
00354 {
00355 finished = true;
00356 }
00357 else
00358 {
00359 finished = false;
00360 }
00361 }
00362 usleep(10000);
00363
00364
00365 }
00366
00367
00368 ROS_INFO("%s: Succeeded", action_name_.c_str());
00369
00370
00371
00372 as_.setSucceeded();
00373 }
00374
00375 void topicCallback_setVelocitiesRaw(const std_msgs::Float64MultiArrayPtr& velocities)
00376 {
00377 if (!isInitialized_)
00378 {
00379 ROS_ERROR("%s: Rejected, sdh not initialized", action_name_.c_str());
00380 return;
00381 }
00382 if (velocities->data.size() != velocities_.size())
00383 {
00384 ROS_ERROR("Velocity array dimension mismatch");
00385 return;
00386 }
00387 if (operationMode_ != "velocity")
00388 {
00389 ROS_ERROR("%s: Rejected, sdh not in velocity mode", action_name_.c_str());
00390 return;
00391 }
00392
00393
00394 while (hasNewGoal_ == true)
00395 usleep(10000);
00396
00397 velocities_[0] = velocities->data[0] * 180.0 / pi_;
00398 velocities_[1] = velocities->data[5] * 180.0 / pi_;
00399 velocities_[2] = velocities->data[6] * 180.0 / pi_;
00400 velocities_[3] = velocities->data[1] * 180.0 / pi_;
00401 velocities_[4] = velocities->data[2] * 180.0 / pi_;
00402 velocities_[5] = velocities->data[3] * 180.0 / pi_;
00403 velocities_[6] = velocities->data[4] * 180.0 / pi_;
00404
00405 hasNewGoal_ = true;
00406 }
00407
00415 bool srvCallback_Init(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
00416 {
00417 if (isInitialized_ == false)
00418 {
00419
00420
00421 try
00422 {
00423 if (sdhdevicetype_.compare("RS232") == 0)
00424 {
00425 sdh_->OpenRS232(sdhdevicenum_, 115200, 1, sdhdevicestring_.c_str());
00426 ROS_INFO("Initialized RS232 for SDH");
00427 isInitialized_ = true;
00428 }
00429 else if (sdhdevicetype_.compare("PCAN") == 0)
00430 {
00431 ROS_INFO("Starting initializing PEAKCAN");
00432 sdh_->OpenCAN_PEAK(baudrate_, timeout_, id_read_, id_write_, sdhdevicestring_.c_str());
00433 ROS_INFO("Initialized PEAK CAN for SDH");
00434 isInitialized_ = true;
00435 }
00436 else if (sdhdevicetype_.compare("ESD") == 0)
00437 {
00438 ROS_INFO("Starting initializing ESD");
00439 if (strcmp(sdhdevicestring_.c_str(), "/dev/can0") == 0)
00440 {
00441 ROS_INFO("Initializing ESD on device %s", sdhdevicestring_.c_str());
00442 sdh_->OpenCAN_ESD(0, baudrate_, timeout_, id_read_, id_write_);
00443 }
00444 else if (strcmp(sdhdevicestring_.c_str(), "/dev/can1") == 0)
00445 {
00446 ROS_INFO("Initializin ESD on device %s", sdhdevicestring_.c_str());
00447 sdh_->OpenCAN_ESD(1, baudrate_, timeout_, id_read_, id_write_);
00448 }
00449 else
00450 {
00451 ROS_ERROR("Currently only support for /dev/can0 and /dev/can1");
00452 res.success = false;
00453 res.message = "Currently only support for /dev/can0 and /dev/can1";
00454 return true;
00455 }
00456 ROS_INFO("Initialized ESDCAN for SDH");
00457 isInitialized_ = true;
00458 }
00459 else
00460 {
00461 ROS_ERROR("Unknown SDH device type: %s", sdhdevicetype_.c_str());
00462 res.success = false;
00463 res.message = "Unknown SDH device type: " + sdhdevicetype_;
00464 }
00465 }
00466 catch (SDH::cSDHLibraryException* e)
00467 {
00468 ROS_ERROR("An exception was caught: %s", e->what());
00469 res.success = false;
00470 res.message = e->what();
00471 delete e;
00472 return true;
00473 }
00474
00475
00476 if (!dsadevicestring_.empty())
00477 {
00478 try
00479 {
00480 dsa_ = new SDH::cDSA(dsa_dbg_level_, dsadevicenum_, dsadevicestring_.c_str());
00481
00482 dsa_->SetFramerate(1, true);
00483 ROS_INFO("Initialized RS232 for DSA Tactile Sensors on device %s", dsadevicestring_.c_str());
00484 for(int imat=0; imat<dsa_->GetSensorInfo().nb_matrices; imat++) {
00485 dsa_->SetMatrixSensitivity(imat, dsa_sensitivity_);
00486 }
00487 isDSAInitialized_ = true;
00488 }
00489 catch (SDH::cSDHLibraryException* e)
00490 {
00491 isDSAInitialized_ = false;
00492 ROS_ERROR("An exception was caught: %s", e->what());
00493 res.success = false;
00494 res.message = e->what();
00495 delete e;
00496 return true;
00497 }
00498 }
00499 if (!switchOperationMode(operationMode_))
00500 {
00501 res.success = false;
00502 res.message = "Could not set operation mode to '" + operationMode_ + "'";
00503 return true;
00504 }
00505 }
00506 else
00507 {
00508 ROS_WARN("...sdh already initialized...");
00509 res.success = true;
00510 res.message = "sdh already initialized";
00511 }
00512
00513 res.success = true;
00514 res.message = "SDH initialised";
00515 return true;
00516 }
00517
00525 bool srvCallback_Stop(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res)
00526 {
00527 ROS_INFO("Stopping sdh");
00528
00529
00530 try
00531 {
00532 sdh_->Stop();
00533 }
00534 catch (SDH::cSDHLibraryException* e)
00535 {
00536 ROS_ERROR("An exception was caught: %s", e->what());
00537 delete e;
00538 }
00539
00540 ROS_INFO("Stopping sdh succesfull");
00541 res.success = true;
00542 res.message = "stopped SDH";
00543 return true;
00544 }
00545
00553 bool srvCallback_SetOperationMode(cob_srvs::SetString::Request &req, cob_srvs::SetString::Response &res)
00554 {
00555 hasNewGoal_ = false;
00556 sdh_->Stop();
00557 ROS_INFO("Set operation mode to [%s]", req.data.c_str());
00558 operationMode_ = req.data;
00559 res.success = true;
00560 res.message = "Set operation mode to "+req.data;
00561 if (operationMode_ == "position")
00562 {
00563 sdh_->SetController(SDH::cSDH::eCT_POSE);
00564 }
00565 else if (operationMode_ == "velocity")
00566 {
00567 try
00568 {
00569 sdh_->SetController(SDH::cSDH::eCT_VELOCITY);
00570 sdh_->SetAxisEnable(sdh_->All, 1.0);
00571 }
00572 catch (SDH::cSDHLibraryException* e)
00573 {
00574 ROS_ERROR("An exception was caught: %s", e->what());
00575 delete e;
00576 }
00577 }
00578 else
00579 {
00580 ROS_ERROR_STREAM("Operation mode '" << req.data << "' not supported");
00581 }
00582 return true;
00583 }
00584
00592 bool srvCallback_EmergencyStop(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) {
00593 try {
00594 isInitialized_ = false;
00595 sdh_->EmergencyStop();
00596 sdh_->SetAxisEnable(sdh_->All, 0.0);
00597 sdh_->SetAxisMotorCurrent(sdh_->All, 0.0);
00598 }
00599 catch(const SDH::cSDHLibraryException* e) {
00600 ROS_ERROR("An exception was caught: %s", e->what());
00601 res.success = false;
00602 res.message = e->what();
00603 return true;
00604 }
00605
00606 res.success = true;
00607 res.message = "EMERGENCY stop";
00608 return true;
00609 }
00610
00618 bool srvCallback_Disconnect(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) {
00619 try {
00620 isInitialized_ = false;
00621 isDSAInitialized_ = false;
00622
00623 sdh_->SetAxisEnable(sdh_->All, 0.0);
00624 sdh_->SetAxisMotorCurrent(sdh_->All, 0.0);
00625
00626 sdh_->Close();
00627 dsa_->Close();
00628 }
00629 catch(const SDH::cSDHLibraryException* e) {
00630 ROS_ERROR("An exception was caught: %s", e->what());
00631 res.success = false;
00632 res.message = e->what();
00633 return true;
00634 }
00635
00636 ROS_INFO("Disconnected");
00637 res.success = true;
00638 res.message = "disconnected from SDH";
00639 return true;
00640 }
00641
00647 bool srvCallback_MotorPowerOn(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) {
00648 try {
00649 sdh_->SetAxisEnable(sdh_->All, 1.0);
00650 sdh_->SetAxisMotorCurrent(sdh_->All, 0.5);
00651 }
00652 catch (const SDH::cSDHLibraryException* e) {
00653 ROS_ERROR("An exception was caught: %s", e->what());
00654 res.success = false;
00655 res.message = e->what();
00656 return true;
00657 }
00658 ROS_INFO("Motor power ON");
00659 res.success = true;
00660 res.message = "Motor ON";
00661 return true;
00662 }
00663
00669 bool srvCallback_MotorPowerOff(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) {
00670 try {
00671 sdh_->SetAxisEnable(sdh_->All, 0.0);
00672 sdh_->SetAxisMotorCurrent(sdh_->All, 0.0);
00673 }
00674 catch (const SDH::cSDHLibraryException* e) {
00675 ROS_ERROR("An exception was caught: %s", e->what());
00676 res.success = false;
00677 res.message = e->what();
00678 return true;
00679 }
00680 ROS_INFO("Motor power OFF");
00681 res.success = true;
00682 res.message = "Motor OFF";
00683 return true;
00684 }
00685
00691 void updateSdh()
00692 {
00693 const ros::Time time = ros::Time::now();
00694 ROS_DEBUG("updateJointState");
00695 if (isInitialized_ == true)
00696 {
00697 if (hasNewGoal_ == true)
00698 {
00699
00700 try
00701 {
00702 sdh_->Stop();
00703 }
00704 catch (SDH::cSDHLibraryException* e)
00705 {
00706 ROS_ERROR("An exception was caught: %s", e->what());
00707 delete e;
00708 }
00709
00710 if (operationMode_ == "position")
00711 {
00712 ROS_DEBUG("moving sdh in position mode");
00713
00714 try
00715 {
00716 sdh_->SetAxisTargetAngle(axes_, targetAngles_);
00717 sdh_->MoveHand(false);
00718 }
00719 catch (SDH::cSDHLibraryException* e)
00720 {
00721 ROS_ERROR("An exception was caught: %s", e->what());
00722 delete e;
00723 }
00724 }
00725 else if (operationMode_ == "velocity")
00726 {
00727 ROS_DEBUG("moving sdh in velocity mode");
00728 try
00729 {
00730 sdh_->SetAxisTargetVelocity(axes_, velocities_);
00731
00732 }
00733 catch (SDH::cSDHLibraryException* e)
00734 {
00735 ROS_ERROR("An exception was caught: %s", e->what());
00736 delete e;
00737 }
00738 }
00739 else if (operationMode_ == "effort")
00740 {
00741 ROS_DEBUG("moving sdh in effort mode");
00742
00743 ROS_WARN("Moving in effort mode currently disabled");
00744 }
00745 else
00746 {
00747 ROS_ERROR("sdh neither in position nor in velocity nor in effort mode. OperationMode = [%s]",
00748 operationMode_.c_str());
00749 }
00750
00751 hasNewGoal_ = false;
00752 }
00753
00754
00755 std::vector<double> actualAngles;
00756 try
00757 {
00758 actualAngles = sdh_->GetAxisActualAngle(axes_);
00759 }
00760 catch (SDH::cSDHLibraryException* e)
00761 {
00762 ROS_ERROR("An exception was caught: %s", e->what());
00763 delete e;
00764 }
00765 std::vector<double> actualVelocities;
00766 try
00767 {
00768 actualVelocities = sdh_->GetAxisActualVelocity(axes_);
00769 }
00770 catch (SDH::cSDHLibraryException* e)
00771 {
00772 ROS_ERROR("An exception was caught: %s", e->what());
00773 delete e;
00774 }
00775
00776 ROS_DEBUG("received %d angles from sdh", static_cast<int>(actualAngles.size()));
00777
00778
00779 sensor_msgs::JointState msg;
00780 msg.header.stamp = time;
00781 msg.name.resize(DOF_);
00782 msg.position.resize(DOF_);
00783 msg.velocity.resize(DOF_);
00784 msg.effort.resize(DOF_);
00785
00786 msg.name = joint_names_;
00787
00788
00789 msg.position[0] = actualAngles[0] * pi_ / 180.0;
00790 msg.position[1] = actualAngles[3] * pi_ / 180.0;
00791 msg.position[2] = actualAngles[4] * pi_ / 180.0;
00792 msg.position[3] = actualAngles[5] * pi_ / 180.0;
00793 msg.position[4] = actualAngles[6] * pi_ / 180.0;
00794 msg.position[5] = actualAngles[1] * pi_ / 180.0;
00795 msg.position[6] = actualAngles[2] * pi_ / 180.0;
00796
00797 msg.velocity[0] = actualVelocities[0] * pi_ / 180.0;
00798 msg.velocity[1] = actualVelocities[3] * pi_ / 180.0;
00799 msg.velocity[2] = actualVelocities[4] * pi_ / 180.0;
00800 msg.velocity[3] = actualVelocities[5] * pi_ / 180.0;
00801 msg.velocity[4] = actualVelocities[6] * pi_ / 180.0;
00802 msg.velocity[5] = actualVelocities[1] * pi_ / 180.0;
00803 msg.velocity[6] = actualVelocities[2] * pi_ / 180.0;
00804
00805 topicPub_JointState_.publish(msg);
00806
00807
00808 sensor_msgs::JointState mimicjointmsg;
00809 mimicjointmsg.header.stamp = time;
00810 mimicjointmsg.name.resize(1);
00811 mimicjointmsg.position.resize(1);
00812 mimicjointmsg.velocity.resize(1);
00813 mimicjointmsg.name[0] = "sdh_finger_21_joint";
00814 mimicjointmsg.position[0] = msg.position[0];
00815 mimicjointmsg.velocity[0] = msg.velocity[0];
00816 topicPub_JointState_.publish(mimicjointmsg);
00817
00818
00819 control_msgs::JointTrajectoryControllerState controllermsg;
00820 controllermsg.header.stamp = time;
00821 controllermsg.joint_names.resize(DOF_);
00822 controllermsg.desired.positions.resize(DOF_);
00823 controllermsg.desired.velocities.resize(DOF_);
00824 controllermsg.actual.positions.resize(DOF_);
00825 controllermsg.actual.velocities.resize(DOF_);
00826 controllermsg.error.positions.resize(DOF_);
00827 controllermsg.error.velocities.resize(DOF_);
00828
00829 controllermsg.joint_names = joint_names_;
00830
00831
00832 if (targetAngles_.size() != 0)
00833 {
00834 controllermsg.desired.positions[0] = targetAngles_[0] * pi_ / 180.0;
00835 controllermsg.desired.positions[1] = targetAngles_[3] * pi_ / 180.0;
00836 controllermsg.desired.positions[2] = targetAngles_[4] * pi_ / 180.0;
00837 controllermsg.desired.positions[3] = targetAngles_[5] * pi_ / 180.0;
00838 controllermsg.desired.positions[4] = targetAngles_[6] * pi_ / 180.0;
00839 controllermsg.desired.positions[5] = targetAngles_[1] * pi_ / 180.0;
00840 controllermsg.desired.positions[6] = targetAngles_[2] * pi_ / 180.0;
00841 }
00842
00843
00844
00845 controllermsg.actual.positions = msg.position;
00846
00847 controllermsg.actual.velocities = msg.velocity;
00848
00849 for (int i = 0; i < DOF_; i++)
00850 {
00851 controllermsg.error.positions[i] = controllermsg.desired.positions[i] - controllermsg.actual.positions[i];
00852 controllermsg.error.velocities[i] = controllermsg.desired.velocities[i] - controllermsg.actual.velocities[i];
00853 }
00854
00855 topicPub_ControllerState_.publish(controllermsg);
00856
00857
00858 state_ = sdh_->GetAxisActualState(axes_);
00859
00860
00861 schunk_sdh::TemperatureArray temp_array;
00862 temp_array.header.stamp = time;
00863 const std::vector<double> temp_value = sdh_->GetTemperature(sdh_->all_temperature_sensors);
00864 if(temp_value.size()==temperature_names_.size()) {
00865 temp_array.name = temperature_names_;
00866 temp_array.temperature = temp_value;
00867 }
00868 else {
00869 ROS_ERROR("amount of temperatures mismatch with stored names");
00870 }
00871 topicPub_Temperature_.publish(temp_array);
00872 }
00873 else
00874 {
00875 ROS_DEBUG("sdh not initialized");
00876 }
00877
00878 diagnostic_msgs::DiagnosticArray diagnostics;
00879 diagnostics.status.resize(1);
00880
00881 if (isError_)
00882 {
00883 diagnostics.status[0].level = 2;
00884 diagnostics.status[0].name = "schunk_powercube_chain";
00885 diagnostics.status[0].message = "one or more drives are in Error mode";
00886 }
00887 else
00888 {
00889 if (isInitialized_)
00890 {
00891 diagnostics.status[0].level = 0;
00892 diagnostics.status[0].name = nh_.getNamespace();
00893 if (isDSAInitialized_)
00894 diagnostics.status[0].message = "sdh with tactile sensing initialized and running";
00895 else
00896 diagnostics.status[0].message = "sdh initialized and running, tactile sensors not connected";
00897 }
00898 else
00899 {
00900 diagnostics.status[0].level = 1;
00901 diagnostics.status[0].name = nh_.getNamespace();
00902 diagnostics.status[0].message = "sdh not initialized";
00903 }
00904 }
00905
00906 topicPub_Diagnostics_.publish(diagnostics);
00907 }
00908
00914 void updateDsa()
00915 {
00916 static const int dsa_reorder[6] = {2, 3, 4, 5, 0, 1};
00917 ROS_DEBUG("updateTactileData");
00918
00919 if (isDSAInitialized_)
00920 {
00921
00922 for (int i = 0; i < 7; i++)
00923 {
00924 try
00925 {
00926
00927 dsa_->UpdateFrame();
00928 }
00929 catch (SDH::cSDHLibraryException* e)
00930 {
00931 ROS_ERROR("An exception was caught: %s", e->what());
00932 delete e;
00933 }
00934 }
00935
00936 schunk_sdh::TactileSensor msg;
00937 msg.header.stamp = ros::Time::now();
00938 msg.tactile_matrix.resize(dsa_->GetSensorInfo().nb_matrices);
00939 for (int i = 0; i < dsa_->GetSensorInfo().nb_matrices; i++)
00940 {
00941 const int m = dsa_reorder[i];
00942 schunk_sdh::TactileMatrix &tm = msg.tactile_matrix[i];
00943 tm.matrix_id = i;
00944 tm.cells_x = dsa_->GetMatrixInfo(m).cells_x;
00945 tm.cells_y = dsa_->GetMatrixInfo(m).cells_y;
00946 tm.tactile_array.resize(tm.cells_x * tm.cells_y);
00947 for (uint y = 0; y < tm.cells_y; y++)
00948 {
00949 for (uint x = 0; x < tm.cells_x; x++)
00950 tm.tactile_array[tm.cells_x * y + x] = dsa_->GetTexel(m, x, y);
00951 }
00952 }
00953
00954 topicPub_TactileSensor_.publish(msg);
00955
00956
00957 schunk_sdh::PressureArrayList msg_pressure_list;
00958 msg_pressure_list.header.stamp = ros::Time::now();
00959 const SDH::cDSA::sSensorInfo &sensor_info = dsa_->GetSensorInfo();
00960 msg_pressure_list.pressure_list.resize(sensor_info.nb_matrices);
00961 for(const uint &fi : {0,1,2}) {
00962 for(const uint &part : {0,1}) {
00963
00964 const int mid = dsa_->GetMatrixIndex(fi, part);
00965 msg_pressure_list.pressure_list[mid].sensor_name = "sdh_"+finger_names_[fi]+std::to_string(part+2)+"_link";
00966
00967
00968 const SDH::cDSA::sMatrixInfo &matrix_info = dsa_->GetMatrixInfo(mid);
00969 msg_pressure_list.pressure_list[mid].cells_x = matrix_info.cells_x;
00970 msg_pressure_list.pressure_list[mid].cells_y = matrix_info.cells_y;
00971 msg_pressure_list.pressure_list[mid].pressure.resize(matrix_info.cells_x * matrix_info.cells_y);
00972
00973 for (uint y(0); y < matrix_info.cells_y; y++) {
00974 for (uint x(0); x < matrix_info.cells_x; x++) {
00975
00976 msg_pressure_list.pressure_list[mid].pressure[matrix_info.cells_x * y + x] =
00977 dsa_->GetTexel(mid, x, y) * dsa_calib_pressure_ / dsa_calib_voltage_ * 1e6;
00978 }
00979 }
00980 }
00981 }
00982 topicPub_Pressure_.publish(msg_pressure_list);
00983 }
00984 }
00985 };
00986
00987 const std::vector<std::string> SdhNode::temperature_names_ = {
00988 "root",
00989 "proximal_finger_1", "distal_finger_1",
00990 "proximal_finger_2", "distal_finger_2",
00991 "proximal_finger_3", "distal_finger_3",
00992 "controller", "pcb"
00993 };
00994
00995 const std::vector<std::string> SdhNode::finger_names_ = {
00996 "finger_2", "thumb_", "finger_1"
00997 };
00998
00999
01005 int main(int argc, char** argv)
01006 {
01007
01008 ros::init(argc, argv, "schunk_sdh");
01009
01010
01011 SdhNode sdh_node(ros::this_node::getName() + "/follow_joint_trajectory");
01012 if (!sdh_node.init())
01013 return 0;
01014
01015 ROS_INFO("...sdh node running...");
01016
01017 double frequency;
01018 if (sdh_node.nh_.hasParam("frequency"))
01019 {
01020 sdh_node.nh_.getParam("frequency", frequency);
01021 }
01022 else
01023 {
01024 frequency = 5;
01025 ROS_WARN("Parameter frequency not available, setting to default value: %f Hz", frequency);
01026 }
01027
01028
01029 ros::Rate loop_rate(frequency);
01030 while (sdh_node.nh_.ok())
01031 {
01032
01033 sdh_node.updateSdh();
01034
01035
01036 sdh_node.updateDsa();
01037
01038
01039 ros::spinOnce();
01040 loop_rate.sleep();
01041 }
01042
01043 return 0;
01044 }