SchunkCanopenNode.cpp
Go to the documentation of this file.
00001 // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
00002 
00003 // -- BEGIN LICENSE BLOCK ----------------------------------------------
00004 // This file is part of the SCHUNK Canopen Driver suite.
00005 //
00006 // This program is free software licensed under the LGPL
00007 // (GNU LESSER GENERAL PUBLIC LICENSE Version 3).
00008 // You can find a copy of this license in LICENSE folder in the top
00009 // directory of the source code.
00010 //
00011 // © Copyright 2016 SCHUNK GmbH, Lauffen/Neckar Germany
00012 // © Copyright 2016 FZI Forschungszentrum Informatik, Karlsruhe, Germany
00013 // -- END LICENSE BLOCK ------------------------------------------------
00014 
00015 //----------------------------------------------------------------------
00022 //----------------------------------------------------------------------
00023 
00024 
00025 #include "control_msgs/FollowJointTrajectoryActionFeedback.h"
00026 #include "ros/service_server.h"
00027 #include "std_msgs/Int16MultiArray.h"
00028 
00029 
00030 #include "schunk_canopen_driver/SchunkCanopenNode.h"
00031 #include <icl_hardware_canopen/SchunkPowerBallNode.h>
00032 #include "icl_core_logging/Logging.h"
00033 
00034 
00035 
00036 
00037 SchunkCanopenNode::SchunkCanopenNode()
00038   : m_priv_nh("~"),
00039     m_action_server(m_priv_nh, "follow_joint_trajectory",
00040     boost::bind(&SchunkCanopenNode::goalCB, this, _1),
00041     boost::bind(&SchunkCanopenNode::cancelCB, this, _1), false),
00042     m_has_goal(false),
00043     m_use_ros_control(false),
00044     m_was_disabled(false),
00045     m_is_enabled(false),
00046     m_homing_active(false),
00047     m_nodes_initialized(false)
00048 {
00049   std::string can_device_name;
00050   int frequency = 30;
00051   std::vector<std::string> chain_names;
00052   std::map<std::string, std::vector<int> > chain_configuratuions;
00053   m_chain_handles.clear();
00054   sensor_msgs::JointState joint_msg;
00055   bool autostart = true;
00056 
00057   m_priv_nh.getParam("chain_names", chain_names);
00058   ros::param::get("~use_ros_control", m_use_ros_control);
00059   m_priv_nh.param<std::string>("can_device_name", can_device_name, "auto");
00060   m_priv_nh.param<float>  ("ppm_profile_velocity",       m_ppm_config.profile_velocity,         0.2);
00061   m_priv_nh.param<float>  ("ppm_profile_acceleration",   m_ppm_config.profile_acceleration,     0.2);
00062   m_priv_nh.param<bool>   ("ppm_use_relative_targets",   m_ppm_config.use_relative_targets,     false);
00063   m_priv_nh.param<bool>   ("ppm_change_set_immediately", m_ppm_config.change_set_immediately,   true);
00064   m_priv_nh.param<bool>   ("ppm_use_blending",           m_ppm_config.use_blending,             true);
00065   m_priv_nh.param<bool>   ("autostart",                  autostart,                             true);
00066   m_priv_nh.param<std::string>("traj_controller_name", m_traj_controller_name, "pos_based_pos_traj_controller");
00067 
00068   // Create a canopen controller
00069   try
00070   {
00071     m_controller = boost::make_shared<CanOpenController>(can_device_name);
00072   }
00073   catch (const DeviceException& e)
00074   {
00075     ROS_ERROR_STREAM ("Initializing CAN device failed. Reason: " << e.what());
00076     ROS_INFO ("Shutting down now.");
00077     return;
00078   }
00079 
00080   // Load SCHUNK powerball specific error codes
00081   char const* tmp = std::getenv("CANOPEN_RESOURCE_PATH");
00082   if (tmp == NULL)
00083   {
00084     LOGGING_WARNING_C(
00085         CanOpen,
00086         CanOpenController,
00087         "The environment variable 'CANOPEN_RESOURCE_PATH' could not be read. No Schunk specific error codes will be used." << endl);
00088   }
00089   else
00090   {
00091     std::string emcy_emergency_errors_filename = boost::filesystem::path(tmp / boost::filesystem::path("EMCY_schunk.ini")).string();
00092     EMCY::addEmergencyErrorMap( emcy_emergency_errors_filename, "schunk_error_codes");
00093   }
00094 
00095 
00096   // Get chain configuration from parameter server
00097   ROS_INFO_STREAM ("Can device identifier: " << can_device_name);
00098   ROS_INFO_STREAM ("Found " << chain_names.size() << " chains");
00099 
00100   for (size_t i = 0; i < chain_names.size(); ++i)
00101   {
00102     std::string name = "chain_" + chain_names[i];
00103     m_controller->addGroup<DS402Group>(chain_names[i]);
00104     m_chain_handles.push_back(m_controller->getGroup<DS402Group>(chain_names[i]));
00105     std::vector<int> chain;
00106     try
00107     {
00108       m_priv_nh.getParam(name, chain);
00109     }
00110     catch (ros::InvalidNameException e)
00111     {
00112       ROS_ERROR_STREAM("Parameter Error!");
00113     }
00114     if (chain.size() == 0)
00115     {
00116       ROS_ERROR_STREAM("Did not find device list for chain " << chain_names[i] << ". Make sure, that an entry " << name << " exists.");
00117       continue;
00118     }
00119 
00120     // Get the chain type
00121     name = "chain_" + chain_names[i] + "_type";
00122     std::string chain_type = "PowerBall";
00123     try
00124     {
00125       m_priv_nh.getParam(name, chain_type);
00126     }
00127     catch (ros::InvalidNameException e)
00128     {
00129       ROS_ERROR_STREAM("Parameter Error!");
00130     }
00131 
00132     // Get the chain transmission factor
00133     name = "chain_" + chain_names[i] + "_transmission_factor";
00134     double transmission_factor = 1.0;
00135     try
00136     {
00137       m_priv_nh.getParam(name, transmission_factor);
00138       if (chain_type != "PowerBall")
00139       {
00140         ROS_INFO_STREAM ("Chain transmission factor is " << transmission_factor);
00141       }
00142     }
00143     catch (ros::InvalidNameException e)
00144     {
00145       ROS_ERROR_STREAM("Parameter Error!");
00146     }
00147 
00148 
00149     ROS_INFO_STREAM ("Found chain with name " << chain_names[i] << " of type " << chain_type << " containing " << chain.size() << " nodes.");
00150     chain_configuratuions[name] = chain;
00151     for (size_t j = 0; j < chain.size(); ++j)
00152     {
00153       if (chain_type == "DS402")
00154       {
00155         m_controller->addNode<DS402Node>(chain[j], chain_names[i]);
00156         m_controller->getNode<DS402Node>(chain[j])->setTransmissionFactor(transmission_factor);
00157       }
00158       else
00159       {
00160         m_controller->addNode<SchunkPowerBallNode>(chain[j], chain_names[i]);
00161       }
00162 
00163       std::string joint_name = "";
00164       std::string mapping_key = "~node_mapping_" + boost::lexical_cast<std::string>( chain[j]);
00165       ros::param::get(mapping_key, joint_name);
00166 
00167       m_joint_name_mapping[joint_name] =  static_cast<uint8_t>(chain[j]);
00168       joint_msg.name.push_back(joint_name);
00169     }
00170   }
00171 
00172   if (autostart)
00173   {
00174     initDevices();
00175   }
00176   else
00177   {
00178     m_init_service = m_priv_nh.advertiseService("init_devices",
00179      &SchunkCanopenNode::initDevicesCb, this);
00180   }
00181 
00182 
00183   ros::Rate loop_rate(frequency);
00184 
00185   DS402Node::Ptr node;
00186   std_msgs::Int16MultiArray currents;
00187 
00188   // The robot's status (joint values and currents) will be published periodically in here
00189   while (ros::ok())
00190   {
00191     ros::spinOnce();
00192 
00193     if (m_nodes_initialized)
00194     {
00195       joint_msg.position.clear();
00196       currents.data.clear();
00197       // TODO: Go over all groups. to handle different things. For example, we might not want to
00198       // read the current from a gripper...
00199       for (std::map<std::string, uint8_t>::iterator it = m_joint_name_mapping.begin();
00200            it != m_joint_name_mapping.end();
00201            ++it)
00202       {
00203         const uint8_t& nr = it->second;
00204         node = m_controller->getNode<DS402Node>(nr);
00205         joint_msg.position.push_back(node->getTargetFeedback());
00206 
00207         // Schunk nodes write currents into the torque_actual register
00208         try
00209         {
00210           currents.data.push_back(node->getTPDOValue<int16_t>("measured_torque"));
00211         }
00212         catch (PDOException& e)
00213         {
00214           ROS_ERROR_STREAM(e.what());
00215           currents.data.push_back(0);
00216         }
00217       }
00218       joint_msg.header.stamp = ros::Time::now();
00219       m_joint_pub.publish(joint_msg);
00220 
00221       m_current_pub.publish(currents);
00222     }
00223     loop_rate.sleep();
00224   }
00225 }
00226 
00227 
00228 bool SchunkCanopenNode::initDevicesCb(std_srvs::TriggerRequest& req,
00229                                       std_srvs::TriggerResponse& resp)
00230 {
00231   initDevices();
00232   resp.success = true;
00233   return resp.success;
00234 }
00235 
00236 
00237 void SchunkCanopenNode::initDevices()
00238 {
00239 
00240   // initialize all nodes, by default this will start ProfilePosition mode, so we're good to enable nodes
00241   try {
00242     m_controller->initNodes();
00243   }
00244   catch (const ProtocolException& e)
00245   {
00246     ROS_ERROR_STREAM ("Caught ProtocolException while initializing devices: " << e.what());
00247     ROS_INFO ("Going to shut down now");
00248     exit (-1);
00249   }
00250   catch (const PDOException& e)
00251   {
00252     ROS_ERROR_STREAM ("Caught PDOException while initializing devices: " << e.what());
00253     ROS_INFO ("Going to shut down now");
00254     exit (-1);
00255   }
00256 
00257   ds402::eModeOfOperation mode = ds402::MOO_PROFILE_POSITION_MODE;
00258   if (m_use_ros_control)
00259   {
00260     mode = ds402::MOO_INTERPOLATED_POSITION_MODE;
00261     m_hardware_interface.reset(
00262       new SchunkCanopenHardwareInterface(m_pub_nh, m_controller));
00263     m_controller_manager.reset(
00264       new controller_manager::ControllerManager( m_hardware_interface.get(), m_pub_nh));
00265 
00266   }
00267 
00268   // Start interface (either action server or ros_control)
00269 
00270   if (m_use_ros_control)
00271   {
00272     m_ros_control_thread = boost::thread(&SchunkCanopenNode::rosControlLoop, this);
00273   }
00274   else
00275   {
00276     for (size_t i = 0; i < m_chain_handles.size(); ++i)
00277     {
00278       try {
00279         m_chain_handles[i]->setupProfilePositionMode(m_ppm_config);
00280         m_chain_handles[i]->enableNodes(mode);
00281       }
00282       catch (const ProtocolException& e)
00283       {
00284         ROS_ERROR_STREAM ("Caught ProtocolException while enabling nodes from chain " <<
00285           m_chain_handles[i]->getName() << ". Nodes from this group won't be enabled.");
00286         continue;
00287       }
00288       ROS_INFO_STREAM ("Enabled nodes from chain " << m_chain_handles[i]->getName());
00289     }
00290     m_action_server.start();
00291   }
00292 
00293   // Create joint state publisher
00294   m_joint_pub = m_pub_nh.advertise<sensor_msgs::JointState>("joint_states", 1);
00295   m_current_pub = m_pub_nh.advertise<std_msgs::Int16MultiArray>("joint_currents", 1);
00296 
00297   // services
00298   m_enable_service =  m_priv_nh.advertiseService("enable_nodes", &SchunkCanopenNode::enableNodes, this);
00299   m_close_brakes_service =  m_priv_nh.advertiseService("close_brakes",
00300      &SchunkCanopenNode::closeBrakes, this);
00301   m_quick_stop_service =  m_priv_nh.advertiseService("quick_stop_nodes",
00302      &SchunkCanopenNode::quickStopNodes, this);
00303   m_home_service_all = m_priv_nh.advertiseService("home_reset_offset_all",
00304      &SchunkCanopenNode::homeAllNodes, this);
00305   m_home_service_joint_names = m_priv_nh.advertiseService("home_reset_offset_by_id",
00306      &SchunkCanopenNode::homeNodesCanIds, this);
00307   m_home_service_canopen_ids = m_priv_nh.advertiseService("home_reset_offset_by_name",
00308      &SchunkCanopenNode::homeNodesJointNames, this);
00309 
00310   m_nodes_initialized = true;
00311 }
00312 
00313 
00314 void SchunkCanopenNode::goalCB (actionlib::ServerGoalHandle< control_msgs::FollowJointTrajectoryAction > gh)
00315 {
00316   ROS_INFO ("Executing Trajectory action");
00317 
00318   /* TODO: Catch errors:
00319    * - Joint not enabled
00320    * - EmergencyStopState
00321    * - Overwriting trajectory
00322    * - invalid positions
00323    */
00324 
00325 
00326   if (gh.getGoal()->trajectory.joint_names.size() != gh.getGoal()->trajectory.points[0].positions.size())
00327   {
00328     ROS_ERROR_STREAM ("Number of given joint names (" << gh.getGoal()->trajectory.joint_names.size() <<
00329       ") and joint states (" << gh.getGoal()->trajectory.points.size() << ") do not match! Aborting goal!");
00330     return;
00331   }
00332 
00333   if (m_has_goal)
00334   {
00335     ROS_WARN_STREAM ("Sent a new goal while another goal was still running. Depending on the " <<
00336       "device configuration this will either result in a queuing or the current trajectory " <<
00337       "will be overwritten."
00338     );
00339   }
00340 
00341 
00342   gh.setAccepted();
00343   m_has_goal = true;
00344   m_traj_thread = boost::thread(&SchunkCanopenNode::trajThread, this, gh);
00345 
00346 }
00347 
00348 void SchunkCanopenNode::trajThread(actionlib::ServerGoalHandle< control_msgs::FollowJointTrajectoryAction >& gh)
00349 {
00350   control_msgs::FollowJointTrajectoryActionResult result;
00351   control_msgs::FollowJointTrajectoryActionFeedback feedback;
00352   feedback.feedback.header = gh.getGoal()->trajectory.header;
00353   result.header = gh.getGoal()->trajectory.header;
00354 
00355   ros::Time start = ros::Time::now();
00356   bool targets_reached = true;
00357 
00358   for (size_t waypoint = 0; waypoint < gh.getGoal()->trajectory.points.size(); ++waypoint)
00359   {
00360     feedback.feedback.desired.positions.clear();
00361     feedback.feedback.joint_names.clear();
00362     feedback.feedback.actual.positions.clear();
00363     for (size_t i = 0; i < gh.getGoal()->trajectory.joint_names.size(); ++i)
00364     {
00365 
00366       uint8_t nr = m_joint_name_mapping[gh.getGoal()->trajectory.joint_names[i]];
00367       float pos = boost::lexical_cast<float>(gh.getGoal()->trajectory.points[waypoint].positions[i]);
00368       ROS_INFO_STREAM ("Joint " << static_cast<int>(nr) << ": " << pos);
00369       SchunkPowerBallNode::Ptr node;
00370       try
00371       {
00372         node = m_controller->getNode<SchunkPowerBallNode>(nr);
00373       }
00374       catch (const NotFoundException& e)
00375       {
00376         ROS_ERROR_STREAM ("One or more nodes could not be found in the controller. " << e.what());
00377         result.result.error_code = -2;
00378         result.result.error_string = e.what();
00379         gh.setAborted(result.result);
00380         return;
00381       }
00382       m_controller->getNode<SchunkPowerBallNode>(nr)->setTarget(pos);
00383       feedback.feedback.desired.positions.push_back(pos);
00384       feedback.feedback.joint_names.push_back(gh.getGoal()->trajectory.joint_names[i]);
00385 
00386 
00387       pos = node->getTargetFeedback();
00388       feedback.feedback.actual.positions.push_back(pos);
00389     }
00390 
00391     m_controller->syncAll();
00392     m_controller->enablePPMotion();
00393 
00394     ros::Duration max_time = gh.getGoal()->goal_time_tolerance;
00395 
00396     ros::Duration spent_time = start - start;
00397     std::vector<bool> reached_vec;
00398 
00399     // Give the brakes time to open up
00400     sleep(1);
00401 
00402     while ( spent_time < max_time || max_time.isZero() )
00403     {
00404       try {
00405         m_controller->syncAll();
00406       }
00407       catch (const std::exception& e)
00408       {
00409         ROS_ERROR_STREAM (e.what());
00410         gh.setAborted();
00411         return;
00412       }
00413       usleep(10000);
00414 
00415       bool waypoint_reached = true;
00416       for (size_t i = 0; i < gh.getGoal()->trajectory.joint_names.size(); ++i)
00417       {
00418         uint8_t nr = m_joint_name_mapping[gh.getGoal()->trajectory.joint_names[i]];
00419         SchunkPowerBallNode::Ptr node = m_controller->getNode<SchunkPowerBallNode>(nr);
00420         waypoint_reached &= node->isTargetReached();
00421         float pos = node->getTargetFeedback();
00422   //       ROS_INFO_STREAM ("Node " << nr << " target reached: " << waypoint_reached <<
00423   //         ", position is: " << pos
00424   //       );
00425         feedback.feedback.actual.time_from_start = spent_time;
00426         feedback.feedback.actual.positions.at(i) = (pos);
00427       }
00428 
00429 
00430       gh.publishFeedback(feedback.feedback);
00431       targets_reached = waypoint_reached;
00432 
00433 
00434 
00435       if (waypoint_reached)
00436       {
00437         ROS_INFO_STREAM ("Waypoint " << waypoint <<" reached" );
00438         break;
00439       }
00440       spent_time = ros::Time::now() - start;
00441       if (spent_time > max_time && !max_time.isZero())
00442       {
00443         result.result.error_code = -5;
00444         result.result.error_string = "Did not reach targets in specified time";
00445         gh.setAborted();
00446         m_has_goal = false;
00447         return;
00448       }
00449       if (boost::this_thread::interruption_requested() )
00450       {
00451         ROS_ERROR ("Interruption requested");
00452         m_has_goal = false;
00453         return;
00454       }
00455     }
00456   }
00457 
00458   if (targets_reached)
00459   {
00460     ROS_INFO ("All targets reached" );
00461     result.result.error_code = 0;
00462     result.result.error_string = "All targets successfully reached";
00463     gh.setSucceeded(result.result);
00464   }
00465   else
00466   {
00467     ROS_INFO ("Not all targets reached" );
00468     result.result.error_code = -5;
00469     result.result.error_string = "Did not reach targets in specified time";
00470     gh.setAborted();
00471   }
00472   m_has_goal = false;
00473 }
00474 
00475 
00476 void SchunkCanopenNode::cancelCB (actionlib::ServerGoalHandle< control_msgs::FollowJointTrajectoryAction > gh)
00477 {
00478   m_is_enabled = false;
00479   ROS_INFO ("Canceling Trajectory action");
00480 
00481   m_traj_thread.interrupt();
00482   ROS_INFO ("Stopped trajectory thread");
00483 
00484   for (size_t i = 0; i < m_chain_handles.size(); ++i)
00485   {
00486     m_chain_handles[i]->quickStop();
00487   }
00488   ROS_INFO ("Device stopped");
00489   sleep(1);
00490   for (size_t i = 0; i < m_chain_handles.size(); ++i)
00491   {
00492     m_chain_handles[i]->enableNodes();
00493     m_is_enabled = true;
00494   }
00495 
00496   control_msgs::FollowJointTrajectoryActionResult result;
00497   result.result.error_code = 0;
00498   result.result.error_string = "Trajectory preempted by user";
00499 
00500   gh.setCanceled(result.result);
00501 }
00502 
00503 void SchunkCanopenNode::rosControlLoop()
00504 {
00505   ros::Duration elapsed_time;
00506   ros::Time last_time = ros::Time::now();
00507   ros::Time current_time = ros::Time::now();
00508 
00509   m_controller->syncAll();
00510   sleep(0.5);
00511 
00512   for (size_t i = 0; i < m_chain_handles.size(); ++i)
00513   {
00514     try {
00515       m_chain_handles[i]->enableNodes(ds402::MOO_INTERPOLATED_POSITION_MODE);
00516     }
00517     catch (const ProtocolException& e)
00518     {
00519       ROS_ERROR_STREAM ("Caught ProtocolException while enabling nodes");
00520       continue;
00521     }
00522     m_controller->syncAll();
00523   }
00524   m_is_enabled = true;
00525 
00526   size_t counter = 0;
00527 
00528   while (ros::ok() && !m_homing_active) {
00529     current_time = ros::Time::now();
00530     elapsed_time = current_time - last_time;
00531     last_time = current_time;
00532     // Input
00533     m_hardware_interface->read();
00534     sensor_msgs::JointState joint_msg = m_hardware_interface->getJointMessage();
00535     if (m_joint_pub)
00536     {
00537       m_joint_pub.publish (joint_msg);
00538     }
00539     if (m_hardware_interface->isFault() && m_is_enabled)
00540     {
00541       m_controller_manager->getControllerByName(m_traj_controller_name)->stopRequest(ros::Time::now());
00542       ROS_ERROR ("Some nodes are in FAULT state! No commands will be sent. Once the fault is removed, call the enable_nodes service.");
00543       m_is_enabled = false;
00544     }
00545     // Control
00546     m_controller->syncAll();
00547     if (m_was_disabled && m_is_enabled)
00548     {
00549       ROS_INFO ("Recovering from FAULT state. Resetting controller");
00550       m_controller_manager->update(current_time, elapsed_time, true);
00551       m_was_disabled = false;
00552     }
00553     else if (m_is_enabled)
00554     {
00555       m_controller_manager->update(current_time, elapsed_time);
00556       /* Give the controller some time, otherwise it will send a 0 waypoint vector which
00557        * might lead into a following error, if joints are not at 0
00558        * TODO: Find a better solution for that.
00559        */
00560       if (counter > 20)
00561       {
00562       // Output
00563         m_hardware_interface->write(current_time, elapsed_time);
00564       }
00565     }
00566 
00567 //     node->printStatus();
00568 
00569 
00570     ++counter;
00571     usleep(10000);
00572   }
00573 
00574   ROS_INFO ("Shutting down ros_control_loop thread.");
00575 }
00576 
00577 bool SchunkCanopenNode::enableNodes(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& resp)
00578 {
00579   m_controller_manager->getControllerByName(m_traj_controller_name)->startRequest(ros::Time::now());
00580   try
00581   {
00582     for (size_t i = 0; i < m_chain_handles.size(); ++i)
00583     {
00584       m_chain_handles[i]->enableNodes();
00585     }
00586   }
00587   catch (const ProtocolException& e)
00588   {
00589     ROS_ERROR_STREAM ( "Error while enabling nodes: " << e.what());
00590   }
00591   m_was_disabled = true;
00592   m_is_enabled = true;
00593   resp.success = true;
00594   return true;
00595 }
00596 
00597 bool SchunkCanopenNode::closeBrakes(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& resp)
00598 {
00599   try
00600   {
00601     for (size_t i = 0; i < m_chain_handles.size(); ++i)
00602     {
00603       m_chain_handles[i]->closeBrakes();
00604     }
00605   }
00606   catch (const ProtocolException& e)
00607   {
00608     ROS_ERROR_STREAM ( "Error while enabling nodes: " << e.what());
00609   }
00610   resp.success = true;
00611   m_was_disabled = true;
00612   m_is_enabled = false;
00613   m_controller_manager->getControllerByName(m_traj_controller_name)->stopRequest(ros::Time::now());
00614 
00615   ROS_INFO ("Closed brakes for all nodes. For reenabling, please use the enable_nodes service. Thank you for your attention.");
00616   return true;
00617 }
00618 
00619 bool SchunkCanopenNode::quickStopNodes(std_srvs::TriggerRequest& req, std_srvs::TriggerResponse& resp)
00620 {
00621   m_is_enabled = false;
00622   if (!m_use_ros_control)
00623   {
00624     m_traj_thread.interrupt();
00625     ROS_INFO ("Stopped trajectory thread");
00626   }
00627 
00628   try
00629   {
00630     for (size_t i = 0; i < m_chain_handles.size(); ++i)
00631     {
00632       m_chain_handles[i]->quickStop();
00633     }
00634   }
00635   catch (const ProtocolException& e)
00636   {
00637     ROS_ERROR_STREAM ( "Error while quick stopping nodes: " << e.what());
00638   }
00639   resp.success = true;
00640   m_was_disabled = false;
00641   m_controller_manager->getControllerByName(m_traj_controller_name)->stopRequest(ros::Time::now());
00642   ROS_INFO ("Quick stopped all nodes. For reenabling, please use the enable_nodes service. Thank you for your attention.");
00643   return true;
00644 }
00645 
00646 bool SchunkCanopenNode::homeAllNodes(schunk_canopen_driver::HomeAllRequest& req,
00647                                      schunk_canopen_driver::HomeAllResponse& resp)
00648 {
00649   schunk_canopen_driver::HomeWithIDsRequest req_fwd;
00650   schunk_canopen_driver::HomeWithIDsResponse resp_fwd;
00651   req_fwd.node_ids = m_controller->getNodeList();
00652 
00653 
00654   homeNodesCanIds (req_fwd, resp_fwd);
00655   resp.success = resp_fwd.success;
00656   return resp.success;
00657 }
00658 
00659 
00660 bool SchunkCanopenNode::homeNodesJointNames(schunk_canopen_driver::HomeWithJointNamesRequest& req,
00661                                             schunk_canopen_driver::HomeWithJointNamesResponse& resp)
00662 {
00663   schunk_canopen_driver::HomeWithIDsRequest req_fwd;
00664   schunk_canopen_driver::HomeWithIDsResponse resp_fwd;
00665   for (std::vector<std::string>::iterator it = req.joint_names.begin();
00666        it != req.joint_names.end();
00667   ++it)
00668   {
00669     if (m_joint_name_mapping.find(*it) != m_joint_name_mapping.end())
00670     {
00671       req_fwd.node_ids.push_back(m_joint_name_mapping[*it]);
00672     }
00673     else
00674     {
00675       ROS_ERROR_STREAM ("Could not find joint " << *it << ". No homing will be performed for this joint.");
00676     }
00677   }
00678   homeNodesCanIds (req_fwd, resp_fwd);
00679   resp.success = resp_fwd.success;
00680   return resp.success;
00681 }
00682 
00683 
00684 bool SchunkCanopenNode::homeNodesCanIds(schunk_canopen_driver::HomeWithIDsRequest& req,
00685                                         schunk_canopen_driver::HomeWithIDsResponse& resp)
00686 {
00687   try
00688   {
00689     for (size_t i = 0; i < m_chain_handles.size(); ++i)
00690     {
00691       m_chain_handles[i]->disableNodes();
00692     }
00693   }
00694   catch (const ProtocolException& e)
00695   {
00696     ROS_ERROR_STREAM ( "Error while stopping nodes: " << e.what());
00697   }
00698 
00699   m_homing_active = true;
00700   m_is_enabled = false;
00701 
00702   for (std::vector<uint8_t>::iterator it = req.node_ids.begin(); it != req.node_ids.end(); ++it)
00703   {
00704     const uint8_t& id = *it;
00705     SchunkPowerBallNode::Ptr node = m_controller->getNode<SchunkPowerBallNode>(id);
00706     try
00707     {
00708       node->home();
00709     }
00710     catch (const DeviceException& e)
00711     {
00712       ROS_ERROR_STREAM ( "Error while homing node " << static_cast<int>(id) << ": " << e.what());
00713     }
00714   }
00715 
00716   if (m_use_ros_control)
00717   {
00718     m_ros_control_thread = boost::thread(&SchunkCanopenNode::rosControlLoop, this);
00719   }
00720 
00721   m_homing_active = false;
00722   m_was_disabled = true;
00723   m_is_enabled = true;
00724   resp.success = true;
00725   return resp.success;
00726 }
00727 
00728 
00729 int main(int argc, char **argv)
00730 {
00731   ros::init(argc, argv, "schunk_chain");
00732   icl_core::logging::initialize(argc, argv);
00733 
00734   SchunkCanopenNode my_schunk_node;
00735 
00736   return 0;
00737 }


schunk_canopen_driver
Author(s): Felix Mauch , Georg Heppner
autogenerated on Sun May 22 2016 03:30:56