From 1a273a9bdb73583d10a643770e28b8e0629728e2 Mon Sep 17 00:00:00 2001
From: Olivier Stasse <ostasse@laas.fr>
Date: Mon, 26 Feb 2018 13:24:10 +0100
Subject: [PATCH] Makes the code compiling both for real robot and simulation
environment.
---
.../src/roscontrol-sot-controller.cpp | 226 +++++++++---------
.../src/roscontrol-sot-controller.hh | 48 ++--
2 files changed, 151 insertions(+), 123 deletions(-)
diff --git a/talos_roscontrol_sot/src/roscontrol-sot-controller.cpp b/talos_roscontrol_sot/src/roscontrol-sot-controller.cpp
index 135bc4c..97d2cd0 100644
--- a/talos_roscontrol_sot/src/roscontrol-sot-controller.cpp
+++ b/talos_roscontrol_sot/src/roscontrol-sot-controller.cpp
@@ -35,7 +35,16 @@
#define ODEBUG4FULL(x)
#define ODEBUG4(x)
-using namespace talos_hardware_interface;
+/// lhi: nickname for local_hardware_interface
+/// Depends if we are on the real robot or not.
+
+#ifdef REAL_ROBOT
+namespace lhi=hardware_interface;
+#else
+namespace lhi=talos_hardware_interface;
+#endif
+using namespace lhi;
+
using namespace rc_sot_system;
namespace talos_sot_controller
@@ -53,7 +62,7 @@ namespace talos_sot_controller
{
pid_controller.initParam(prefix);
}
-
+
RCSotController::
RCSotController():
// Store 32 DoFs for 5 minutes (1 Khz: 5*60*1000)
@@ -80,7 +89,7 @@ namespace talos_sot_controller
}
bool RCSotController::
- initRequest (talos_hardware_interface::RobotHW * robot_hw,
+ initRequest (lhi::RobotHW * robot_hw,
ros::NodeHandle &robot_nh,
ros::NodeHandle &controller_nh,
std::set<std::string> & claimed_resources)
@@ -106,7 +115,7 @@ namespace talos_sot_controller
std::string joint_name = joints_name_[idJoint];
std::map<std::string,EffortControlPDMotorControlData>::iterator
search_ecpd = effort_mode_pd_motors_.find(joint_name);
-
+
if (search_ecpd!=effort_mode_pd_motors_.end())
{
EffortControlPDMotorControlData & ecpdcdata =
@@ -119,11 +128,17 @@ namespace talos_sot_controller
}
bool RCSotController::
- initInterfaces(talos_hardware_interface::RobotHW * robot_hw,
+ initInterfaces(lhi::RobotHW * robot_hw,
ros::NodeHandle &,
ros::NodeHandle &,
std::set<std::string> & claimed_resources)
{
+ std::string lns;
+ #ifdef REAL_ROBOT
+ lns="hardware_interface";
+ #else
+ lns="talos_hardware_interface";
+ #endif
// Check if construction finished cleanly
if (state_!=CONSTRUCTED)
{
@@ -135,8 +150,8 @@ namespace talos_sot_controller
if (! pos_iface_)
{
ROS_ERROR("This controller requires a hardware interface of type '%s'."
- " Make sure this is registered in the talos_hardware_interface::RobotHW class.",
- getHardwareInterfaceType().c_str());
+ " Make sure this is registered in the %s::RobotHW class.",
+ getHardwareInterfaceType().c_str(), lns.c_str());
return false ;
}
@@ -145,8 +160,8 @@ namespace talos_sot_controller
if (! effort_iface_)
{
ROS_ERROR("This controller requires a hardware interface of type '%s'."
- " Make sure this is registered in the talos_hardware_interface::RobotHW class.",
- getHardwareInterfaceType().c_str());
+ " Make sure this is registered in the %s::RobotHW class.",
+ getHardwareInterfaceType().c_str(),lns.c_str());
return false ;
}
@@ -155,8 +170,8 @@ namespace talos_sot_controller
if (! ft_iface_ )
{
ROS_ERROR("This controller requires a hardware interface of type '%s '. "
- " Make sure this is registered inthe talos_hardware_interface::RobotHW class.",
- internal :: demangledTypeName<ForceTorqueSensorInterface>().c_str());
+ " Make sure this is registered inthe %s::RobotHW class.",
+ internal :: demangledTypeName<ForceTorqueSensorInterface>().c_str(),lns.c_str());
return false ;
}
// Get a pointer to the IMU sensor interface
@@ -164,8 +179,8 @@ namespace talos_sot_controller
if (! imu_iface_)
{
ROS_ERROR("This controller requires a hardware interface of type '%s'."
- " Make sure this is registered inthe talos_hardware_interface::RobotHW class.",
- internal :: demangledTypeName<ImuSensorInterface>().c_str());
+ " Make sure this is registered in the %s::RobotHW class.",
+ internal :: demangledTypeName<ImuSensorInterface>().c_str(),lns.c_str());
return false ;
}
@@ -177,8 +192,8 @@ namespace talos_sot_controller
if (!act_temp_iface_)
{
ROS_ERROR("This controller requires a hardware interface of type '%s'."
- " Make sure this is registered inthe talos_hardware_interface::RobotHW class.",
- internal :: demangledTypeName<ActuatorTemperatureSensorInterface>().c_str());
+ " Make sure this is registered in the %s::RobotHW class.",
+ internal :: demangledTypeName<ActuatorTemperatureSensorInterface>().c_str(),lns.c_str());
return false ;
}
}
@@ -268,42 +283,42 @@ namespace talos_sot_controller
// Read libname
if (robot_nh.hasParam("/sot_controller/effort_control_pd_motor_init/gains"))
{
- XmlRpc::XmlRpcValue xml_rpc_ecpd_init;
- robot_nh.getParamCached("/sot_controller/effort_control_pd_motor_init/gains",
- xml_rpc_ecpd_init);
-
- ROS_INFO("/sot_controller/effort_control_pd_motor_init/gains: %d %d %d\n",
- xml_rpc_ecpd_init.getType(),XmlRpc::XmlRpcValue::TypeArray,XmlRpc::XmlRpcValue::TypeStruct);
- effort_mode_pd_motors_.clear();
-
- for (int i=0;i<joints_name_.size();i++)
- {
- if (xml_rpc_ecpd_init.hasMember(joints_name_[i]))
- {
- /*
- XmlRpc::XmlRpcValue &aDataSet= xml_rpc_ecpd_init[joints_name_[i]];
- ROS_INFO("/sot_controller/effort_control_pd_motor_init %s type: %d\n",joints_name_[i],aDataSet.getType());
- if (aDataSet.getType()!=XmlRpc::XmlRpcValue::TypeStruct)
- {
- ROS_ERROR("In /sot_controller/effort_control_pd_motor_init/gains/%s not a struct"
- ,joints_name_[i]);
- throw XmlrpcHelperException("Pb in readParamsEffortControlPDMotorControlData");
- }
- */
- std::string prefix= "/sot_controller/effort_control_pd_motor_init/gains/" + joints_name_[i];
- effort_mode_pd_motors_[joints_name_[i]].read_from_xmlrpc_value(prefix);
- }
- else
- ROS_INFO("joint %s not in /sot_controller/effort_control_pd_motor_init/gains\n",
- joints_name_[i].c_str());
- }
- return true;
+ XmlRpc::XmlRpcValue xml_rpc_ecpd_init;
+ robot_nh.getParamCached("/sot_controller/effort_control_pd_motor_init/gains",
+ xml_rpc_ecpd_init);
+
+ ROS_INFO("/sot_controller/effort_control_pd_motor_init/gains: %d %d %d\n",
+ xml_rpc_ecpd_init.getType(),XmlRpc::XmlRpcValue::TypeArray,XmlRpc::XmlRpcValue::TypeStruct);
+ effort_mode_pd_motors_.clear();
+
+ for (int i=0;i<joints_name_.size();i++)
+ {
+ if (xml_rpc_ecpd_init.hasMember(joints_name_[i]))
+ {
+ /*
+ XmlRpc::XmlRpcValue &aDataSet= xml_rpc_ecpd_init[joints_name_[i]];
+ ROS_INFO("/sot_controller/effort_control_pd_motor_init %s type: %d\n",joints_name_[i],aDataSet.getType());
+ if (aDataSet.getType()!=XmlRpc::XmlRpcValue::TypeStruct)
+ {
+ ROS_ERROR("In /sot_controller/effort_control_pd_motor_init/gains/%s not a struct"
+ ,joints_name_[i]);
+ throw XmlrpcHelperException("Pb in readParamsEffortControlPDMotorControlData");
+ }
+ */
+ std::string prefix= "/sot_controller/effort_control_pd_motor_init/gains/" + joints_name_[i];
+ effort_mode_pd_motors_[joints_name_[i]].read_from_xmlrpc_value(prefix);
+ }
+ else
+ ROS_INFO("joint %s not in /sot_controller/effort_control_pd_motor_init/gains\n",
+ joints_name_[i].c_str());
+ }
+ return true;
}
ROS_ERROR("No parameter /sot_controller/effort_controler_pd_motor_init");
return false;
}
-
+
bool RCSotController::
readParamsFromRCToSotDevice(ros::NodeHandle &robot_nh)
{
@@ -420,6 +435,7 @@ namespace talos_sot_controller
if (control_mode_==EFFORT)
readParamsEffortControlPDMotorControlData(robot_nh);
+
return true;
}
@@ -706,12 +722,7 @@ namespace talos_sot_controller
sotController_->nominalSetSensors(sensorsIn_);
sotController_->getControl(controlValues_);
}
- catch(std::exception &e)
- {
- std::cerr << "Failure happened during one_iteration of RCSotController::one_iteration: std_exception" << std::endl;
- std::cerr << "Use gdb on this line together with gdb to investiguate the problem: " <<std::endl;
- std::cerr << __FILE__ << " " << __LINE__ << std::endl;
- }
+ catch(std::exception &e) { throw e;}
/// Read the control values
readControl(controlValues_);
@@ -722,64 +733,63 @@ namespace talos_sot_controller
void RCSotController::
update(const ros::Time&, const ros::Duration& period)
- {
+ {
// Do not send any control if the dynamic graph is not started
- if (!isDynamicGraphStopped())
+ if (!isDynamicGraphStopped())
{
- try
- {
- one_iteration();
- }
- catch (std::exception const &exc)
- {
- std::cerr << "Failure happened during one_iteration evaluation: std_exception" << std::endl;
- std::cerr << "Use gdb on this line together with gdb to investiguate the problem: " <<std::endl;
- std::cerr << __FILE__ << " " << __LINE__ << std::endl;
- throw exc;
- }
- catch (...)
- {
- std::cerr << "Failure happened during one_iteration evaluation: unknown exception" << std::endl;
- std::cerr << "Use gdb on this line together with gdb to investiguate the problem: " <<std::endl;
- std::cerr << __FILE__ << " " << __LINE__ << std::endl;
- }
+ try
+ {
+ one_iteration();
+ }
+ catch (std::exception const &exc)
+ {
+ std::cerr << "Failure happened during one_iteration evaluation: std_exception" << std::endl;
+ std::cerr << "Use gdb on this line together with gdb to investiguate the problem: " <<std::endl;
+ std::cerr << __FILE__ << " " << __LINE__ << std::endl;
+ throw exc;
+ }
+ catch (...)
+ {
+ std::cerr << "Failure happened during one_iteration evaluation: unknown exception" << std::endl;
+ std::cerr << "Use gdb on this line together with gdb to investiguate the problem: " <<std::endl;
+ std::cerr << __FILE__ << " " << __LINE__ << std::endl;
+ }
}
else
// But in effort mode it means that we are sending 0
// Therefore implements a default PD controller on the system.
if (control_mode_==EFFORT)
- {
- // ROS_INFO("Compute command for effort mode: %d %d",joints_.size(),effort_mode_pd_motors_.size());
- for(unsigned int idJoint=0;idJoint<joints_.size();idJoint++)
- {
- std::string joint_name = joints_name_[idJoint];
- std::map<std::string,EffortControlPDMotorControlData>::iterator
- search_ecpd = effort_mode_pd_motors_.find(joint_name);
-
- if (search_ecpd!=effort_mode_pd_motors_.end())
- {
- EffortControlPDMotorControlData & ecpdcdata =
- search_ecpd->second;
-
- double vel_err = 0 - joints_[idJoint].getVelocity();
- double err = ecpdcdata.des_pos - joints_[idJoint].getPosition();
-
- ecpdcdata.integ_err +=err;
-
- double local_command = ecpdcdata.pid_controller.computeCommand(err,vel_err,period);
- // Apply command
- control_toolbox::Pid::Gains gains = ecpdcdata.pid_controller.getGains();
- //ROS_INFO("command: %d %s %f %f (%f %f %f)",idJoint, joints_name_[idJoint].c_str(),
- //local_command, DataOneIter_.motor_angle[idJoint],
- //gains.p_gain_,gains.d_gain_, gains.i_gain_);
- joints_[idJoint].setCommand(local_command);
-
- // Update previous value.
- ecpdcdata.prev = DataOneIter_.motor_angle[idJoint];
- }
- }
- }
- }
+ {
+ // ROS_INFO("Compute command for effort mode: %d %d",joints_.size(),effort_mode_pd_motors_.size());
+ for(unsigned int idJoint=0;idJoint<joints_.size();idJoint++)
+ {
+ std::string joint_name = joints_name_[idJoint];
+ std::map<std::string,EffortControlPDMotorControlData>::iterator
+ search_ecpd = effort_mode_pd_motors_.find(joint_name);
+
+ if (search_ecpd!=effort_mode_pd_motors_.end())
+ {
+ EffortControlPDMotorControlData & ecpdcdata =
+ search_ecpd->second;
+ double vel_err = 0 - joints_[idJoint].getVelocity();
+ double err = ecpdcdata.des_pos - joints_[idJoint].getPosition();
+
+ ecpdcdata.integ_err +=err;
+
+ double local_command = ecpdcdata.pid_controller.computeCommand(err,vel_err,period);
+ // Apply command
+ control_toolbox::Pid::Gains gains = ecpdcdata.pid_controller.getGains();
+ //ROS_INFO("command: %d %s %f %f (%f %f %f)",idJoint, joints_name_[idJoint].c_str(),
+ //local_command, DataOneIter_.motor_angle[idJoint],
+ //gains.p_gain_,gains.d_gain_, gains.i_gain_);
+ joints_[idJoint].setCommand(local_command);
+
+ // Update previous value.
+ ecpdcdata.prev = DataOneIter_.motor_angle[idJoint];
+ }
+ }
+ }
+ }
void RCSotController::
starting(const ros::Time &)
@@ -799,16 +809,16 @@ namespace talos_sot_controller
{
//return type_name_;
if (control_mode_==POSITION)
- return talos_hardware_interface::internal::
- demangledTypeName<talos_hardware_interface::PositionJointInterface>();
+ return lhi::internal::
+ demangledTypeName<lhi::PositionJointInterface>();
else if (control_mode_==EFFORT)
- return talos_hardware_interface::internal::
- demangledTypeName<talos_hardware_interface::EffortJointInterface>();
+ return lhi::internal::
+ demangledTypeName<lhi::EffortJointInterface>();
std::string voidstring("");
return voidstring;
}
PLUGINLIB_EXPORT_CLASS(talos_sot_controller::RCSotController,
- talos_controller_interface::ControllerBase);
+ lci::ControllerBase);
}
diff --git a/talos_roscontrol_sot/src/roscontrol-sot-controller.hh b/talos_roscontrol_sot/src/roscontrol-sot-controller.hh
index e45c20a..1104645 100644
--- a/talos_roscontrol_sot/src/roscontrol-sot-controller.hh
+++ b/talos_roscontrol_sot/src/roscontrol-sot-controller.hh
@@ -35,18 +35,26 @@
#include <string>
#include <map>
+#ifdef REAL_ROBOT
+#include <controller_interface/controller.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <hardware_interface/imu_sensor_interface.h>
+#include <hardware_interface/force_torque_sensor_interface.h>
+#include <pal_hardware_interfaces/actuator_temperature_interface.h>
+#else
#include <talos_controller_interface/controller.h>
#include <talos_hardware_interface/joint_command_interface.h>
#include <talos_hardware_interface/imu_sensor_interface.h>
#include <talos_hardware_interface/force_torque_sensor_interface.h>
#include <talos_pal_hardware_interfaces/actuator_temperature_interface.h>
+#endif
#include <dynamic_graph_bridge/sot_loader_basic.hh>
-
-#include "log.hh"
#include <ros/ros.h>
#include <control_toolbox/pid.h>
+#include "log.hh"
+
namespace talos_sot_controller
{
enum SotControlMode { POSITION, EFFORT};
@@ -73,11 +81,20 @@ namespace talos_sot_controller
// void read_from_xmlrpc_value(XmlRpc::XmlRpcValue &aXRV);
void read_from_xmlrpc_value(const std::string &prefix);
};
+
/**
This class encapsulates the Stack of Tasks inside the ros-control infra-structure.
*/
- class RCSotController : public talos_controller_interface::ControllerBase,
+#ifdef REAL_ROBOT
+ namespace lhi = hardware_interface;
+ namespace lci = controller_interface;
+#else
+ namespace lhi = talos_hardware_interface;
+ namespace lci = talos_controller_interface;
+#endif
+
+ class RCSotController : public lci::ControllerBase,
SotLoaderBasic
{
@@ -92,33 +109,33 @@ namespace talos_sot_controller
/// @{ \name Ros-control related fields
/// \brief Vector of joint handles.
- std::vector<talos_hardware_interface::JointHandle> joints_;
+ std::vector<lhi::JointHandle> joints_;
std::vector<std::string> joints_name_;
/// \brief Vector towards the IMU.
- std::vector<talos_hardware_interface::ImuSensorHandle> imu_sensor_;
+ std::vector<lhi::ImuSensorHandle> imu_sensor_;
/// \brief Vector of 6D force sensor.
- std::vector<talos_hardware_interface::ForceTorqueSensorHandle> ft_sensors_;
+ std::vector<lhi::ForceTorqueSensorHandle> ft_sensors_;
/// \brief Vector of temperature sensors for the actuators.
- std::vector<talos_hardware_interface::ActuatorTemperatureSensorHandle>
+ std::vector<lhi::ActuatorTemperatureSensorHandle>
act_temp_sensors_;
/// \brief Interface to the joints controlled in position.
- talos_hardware_interface::PositionJointInterface * pos_iface_;
+ lhi::PositionJointInterface * pos_iface_;
/// \brief Interface to the joints controlled in force.
- talos_hardware_interface::EffortJointInterface * effort_iface_;
+ lhi::EffortJointInterface * effort_iface_;
/// \brief Interface to the sensors (IMU).
- talos_hardware_interface::ImuSensorInterface* imu_iface_;
+ lhi::ImuSensorInterface* imu_iface_;
/// \brief Interface to the sensors (Force).
- talos_hardware_interface::ForceTorqueSensorInterface* ft_iface_;
+ lhi::ForceTorqueSensorInterface* ft_iface_;
/// \brief Interface to the actuator temperature sensor.
- talos_hardware_interface::ActuatorTemperatureSensorInterface * act_temp_iface_;
+ lhi::ActuatorTemperatureSensorInterface * act_temp_iface_;
/// @}
@@ -134,10 +151,11 @@ namespace talos_sot_controller
/// \brief The robot can controlled in effort or position mode (default).
SotControlMode control_mode_;
+
/// \brief Implement a PD controller for the robot when the dynamic graph
/// is not on.
std::map<std::string,EffortControlPDMotorControlData> effort_mode_pd_motors_;
-
+
/// \brief Map from ros-control quantities to robot device
/// ros-control quantities are for the sensors:
/// * motor-angles
@@ -155,7 +173,7 @@ namespace talos_sot_controller
/// \brief Read the configuration files,
/// claims the request to the robot and initialize the Stack-Of-Tasks.
- bool initRequest (talos_hardware_interface::RobotHW * robot_hw,
+ bool initRequest (lhi::RobotHW * robot_hw,
ros::NodeHandle &robot_nh,
ros::NodeHandle &controller_nh,
std::set<std::string> & claimed_resources);
@@ -178,7 +196,7 @@ namespace talos_sot_controller
protected:
/// Initialize the roscontrol interfaces
- bool initInterfaces(talos_hardware_interface::RobotHW * robot_hw,
+ bool initInterfaces(lhi::RobotHW * robot_hw,
ros::NodeHandle &,
ros::NodeHandle &,
std::set<std::string> & claimed_resources);
--
GitLab