Adapt automatically to temperature cotnroller if present.

CMakeLists.txt

@@ -70,6 +70,7 @@ add_required_dependency("realtime_tools >= 1.8")
 add_required_dependency("dynamic_graph_bridge")
 add_required_dependency("controller_interface")
 add_required_dependency("pal_hardware_interfaces")
+add_optional_dependency("temperature_sensor_controller")
 
 # This is necessary so that the pc file generated by catking is similar to the on
 # done directly by jrl-cmake-modules
@@ -77,13 +78,19 @@ catkin_package(CATKIN_DEPENDS
 roscpp realtime_tools message_runtime dynamic_graph_bridge pal_hardware_interfaces controller_interface
 LIBRARIES rcsot_controller)
 
-# Detect the prf-ros-control version to switch code
+# Detect the controller interface version to switch code
 if(CONTROLLER_INTERFACE_FOUND)
   if (${CONTROLLER_INTERFACE_VERSION} VERSION_GREATER "0.2.5")
     add_definitions(-DCONTROLLER_INTERFACE_KINETIC)
   endif(${CONTROLLER_INTERFACE_VERSION} VERSION_GREATER "0.2.5")
 endif(CONTROLLER_INTERFACE_FOUND)
 
+# Detect if temperature sensor controller package is found
+# if yes then it is a PARL Robotics Forked code.
+if(TEMPERATURE_SENSOR_CONTROLLER_FOUND)
+  add_definitions(-DTEMPERATURE_SENSOR_CONTROLLER)
+endif(TEMPERATURE_SENSOR_CONTROLLER_FOUND)
+
 
 ###########
 ## Build ##

src/roscontrol-sot-controller.cpp

@@ -7,6 +7,7 @@
 #include <pluginlib/class_list_macros.h>
 #include "roscontrol-sot-controller.hh"
 
+#include<ros/console.h>
 
 #if DEBUG
 #define ODEBUG(x) std::cout << x << std::endl
@@ -164,55 +165,53 @@ namespace sot_controller
 
     // Get a pointer to the joint position control interface
     pos_iface_ = robot_hw->get<PositionJointInterface>();
-    if (! pos_iface_)
+    if (!pos_iface_)
       {
-	ROS_ERROR("This controller requires a hardware interface of type '%s'."
-		  " Make sure this is registered in the %s::RobotHW class.",
+	ROS_WARN("This controller did not find  a hardware interface of type '%s'."
+		 " Make sure this is registered in the %s::RobotHW class if it is required.",
 		  getHardwareInterfaceType().c_str(), lns.c_str());
-	return false ;
       }
 
     // Get a pointer to the joint effort control interface
     effort_iface_ = robot_hw->get<EffortJointInterface>();
     if (! effort_iface_)
       {
-	ROS_ERROR("This controller requires a hardware interface of type '%s'."
-		  " Make sure this is registered in the %s::RobotHW class.",
-		  getHardwareInterfaceType().c_str(),lns.c_str());
-	    return false ;
+	ROS_WARN("This controller did not find a hardware interface of type '%s'."
+		 " Make sure this is registered in the %s::RobotHW class if it is required.",
+		    getHardwareInterfaceType().c_str(),lns.c_str());
       }
 
     // Get a pointer to the force-torque sensor interface
     ft_iface_ = robot_hw->get<ForceTorqueSensorInterface>();
     if (! ft_iface_ )
       {
-	ROS_ERROR("This controller requires a hardware interface of type '%s '. " 
-		  " Make sure this is registered inthe %s::RobotHW class.",
+	ROS_WARN("This controller did not find a hardware interface of type '%s '. " 
+		 " Make sure this is registered inthe %s::RobotHW class if it is required.",
 		  internal :: demangledTypeName<ForceTorqueSensorInterface>().c_str(),lns.c_str());
-	return false ;
       }
+    
     // Get a pointer to the IMU sensor interface
     imu_iface_ = robot_hw->get<ImuSensorInterface>();
     if (! imu_iface_)
       {
-	ROS_ERROR("This controller requires a hardware interface of type '%s'."
-		  " Make sure this is registered in the %s::RobotHW class.",
-		  internal :: demangledTypeName<ImuSensorInterface>().c_str(),lns.c_str());
-	return false ;
+	ROS_WARN("This controller did not find a hardware interface of type '%s'."
+		 " Make sure this is registered in the %s::RobotHW class if it is required.",
+		    internal :: demangledTypeName<ImuSensorInterface>().c_str(),lns.c_str());
       }
 
     // Temperature sensor not available in simulation mode
     if (!simulation_mode_)
       {
+#ifdef TEMPERATURE_SENSOR_CONTROLLER_FOUND
 	// Get a pointer to the actuator temperature sensor interface
 	act_temp_iface_ = robot_hw->get<ActuatorTemperatureSensorInterface>();
 	if (!act_temp_iface_)
 	  {
-	    ROS_ERROR("This controller requires a hardware interface of type '%s'."
-		      " Make sure this is registered in the %s::RobotHW class.",
+	    ROS_WARN("This controller did not find a hardware interface of type '%s'."
+		     " Make sure this is registered in the %s::RobotHW class if it is required.",
 		      internal :: demangledTypeName<ActuatorTemperatureSensorInterface>().c_str(),lns.c_str());
-	    return false ;	  
 	  }
+#endif
       }
 	
       
@@ -330,15 +329,17 @@ namespace sot_controller
          {
 	   XmlRpc::XmlRpcValue xml_rpc_pci_el;
 
-	   ROS_INFO("/sot_controller/position_control_init_pos/ %ld: %d %d %d\n",i,
-		    xml_rpc_pci_pose[i].getType(),XmlRpc::XmlRpcValue::TypeArray,XmlRpc::XmlRpcValue::TypeStruct);
+	   ROS_INFO("/sot_controller/position_control_init_pos/ %ld: %ld %d %d\n",i,
+		    xml_rpc_pci_pose[(int)i].getType(),
+		    XmlRpc::XmlRpcValue::TypeArray,
+		    XmlRpc::XmlRpcValue::TypeStruct);
 	   
-           if (xml_rpc_pci_pose[i].hasMember("name"))
+           if (xml_rpc_pci_pose[(int)i].hasMember("name"))
              {
-	       std::string local_joint_name = std::string(xml_rpc_pci_pose[i]["name"]);
-	       if (xml_rpc_pci_pose[i].hasMember("name"))
+	       std::string local_joint_name = std::string(xml_rpc_pci_pose[(int)i]["name"]);
+	       if (xml_rpc_pci_pose[(int)i].hasMember("name"))
 		 {
-		   double local_des_pose = double(xml_rpc_pci_pose[i]["des_pos"]);
+		   double local_des_pose = double(xml_rpc_pci_pose[(int)i]["des_pos"]);
 		   desired_init_pose_[local_joint_name] = local_des_pose;
 		 }
 	       else
@@ -441,8 +442,26 @@ namespace sot_controller
 	robot_nh.getParam("/sot_controller/joint_names",
 			  joints_name_);
 	for(std::vector<std::string>::size_type i=0;i<joints_name_.size();i++)
-	  {ROS_INFO_STREAM("joints_name_[" << i << "]=" << joints_name_[i]);}
-	
+	  {
+	    ROS_INFO_STREAM("joints_name_[" << i << "]=" << joints_name_[i]);
+
+	    if (modelURDF_.use_count())
+	      {
+		urdf::JointConstSharedPtr aJCSP = modelURDF_->getJoint(joints_name_[i]);
+		if (aJCSP.use_count()!=0)
+		  ROS_INFO_STREAM( joints_name_[i] + " found in the robot model" );
+		else
+		  {
+		    ROS_ERROR(" %s not found in the robot model",joints_name_[i]);
+		    return false;
+		  }
+	      }
+	    else
+	      {
+		ROS_ERROR("No robot model loaded in /robot_description");
+		return false;
+	      }
+	  }
       }
     else
       return false;
@@ -510,6 +529,25 @@ namespace sot_controller
     ROS_ERROR("You need to define a control period in param /sot_controller/dt");
     return false;
   }
+
+  bool RCSotController::
+  readUrdf(ros::NodeHandle &robot_nh)
+  {
+    /// Reading the parameter /robot_description which contains the robot
+    /// description
+    if (!robot_nh.hasParam("/robot_description"))
+      {
+	ROS_ERROR("ROS application does not have robot_description");
+	return false;
+      }
+    std::string robot_description_str;
+    
+    robot_nh.getParam("/robot_description",robot_description_str);
+
+    modelURDF_ = urdf::parseURDF(robot_description_str);
+    ROS_INFO("Loaded /robot_description %d",modelURDF_.use_count());
+    return true;
+  }
   
   bool RCSotController::
   readParams(ros::NodeHandle &robot_nh)
@@ -525,6 +563,9 @@ namespace sot_controller
     if (robot_nh.hasParam("/sot_controller/simulation_mode")) 
       simulation_mode_ = true;
     
+    /// Read URDF file.
+    readUrdf(robot_nh);
+    
     /// Calls readParamsJointNames
     // Reads the list of joints to be controlled.
     if (!readParamsJointNames(robot_nh))
@@ -672,12 +713,15 @@ namespace sot_controller
     for(unsigned int idJoint=0;idJoint<joints_.size();idJoint++)
       {
 	DataOneIter_.motor_angle[idJoint] = joints_[idJoint].getPosition();
-	//if (!simulation_mode_)
-	  DataOneIter_.joint_angle[idJoint] = joints_[idJoint].getAbsolutePosition();
 
+#ifdef TEMPERATURE_SENSOR_CONTROLLER_FOUND
+	DataOneIter_.joint_angle[idJoint] = joints_[idJoint].getAbsolutePosition();
+#endif	  
 	DataOneIter_.velocities[idJoint] = joints_[idJoint].getVelocity();
-	//if (!simulation_mode_)
-	  DataOneIter_.torques[idJoint] = joints_[idJoint].getTorqueSensor();
+
+#ifdef TEMPERATURE_SENSOR_CONTROLLER_FOUND	
+	DataOneIter_.torques[idJoint] = joints_[idJoint].getTorqueSensor();
+#endif
 	DataOneIter_.motor_currents[idJoint] = joints_[idJoint].getEffort();
       }
     

src/roscontrol-sot-controller.hh

@@ -45,6 +45,10 @@
 #include <ros/ros.h>
 #include <control_toolbox/pid.h>
 
+/** URDF DOM*/
+#include <urdf_parser/urdf_parser.h>
+
+/* Local header */
 #include "log.hh"
 
 namespace sot_controller 
@@ -165,7 +169,10 @@ namespace sot_controller
 
     /// Jitter for the subsampling.
     double jitter_;
-    
+
+    /// URDF model of the robot.
+    urdf::ModelInterfaceSharedPtr modelURDF_;    
+
   public :
 
     RCSotController ();
@@ -287,6 +294,8 @@ namespace sot_controller
     /// One iteration: read sensor, compute the control law, apply control.
     void one_iteration();
 
+    /// Read URDF model from /robot_description parameter.
+    bool readUrdf(ros::NodeHandle &robot_nh);
   };
 }