Skip to content
Snippets Groups Projects
Commit 48ce14ca authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Adapt automatically to temperature cotnroller if present.

parent ab3593c0
No related branches found
Tags v0.0.4
No related merge requests found
......@@ -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);
};
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment