SVHNode.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 SVH 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 2014 SCHUNK Mobile Greifsysteme GmbH, Lauffen/Neckar Germany
00012 // © Copyright 2014 FZI Forschungszentrum Informatik, Karlsruhe, Germany
00013 //
00014 // -- END LICENSE BLOCK ------------------------------------------------
00015 
00016 //----------------------------------------------------------------------
00024 //----------------------------------------------------------------------
00025 // ROS includes.
00026 #include <ros/ros.h>
00027 #include <string>
00028 
00029 // Custom includes
00030 #include "DynamicParameterClass.h"
00031 #include "SVHNode.h"
00032 #include <icl_core/EnumHelper.h>
00033 
00034 
00035 /*--------------------------------------------------------------------
00036  * Callback functions
00037  *------------------------------------------------------------------*/
00038 
00039 SVHNode::SVHNode(const ros::NodeHandle& nh)
00040 {
00041   //==========
00042   // Params
00043   //==========
00044 
00045   bool autostart, use_internal_logging;
00046   int reset_timeout;
00047   std::vector<bool> disable_flags(driver_svh::eSVH_DIMENSION, false);
00048   // Config that contains the log stream configuration without the file names
00049   std::string logging_config_file;
00050 
00051   // Parameters that depend on the hardware version of the hand.
00052   XmlRpc::XmlRpcValue dynamic_parameters;
00053 
00054   uint16_t manual_major_version = 0;
00055   int manual_major_version_int = 0;
00056   uint16_t manual_minor_version = 0;
00057   int manual_minor_version_int = 0;
00058 
00059   try
00060   {
00061     nh.param<bool>("autostart", autostart, false);
00062     nh.param<bool>("use_internal_logging", use_internal_logging, false);
00063     nh.param<std::string>("serial_device", serial_device_name_, "/dev/ttyUSB0");
00064     // Note : Wrong values (like numerics) in the launch file will lead to a "true" value here
00065     nh.getParam("disable_flags", disable_flags);
00066     nh.param<int>("reset_timeout", reset_timeout, 5);
00067     nh.getParam("logging_config", logging_config_file);
00068     nh.param<std::string>("name_prefix", name_prefix, "left_hand");
00069     nh.param<int>("connect_retry_count", connect_retry_count, 3);
00070     nh.getParam("VERSIONS_PARAMETERS", dynamic_parameters);
00071     nh.param<int>("use_major_version", manual_major_version_int, 0);
00072     manual_major_version = static_cast<uint16_t>(manual_major_version_int);
00073     nh.param<int>("use_minor_version", manual_minor_version_int, 0);
00074     manual_minor_version = static_cast<uint16_t>(manual_minor_version_int);
00075   }
00076   catch (ros::InvalidNameException e)
00077   {
00078     ROS_ERROR("Parameter Error!");
00079   }
00080 
00081   // Tell the user what we are using
00082   ROS_INFO("Name prefix for this Hand was set to :%s", name_prefix.c_str());
00083 
00084   // Initialize the icl_library logging framework ( THIS NEEDS TO BE DONE BEFORE ANY LIB OBJECT IS
00085   // CREATED)
00086   if (use_internal_logging)
00087   {
00088     // Fake an input to the logging call to tell it where to look for the logging config
00089 
00090     // Strdup to create non const chars as it is required by the initialize function.
00091     // not really beatiful but it works.
00092     char* argv[] = {strdup("Logging"), strdup("-c"), strdup(logging_config_file.c_str())};
00093     int argc     = 3; // number of elements above
00094 
00095     // In case the file is not present (event though the parameter is) the logging will just put out
00096     // a
00097     // warning so we dont need to check it further. However the log level will only be Info (out of
00098     // the available Error, Warning, Info, Debug, Trace)
00099     // in that case also the log files will be disregarded
00100     if (icl_core::logging::initialize(argc, argv))
00101     {
00102       ROS_INFO("Internal logging was activated, output will be written as configured in "
00103                "logging.xml (default to ~/.ros/log)");
00104     }
00105     else
00106     {
00107       ROS_WARN("Internal logging was enabled but config file could not be read. Please make sure "
00108                "the provided path to the config file is correct.");
00109     }
00110   }
00111   else
00112   {
00113     icl_core::logging::initialize();
00114   }
00115 
00116   for (size_t i = 0; i < 9; ++i)
00117   {
00118     if (disable_flags[i])
00119     {
00120       ROS_WARN_STREAM("svh_controller disabling channel nr " << i);
00121     }
00122   }
00123 
00124   // Init the actual driver hook (after logging initialize)
00125   fm_.reset(new driver_svh::SVHFingerManager(disable_flags, reset_timeout));
00126 
00127   // Receives current Firmware Version
00128   // because some parameters depend on the version
00129   if (manual_major_version == 0 && manual_minor_version == 0)
00130   {
00131     fm_->connect(serial_device_name_, connect_retry_count);
00132     driver_svh::SVHFirmwareInfo version_info = fm_->getFirmwareInfo();
00133     ROS_INFO("Hand hardware controller version:  %d.%d",
00134              version_info.version_major,
00135              version_info.version_minor);
00136 
00137     manual_major_version = version_info.version_major;
00138     manual_minor_version = version_info.version_minor;
00139 
00140     fm_->disconnect();
00141   }
00142   // get the the individual finger parameters
00143   // We will read out all of them, so that in case we fail half way we do not set anything
00144   try
00145   {
00146     // Loading hand parameters
00147     DynamicParameter dyn_parameters(manual_major_version, manual_minor_version, dynamic_parameters);
00148 
00149 
00150     for (size_t channel = 0; channel < driver_svh::eSVH_DIMENSION; ++channel)
00151     {
00152       // Only update the values in case actually have some. Otherwise the driver will use internal
00153       // defaults. Overwriting them with zeros would be counter productive
00154       if (dyn_parameters.getSettings().current_settings_given[channel])
00155       {
00156         fm_->setCurrentSettings(
00157           static_cast<driver_svh::SVHChannel>(channel),
00158           driver_svh::SVHCurrentSettings(dyn_parameters.getSettings().current_settings[channel]));
00159       }
00160       if (dyn_parameters.getSettings().position_settings_given[channel])
00161       {
00162         fm_->setPositionSettings(
00163           static_cast<driver_svh::SVHChannel>(channel),
00164           driver_svh::SVHPositionSettings(dyn_parameters.getSettings().position_settings[channel]));
00165       }
00166       if (dyn_parameters.getSettings().home_settings_given[channel])
00167       {
00168         fm_->setHomeSettings(static_cast<driver_svh::SVHChannel>(channel),
00169                              driver_svh::SVHHomeSettings(
00170                                dyn_parameters.getSettings().home_settings[channel]
00171                              ));
00172       }
00173     }
00174   }
00175   catch (ros::InvalidNameException e)
00176   {
00177     ROS_ERROR("Parameter Error! While reading the controller settings. Will use default settings");
00178   }
00179 
00180   // prepare the channel position message for later sending
00181   channel_pos_.name.resize(driver_svh::eSVH_DIMENSION);
00182   channel_pos_.position.resize(driver_svh::eSVH_DIMENSION, 0.0);
00183   channel_pos_.effort.resize(driver_svh::eSVH_DIMENSION, 0.0);
00184   for (size_t channel = 0; channel < driver_svh::eSVH_DIMENSION; ++channel)
00185   {
00186     channel_pos_.name[channel] =
00187       name_prefix + "_" + driver_svh::SVHController::m_channel_description[channel];
00188   }
00189 
00190   // Prepare the channel current message for later sending
00191   channel_currents.data.clear();
00192   channel_currents.data.resize(driver_svh::eSVH_DIMENSION, 0.0);
00193   channel_currents.layout.data_offset = 0;
00194   std_msgs::MultiArrayDimension dim;
00195   dim.label  = "channel currents";
00196   dim.size   = 9;
00197   dim.stride = 0;
00198   channel_currents.layout.dim.push_back(dim);
00199 
00200   // Connect and start the reset so that the hand is ready for use
00201   if (autostart && fm_->connect(serial_device_name_, connect_retry_count))
00202   {
00203     fm_->resetChannel(driver_svh::eSVH_ALL);
00204     ROS_INFO("Driver was autostarted! Input can now be sent. Have a safe and productive day!");
00205   }
00206   else
00207   {
00208     ROS_INFO("SVH Driver Ready, you will need to connect and reset the fingers before you can use "
00209              "the hand.");
00210   }
00211 }
00212 
00213 SVHNode::~SVHNode()
00214 {
00215   // Tell the driver to close connections
00216   fm_->disconnect();
00217 }
00218 
00219 // Callback function for changing parameters dynamically
00220 void SVHNode::dynamic_reconfigure_callback(svh_controller::svhConfig& config, uint32_t level)
00221 {
00222   serial_device_name_ = config.serial_device;
00223 
00224   fm_->setResetSpeed(config.finger_reset_speed);
00225   fm_->setResetTimeout(config.reset_timeout);
00226 }
00227 
00228 // Callback function for connecting to SCHUNK five finger hand
00229 void SVHNode::connectCallback(const std_msgs::Empty&)
00230 {
00231   if (fm_->isConnected())
00232   {
00233     fm_->disconnect();
00234   }
00235 
00236   if (!fm_->connect(serial_device_name_, connect_retry_count))
00237   {
00238     ROS_ERROR(
00239       "Could not connect to SCHUNK five finger hand with serial device %s, and retry count %i",
00240       serial_device_name_.c_str(),
00241       connect_retry_count);
00242   }
00243 }
00244 
00245 // Callback function to reset/home channels of SCHUNK five finger hand
00246 void SVHNode::resetChannelCallback(const std_msgs::Int8ConstPtr& channel)
00247 {
00248   // convert int8 channel into SVHChannel enum
00249   driver_svh::SVHChannel svh_channel = static_cast<driver_svh::SVHChannel>(channel->data);
00250 
00251   if (fm_->resetChannel(svh_channel))
00252   {
00253     ROS_INFO("Channel %i successfully homed!", svh_channel);
00254   }
00255   else
00256   {
00257     ROS_ERROR("Could not reset channel %i !", svh_channel);
00258   }
00259 }
00260 
00261 // Callback function to enable channels of SCHUNK five finger hand
00262 void SVHNode::enableChannelCallback(const std_msgs::Int8ConstPtr& channel)
00263 {
00264   fm_->enableChannel(static_cast<driver_svh::SVHChannel>(channel->data));
00265 }
00266 
00267 // Callback function for setting channel target positions to SCHUNK five finger hand
00268 void SVHNode::jointStateCallback(const sensor_msgs::JointStateConstPtr& input)
00269 {
00270   // vector to read target positions from joint states
00271   std::vector<double> target_positions(driver_svh::eSVH_DIMENSION, 0.0);
00272   // bool vector storing true, if new target position read
00273   std::vector<bool> pos_read(driver_svh::eSVH_DIMENSION, false);
00274   // target positions read counter
00275   uint8_t pos_read_counter = 0;
00276 
00277   size_t index = 0;
00278   std::vector<std::string>::const_iterator joint_name;
00279   for (joint_name = input->name.begin(); joint_name != input->name.end(); ++joint_name, ++index)
00280   {
00281     int32_t channel = 0;
00282 
00283     // Find the corresponding channels to the input joint names
00284     bool valid_input = false;
00285     for (channel = 0; !valid_input && (channel < driver_svh::eSVH_DIMENSION) &&
00286                       (driver_svh::SVHController::m_channel_description[channel] != NULL);
00287          ++channel)
00288     {
00289       valid_input =
00290         (joint_name->compare(name_prefix + "_" +
00291                              driver_svh::SVHController::m_channel_description[channel]) == 0);
00292     }
00293 
00294     // We count one to high with the for loop so we have to correct that
00295     --channel;
00296 
00297 
00298     if (valid_input) //(icl_core::string2Enum((*joint_name), channel,
00299                      //driver_svh::SVHController::m_channel_description))
00300     {
00301       if (input->position.size() > index)
00302       {
00303         target_positions[channel] = input->position[index];
00304         pos_read[channel]         = true;
00305         pos_read_counter++;
00306       }
00307       else
00308       {
00309         ROS_WARN_STREAM("Vector of input joint state is too small! Cannot access element nr "
00310                         << index);
00311       }
00312     }
00313     else
00314     {
00315       // ROS_WARN("Could not map joint name %s to channel!", (*joint_name).c_str());
00316     }
00317   }
00318 
00319   // send target values at once
00320   if (pos_read_counter == driver_svh::eSVH_DIMENSION)
00321   {
00322     if (!fm_->setAllTargetPositions(target_positions))
00323     {
00324       ROS_WARN_ONCE("Set target position command rejected!");
00325     }
00326   }
00327   // not all positions has been set: send only the available positions
00328   else
00329   {
00330     for (size_t i = 0; i < driver_svh::eSVH_DIMENSION; ++i)
00331     {
00332       if (pos_read[i])
00333       {
00334         fm_->setTargetPosition(static_cast<driver_svh::SVHChannel>(i), target_positions[i], 0.0);
00335       }
00336     }
00337   }
00338 }
00339 
00340 
00341 sensor_msgs::JointState SVHNode::getChannelFeedback()
00342 {
00343   if (fm_->isConnected())
00344   {
00345     // Get positions in rad
00346     for (size_t channel = 0; channel < driver_svh::eSVH_DIMENSION; ++channel)
00347     {
00348       double cur_pos = 0.0;
00349       double cur_cur = 0.0;
00350       if (fm_->isHomed(static_cast<driver_svh::SVHChannel>(channel)))
00351       {
00352         fm_->getPosition(static_cast<driver_svh::SVHChannel>(channel), cur_pos);
00353         // Read out currents if you want to
00354         fm_->getCurrent(static_cast<driver_svh::SVHChannel>(channel), cur_cur);
00355       }
00356       channel_pos_.position[channel] = cur_pos;
00357       channel_pos_.effort[channel] =
00358         cur_cur * driver_svh::SVHController::channel_effort_constants[channel];
00359     }
00360   }
00361 
00362   channel_pos_.header.stamp = ros::Time::now();
00363   return channel_pos_;
00364 }
00365 
00366 std_msgs::Float64MultiArray SVHNode::getChannelCurrents()
00367 {
00368   if (fm_->isConnected())
00369   {
00370     // Get currents
00371     for (size_t channel = 0; channel < driver_svh::eSVH_DIMENSION; ++channel)
00372     {
00373       double cur_cur = 0.0;
00374       if (fm_->isHomed(static_cast<driver_svh::SVHChannel>(channel)))
00375       {
00376         fm_->getCurrent(static_cast<driver_svh::SVHChannel>(channel), cur_cur);
00377       }
00378       channel_currents.data[channel] = cur_cur;
00379     }
00380   }
00381 
00382   return channel_currents;
00383 }
00384 
00385 
00386 /*--------------------------------------------------------------------
00387  * main()
00388  * Main function to set up ROS node.
00389  *------------------------------------------------------------------*/
00390 
00391 int main(int argc, char** argv)
00392 {
00393   //==========
00394   // ROS
00395   //==========
00396 
00397   // Set up ROS.
00398   ros::init(argc, argv, "svh_controller");
00399   // Private NH for general params
00400   ros::NodeHandle nh("~");
00401 
00402 
00403   // Tell ROS how fast to run this node. (100 = 100 Hz = 10 ms)
00404   ros::Rate rate(50);
00405 
00406   //==========
00407   // Logic
00408   //==========
00409   // Node object holding all the relevant functions
00410   SVHNode svh_node(nh);
00411 
00412   //==========
00413   // Dynamic Reconfigure
00414   //==========
00415 
00416   dynamic_reconfigure::Server<svh_controller::svhConfig> server;
00417   dynamic_reconfigure::Server<svh_controller::svhConfig>::CallbackType f;
00418 
00419   f = boost::bind(&SVHNode::dynamic_reconfigure_callback, &svh_node, _1, _2);
00420   server.setCallback(f);
00421 
00422   //==========
00423   // Callbacks
00424   //==========
00425 
00426   // Subscribe connect topic (Empty)
00427   ros::Subscriber connect_sub = nh.subscribe("connect", 1, &SVHNode::connectCallback, &svh_node);
00428   // Subscribe reset channel topic (Int8)
00429   ros::Subscriber reset_sub =
00430     nh.subscribe("reset_channel", 1, &SVHNode::resetChannelCallback, &svh_node);
00431   // Subscribe enable channel topic (Int8)
00432   ros::Subscriber enable_sub =
00433     nh.subscribe("enable_channel", 1, &SVHNode::enableChannelCallback, &svh_node);
00434   // Subscribe joint state topic
00435   ros::Subscriber channel_target_sub =
00436     nh.subscribe<sensor_msgs::JointState>("channel_targets",
00437                                           1,
00438                                           &SVHNode::jointStateCallback,
00439                                           &svh_node,
00440                                           ros::TransportHints().tcpNoDelay());
00441   // Publish current channel positions
00442   ros::Publisher channel_pos_pub = nh.advertise<sensor_msgs::JointState>("channel_feedback", 1);
00443   // Additionally publish just the current values of the motors
00444   ros::Publisher channel_current_pub =
00445     nh.advertise<std_msgs::Float64MultiArray>("channel_currents", 1);
00446 
00447   //==========
00448   // Messaging
00449   //==========
00450 
00451   // Main loop.
00452   while (nh.ok())
00453   {
00454     // get the current positions of all joints and publish them
00455     channel_pos_pub.publish(svh_node.getChannelFeedback());
00456     channel_current_pub.publish(svh_node.getChannelCurrents());
00457 
00458     ros::spinOnce();
00459     rate.sleep();
00460   }
00461 
00462 
00463   return 0;
00464 }


schunk_svh_driver
Author(s): Georg Heppner
autogenerated on Thu Jun 6 2019 18:29:08