Skip to content
GitLab
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
Hide whitespace changes
Inline Side-by-side
.clang-format 0 → 100644
View file @ 3d9203f0
---
ColumnLimit: 80
...
src/log.cpp
View file @ 3d9203f0
......@@ -4,17 +4,17 @@
Object to log the low-level informations of a robot.
*/
#include "log.hh"
#include <sys/time.h>
#include <sstream>
#include <fstream>
#include <iomanip>
#include <sstream>
#include <sys/time.h>
using namespace std;
using namespace rc_sot_system;
DataToLog::DataToLog() {}
void DataToLog::init(ProfileLog& aProfileLog) {
void DataToLog::init(ProfileLog &aProfileLog) {
profileLog_ = aProfileLog;
motor_angle.resize(aProfileLog.nbDofs * aProfileLog.length);
joint_angle.resize(aProfileLog.nbDofs * aProfileLog.length);
......@@ -37,7 +37,7 @@ void DataToLog::init(ProfileLog& aProfileLog) {
Log::Log() : lref_(0), lrefts_(0) {}
void Log::init(ProfileLog& aProfileLog) {
void Log::init(ProfileLog &aProfileLog) {
profileLog_ = aProfileLog;
lref_ = 0;
lrefts_ = 0;
......@@ -48,7 +48,7 @@ void Log::init(ProfileLog& aProfileLog) {
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) ||
(aDataToLog.velocities.size() != profileLog_.nbDofs))
return;
......@@ -122,7 +122,7 @@ void Log::stop_it() {
timeorigin_;
}
void Log::save(std::string& fileName) {
void Log::save(std::string &fileName) {
std::string suffix("-mastate.log");
saveVector(fileName, suffix, StoredData_.motor_angle, profileLog_.nbDofs);
suffix = "-jastate.log";
......@@ -154,22 +154,22 @@ void Log::save(std::string& fileName) {
saveVector(fileName, suffix, StoredData_.duration, 1);
}
inline void writeHeaderToBinaryBuffer(ofstream& of, const std::size_t& nVector,
const std::size_t& vectorSize) {
of.write((const char*)(&nVector), sizeof(std::size_t));
of.write((const char*)(&vectorSize), sizeof(std::size_t));
inline void writeHeaderToBinaryBuffer(ofstream &of, const std::size_t &nVector,
const std::size_t &vectorSize) {
of.write((const char *)(&nVector), sizeof(std::size_t));
of.write((const char *)(&vectorSize), sizeof(std::size_t));
}
inline void writeToBinaryFile(ofstream& of, const double& t, const double& dt,
const std::vector<double>& data,
const std::size_t& idx, const std::size_t& size) {
of.write((const char*)&t, sizeof(double));
of.write((const char*)&dt, sizeof(double));
of.write((const char*)(&data[idx]), size * (sizeof(double)));
inline void writeToBinaryFile(ofstream &of, const double &t, const double &dt,
const std::vector<double> &data,
const std::size_t &idx, const std::size_t &size) {
of.write((const char *)&t, sizeof(double));
of.write((const char *)&dt, sizeof(double));
of.write((const char *)(&data[idx]), size * (sizeof(double)));
}
void Log::saveVector(std::string& fileName, std::string& suffix,
const std::vector<double>& avector, std::size_t size) {
void Log::saveVector(std::string &fileName, std::string &suffix,
const std::vector<double> &avector, std::size_t size) {
ostringstream oss;
oss << fileName;
oss << suffix.c_str();
......
src/log.hh
View file @ 3d9203f0
......@@ -7,8 +7,8 @@
#ifndef _RC_SOT_SYSTEM_LOG_H_
#define _RC_SOT_SYSTEM_LOG_H_
#include <vector>
#include <string>
#include <vector>
namespace rc_sot_system {
......@@ -57,7 +57,7 @@ struct DataToLog {
};
class Log {
private:
private:
/// Profile Log
ProfileLog profileLog_;
......@@ -78,7 +78,7 @@ class Log {
void saveVector(std::string &filename, std::string &suffix,
const std::vector<double> &avector, std::size_t);
public:
public:
Log();
void init(ProfileLog &aProfileLog);
......@@ -88,7 +88,7 @@ class Log {
void start_it();
void stop_it();
};
} // namespace rc_sot_system
} // namespace rc_sot_system
#pragma GCC diagnostic push
#pragma GCC system_header
......
src/roscontrol-sot-controller.cpp
View file @ 3d9203f0
#include <dlfcn.h>
#include <fstream>
#include <iomanip>
#include <dlfcn.h>
#include <sstream>
#include "roscontrol-sot-controller.hh"
......@@ -19,26 +19,26 @@
#define DBGFILE "/tmp/rcoh2sot.dat"
#define RESETDEBUG5() \
{ \
std::ofstream DebugFile; \
DebugFile.open(DBGFILE, std::ofstream::out); \
DebugFile.close(); \
#define RESETDEBUG5() \
{ \
std::ofstream DebugFile; \
DebugFile.open(DBGFILE, std::ofstream::out); \
DebugFile.close(); \
}
#define ODEBUG5FULL(x) \
{ \
std::ofstream DebugFile; \
DebugFile.open(DBGFILE, std::ofstream::app); \
DebugFile << __FILE__ << ":" << __FUNCTION__ << "(#" << __LINE__ \
<< "):" << x << std::endl; \
DebugFile.close(); \
#define ODEBUG5FULL(x) \
{ \
std::ofstream DebugFile; \
DebugFile.open(DBGFILE, std::ofstream::app); \
DebugFile << __FILE__ << ":" << __FUNCTION__ << "(#" << __LINE__ \
<< "):" << x << std::endl; \
DebugFile.close(); \
}
#define ODEBUG5(x) \
{ \
std::ofstream DebugFile; \
DebugFile.open(DBGFILE, std::ofstream::app); \
DebugFile << x << std::endl; \
DebugFile.close(); \
#define ODEBUG5(x) \
{ \
std::ofstream DebugFile; \
DebugFile.open(DBGFILE, std::ofstream::app); \
DebugFile << x << std::endl; \
DebugFile.close(); \
}
#define RESETDEBUG4()
......@@ -46,7 +46,7 @@
#define ODEBUG4(x)
class LoggerROSStream : public ::dynamicgraph::LoggerStream {
public:
public:
void write(const char *c) { ROS_ERROR("%s", c); }
};
......@@ -70,13 +70,10 @@ void ControlPDMotorControlData::read_from_xmlrpc_value(
}
RCSotController::RCSotController()
: // Store 32 DoFs for 5 minutes (1 Khz: 5*60*1000)
// -> 124 Mo of data.
type_name_("RCSotController"),
simulation_mode_(false),
accumulated_time_(0.0),
jitter_(0.0),
verbosity_level_(0) {
: // Store 32 DoFs for 5 minutes (1 Khz: 5*60*1000)
// -> 124 Mo of data.
type_name_("RCSotController"), simulation_mode_(false),
accumulated_time_(0.0), jitter_(0.0), verbosity_level_(0) {
RESETDEBUG4();
profileLog_.length = 300000;
}
......@@ -129,7 +126,8 @@ bool RCSotController::initRequest(lhi::RobotHW *robot_hw,
ClaimedResources &claimed_resources) {
ROS_WARN("initRequest 1");
/// Read the parameter server
if (!readParams(robot_nh)) return false;
if (!readParams(robot_nh))
return false;
ROS_WARN("initRequest 2");
/// Create ros control interfaces to hardware
/// Recalls: init() is called by initInterfaces()
......@@ -166,64 +164,58 @@ bool RCSotController::initInterfaces(lhi::RobotHW *robot_hw, ros::NodeHandle &,
// Check if construction finished cleanly
if (state_ != CONSTRUCTED) {
ROS_ERROR(
"Cannot initialize this controller because it "
"failed to be constructed");
ROS_ERROR("Cannot initialize this controller because it "
"failed to be constructed");
}
// Get a pointer to the joint position control interface
pos_iface_ = robot_hw->get<PositionJointInterface>();
if (!pos_iface_) {
ROS_WARN(
"This controller did not find a hardware interface of type "
"PositionJointInterface."
" Make sure this is registered in the %s::RobotHW class "
"if it is required.",
lns.c_str());
ROS_WARN("This controller did not find a hardware interface of type "
"PositionJointInterface."
" Make sure this is registered in the %s::RobotHW class "
"if it is required.",
lns.c_str());
}
// Get a pointer to the joint velocity control interface
vel_iface_ = robot_hw->get<VelocityJointInterface>();
if (!vel_iface_) {
ROS_WARN(
"This controller did not find a hardware interface of type "
"VelocityJointInterface."
" Make sure this is registered in the %s::RobotHW class "
"if it is required.",
lns.c_str());
ROS_WARN("This controller did not find a hardware interface of type "
"VelocityJointInterface."
" Make sure this is registered in the %s::RobotHW class "
"if it is required.",
lns.c_str());
}
// Get a pointer to the joint effort control interface
effort_iface_ = robot_hw->get<EffortJointInterface>();
if (!effort_iface_) {
ROS_WARN(
"This controller did not find a hardware interface of type "
"EffortJointInterface."
" Make sure this is registered in the %s::RobotHW class "
"if it is required.",
lns.c_str());
ROS_WARN("This controller did not find a hardware interface of type "
"EffortJointInterface."
" Make sure this is registered in the %s::RobotHW class "
"if it is required.",
lns.c_str());
}
// Get a pointer to the force-torque sensor interface
ft_iface_ = robot_hw->get<ForceTorqueSensorInterface>();
if (!ft_iface_) {
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());
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());
}
// Get a pointer to the IMU sensor interface
imu_iface_ = robot_hw->get<ImuSensorInterface>();
if (!imu_iface_) {
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());
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
......@@ -244,9 +236,12 @@ bool RCSotController::initInterfaces(lhi::RobotHW *robot_hw, ros::NodeHandle &,
}
// Return which resources are claimed by this controller
if (pos_iface_) pos_iface_->clearClaims();
if (vel_iface_) vel_iface_->clearClaims();
if (effort_iface_) effort_iface_->clearClaims();
if (pos_iface_)
pos_iface_->clearClaims();
if (vel_iface_)
vel_iface_->clearClaims();
if (effort_iface_)
effort_iface_->clearClaims();
if (!init()) {
ROS_ERROR("Failed to initialize sot-controller");
......@@ -280,27 +275,37 @@ bool RCSotController::initInterfaces(lhi::RobotHW *robot_hw, ros::NodeHandle &,
}
/// 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
claimed_resources = pos_iface_->getClaims();
/// Display claimed ressources
if (verbosity_level_ > 0) displayClaimedResources(claimed_resources);
if (pos_iface_ != 0) pos_iface_->clearClaims();
if (verbosity_level_ > 0)
displayClaimedResources(claimed_resources);
if (pos_iface_ != 0)
pos_iface_->clearClaims();
claimed_resources = vel_iface_->getClaims();
/// Display claimed ressources
if (verbosity_level_ > 0) displayClaimedResources(claimed_resources);
if (vel_iface_ != 0) vel_iface_->clearClaims();
if (verbosity_level_ > 0)
displayClaimedResources(claimed_resources);
if (vel_iface_ != 0)
vel_iface_->clearClaims();
claimed_resources = effort_iface_->getClaims();
if (verbosity_level_ > 0) displayClaimedResources(claimed_resources);
if (effort_iface_ != 0) effort_iface_->clearClaims();
if (verbosity_level_ > 0)
displayClaimedResources(claimed_resources);
if (effort_iface_ != 0)
effort_iface_->clearClaims();
#endif
if (verbosity_level_ > 0)
ROS_INFO_STREAM("Initialization of sot-controller Ok !");
......@@ -311,7 +316,8 @@ bool RCSotController::initInterfaces(lhi::RobotHW *robot_hw, ros::NodeHandle &,
}
bool RCSotController::init() {
if (!initJoints()) return false;
if (!initJoints())
return false;
if (!initIMU()) {
ROS_WARN("could not initialize IMU sensor(s).");
}
......@@ -404,10 +410,9 @@ bool RCSotController::readParamsEffortControlPDMotorControlData(
effort_mode_pd_motors_[joints_name_[i]].read_from_xmlrpc_value(
prefix);
} else
ROS_ERROR(
"No PID data for effort controlled joint %s in "
"/sot_controller/effort_control_pd_motor_init/gains/",
joints_name_[i].c_str());
ROS_ERROR("No PID data for effort controlled joint %s in "
"/sot_controller/effort_control_pd_motor_init/gains/",
joints_name_[i].c_str());
}
}
}
......@@ -452,10 +457,9 @@ bool RCSotController::readParamsVelocityControlPDMotorControlData(
velocity_mode_pd_motors_[joints_name_[i]].read_from_xmlrpc_value(
prefix);
} else
ROS_ERROR(
"No PID data for velocity controlled joint %s in "
"/sot_controller/velocity_control_pd_motor_init/gains/",
joints_name_[i].c_str());
ROS_ERROR("No PID data for velocity controlled joint %s in "
"/sot_controller/velocity_control_pd_motor_init/gains/",
joints_name_[i].c_str());
}
}
}
......@@ -480,15 +484,13 @@ bool RCSotController::readParamsFromRCToSotDevice(ros::NodeHandle &robot_nh) {
ROS_INFO_STREAM(it->first << ", " << it->second);
}
} else {
ROS_ERROR_STREAM(
"Could not read param /sot_controller/"
"map_rc_to_sot_device");
ROS_ERROR_STREAM("Could not read param /sot_controller/"
"map_rc_to_sot_device");
return false;
}
} else {
ROS_INFO_STREAM(
"Param /sot_controller/map_rc_to_sot_device "
"does not exists !");
ROS_INFO_STREAM("Param /sot_controller/map_rc_to_sot_device "
"does not exists !");
return false;
}
return true;
......@@ -574,7 +576,8 @@ bool RCSotController::readParamsControlMode(ros::NodeHandle &robot_nh) {
for (unsigned int idJoint = 0; idJoint < joints_name_.size(); idJoint++) {
std::string joint_name = joints_name_[idJoint];
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,
joint_name.c_str(), aJoint.ros_control_mode);
}
......@@ -589,13 +592,15 @@ bool RCSotController::readParamsdt(ros::NodeHandle &robot_nh) {
/// Reading the jitter is optional but it is a very good idea.
if (robot_nh.hasParam("/sot_controller/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
if (robot_nh.hasParam("/sot_controller/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;
}
......@@ -627,7 +632,8 @@ bool RCSotController::readParams(ros::NodeHandle &robot_nh) {
readParamsVerbosityLevel(robot_nh);
/// 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
/// if we are in simulation mode
......@@ -640,18 +646,21 @@ bool RCSotController::readParams(ros::NodeHandle &robot_nh) {
/// Calls readParamsJointNames
// Reads the list of joints to be controlled.
if (!readParamsJointNames(robot_nh)) return false;
if (!readParamsJointNames(robot_nh))
return false;
/// Calls readParamsControlMode.
// Defines if the control mode is position or effort
if (!readParamsControlMode(robot_nh)) return false;
if (!readParamsControlMode(robot_nh))
return false;
/// Calls readParamsFromRCToSotDevice
// Mapping from ros-controll to sot device
readParamsFromRCToSotDevice(robot_nh);
/// Get control period
if (!readParamsdt(robot_nh)) return false;
if (!readParamsdt(robot_nh))
return false;
readParamsEffortControlPDMotorControlData(robot_nh);
readParamsVelocityControlPDMotorControlData(robot_nh);
......@@ -671,26 +680,26 @@ bool RCSotController::initJoints() {
try {
JointSotHandle &aJointSotHandle = joints_[joint_name];
switch (aJointSotHandle.ros_control_mode) {
case POSITION:
aJointSotHandle.joint = pos_iface_->getHandle(joint_name);
if (verbosity_level_ > 0)
ROS_INFO_STREAM("Found joint "
<< joint_name << " in position " << i << " "
<< aJointSotHandle.joint.getName());
break;
case VELOCITY:
aJointSotHandle.joint = vel_iface_->getHandle(joint_name);
if (verbosity_level_ > 0)
ROS_INFO_STREAM("Found joint "
<< joint_name << " in velocity " << i << " "
<< aJointSotHandle.joint.getName());
break;
case EFFORT:
aJointSotHandle.joint = effort_iface_->getHandle(joint_name);
if (verbosity_level_ > 0)
ROS_INFO_STREAM("Found joint "
<< joint_name << " in effort " << i << " "
<< aJointSotHandle.joint.getName());
case POSITION:
aJointSotHandle.joint = pos_iface_->getHandle(joint_name);
if (verbosity_level_ > 0)
ROS_INFO_STREAM("Found joint " << joint_name << " in position " << i
<< " "
<< aJointSotHandle.joint.getName());
break;
case VELOCITY:
aJointSotHandle.joint = vel_iface_->getHandle(joint_name);
if (verbosity_level_ > 0)
ROS_INFO_STREAM("Found joint " << joint_name << " in velocity " << i
<< " "
<< aJointSotHandle.joint.getName());
break;
case EFFORT:
aJointSotHandle.joint = effort_iface_->getHandle(joint_name);
if (verbosity_level_ > 0)
ROS_INFO_STREAM("Found joint " << joint_name << " in effort " << i
<< " "
<< aJointSotHandle.joint.getName());
}
// throws on failure
......@@ -708,7 +717,8 @@ bool RCSotController::initJoints() {
}
bool RCSotController::initIMU() {
if (!imu_iface_) return false;
if (!imu_iface_)
return false;
// get all imu sensor names
const std ::vector<std ::string> &imu_iface_names = imu_iface_->getNames();
......@@ -725,7 +735,8 @@ bool RCSotController::initIMU() {
}
bool RCSotController::initForceSensors() {
if (!ft_iface_) return false;
if (!ft_iface_)
return false;
// get force torque sensors names package.
const std::vector<std::string> &ft_iface_names = ft_iface_->getNames();
......@@ -744,7 +755,8 @@ bool RCSotController::initForceSensors() {
bool RCSotController::initTemperatureSensors() {
if (!simulation_mode_) {
#ifdef TEMPERATURE_SENSOR_CONTROLLER
if (!act_temp_iface_) return false;
if (!act_temp_iface_)
return false;
// get temperature sensors names
const std::vector<std::string> &act_temp_iface_names =
......@@ -1053,7 +1065,9 @@ void RCSotController::update(const ros::Time &, const ros::Duration &period) {
fillSensors();
try {
sotController_->setupSetSensors(sensorsIn_);
} catch(std::exception &e) { throw e;}
} catch (std::exception &e) {
throw e;
}
}
}
......@@ -1076,4 +1090,4 @@ void RCSotController::stopping(const ros::Time &) {
}
PLUGINLIB_EXPORT_CLASS(sot_controller::RCSotController, lci::ControllerBase)
} // namespace sot_controller
} // namespace sot_controller
src/roscontrol-sot-controller.hh
View file @ 3d9203f0
......@@ -5,22 +5,22 @@
#ifndef RC_SOT_CONTROLLER_H
#define RC_SOT_CONTROLLER_H
#include <string>
#include <map>
#include <string>
#pragma GCC diagnostic push
#pragma GCC system_header
#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 <hardware_interface/imu_sensor_interface.h>
#include <hardware_interface/joint_command_interface.h>
#include <pal_hardware_interfaces/actuator_temperature_interface.h>
#include <pluginlib/class_list_macros.h>
#pragma GCC diagnostic pop
#include <control_toolbox/pid.h>
#include <dynamic_graph_bridge/sot_loader_basic.hh>
#include <ros/ros.h>
#include <control_toolbox/pid.h>
/** URDF DOM*/
#include <urdf_parser/urdf_parser.h>
......@@ -34,7 +34,7 @@ namespace lhi = hardware_interface;
namespace lci = controller_interface;
class XmlrpcHelperException : public ros::Exception {
public:
public:
XmlrpcHelperException(const std::string &what) : ros::Exception(what) {}
};
......@@ -66,14 +66,14 @@ typedef std::set<std::string> ClaimedResources;
*/
class RCSotController : public lci::ControllerBase, SotLoaderBasic {
protected:
protected:
/// Robot nb dofs.
size_t nbDofs_;
/// Data log.