Commit 3d9203f0 authored by Olivier Stasse's avatar Olivier Stasse
Browse files

clang-format + Fix pinocchio header order.

parent 9dd2e8a2
Pipeline #6285 failed with stage
in 2 minutes and 31 seconds
---
ColumnLimit: 80
...
...@@ -4,17 +4,17 @@ ...@@ -4,17 +4,17 @@
Object to log the low-level informations of a robot. Object to log the low-level informations of a robot.
*/ */
#include "log.hh" #include "log.hh"
#include <sys/time.h>
#include <sstream>
#include <fstream> #include <fstream>
#include <iomanip> #include <iomanip>
#include <sstream>
#include <sys/time.h>
using namespace std; using namespace std;
using namespace rc_sot_system; using namespace rc_sot_system;
DataToLog::DataToLog() {} DataToLog::DataToLog() {}
void DataToLog::init(ProfileLog& aProfileLog) { void DataToLog::init(ProfileLog &aProfileLog) {
profileLog_ = aProfileLog; profileLog_ = aProfileLog;
motor_angle.resize(aProfileLog.nbDofs * aProfileLog.length); motor_angle.resize(aProfileLog.nbDofs * aProfileLog.length);
joint_angle.resize(aProfileLog.nbDofs * aProfileLog.length); joint_angle.resize(aProfileLog.nbDofs * aProfileLog.length);
...@@ -37,7 +37,7 @@ void DataToLog::init(ProfileLog& aProfileLog) { ...@@ -37,7 +37,7 @@ void DataToLog::init(ProfileLog& aProfileLog) {
Log::Log() : lref_(0), lrefts_(0) {} Log::Log() : lref_(0), lrefts_(0) {}
void Log::init(ProfileLog& aProfileLog) { void Log::init(ProfileLog &aProfileLog) {
profileLog_ = aProfileLog; profileLog_ = aProfileLog;
lref_ = 0; lref_ = 0;
lrefts_ = 0; lrefts_ = 0;
...@@ -48,7 +48,7 @@ void Log::init(ProfileLog& aProfileLog) { ...@@ -48,7 +48,7 @@ void Log::init(ProfileLog& aProfileLog) {
timeorigin_ = (double)current.tv_sec + 0.000001 * ((double)current.tv_usec); timeorigin_ = (double)current.tv_sec + 0.000001 * ((double)current.tv_usec);
} }
void Log::record(DataToLog& aDataToLog) { void Log::record(DataToLog &aDataToLog) {
if ((aDataToLog.motor_angle.size() != profileLog_.nbDofs) || if ((aDataToLog.motor_angle.size() != profileLog_.nbDofs) ||
(aDataToLog.velocities.size() != profileLog_.nbDofs)) (aDataToLog.velocities.size() != profileLog_.nbDofs))
return; return;
...@@ -122,7 +122,7 @@ void Log::stop_it() { ...@@ -122,7 +122,7 @@ void Log::stop_it() {
timeorigin_; timeorigin_;
} }
void Log::save(std::string& fileName) { void Log::save(std::string &fileName) {
std::string suffix("-mastate.log"); std::string suffix("-mastate.log");
saveVector(fileName, suffix, StoredData_.motor_angle, profileLog_.nbDofs); saveVector(fileName, suffix, StoredData_.motor_angle, profileLog_.nbDofs);
suffix = "-jastate.log"; suffix = "-jastate.log";
...@@ -154,22 +154,22 @@ void Log::save(std::string& fileName) { ...@@ -154,22 +154,22 @@ void Log::save(std::string& fileName) {
saveVector(fileName, suffix, StoredData_.duration, 1); saveVector(fileName, suffix, StoredData_.duration, 1);
} }
inline void writeHeaderToBinaryBuffer(ofstream& of, const std::size_t& nVector, inline void writeHeaderToBinaryBuffer(ofstream &of, const std::size_t &nVector,
const std::size_t& vectorSize) { const std::size_t &vectorSize) {
of.write((const char*)(&nVector), sizeof(std::size_t)); of.write((const char *)(&nVector), sizeof(std::size_t));
of.write((const char*)(&vectorSize), sizeof(std::size_t)); of.write((const char *)(&vectorSize), sizeof(std::size_t));
} }
inline void writeToBinaryFile(ofstream& of, const double& t, const double& dt, inline void writeToBinaryFile(ofstream &of, const double &t, const double &dt,
const std::vector<double>& data, const std::vector<double> &data,
const std::size_t& idx, const std::size_t& size) { const std::size_t &idx, const std::size_t &size) {
of.write((const char*)&t, sizeof(double)); of.write((const char *)&t, sizeof(double));
of.write((const char*)&dt, sizeof(double)); of.write((const char *)&dt, sizeof(double));
of.write((const char*)(&data[idx]), size * (sizeof(double))); of.write((const char *)(&data[idx]), size * (sizeof(double)));
} }
void Log::saveVector(std::string& fileName, std::string& suffix, void Log::saveVector(std::string &fileName, std::string &suffix,
const std::vector<double>& avector, std::size_t size) { const std::vector<double> &avector, std::size_t size) {
ostringstream oss; ostringstream oss;
oss << fileName; oss << fileName;
oss << suffix.c_str(); oss << suffix.c_str();
......
...@@ -7,8 +7,8 @@ ...@@ -7,8 +7,8 @@
#ifndef _RC_SOT_SYSTEM_LOG_H_ #ifndef _RC_SOT_SYSTEM_LOG_H_
#define _RC_SOT_SYSTEM_LOG_H_ #define _RC_SOT_SYSTEM_LOG_H_
#include <vector>
#include <string> #include <string>
#include <vector>
namespace rc_sot_system { namespace rc_sot_system {
...@@ -57,7 +57,7 @@ struct DataToLog { ...@@ -57,7 +57,7 @@ struct DataToLog {
}; };
class Log { class Log {
private: private:
/// Profile Log /// Profile Log
ProfileLog profileLog_; ProfileLog profileLog_;
...@@ -78,7 +78,7 @@ class Log { ...@@ -78,7 +78,7 @@ class Log {
void saveVector(std::string &filename, std::string &suffix, void saveVector(std::string &filename, std::string &suffix,
const std::vector<double> &avector, std::size_t); const std::vector<double> &avector, std::size_t);
public: public:
Log(); Log();
void init(ProfileLog &aProfileLog); void init(ProfileLog &aProfileLog);
...@@ -88,7 +88,7 @@ class Log { ...@@ -88,7 +88,7 @@ class Log {
void start_it(); void start_it();
void stop_it(); void stop_it();
}; };
} // namespace rc_sot_system } // namespace rc_sot_system
#pragma GCC diagnostic push #pragma GCC diagnostic push
#pragma GCC system_header #pragma GCC system_header
......
#include <dlfcn.h>
#include <fstream> #include <fstream>
#include <iomanip> #include <iomanip>
#include <dlfcn.h>
#include <sstream> #include <sstream>
#include "roscontrol-sot-controller.hh" #include "roscontrol-sot-controller.hh"
...@@ -19,26 +19,26 @@ ...@@ -19,26 +19,26 @@
#define DBGFILE "/tmp/rcoh2sot.dat" #define DBGFILE "/tmp/rcoh2sot.dat"
#define RESETDEBUG5() \ #define RESETDEBUG5() \
{ \ { \
std::ofstream DebugFile; \ std::ofstream DebugFile; \
DebugFile.open(DBGFILE, std::ofstream::out); \ DebugFile.open(DBGFILE, std::ofstream::out); \
DebugFile.close(); \ DebugFile.close(); \
} }
#define ODEBUG5FULL(x) \ #define ODEBUG5FULL(x) \
{ \ { \
std::ofstream DebugFile; \ std::ofstream DebugFile; \
DebugFile.open(DBGFILE, std::ofstream::app); \ DebugFile.open(DBGFILE, std::ofstream::app); \
DebugFile << __FILE__ << ":" << __FUNCTION__ << "(#" << __LINE__ \ DebugFile << __FILE__ << ":" << __FUNCTION__ << "(#" << __LINE__ \
<< "):" << x << std::endl; \ << "):" << x << std::endl; \
DebugFile.close(); \ DebugFile.close(); \
} }
#define ODEBUG5(x) \ #define ODEBUG5(x) \
{ \ { \
std::ofstream DebugFile; \ std::ofstream DebugFile; \
DebugFile.open(DBGFILE, std::ofstream::app); \ DebugFile.open(DBGFILE, std::ofstream::app); \
DebugFile << x << std::endl; \ DebugFile << x << std::endl; \
DebugFile.close(); \ DebugFile.close(); \
} }
#define RESETDEBUG4() #define RESETDEBUG4()
...@@ -46,7 +46,7 @@ ...@@ -46,7 +46,7 @@
#define ODEBUG4(x) #define ODEBUG4(x)
class LoggerROSStream : public ::dynamicgraph::LoggerStream { class LoggerROSStream : public ::dynamicgraph::LoggerStream {
public: public:
void write(const char *c) { ROS_ERROR("%s", c); } void write(const char *c) { ROS_ERROR("%s", c); }
}; };
...@@ -70,13 +70,10 @@ void ControlPDMotorControlData::read_from_xmlrpc_value( ...@@ -70,13 +70,10 @@ void ControlPDMotorControlData::read_from_xmlrpc_value(
} }
RCSotController::RCSotController() RCSotController::RCSotController()
: // Store 32 DoFs for 5 minutes (1 Khz: 5*60*1000) : // Store 32 DoFs for 5 minutes (1 Khz: 5*60*1000)
// -> 124 Mo of data. // -> 124 Mo of data.
type_name_("RCSotController"), type_name_("RCSotController"), simulation_mode_(false),
simulation_mode_(false), accumulated_time_(0.0), jitter_(0.0), verbosity_level_(0) {
accumulated_time_(0.0),
jitter_(0.0),
verbosity_level_(0) {
RESETDEBUG4(); RESETDEBUG4();
profileLog_.length = 300000; profileLog_.length = 300000;
} }
...@@ -129,7 +126,8 @@ bool RCSotController::initRequest(lhi::RobotHW *robot_hw, ...@@ -129,7 +126,8 @@ bool RCSotController::initRequest(lhi::RobotHW *robot_hw,
ClaimedResources &claimed_resources) { ClaimedResources &claimed_resources) {
ROS_WARN("initRequest 1"); ROS_WARN("initRequest 1");
/// Read the parameter server /// Read the parameter server
if (!readParams(robot_nh)) return false; if (!readParams(robot_nh))
return false;
ROS_WARN("initRequest 2"); ROS_WARN("initRequest 2");
/// Create ros control interfaces to hardware /// Create ros control interfaces to hardware
/// Recalls: init() is called by initInterfaces() /// Recalls: init() is called by initInterfaces()
...@@ -166,64 +164,58 @@ bool RCSotController::initInterfaces(lhi::RobotHW *robot_hw, ros::NodeHandle &, ...@@ -166,64 +164,58 @@ bool RCSotController::initInterfaces(lhi::RobotHW *robot_hw, ros::NodeHandle &,
// Check if construction finished cleanly // Check if construction finished cleanly
if (state_ != CONSTRUCTED) { if (state_ != CONSTRUCTED) {
ROS_ERROR( ROS_ERROR("Cannot initialize this controller because it "
"Cannot initialize this controller because it " "failed to be constructed");
"failed to be constructed");
} }
// Get a pointer to the joint position control interface // Get a pointer to the joint position control interface
pos_iface_ = robot_hw->get<PositionJointInterface>(); pos_iface_ = robot_hw->get<PositionJointInterface>();
if (!pos_iface_) { if (!pos_iface_) {
ROS_WARN( ROS_WARN("This controller did not find a hardware interface of type "
"This controller did not find a hardware interface of type " "PositionJointInterface."
"PositionJointInterface." " Make sure this is registered in the %s::RobotHW class "
" Make sure this is registered in the %s::RobotHW class " "if it is required.",
"if it is required.", lns.c_str());
lns.c_str());
} }
// Get a pointer to the joint velocity control interface // Get a pointer to the joint velocity control interface
vel_iface_ = robot_hw->get<VelocityJointInterface>(); vel_iface_ = robot_hw->get<VelocityJointInterface>();
if (!vel_iface_) { if (!vel_iface_) {
ROS_WARN( ROS_WARN("This controller did not find a hardware interface of type "
"This controller did not find a hardware interface of type " "VelocityJointInterface."
"VelocityJointInterface." " Make sure this is registered in the %s::RobotHW class "
" Make sure this is registered in the %s::RobotHW class " "if it is required.",
"if it is required.", lns.c_str());
lns.c_str());
} }
// Get a pointer to the joint effort control interface // Get a pointer to the joint effort control interface
effort_iface_ = robot_hw->get<EffortJointInterface>(); effort_iface_ = robot_hw->get<EffortJointInterface>();
if (!effort_iface_) { if (!effort_iface_) {
ROS_WARN( ROS_WARN("This controller did not find a hardware interface of type "
"This controller did not find a hardware interface of type " "EffortJointInterface."
"EffortJointInterface." " Make sure this is registered in the %s::RobotHW class "
" Make sure this is registered in the %s::RobotHW class " "if it is required.",
"if it is required.", lns.c_str());
lns.c_str());
} }
// Get a pointer to the force-torque sensor interface // Get a pointer to the force-torque sensor interface
ft_iface_ = robot_hw->get<ForceTorqueSensorInterface>(); ft_iface_ = robot_hw->get<ForceTorqueSensorInterface>();
if (!ft_iface_) { if (!ft_iface_) {
ROS_WARN( ROS_WARN("This controller did not find a hardware interface of type '%s '. "
"This controller did not find a hardware interface of type '%s '. " " Make sure this is registered inthe %s::RobotHW class "
" Make sure this is registered inthe %s::RobotHW class " "if it is required.",
"if it is required.", internal ::demangledTypeName<ForceTorqueSensorInterface>().c_str(),
internal ::demangledTypeName<ForceTorqueSensorInterface>().c_str(), lns.c_str());
lns.c_str());
} }
// Get a pointer to the IMU sensor interface // Get a pointer to the IMU sensor interface
imu_iface_ = robot_hw->get<ImuSensorInterface>(); imu_iface_ = robot_hw->get<ImuSensorInterface>();
if (!imu_iface_) { if (!imu_iface_) {
ROS_WARN( ROS_WARN("This controller did not find a hardware interface of type '%s'."
"This controller did not find a hardware interface of type '%s'." " Make sure this is registered in the %s::RobotHW class "
" Make sure this is registered in the %s::RobotHW class " "if it is required.",
"if it is required.", internal ::demangledTypeName<ImuSensorInterface>().c_str(),
internal ::demangledTypeName<ImuSensorInterface>().c_str(), lns.c_str());
lns.c_str());
} }
// Temperature sensor not available in simulation mode // Temperature sensor not available in simulation mode
...@@ -244,9 +236,12 @@ bool RCSotController::initInterfaces(lhi::RobotHW *robot_hw, ros::NodeHandle &, ...@@ -244,9 +236,12 @@ bool RCSotController::initInterfaces(lhi::RobotHW *robot_hw, ros::NodeHandle &,
} }
// Return which resources are claimed by this controller // Return which resources are claimed by this controller
if (pos_iface_) pos_iface_->clearClaims(); if (pos_iface_)
if (vel_iface_) vel_iface_->clearClaims(); pos_iface_->clearClaims();
if (effort_iface_) effort_iface_->clearClaims(); if (vel_iface_)
vel_iface_->clearClaims();
if (effort_iface_)
effort_iface_->clearClaims();
if (!init()) { if (!init()) {
ROS_ERROR("Failed to initialize sot-controller"); ROS_ERROR("Failed to initialize sot-controller");
...@@ -280,27 +275,37 @@ bool RCSotController::initInterfaces(lhi::RobotHW *robot_hw, ros::NodeHandle &, ...@@ -280,27 +275,37 @@ bool RCSotController::initInterfaces(lhi::RobotHW *robot_hw, ros::NodeHandle &,
} }
/// Display claimed ressources /// Display claimed ressources
if (verbosity_level_ > 0) displayClaimedResources(claimed_resources); if (verbosity_level_ > 0)
displayClaimedResources(claimed_resources);
if (pos_iface_ != 0) pos_iface_->clearClaims(); if (pos_iface_ != 0)
pos_iface_->clearClaims();
if (vel_iface_ != 0) vel_iface_->clearClaims(); if (vel_iface_ != 0)
vel_iface_->clearClaims();
if (effort_iface_ != 0) effort_iface_->clearClaims(); if (effort_iface_ != 0)
effort_iface_->clearClaims();
#else #else
claimed_resources = pos_iface_->getClaims(); claimed_resources = pos_iface_->getClaims();
/// Display claimed ressources /// Display claimed ressources
if (verbosity_level_ > 0) displayClaimedResources(claimed_resources); if (verbosity_level_ > 0)
if (pos_iface_ != 0) pos_iface_->clearClaims(); displayClaimedResources(claimed_resources);
if (pos_iface_ != 0)
pos_iface_->clearClaims();
claimed_resources = vel_iface_->getClaims(); claimed_resources = vel_iface_->getClaims();
/// Display claimed ressources /// Display claimed ressources
if (verbosity_level_ > 0) displayClaimedResources(claimed_resources); if (verbosity_level_ > 0)
if (vel_iface_ != 0) vel_iface_->clearClaims(); displayClaimedResources(claimed_resources);
if (vel_iface_ != 0)
vel_iface_->clearClaims();
claimed_resources = effort_iface_->getClaims(); claimed_resources = effort_iface_->getClaims();
if (verbosity_level_ > 0) displayClaimedResources(claimed_resources); if (verbosity_level_ > 0)
if (effort_iface_ != 0) effort_iface_->clearClaims(); displayClaimedResources(claimed_resources);
if (effort_iface_ != 0)
effort_iface_->clearClaims();
#endif #endif
if (verbosity_level_ > 0) if (verbosity_level_ > 0)
ROS_INFO_STREAM("Initialization of sot-controller Ok !"); ROS_INFO_STREAM("Initialization of sot-controller Ok !");
...@@ -311,7 +316,8 @@ bool RCSotController::initInterfaces(lhi::RobotHW *robot_hw, ros::NodeHandle &, ...@@ -311,7 +316,8 @@ bool RCSotController::initInterfaces(lhi::RobotHW *robot_hw, ros::NodeHandle &,
} }
bool RCSotController::init() { bool RCSotController::init() {
if (!initJoints()) return false; if (!initJoints())
return false;
if (!initIMU()) { if (!initIMU()) {
ROS_WARN("could not initialize IMU sensor(s)."); ROS_WARN("could not initialize IMU sensor(s).");
} }
...@@ -404,10 +410,9 @@ bool RCSotController::readParamsEffortControlPDMotorControlData( ...@@ -404,10 +410,9 @@ bool RCSotController::readParamsEffortControlPDMotorControlData(
effort_mode_pd_motors_[joints_name_[i]].read_from_xmlrpc_value( effort_mode_pd_motors_[joints_name_[i]].read_from_xmlrpc_value(
prefix); prefix);
} else } else
ROS_ERROR( ROS_ERROR("No PID data for effort controlled joint %s in "
"No PID data for effort controlled joint %s in " "/sot_controller/effort_control_pd_motor_init/gains/",
"/sot_controller/effort_control_pd_motor_init/gains/", joints_name_[i].c_str());
joints_name_[i].c_str());
} }
} }
} }
...@@ -452,10 +457,9 @@ bool RCSotController::readParamsVelocityControlPDMotorControlData( ...@@ -452,10 +457,9 @@ bool RCSotController::readParamsVelocityControlPDMotorControlData(
velocity_mode_pd_motors_[joints_name_[i]].read_from_xmlrpc_value( velocity_mode_pd_motors_[joints_name_[i]].read_from_xmlrpc_value(
prefix); prefix);
} else } else
ROS_ERROR( ROS_ERROR("No PID data for velocity controlled joint %s in "
"No PID data for velocity controlled joint %s in " "/sot_controller/velocity_control_pd_motor_init/gains/",
"/sot_controller/velocity_control_pd_motor_init/gains/", joints_name_[i].c_str());
joints_name_[i].c_str());
} }
} }
} }
...@@ -480,15 +484,13 @@ bool RCSotController::readParamsFromRCToSotDevice(ros::NodeHandle &robot_nh) { ...@@ -480,15 +484,13 @@ bool RCSotController::readParamsFromRCToSotDevice(ros::NodeHandle &robot_nh) {
ROS_INFO_STREAM(it->first << ", " << it->second); ROS_INFO_STREAM(it->first << ", " << it->second);
} }
} else { } else {
ROS_ERROR_STREAM( ROS_ERROR_STREAM("Could not read param /sot_controller/"
"Could not read param /sot_controller/" "map_rc_to_sot_device");
"map_rc_to_sot_device");
return false; return false;
} }
} else { } else {
ROS_INFO_STREAM( ROS_INFO_STREAM("Param /sot_controller/map_rc_to_sot_device "
"Param /sot_controller/map_rc_to_sot_device " "does not exists !");
"does not exists !");
return false; return false;
} }
return true; return true;
...@@ -574,7 +576,8 @@ bool RCSotController::readParamsControlMode(ros::NodeHandle &robot_nh) { ...@@ -574,7 +576,8 @@ bool RCSotController::readParamsControlMode(ros::NodeHandle &robot_nh) {
for (unsigned int idJoint = 0; idJoint < joints_name_.size(); idJoint++) { for (unsigned int idJoint = 0; idJoint < joints_name_.size(); idJoint++) {
std::string joint_name = joints_name_[idJoint]; std::string joint_name = joints_name_[idJoint];
JointSotHandle &aJoint = joints_[joint_name]; JointSotHandle &aJoint = joints_[joint_name];
if (!getJointControlMode(joint_name, aJoint)) return false; if (!getJointControlMode(joint_name, aJoint))
return false;
ROS_INFO("joint_name[%d]=%s, control_mode=%d", idJoint, ROS_INFO("joint_name[%d]=%s, control_mode=%d", idJoint,
joint_name.c_str(), aJoint.ros_control_mode); joint_name.c_str(), aJoint.ros_control_mode);
} }
...@@ -589,13 +592,15 @@ bool RCSotController::readParamsdt(ros::NodeHandle &robot_nh) { ...@@ -589,13 +592,15 @@ bool RCSotController::readParamsdt(ros::NodeHandle &robot_nh) {
/// Reading the jitter is optional but it is a very good idea. /// Reading the jitter is optional but it is a very good idea.
if (robot_nh.hasParam("/sot_controller/jitter")) { if (robot_nh.hasParam("/sot_controller/jitter")) {
robot_nh.getParam("/sot_controller/jitter", jitter_); robot_nh.getParam("/sot_controller/jitter", jitter_);
if (verbosity_level_ > 0) ROS_INFO_STREAM("jitter: " << jitter_); if (verbosity_level_ > 0)
ROS_INFO_STREAM("jitter: " << jitter_);
} }
/// Read /sot_controller/dt to know what is the control period /// Read /sot_controller/dt to know what is the control period
if (robot_nh.hasParam("/sot_controller/dt")) { if (robot_nh.hasParam("/sot_controller/dt")) {
robot_nh.getParam("/sot_controller/dt", dt_); robot_nh.getParam("/sot_controller/dt", dt_);
if (verbosity_level_ > 0) ROS_INFO_STREAM("dt: " << dt_); if (verbosity_level_ > 0)
ROS_INFO_STREAM("dt: " << dt_);
return true; return true;
} }
...@@ -627,7 +632,8 @@ bool RCSotController::readParams(ros::NodeHandle &robot_nh) { ...@@ -627,7 +632,8 @@ bool RCSotController::readParams(ros::NodeHandle &robot_nh) {
readParamsVerbosityLevel(robot_nh); readParamsVerbosityLevel(robot_nh);
/// Reads the SoT dynamic library name. /// Reads the SoT dynamic library name.
if (!readParamsSotLibName(robot_nh)) return false; if (!readParamsSotLibName(robot_nh))
return false;
/// Read /sot_controller/simulation_mode to know /// Read /sot_controller/simulation_mode to know
/// if we are in simulation mode /// if we are in simulation mode
...@@ -640,18 +646,21 @@ bool RCSotController::readParams(ros::NodeHandle &robot_nh) { ...@@ -640,18 +646,21 @@ bool RCSotController::readParams(ros::NodeHandle &robot_nh) {
/// Calls readParamsJointNames /// Calls readParamsJointNames
// Reads the list of joints to be controlled. // Reads the list of joints to be controlled.
if (!readParamsJointNames(robot_nh)) return false; if (!readParamsJointNames(robot_nh))
return false;
/// Calls readParamsControlMode. /// Calls readParamsControlMode.
// Defines if the control mode is position or effort // Defines if the control mode is position or effort
if (!readParamsControlMode(robot_nh)) return false; if (!readParamsControlMode(robot_nh))