Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-10 10:00:48 +02:00
Code Releases Activity

Format sources with clang-format

Browse Source
This commit is contained in:
Miguel Prada
2018-11-20 09:47:17 +01:00
parent d7a59872b2
commit 9e276a3368
20 changed files with 177 additions and 163 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
include/ur_modern_driver/pipeline.h
View File

@@ -110,7 +110,6 @@ public:
} }
}; };
template <typename T> template <typename T>
class Pipeline class Pipeline
{ {
@@ -140,7 +139,7 @@ private:
{ {
if (!queue_.try_enqueue(std::move(p))) if (!queue_.try_enqueue(std::move(p)))
{ {
LOG_ERROR("Pipeline producer overflowed! <%s>",name_.c_str()); LOG_ERROR("Pipeline producer overflowed! <%s>", name_.c_str());
} }
} }
@@ -201,7 +200,7 @@ public:
if (!running_) if (!running_)
return; return;
LOG_DEBUG("Stopping pipeline! <%s>",name_.c_str()); LOG_DEBUG("Stopping pipeline! <%s>", name_.c_str());
consumer_.stopConsumer(); consumer_.stopConsumer();
producer_.stopProducer(); producer_.stopProducer();

2
include/ur_modern_driver/ros/action_trajectory_follower_interface.h
View File

@@ -28,5 +28,5 @@ public:
virtual bool start() = 0; virtual bool start() = 0;
virtual bool execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt) = 0; virtual bool execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt) = 0;
virtual void stop() = 0; virtual void stop() = 0;
virtual ~ActionTrajectoryFollowerInterface() {}; virtual ~ActionTrajectoryFollowerInterface(){};
}; };

15
include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h
View File

@@ -9,11 +9,11 @@
#include <thread> #include <thread>
#include <vector> #include <vector>
#include "ur_modern_driver/log.h" #include "ur_modern_driver/log.h"
#include "ur_modern_driver/ros/action_trajectory_follower_interface.h"
#include "ur_modern_driver/ur/commander.h" #include "ur_modern_driver/ur/commander.h"
#include "ur_modern_driver/ur/server.h" #include "ur_modern_driver/ur/server.h"
#include "ur_modern_driver/ros/action_trajectory_follower_interface.h"
class LowBandwidthTrajectoryFollower: public ActionTrajectoryFollowerInterface class LowBandwidthTrajectoryFollower : public ActionTrajectoryFollowerInterface
{ {
private: private:
std::atomic<bool> running_; std::atomic<bool> running_;
@@ -21,14 +21,13 @@ private:
URCommander &commander_; URCommander &commander_;
URServer server_; URServer server_;
double time_interval_, servoj_time_, servoj_time_waiting_, max_waiting_time_, \ double time_interval_, servoj_time_, servoj_time_waiting_, max_waiting_time_, servoj_gain_, servoj_lookahead_time_,
servoj_gain_, servoj_lookahead_time_, max_joint_difference_; max_joint_difference_;
std::string program_; std::string program_;
bool execute(const std::array<double, 6> &positions, bool execute(const std::array<double, 6> &positions, const std::array<double, 6> &velocities, double sample_number,
const std::array<double, 6> &velocities, double time_in_seconds);
double sample_number, double time_in_seconds);
public: public:
LowBandwidthTrajectoryFollower(URCommander &commander, std::string &reverse_ip, int reverse_port, bool version_3); LowBandwidthTrajectoryFollower(URCommander &commander, std::string &reverse_ip, int reverse_port, bool version_3);
@@ -37,5 +36,5 @@ public:
bool execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt); bool execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt);
void stop(); void stop();
virtual ~LowBandwidthTrajectoryFollower() {}; virtual ~LowBandwidthTrajectoryFollower(){};
}; };

3
include/ur_modern_driver/ros/mb_publisher.h
View File

@@ -35,7 +35,8 @@ private:
void publishRobotStatus(const RobotModeData_V3_0__1& data) const; void publishRobotStatus(const RobotModeData_V3_0__1& data) const;
public: public:
MBPublisher() : io_pub_(nh_.advertise<ur_msgs::IOStates>("ur_driver/io_states", 1)) MBPublisher()
: io_pub_(nh_.advertise<ur_msgs::IOStates>("ur_driver/io_states", 1))
, status_pub_(nh_.advertise<industrial_msgs::RobotStatus>("ur_driver/robot_status", 1)) , status_pub_(nh_.advertise<industrial_msgs::RobotStatus>("ur_driver/robot_status", 1))
{ {
} }

4
include/ur_modern_driver/ros/trajectory_follower.h
View File

@@ -9,9 +9,9 @@
#include <thread> #include <thread>
#include <vector> #include <vector>
#include "ur_modern_driver/log.h" #include "ur_modern_driver/log.h"
#include "ur_modern_driver/ros/action_trajectory_follower_interface.h"
#include "ur_modern_driver/ur/commander.h" #include "ur_modern_driver/ur/commander.h"
#include "ur_modern_driver/ur/server.h" #include "ur_modern_driver/ur/server.h"
#include "ur_modern_driver/ros/action_trajectory_follower_interface.h"
class TrajectoryFollower : public ActionTrajectoryFollowerInterface class TrajectoryFollower : public ActionTrajectoryFollowerInterface
{ {
@@ -43,5 +43,5 @@ public:
bool execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt); bool execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt);
void stop(); void stop();
virtual ~TrajectoryFollower() {}; virtual ~TrajectoryFollower(){};
}; };

5
include/ur_modern_driver/ros/urscript_handler.h
View File

@@ -8,10 +8,9 @@
#include "ur_modern_driver/ros/service_stopper.h" #include "ur_modern_driver/ros/service_stopper.h"
#include "ur_modern_driver/ur/commander.h" #include "ur_modern_driver/ur/commander.h"
class URScriptHandler: public Service class URScriptHandler : public Service
{ {
private: private:
ros::NodeHandle nh_; ros::NodeHandle nh_;
URCommander &commander_; URCommander &commander_;
ros::Subscriber urscript_sub_; ros::Subscriber urscript_sub_;
@@ -19,6 +18,6 @@ private:
public: public:
URScriptHandler(URCommander &commander); URScriptHandler(URCommander &commander);
void urscriptInterface(const std_msgs::String::ConstPtr& msg); void urscriptInterface(const std_msgs::String::ConstPtr &msg);
void onRobotStateChange(RobotState state); void onRobotStateChange(RobotState state);
}; };

4
include/ur_modern_driver/test/random_data.h
View File

@@ -1,11 +1,11 @@
#pragma once #pragma once
#include <inttypes.h> #include <inttypes.h>
#include <algorithm> #include <algorithm>
#include <bitset>
#include <climits> #include <climits>
#include <cstddef> #include <cstddef>
#include <functional> #include <functional>
#include <random> #include <random>
#include <bitset>
#include "ur_modern_driver/bin_parser.h" #include "ur_modern_driver/bin_parser.h"
class RandomDataTest class RandomDataTest
@@ -52,7 +52,7 @@ public:
template <typename T> template <typename T>
void set(T data, size_t pos) void set(T data, size_t pos)
{ {
std::memcpy(&data, buf_+pos, sizeof(T)); std::memcpy(&data, buf_ + pos, sizeof(T));
} }
void skip(size_t n) void skip(size_t n)

2
include/ur_modern_driver/ur/server.h
View File

@@ -26,6 +26,6 @@ public:
bool bind(); bool bind();
bool accept(); bool accept();
void disconnectClient(); void disconnectClient();
bool readLine(char* buffer, size_t buf_len); bool readLine(char *buffer, size_t buf_len);
bool write(const uint8_t *buf, size_t buf_len, size_t &written); bool write(const uint8_t *buf, size_t buf_len, size_t &written);
}; };

11
src/ros/action_server.cpp
View File

@@ -1,7 +1,8 @@
#include "ur_modern_driver/ros/action_server.h" #include "ur_modern_driver/ros/action_server.h"
#include <cmath> #include <cmath>
ActionServer::ActionServer(ActionTrajectoryFollowerInterface& follower, std::vector<std::string>& joint_names, double max_velocity) ActionServer::ActionServer(ActionTrajectoryFollowerInterface& follower, std::vector<std::string>& joint_names,
double max_velocity)
: as_(nh_, "follow_joint_trajectory", boost::bind(&ActionServer::onGoal, this, _1), : as_(nh_, "follow_joint_trajectory", boost::bind(&ActionServer::onGoal, this, _1),
boost::bind(&ActionServer::onCancel, this, _1), false) boost::bind(&ActionServer::onCancel, this, _1), false)
, joint_names_(joint_names) , joint_names_(joint_names)
@@ -147,9 +148,10 @@ bool ActionServer::validateJoints(GoalHandle& gh, Result& res)
res.error_code = Result::INVALID_JOINTS; res.error_code = Result::INVALID_JOINTS;
res.error_string = "Invalid joint names for goal\n"; res.error_string = "Invalid joint names for goal\n";
res.error_string += "Expected: "; res.error_string += "Expected: ";
std::for_each(goal_joints.begin(), goal_joints.end(), [&res](std::string joint){res.error_string += joint + ", ";}); std::for_each(goal_joints.begin(), goal_joints.end(),
[&res](std::string joint) { res.error_string += joint + ", "; });
res.error_string += "\nFound: "; res.error_string += "\nFound: ";
std::for_each(joint_set_.begin(), joint_set_.end(), [&res](std::string joint){res.error_string += joint + ", ";}); std::for_each(joint_set_.begin(), joint_set_.end(), [&res](std::string joint) { res.error_string += joint + ", "; });
return false; return false;
} }
@@ -187,7 +189,8 @@ bool ActionServer::validateTrajectory(GoalHandle& gh, Result& res)
} }
if (std::fabs(velocity) > max_velocity_) if (std::fabs(velocity) > max_velocity_)
{ {
res.error_string = "Received a goal with velocities that are higher than max_velocity_ " + std::to_string(max_velocity_); res.error_string =
"Received a goal with velocities that are higher than max_velocity_ " + std::to_string(max_velocity_);
return false; return false;
} }
} }

2
src/ros/controller.cpp
View File

@@ -58,7 +58,7 @@ void ROSController::doSwitch(const std::list<hardware_interface::ControllerInfo>
return; return;
} }
if(start_list.size() > 0) if (start_list.size() > 0)
LOG_WARN("Failed to start interface!"); LOG_WARN("Failed to start interface!");
} }

32
src/ros/lowbandwidth_trajectory_follower.cpp
View File

@@ -1,9 +1,9 @@
#include "ur_modern_driver/ros/lowbandwidth_trajectory_follower.h" #include "ur_modern_driver/ros/lowbandwidth_trajectory_follower.h"
#include <endian.h> #include <endian.h>
#include <cmath>
#include <ros/ros.h> #include <ros/ros.h>
#include <cmath>
static const std::array<double, 6> EMPTY_VALUES = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; static const std::array<double, 6> EMPTY_VALUES = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
static const std::string TIME_INTERVAL("{{TIME_INTERVAL}}"); static const std::string TIME_INTERVAL("{{TIME_INTERVAL}}");
static const std::string SERVOJ_TIME("{{SERVOJ_TIME}}"); static const std::string SERVOJ_TIME("{{SERVOJ_TIME}}");
@@ -233,8 +233,8 @@ end
)"; )";
LowBandwidthTrajectoryFollower::LowBandwidthTrajectoryFollower(URCommander &commander, std::string &reverse_ip, int reverse_port, LowBandwidthTrajectoryFollower::LowBandwidthTrajectoryFollower(URCommander &commander, std::string &reverse_ip,
bool version_3) int reverse_port, bool version_3)
: running_(false) : running_(false)
, commander_(commander) , commander_(commander)
, server_(reverse_port) , server_(reverse_port)
@@ -256,7 +256,8 @@ LowBandwidthTrajectoryFollower::LowBandwidthTrajectoryFollower(URCommander &comm
std::string res(POSITION_PROGRAM); std::string res(POSITION_PROGRAM);
std::ostringstream out; std::ostringstream out;
if (!version_3) { if (!version_3)
{
LOG_ERROR("Low Bandwidth Trajectory Follower only works for interface version > 3"); LOG_ERROR("Low Bandwidth Trajectory Follower only works for interface version > 3");
std::exit(-1); std::exit(-1);
} }
@@ -307,8 +308,8 @@ bool LowBandwidthTrajectoryFollower::start()
} }
bool LowBandwidthTrajectoryFollower::execute(const std::array<double, 6> &positions, bool LowBandwidthTrajectoryFollower::execute(const std::array<double, 6> &positions,
const std::array<double, 6> &velocities, const std::array<double, 6> &velocities, double sample_number,
double sample_number, double time_in_seconds) double time_in_seconds)
{ {
if (!running_) if (!running_)
return false; return false;
@@ -317,11 +318,11 @@ bool LowBandwidthTrajectoryFollower::execute(const std::array<double, 6> &positi
out << "("; out << "(";
out << sample_number << ","; out << sample_number << ",";
for (auto const &pos: positions) for (auto const &pos : positions)
{ {
out << pos << ","; out << pos << ",";
} }
for (auto const &vel: velocities) for (auto const &vel : velocities)
{ {
out << vel << ","; out << vel << ",";
} }
@@ -331,7 +332,7 @@ bool LowBandwidthTrajectoryFollower::execute(const std::array<double, 6> &positi
// We have only ASCII characters and we can cast char -> uint_8 // We have only ASCII characters and we can cast char -> uint_8
const std::string tmp = out.str(); const std::string tmp = out.str();
const char *formatted_message = tmp.c_str(); const char *formatted_message = tmp.c_str();
const uint8_t *buf = (uint8_t *) formatted_message; const uint8_t *buf = (uint8_t *)formatted_message;
size_t written; size_t written;
LOG_DEBUG("Sending message %s", formatted_message); LOG_DEBUG("Sending message %s", formatted_message);
@@ -346,7 +347,7 @@ bool LowBandwidthTrajectoryFollower::execute(std::vector<TrajectoryPoint> &traje
bool finished = false; bool finished = false;
char* line[MAX_SERVER_BUF_LEN]; char *line[MAX_SERVER_BUF_LEN];
bool res = true; bool res = true;
@@ -358,13 +359,14 @@ bool LowBandwidthTrajectoryFollower::execute(std::vector<TrajectoryPoint> &traje
finished = true; finished = true;
break; break;
} }
unsigned int message_num=atoi((const char *) line); unsigned int message_num = atoi((const char *)line);
LOG_DEBUG("Received request %i", message_num); LOG_DEBUG("Received request %i", message_num);
if (message_num < trajectory.size()) if (message_num < trajectory.size())
{ {
res = execute(trajectory[message_num].positions, trajectory[message_num].velocities, res = execute(trajectory[message_num].positions, trajectory[message_num].velocities, message_num,
message_num, trajectory[message_num].time_from_start.count() / 1e6); trajectory[message_num].time_from_start.count() / 1e6);
} else }
else
{ {
res = execute(EMPTY_VALUES, EMPTY_VALUES, message_num, 0.0); res = execute(EMPTY_VALUES, EMPTY_VALUES, message_num, 0.0);
} }

32
src/ros/mb_publisher.cpp
View File

@@ -20,25 +20,25 @@ void MBPublisher::publish(ur_msgs::IOStates& io_msg, SharedMasterBoardData& data
void MBPublisher::publishRobotStatus(industrial_msgs::RobotStatus& status, const SharedRobotModeData& data) const void MBPublisher::publishRobotStatus(industrial_msgs::RobotStatus& status, const SharedRobotModeData& data) const
{ {
//note that this is true as soon as the drives are powered, // note that this is true as soon as the drives are powered,
//even if the breakes are still closed // even if the breakes are still closed
//which is in slight contrast to the comments in the // which is in slight contrast to the comments in the
//message definition // message definition
status.drives_powered.val = data.robot_power_on; status.drives_powered.val = data.robot_power_on;
status.e_stopped.val = data.emergency_stopped; status.e_stopped.val = data.emergency_stopped;
//I found no way to reliably get information if the robot is moving // I found no way to reliably get information if the robot is moving
//data.programm_running would be true when using this driver to move the robot // data.programm_running would be true when using this driver to move the robot
//but it would also be true when another programm is running that might not // but it would also be true when another programm is running that might not
//in fact contain any movement commands // in fact contain any movement commands
status.in_motion.val = industrial_msgs::TriState::UNKNOWN; status.in_motion.val = industrial_msgs::TriState::UNKNOWN;
//the error code, if any, is not transmitted by this protocol // the error code, if any, is not transmitted by this protocol
//it can and should be fetched seperately // it can and should be fetched seperately
status.error_code = 0; status.error_code = 0;
//note that e-stop is handled by a seperate variable // note that e-stop is handled by a seperate variable
status.in_error.val = data.protective_stopped; status.in_error.val = data.protective_stopped;
status_pub_.publish(status); status_pub_.publish(status);
@@ -53,9 +53,9 @@ void MBPublisher::publishRobotStatus(const RobotModeData_V1_X& data) const
else else
msg.mode.val = industrial_msgs::RobotMode::AUTO; msg.mode.val = industrial_msgs::RobotMode::AUTO;
//todo: verify that this correct, there is also ROBOT_READY_MODE // todo: verify that this correct, there is also ROBOT_READY_MODE
msg.motion_possible.val = (data.robot_mode == robot_mode_V1_X::ROBOT_RUNNING_MODE) msg.motion_possible.val = (data.robot_mode == robot_mode_V1_X::ROBOT_RUNNING_MODE) ? industrial_msgs::TriState::ON :
? industrial_msgs::TriState::ON : industrial_msgs::TriState::OFF; industrial_msgs::TriState::OFF;
publishRobotStatus(msg, data); publishRobotStatus(msg, data);
} }
@@ -64,8 +64,8 @@ void MBPublisher::publishRobotStatus(const RobotModeData_V3_0__1& data) const
{ {
industrial_msgs::RobotStatus msg; industrial_msgs::RobotStatus msg;
msg.motion_possible.val = (data.robot_mode == robot_mode_V3_X::RUNNING) msg.motion_possible.val =
? industrial_msgs::TriState::ON : industrial_msgs::TriState::OFF; (data.robot_mode == robot_mode_V3_X::RUNNING) ? industrial_msgs::TriState::ON : industrial_msgs::TriState::OFF;
if (data.control_mode == robot_control_mode_V3_X::TEACH) if (data.control_mode == robot_control_mode_V3_X::TEACH)
msg.mode.val = industrial_msgs::RobotMode::MANUAL; msg.mode.val = industrial_msgs::RobotMode::MANUAL;

3
src/ros/service_stopper.cpp
View File

@@ -20,7 +20,8 @@ ServiceStopper::ServiceStopper(std::vector<Service*> services)
{ {
if (mode != "Never") if (mode != "Never")
{ {
LOG_WARN("Found invalid value for param require_activation: '%s'\nShould be one of Never, OnStartup, Always", mode.c_str()); LOG_WARN("Found invalid value for param require_activation: '%s'\nShould be one of Never, OnStartup, Always",
mode.c_str());
mode = "Never"; mode = "Never";
} }
notify_all(RobotState::Running); notify_all(RobotState::Running);

2
src/ros/trajectory_follower.cpp
View File

@@ -1,7 +1,7 @@
#include "ur_modern_driver/ros/trajectory_follower.h" #include "ur_modern_driver/ros/trajectory_follower.h"
#include <endian.h> #include <endian.h>
#include <cmath>
#include <ros/ros.h> #include <ros/ros.h>
#include <cmath>
static const int32_t MULT_JOINTSTATE_ = 1000000; static const int32_t MULT_JOINTSTATE_ = 1000000;
static const std::string JOINT_STATE_REPLACE("{{JOINT_STATE_REPLACE}}"); static const std::string JOINT_STATE_REPLACE("{{JOINT_STATE_REPLACE}}");

4
src/ros/urscript_handler.cpp
View File

@@ -1,9 +1,7 @@
#include "ur_modern_driver/ros/urscript_handler.h" #include "ur_modern_driver/ros/urscript_handler.h"
#include "ur_modern_driver/log.h" #include "ur_modern_driver/log.h"
URScriptHandler::URScriptHandler(URCommander &commander) URScriptHandler::URScriptHandler(URCommander& commander) : commander_(commander), state_(RobotState::Error)
: commander_(commander)
, state_(RobotState::Error)
{ {
LOG_INFO("Initializing ur_driver/URScript subscriber"); LOG_INFO("Initializing ur_driver/URScript subscriber");
urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1, &URScriptHandler::urscriptInterface, this); urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1, &URScriptHandler::urscriptInterface, this);

29
src/ros_main.cpp
View File

@@ -9,11 +9,11 @@
#include "ur_modern_driver/ros/action_server.h" #include "ur_modern_driver/ros/action_server.h"
#include "ur_modern_driver/ros/controller.h" #include "ur_modern_driver/ros/controller.h"
#include "ur_modern_driver/ros/io_service.h" #include "ur_modern_driver/ros/io_service.h"
#include "ur_modern_driver/ros/lowbandwidth_trajectory_follower.h"
#include "ur_modern_driver/ros/mb_publisher.h" #include "ur_modern_driver/ros/mb_publisher.h"
#include "ur_modern_driver/ros/rt_publisher.h" #include "ur_modern_driver/ros/rt_publisher.h"
#include "ur_modern_driver/ros/service_stopper.h" #include "ur_modern_driver/ros/service_stopper.h"
#include "ur_modern_driver/ros/trajectory_follower.h" #include "ur_modern_driver/ros/trajectory_follower.h"
#include "ur_modern_driver/ros/lowbandwidth_trajectory_follower.h"
#include "ur_modern_driver/ros/urscript_handler.h" #include "ur_modern_driver/ros/urscript_handler.h"
#include "ur_modern_driver/ur/commander.h" #include "ur_modern_driver/ur/commander.h"
#include "ur_modern_driver/ur/factory.h" #include "ur_modern_driver/ur/factory.h"
@@ -62,10 +62,12 @@ public:
class IgnorePipelineStoppedNotifier : public INotifier class IgnorePipelineStoppedNotifier : public INotifier
{ {
public: public:
void started(std::string name){ void started(std::string name)
{
LOG_INFO("Starting pipeline %s", name.c_str()); LOG_INFO("Starting pipeline %s", name.c_str());
} }
void stopped(std::string name){ void stopped(std::string name)
{
LOG_INFO("Stopping pipeline %s", name.c_str()); LOG_INFO("Stopping pipeline %s", name.c_str());
} }
}; };
@@ -73,17 +75,18 @@ public:
class ShutdownOnPipelineStoppedNotifier : public INotifier class ShutdownOnPipelineStoppedNotifier : public INotifier
{ {
public: public:
void started(std::string name){ void started(std::string name)
{
LOG_INFO("Starting pipeline %s", name.c_str()); LOG_INFO("Starting pipeline %s", name.c_str());
} }
void stopped(std::string name){ void stopped(std::string name)
{
LOG_INFO("Shutting down on stopped pipeline %s", name.c_str()); LOG_INFO("Shutting down on stopped pipeline %s", name.c_str());
ros::shutdown(); ros::shutdown();
exit(1); exit(1);
} }
}; };
bool parse_args(ProgArgs &args) bool parse_args(ProgArgs &args)
{ {
if (!ros::param::get(IP_ADDR_ARG, args.host)) if (!ros::param::get(IP_ADDR_ARG, args.host))
@@ -121,9 +124,9 @@ int main(int argc, char **argv)
return EXIT_FAILURE; return EXIT_FAILURE;
} }
//Add prefix to joint names // Add prefix to joint names
std::transform (args.joint_names.begin(), args.joint_names.end(), args.joint_names.begin(), std::transform(args.joint_names.begin(), args.joint_names.end(), args.joint_names.begin(),
[&args](std::string name){return args.prefix + name;}); [&args](std::string name) { return args.prefix + name; });
std::string local_ip(getLocalIPAccessibleFromHost(args.host)); std::string local_ip(getLocalIPAccessibleFromHost(args.host));
@@ -144,8 +147,8 @@ int main(int argc, char **argv)
if (args.use_ros_control) if (args.use_ros_control)
{ {
LOG_INFO("ROS control enabled"); LOG_INFO("ROS control enabled");
TrajectoryFollower *traj_follower = new TrajectoryFollower( TrajectoryFollower *traj_follower =
*rt_commander, local_ip, args.reverse_port, factory.isVersion3()); new TrajectoryFollower(*rt_commander, local_ip, args.reverse_port, factory.isVersion3());
controller = new ROSController(*rt_commander, *traj_follower, args.joint_names, args.max_vel_change, args.tcp_link); controller = new ROSController(*rt_commander, *traj_follower, args.joint_names, args.max_vel_change, args.tcp_link);
rt_vec.push_back(controller); rt_vec.push_back(controller);
services.push_back(controller); services.push_back(controller);
@@ -157,8 +160,8 @@ int main(int argc, char **argv)
if (args.use_lowbandwidth_trajectory_follower) if (args.use_lowbandwidth_trajectory_follower)
{ {
LOG_INFO("Use low bandwidth trajectory follower"); LOG_INFO("Use low bandwidth trajectory follower");
traj_follower = new LowBandwidthTrajectoryFollower(*rt_commander, traj_follower =
local_ip, args.reverse_port,factory.isVersion3()); new LowBandwidthTrajectoryFollower(*rt_commander, local_ip, args.reverse_port, factory.isVersion3());
} }
else else
{ {

3
src/tcp_socket.cpp
View File

@@ -117,10 +117,9 @@ bool TCPSocket::read(char *character)
// It's inefficient, but in our case we read very small messages // It's inefficient, but in our case we read very small messages
// and the overhead connected with reading character by character is // and the overhead connected with reading character by character is
// negligible - adding buffering would complicate the code needlessly. // negligible - adding buffering would complicate the code needlessly.
return read((uint8_t *) character, 1, read_chars); return read((uint8_t *)character, 1, read_chars);
} }
bool TCPSocket::read(uint8_t *buf, size_t buf_len, size_t &read) bool TCPSocket::read(uint8_t *buf, size_t buf_len, size_t &read)
{ {
read = 0; read = 0;

18
src/ur/commander.cpp
View File

@@ -22,7 +22,7 @@ void URCommander::formatArray(std::ostringstream &out, std::array<double, 6> &va
bool URCommander::uploadProg(const std::string &s) bool URCommander::uploadProg(const std::string &s)
{ {
LOG_DEBUG("Sending program [%s]",s.c_str()); LOG_DEBUG("Sending program [%s]", s.c_str());
return write(s); return write(s);
} }
@@ -73,7 +73,9 @@ bool URCommander_V1_X::speedj(std::array<double, 6> &speeds, double acceleration
bool URCommander_V1_X::setAnalogOut(uint8_t pin, double value) bool URCommander_V1_X::setAnalogOut(uint8_t pin, double value)
{ {
std::ostringstream out; std::ostringstream out;
out << "sec io_fun():\n" << "set_analog_out(" << (int)pin << "," << std::fixed << std::setprecision(4) << value << ")\n" << "end\n"; out << "sec io_fun():\n"
<< "set_analog_out(" << (int)pin << "," << std::fixed << std::setprecision(4) << value << ")\n"
<< "end\n";
std::string s(out.str()); std::string s(out.str());
return write(s); return write(s);
} }
@@ -81,7 +83,9 @@ bool URCommander_V1_X::setAnalogOut(uint8_t pin, double value)
bool URCommander_V1_X::setDigitalOut(uint8_t pin, bool value) bool URCommander_V1_X::setDigitalOut(uint8_t pin, bool value)
{ {
std::ostringstream out; std::ostringstream out;
out << "sec io_fun():\n" << "set_digital_out(" << (int)pin << "," << (value ? "True" : "False") << ")\n" << "end\n"; out << "sec io_fun():\n"
<< "set_digital_out(" << (int)pin << "," << (value ? "True" : "False") << ")\n"
<< "end\n";
std::string s(out.str()); std::string s(out.str());
return write(s); return write(s);
} }
@@ -89,7 +93,9 @@ bool URCommander_V1_X::setDigitalOut(uint8_t pin, bool value)
bool URCommander_V3_X::setAnalogOut(uint8_t pin, double value) bool URCommander_V3_X::setAnalogOut(uint8_t pin, double value)
{ {
std::ostringstream out; std::ostringstream out;
out << "sec io_fun():\n" << "set_standard_analog_out(" << (int)pin << "," << std::fixed << std::setprecision(5) << value << ")\n" << "end\n"; out << "sec io_fun():\n"
<< "set_standard_analog_out(" << (int)pin << "," << std::fixed << std::setprecision(5) << value << ")\n"
<< "end\n";
std::string s(out.str()); std::string s(out.str());
return write(s); return write(s);
} }
@@ -116,7 +122,9 @@ bool URCommander_V3_X::setDigitalOut(uint8_t pin, bool value)
else else
return false; return false;
out << "sec io_fun():\n" << func << "(" << (int)pin << "," << (value ? "True" : "False") << ")\n" << "end\n"; out << "sec io_fun():\n"
<< func << "(" << (int)pin << "," << (value ? "True" : "False") << ")\n"
<< "end\n";
std::string s(out.str()); std::string s(out.str());
return write(s); return write(s);
} }

18
src/ur/server.cpp
View File

@@ -31,8 +31,7 @@ std::string URServer::getIP()
return std::string(buf); return std::string(buf);
} }
bool URServer::open(int socket_fd, struct sockaddr* address, size_t address_len)
bool URServer::open(int socket_fd, struct sockaddr *address, size_t address_len)
{ {
int flag = 1; int flag = 1;
setsockopt(socket_fd, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(int)); setsockopt(socket_fd, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(int));
@@ -63,9 +62,10 @@ bool URServer::accept()
int client_fd = -1; int client_fd = -1;
int retry = 0; int retry = 0;
while((client_fd = ::accept(getSocketFD(), &addr, &addr_len)) == -1){ while ((client_fd = ::accept(getSocketFD(), &addr, &addr_len)) == -1)
{
LOG_ERROR("Accepting socket connection failed. (errno: %d)", errno); LOG_ERROR("Accepting socket connection failed. (errno: %d)", errno);
if(retry++ >= 5) if (retry++ >= 5)
return false; return false;
} }
@@ -89,21 +89,23 @@ bool URServer::write(const uint8_t* buf, size_t buf_len, size_t& written)
bool URServer::readLine(char* buffer, size_t buf_len) bool URServer::readLine(char* buffer, size_t buf_len)
{ {
char *current_pointer = buffer; char* current_pointer = buffer;
char ch; char ch;
size_t total_read; size_t total_read;
if (buf_len <= 0 || buffer == NULL) { if (buf_len <= 0 || buffer == NULL)
{
return false; return false;
} }
total_read = 0; total_read = 0;
for (;;) { for (;;)
{
if (client_.read(&ch)) if (client_.read(&ch))
{ {
if (total_read < buf_len - 1) // just in case ... if (total_read < buf_len - 1) // just in case ...
{ {
total_read ++; total_read++;
*current_pointer++ = ch; *current_pointer++ = ch;
} }
if (ch == '\n') if (ch == '\n')

6
tests/ur/master_board.cpp
View File

@@ -9,7 +9,7 @@
TEST(MasterBoardData_V1_X, testRandomDataParsing) TEST(MasterBoardData_V1_X, testRandomDataParsing)
{ {
RandomDataTest rdt(71); RandomDataTest rdt(71);
rdt.set<uint8_t>(1, 58); //sets euromap67_interface_installed to true rdt.set<uint8_t>(1, 58); // sets euromap67_interface_installed to true
BinParser bp = rdt.getParser(); BinParser bp = rdt.getParser();
MasterBoardData_V1_X state; MasterBoardData_V1_X state;
@@ -43,7 +43,7 @@ TEST(MasterBoardData_V1_X, testRandomDataParsing)
TEST(MasterBoardData_V3_0__1, testRandomDataParsing) TEST(MasterBoardData_V3_0__1, testRandomDataParsing)
{ {
RandomDataTest rdt(83); RandomDataTest rdt(83);
rdt.set<uint8_t>(1, 62); //sets euromap67_interface_installed to true rdt.set<uint8_t>(1, 62); // sets euromap67_interface_installed to true
BinParser bp = rdt.getParser(); BinParser bp = rdt.getParser();
MasterBoardData_V3_0__1 state; MasterBoardData_V3_0__1 state;
ASSERT_TRUE(state.parseWith(bp)) << "parse() returned false"; ASSERT_TRUE(state.parseWith(bp)) << "parse() returned false";
@@ -78,7 +78,7 @@ TEST(MasterBoardData_V3_0__1, testRandomDataParsing)
TEST(MasterBoardData_V3_2, testRandomDataParsing) TEST(MasterBoardData_V3_2, testRandomDataParsing)
{ {
RandomDataTest rdt(85); RandomDataTest rdt(85);
rdt.set<uint8_t>(1, 62); //sets euromap67_interface_installed to true rdt.set<uint8_t>(1, 62); // sets euromap67_interface_installed to true
BinParser bp = rdt.getParser(); BinParser bp = rdt.getParser();
MasterBoardData_V3_2 state; MasterBoardData_V3_2 state;
ASSERT_TRUE(state.parseWith(bp)) << "parse() returned false"; ASSERT_TRUE(state.parseWith(bp)) << "parse() returned false";