Commit 48ce14ca authored by Olivier Stasse's avatar Olivier Stasse

Adapt automatically to temperature cotnroller if present.

parent ab3593c0
......@@ -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 ##
......
......@@ -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();
}
......
......@@ -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);
};
}
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment