plugins: simulation and handler

.travis.rosinstall

@@ -7,3 +7,7 @@
     uri: 'https://github.com/iirob/rosparam_handler.git'
     local-name: rosparam_handler
     version: master
+- git:
+    uri: 'https://github.com/KITrobotics/ros_control.git'
+    local-name: ros_control
+    version: combined_robot_sensor_hw

CMakeLists.txt

@@ -68,7 +68,7 @@ generate_messages(
 ###################################
 catkin_package(
  INCLUDE_DIRS include
- CATKIN_DEPENDS geometry_msgs iirob_filters message_runtime roscpp std_msgs std_srvs rospy realtime_tools pluginlib tf2 tf2_ros
+ CATKIN_DEPENDS geometry_msgs iirob_filters message_runtime roscpp std_msgs std_srvs rospy realtime_tools pluginlib tf2 tf2_ros hardware_interface
  DEPENDS Eigen3
  LIBRARIES ${PROJECT_NAME} ${PROJECT_NAME}_sim
 )
@@ -85,7 +85,10 @@ include_directories(
   ${Eigen3_INCLUDE_DIRS}
 )
 
-add_library(${PROJECT_NAME} src/force_torque_sensor_handle.cpp )
+add_library(${PROJECT_NAME}
+	src/force_torque_sensor_hw.cpp
+	src/force_torque_sensor_hw_handler.cpp
+)
 add_dependencies(${PROJECT_NAME}
   ${PROJECT_NAME}_gencfg  # For dynamic_reconfigure
   ${PROJECT_NAME}_generate_messages_cpp
@@ -120,3 +123,6 @@ install(DIRECTORY config/ cfg/
 install(FILES force_torque_sensor_plugin.xml
 DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
 
+install(FILES hardware_interface_plugin.xml
+DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+

hardware_interface_plugin.xml

@@ -0,0 +1,21 @@
+<library path="lib/libforce_torque_sensor">
+	<class name="force_torque_sensor/ForceTorqueSensorHW" type="hardware_interface::ForceTorqueSensorHW" base_class_type="hardware_interface::SensorHW">
+		<description>
+			FTS interface
+		</description>
+	</class>
+
+	<class name="force_torque_sensor/ForceTorqueSensorHWHandler" type="force_torque_sensor::ForceTorqueSensorHWHandler" base_class_type="hardware_interface::SensorHW"> 
+	  <description>
+	    Force Torque Sensor Handler
+	  </description>
+  </class>
+</library>
+
+<library path="lib/libforce_torque_sensor_sim">
+	<class name="force_torque_sensor/ForceTorqueSensorSim" type="force_torque_sensor::ForceTorqueSensorSim" base_class_type="hardware_interface::SensorHW">
+		<description>
+			Simulated Force Torque Sensor
+		</description>
+	</class>
+</library>

include/force_torque_sensor/force_torque_sensor_hw.h

@@ -2,10 +2,12 @@
 #ifndef FORCETORQUESENSORHW_INCLUDEDEF_H
 #define FORCETORQUESENSORHW_INCLUDEDEF_H
 
+#include <hardware_interface/sensor_hw.h>
+
 namespace hardware_interface
 {
 
-class ForceTorqueSensorHW
+class ForceTorqueSensorHW : public hardware_interface::SensorHW
 {
 public:
   ForceTorqueSensorHW() {};

include/force_torque_sensor/force_torque_sensor_handle.h → include/force_torque_sensor/force_torque_sensor_hw_handler.h

@@ -41,8 +41,8 @@
  *
  ****************************************************************/
 
-#ifndef FORCETORQUESENSORHANDLE_INCLUDEDEF_H
-#define FORCETORQUESENSORHANDLE_INCLUDEDEF_H
+#ifndef FORCETORQUESENSORHWHANDLER_INCLUDEDEF_H
+#define FORCETORQUESENSORHWHANDLER_INCLUDEDEF_H
 
 #include <hardware_interface/force_torque_sensor_interface.h>
 #include <force_torque_sensor/force_torque_sensor_hw.h>
@@ -96,13 +96,13 @@ typedef unsigned char uint8_t;
 namespace force_torque_sensor
 {
 
-class ForceTorqueSensorHandle : public hardware_interface::ForceTorqueSensorHandle
+class ForceTorqueSensorHWHandler : public hardware_interface::ForceTorqueSensorHW
 {
 public:
+  ForceTorqueSensorHWHandler();
+  ForceTorqueSensorHWHandler(ros::NodeHandle &nh, hardware_interface::ForceTorqueSensorHW *sensor, std::string sensor_name, std::string output_frame);
+  ForceTorqueSensorHWHandler(ros::NodeHandle &nh, std::string sensor_name, std::string output_frame);
 
-  ForceTorqueSensorHandle(ros::NodeHandle &nh, hardware_interface::ForceTorqueSensorHW *sensor, std::string sensor_name, std::string output_frame);
-  ForceTorqueSensorHandle(ros::NodeHandle &nh, std::string sensor_name, std::string output_frame);
-
   void prepareNode(std::string output_frame);
 
   void init_sensor(std::string &msg, bool &success);
@@ -116,9 +116,13 @@ class ForceTorqueSensorHandle : public hardware_interface::ForceTorqueSensorHand
   bool srvCallback_recalibrate(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res);
   bool srvCallback_setSensorOffset(force_torque_sensor::SetSensorOffset::Request &req,
                                  force_torque_sensor::SetSensorOffset::Response &res);
+  bool init ( ros::NodeHandle& root_nh, ros::NodeHandle &sensor_hw_nh );
+  void read ( const ros::Time& time, const ros::Duration& period );
+  hardware_interface::ForceTorqueSensorHandle getFTSHandle();
 
 private:
   void updateFTData(const ros::TimerEvent &event);
+  void updateFTData_();
   geometry_msgs::Wrench makeAverageMeasurement(uint number_of_measurements, double time_between_meas, std::string frame_id="");
 
   bool transform_wrench(std::string goal_frame, std::string source_frame, geometry_msgs::Wrench wrench, geometry_msgs::Wrench& transformed);
@@ -139,6 +143,7 @@ class ForceTorqueSensorHandle : public hardware_interface::ForceTorqueSensorHand
   std::string sensor_frame_;
 
   void pullFTData(const ros::TimerEvent &event);
+  void pullFTData_();
   void filterFTData();
 
   // Arrays for dumping FT-Data
@@ -189,6 +194,7 @@ class ForceTorqueSensorHandle : public hardware_interface::ForceTorqueSensorHand
   ros::ServiceServer srvServer_SetSensorOffset;
 
   ros::Timer ftUpdateTimer_, ftPullTimer_;
+  bool using_timer;
 
   tf2_ros::TransformListener *p_tfListener;
   tf2::Transform transform_ee_base;
@@ -220,7 +226,16 @@ class ForceTorqueSensorHandle : public hardware_interface::ForceTorqueSensorHand
 
   boost::shared_ptr<pluginlib::ClassLoader<hardware_interface::ForceTorqueSensorHW>> sensor_loader_;
   boost::shared_ptr<hardware_interface::ForceTorqueSensorHW> sensor_;
+
+  hardware_interface::ForceTorqueSensorHandle fts_handle;
+  hardware_interface::ForceTorqueSensorInterface fts_interface_;
+  ros::Time last_publish_time_;
+  ros::Time last_pull_time_;
+
+  void registerHandleAndInterface(std::string sensor_name, std::string output_frame);
+  bool loadSensor(std::string sensor_hw, std::string transform_frame);
 };
 
 }
 #endif
+

include/force_torque_sensor/force_torque_sensor_sim.h

@@ -5,7 +5,7 @@
 #include <ros/ros.h>
 #include <geometry_msgs/Twist.h>
 #include <geometry_msgs/WrenchStamped.h>
-
+#include <hardware_interface/force_torque_sensor_interface.h>
 #include <force_torque_sensor/force_torque_sensor_hw.h>
 
 namespace force_torque_sensor
@@ -26,9 +26,15 @@ class ForceTorqueSensorSim : public  hardware_interface::ForceTorqueSensorHW
   void subscribeData(const geometry_msgs::Twist::ConstPtr& msg);
   geometry_msgs::WrenchStamped joystick_data;
 
+  bool init ( ros::NodeHandle& root_nh, ros::NodeHandle &sensor_hw_nh );
+  void read ( const ros::Time& time, const ros::Duration& period );
+
 private:
   ros::NodeHandle nh_;
   ros::Subscriber force_input_subscriber;
+  hardware_interface::ForceTorqueSensorInterface fts_interface_;
+  double force_[3];
+  double torque_[3];
 };
 
 }

package.xml

@@ -35,5 +35,6 @@
   <!-- The export tag contains other, unspecified, tags -->
   <export>
     <force_torque_sensor plugin="${prefix}/force_torque_sensor_plugin.xml"/>
+    <hardware_interface plugin="${prefix}/hardware_interface_plugin.xml"/>
   </export>
 </package>

src/force_torque_sensor_hw.cpp

@@ -0,0 +1,57 @@
+/****************************************************************
+ *
+ * Copyright 2016 Intelligent Industrial Robotics (IIROB) Group,
+ * Institute for Anthropomatics and Robotics (IAR) -
+ * Intelligent Process Control and Robotics (IPR),
+ * Karlsruhe Institute of Technology
+ *
+ * Maintainers: Denis Štogl, email: [email protected]
+ *               Andreea Tulbure
+ *
+ * Date of update: 2017
+ *
+ * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ *
+ * Copyright (c) 2010
+ *
+ * Fraunhofer Institute for Manufacturing Engineering
+ * and Automation (IPA)
+ *
+ * Author: Alexander Bubeck, email:[email protected]
+ * Supervised by: Alexander Bubeck, email:[email protected]
+ *
+ * Date of creation: June 2010
+ *
+ * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the copyright holder nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU Lesser General Public License LGPL as
+ * published by the Free Software Foundation, either version 3 of the
+ * License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU Lesser General Public License LGPL for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License LGPL along with this program.
+ * If not, see <http://www.gnu.org/licenses/>.
+ *
+ ****************************************************************/
+#include <force_torque_sensor/force_torque_sensor_hw.h> 
+#include <pluginlib/class_list_macros.h>
+
+PLUGINLIB_EXPORT_CLASS(hardware_interface::ForceTorqueSensorHW, hardware_interface::SensorHW)