message: backport temperature publisching to sdh_only node
diff: #include <sensor_msgs/JointState.h>
  #include <control_msgs/FollowJointTrajectoryAction.h>
  #include <control_msgs/JointTrajectoryControllerState.h>
  +#include <schunk_sdh/TemperatureArray.h>
  // ROS service includes
  #include <std_srvs/Trigger.h>
  @@ -62,6 +63,7 @@ private:
  ros::Publisher topicPub_JointState_;
  ros::Publisher topicPub_ControllerState_;
  ros::Publisher topicPub_Diagnostics_;
  + ros::Publisher topicPub_Temperature_;
  // topic subscribers
  ros::Subscriber subSetVelocitiesRaw_;
  @@ -107,6 +109,8 @@ private:
  bool hasNewGoal_;
  std::string operationMode_;
  + static const std::vector<std::string> temperature_names_;
  +
  public:
  /*!
  * \brief Constructor for SdhNode class
  @@ -148,6 +152,7 @@ public:
  topicPub_ControllerState_ = nh_.advertise<control_msgs::JointTrajectoryControllerState>(
  "joint_trajectory_controller/state", 1);
  topicPub_Diagnostics_ = nh_.advertise<diagnostic_msgs::DiagnosticArray>("diagnostics", 1);
  + topicPub_Temperature_ = nh_.advertise<schunk_sdh::TemperatureArray>("temperature", 1);
  // pointer to sdh
  sdh_ = new SDH::cSDH(false, false, 0); // (_use_radians=false, bool _use_fahrenheit=false, int _debug_level=0)
  @@ -801,6 +806,19 @@ public:
  // read sdh status
  state_ = sdh_->GetAxisActualState(axes_);
  +
  + // publish temperature
  + schunk_sdh::TemperatureArray temp_array;
  + temp_array.header.stamp = time;
  + const std::vector<double> temp_value = sdh_->GetTemperature(sdh_->all_temperature_sensors);
  + if(temp_value.size()==temperature_names_.size()) {
  + temp_array.name = temperature_names_;
  + temp_array.temperature = temp_value;
  + }
  + else {
  + ROS_ERROR("amount of temperatures mismatch with stored names");
  + }
  + topicPub_Temperature_.publish(temp_array);
  }
  else
  {
  @@ -835,6 +853,14 @@ public:
  topicPub_Diagnostics_.publish(diagnostics);
  }
  };
  +
  +const std::vector<std::string> SdhNode::temperature_names_ = {
  + "root",
  + "proximal_finger_1", "distal_finger_1",
  + "proximal_finger_2", "distal_finger_2",
  + "proximal_finger_3", "distal_finger_3",
  + "controller", "pcb"
  +};
  // SdhNode
  /*!
  