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

Clang-format run

Browse Source
This commit is contained in:
Simon Rasmussen
2017-07-09 02:54:49 +02:00
parent 577fcdbf98
commit 3a5fa23f6b
31 changed files with 343 additions and 343 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
include/ur_modern_driver/bin_parser.h
View File

@@ -3,11 +3,11 @@
#include <assert.h>
#include <endian.h>
#include <inttypes.h>
#include <array>
#include <bitset>
#include <cstddef>
#include <cstring>
#include <string>
#include <array>
#include <bitset>
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/types.h"

26
include/ur_modern_driver/event_counter.h
View File

@@ -1,11 +1,10 @@
#pragma once
#include <cstdlib>
#include <chrono>
#include <cstdlib>
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/ur/consumer.h"
class EventCounter : public URRTPacketConsumer
{
private:
@@ -13,32 +12,31 @@ private:
Clock::time_point events_[250];
size_t idx_ = 0;
Clock::time_point last_;
public:
void trigger()
{
//auto now = Clock::now();
//LOG_INFO("Time diff: %d ms", std::chrono::duration_cast<std::chrono::microseconds>(now - last_));
//last_ = now;
//return;
// auto now = Clock::now();
// LOG_INFO("Time diff: %d ms", std::chrono::duration_cast<std::chrono::microseconds>(now - last_));
// last_ = now;
// return;
events_[idx_] = Clock::now();
idx_ += 1;
if(idx_ > 250)
if (idx_ > 250)
{
std::chrono::time_point<std::chrono::high_resolution_clock> t_min =
std::chrono::time_point<std::chrono::high_resolution_clock>::max();
std::chrono::time_point<std::chrono::high_resolution_clock>::max();
std::chrono::time_point<std::chrono::high_resolution_clock> t_max =
std::chrono::time_point<std::chrono::high_resolution_clock>::min();
for(auto const& e : events_)
for (auto const& e : events_)
{
if(e < t_min)
if (e < t_min)
t_min = e;
if(e > t_max)
if (e > t_max)
t_max = e;
}
@@ -46,7 +44,7 @@ public:
auto secs = std::chrono::duration_cast<std::chrono::seconds>(diff).count();
auto ms = std::chrono::duration_cast<std::chrono::microseconds>(diff).count();
std::chrono::duration<double> test(t_max - t_min);
LOG_INFO("Recieved 250 messages at %f Hz", (250.0/test.count()));
LOG_INFO("Recieved 250 messages at %f Hz", (250.0 / test.count()));
idx_ = 0;
}
}

14
include/ur_modern_driver/pipeline.h
View File

@@ -43,28 +43,28 @@ public:
virtual void setupConsumer()
{
for(auto &con : consumers_)
for (auto& con : consumers_)
{
con->setupConsumer();
}
}
virtual void teardownConsumer()
{
for(auto &con : consumers_)
for (auto& con : consumers_)
{
con->teardownConsumer();
}
}
virtual void stopConsumer()
{
for(auto &con : consumers_)
for (auto& con : consumers_)
{
con->stopConsumer();
}
}
virtual void onTimeout()
{
for(auto &con : consumers_)
for (auto& con : consumers_)
{
con->onTimeout();
}
@@ -73,9 +73,9 @@ public:
bool consume(shared_ptr<T> product)
{
bool res = true;
for(auto &con : consumers_)
for (auto& con : consumers_)
{
if(!con->consume(product))
if (!con->consume(product))
res = false;
}
return res;
@@ -153,7 +153,7 @@ private:
Time now = Clock::now();
auto pkg_diff = now - last_pkg;
auto warn_diff = now - last_warn;
if(pkg_diff > std::chrono::seconds(1) && warn_diff > std::chrono::seconds(1))
if (pkg_diff > std::chrono::seconds(1) && warn_diff > std::chrono::seconds(1))
{
last_warn = now;
consumer_.onTimeout();

18
include/ur_modern_driver/ros/controller.h
View File

@@ -1,18 +1,18 @@
#pragma once
#include <atomic>
#include <ros/ros.h>
#include <controller_manager/controller_manager.h>
#include <hardware_interface/force_torque_sensor_interface.h>
#include <hardware_interface/internal/demangle_symbol.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/robot_hw.h>
#include <ros/ros.h>
#include <atomic>
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/ros/hardware_interface.h"
#include "ur_modern_driver/ros/service_stopper.h"
#include "ur_modern_driver/ur/commander.h"
#include "ur_modern_driver/ur/consumer.h"
#include "ur_modern_driver/ur/rt_state.h"
#include "ur_modern_driver/ros/hardware_interface.h"
#include "ur_modern_driver/ros/service_stopper.h"
class ROSController : private hardware_interface::RobotHW, public URRTPacketConsumer, public Service
{
@@ -55,10 +55,14 @@ private:
void reset();
public:
ROSController(URCommander& commander, TrajectoryFollower& follower, std::vector<std::string>& joint_names, double max_vel_change);
virtual ~ROSController() { }
ROSController(URCommander& commander, TrajectoryFollower& follower, std::vector<std::string>& joint_names,
double max_vel_change);
virtual ~ROSController()
{
}
// from RobotHW
void doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list, const std::list<hardware_interface::ControllerInfo>& stop_list);
void doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>& stop_list);
// from URRTPacketConsumer
virtual void setupConsumer();
virtual bool consume(RTState_V1_6__7& state)

22
include/ur_modern_driver/ros/hardware_interface.h
View File

@@ -4,17 +4,23 @@
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <algorithm>
#include "ur_modern_driver/ros/trajectory_follower.h"
#include "ur_modern_driver/ur/commander.h"
#include "ur_modern_driver/ur/rt_state.h"
#include "ur_modern_driver/ros/trajectory_follower.h"
class HardwareInterface
{
public:
virtual bool write() = 0;
virtual void start() {}
virtual void stop() {}
virtual void reset() {}
virtual void start()
{
}
virtual void stop()
{
}
virtual void reset()
{
}
};
using hardware_interface::JointHandle;
@@ -48,7 +54,8 @@ private:
double max_vel_change_;
public:
VelocityInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface, std::vector<std::string> &joint_names, double max_vel_change);
VelocityInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface,
std::vector<std::string> &joint_names, double max_vel_change);
virtual bool write();
virtual void reset();
typedef hardware_interface::VelocityJointInterface parent_type;
@@ -57,11 +64,12 @@ public:
class PositionInterface : public HardwareInterface, public hardware_interface::PositionJointInterface
{
private:
TrajectoryFollower& follower_;
TrajectoryFollower &follower_;
std::array<double, 6> position_cmd_;
public:
PositionInterface(TrajectoryFollower& follower, hardware_interface::JointStateInterface &js_interface, std::vector<std::string> &joint_names);
PositionInterface(TrajectoryFollower &follower, hardware_interface::JointStateInterface &js_interface,
std::vector<std::string> &joint_names);
virtual bool write();
virtual void start();
virtual void stop();

20
include/ur_modern_driver/ros/io_service.h
View File

@@ -10,11 +10,11 @@
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/ur/commander.h"
class IOService
class IOService
{
private:
ros::NodeHandle nh_;
URCommander &commander_;
URCommander& commander_;
ros::ServiceServer io_service_;
ros::ServiceServer payload_service_;
@@ -23,20 +23,20 @@ private:
LOG_INFO("setIO called with [%d, %d]", req.fun, req.pin);
bool res = false;
bool flag = req.state > 0.0 ? true : false;
switch(req.fun)
switch (req.fun)
{
case ur_msgs::SetIO::Request::FUN_SET_DIGITAL_OUT:
res = commander_.setDigitalOut(req.pin, flag);
break;
break;
case ur_msgs::SetIO::Request::FUN_SET_ANALOG_OUT:
res = commander_.setAnalogOut(req.pin, req.state);
break;
break;
case ur_msgs::SetIO::Request::FUN_SET_TOOL_VOLTAGE:
res = commander_.setToolVoltage(static_cast<uint8_t>(req.state));
break;
break;
case ur_msgs::SetIO::Request::FUN_SET_FLAG:
res = commander_.setFlag(req.pin, flag);
break;
break;
default:
LOG_WARN("Invalid setIO function called (%d)", req.fun);
}
@@ -47,17 +47,15 @@ private:
bool setPayload(ur_msgs::SetPayloadRequest& req, ur_msgs::SetPayloadResponse& resp)
{
LOG_INFO("setPayload called");
//TODO check min and max payload?
// TODO check min and max payload?
return (resp.success = commander_.setPayload(req.payload));
}
public:
IOService(URCommander &commander)
IOService(URCommander& commander)
: commander_(commander)
, io_service_(nh_.advertiseService("ur_driver/set_io", &IOService::setIO, this))
, payload_service_(nh_.advertiseService("ur_driver/set_payload", &IOService::setPayload, this))
{
}
};

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

@@ -16,22 +16,21 @@ private:
Publisher io_pub_;
template <size_t N>
inline void appendDigital(std::vector<ur_msgs::Digital> &vec, std::bitset<N> bits)
inline void appendDigital(std::vector<ur_msgs::Digital>& vec, std::bitset<N> bits)
{
for(size_t i = 0; i < N; i++)
for (size_t i = 0; i < N; i++)
{
ur_msgs::Digital digi;
digi.pin = static_cast<uint8_t>(i);
digi.state = bits.test(i);
vec.push_back(digi);
ur_msgs::Digital digi;
digi.pin = static_cast<uint8_t>(i);
digi.state = bits.test(i);
vec.push_back(digi);
}
}
void publish(ur_msgs::IOStates &io_msg, SharedMasterBoardData& data);
void publish(ur_msgs::IOStates& io_msg, SharedMasterBoardData& data);
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))
{
}

32
include/ur_modern_driver/ros/service_stopper.h
View File

@@ -4,7 +4,7 @@
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/ur/consumer.h"
enum class RobotState
enum class RobotState
{
Running,
Error,
@@ -18,15 +18,16 @@ public:
virtual void onRobotStateChange(RobotState state) = 0;
};
class ServiceStopper : public URStatePacketConsumer {
class ServiceStopper : public URStatePacketConsumer
{
private:
ros::NodeHandle nh_;
ros::ServiceServer enable_service_;
std::vector<Service*> services_;
RobotState last_state_;
void notify_all(RobotState state);
bool handle(SharedRobotModeData &data, bool error);
bool handle(SharedRobotModeData& data, bool error);
bool enableCallback(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& resp);
public:
@@ -34,19 +35,28 @@ public:
virtual bool consume(RobotModeData_V1_X& data)
{
return handle(data, data.robot_mode != robot_mode_V1_X::ROBOT_RUNNING_MODE);
return handle(data, data.robot_mode != robot_mode_V1_X::ROBOT_RUNNING_MODE);
}
virtual bool consume(RobotModeData_V3_0__1& data)
{
return handle(data, data.robot_mode != robot_mode_V3_X::RUNNING);
return handle(data, data.robot_mode != robot_mode_V3_X::RUNNING);
}
virtual bool consume(RobotModeData_V3_2& data)
{
return handle(data, data.robot_mode != robot_mode_V3_X::RUNNING);
return handle(data, data.robot_mode != robot_mode_V3_X::RUNNING);
}
//unused
virtual bool consume(MasterBoardData_V1_X& data) { return true; }
virtual bool consume(MasterBoardData_V3_0__1& data) { return true; }
virtual bool consume(MasterBoardData_V3_2& data) { return true; }
// unused
virtual bool consume(MasterBoardData_V1_X& data)
{
return true;
}
virtual bool consume(MasterBoardData_V3_0__1& data)
{
return true;
}
virtual bool consume(MasterBoardData_V3_2& data)
{
return true;
}
};

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

@@ -1,13 +1,13 @@
#pragma once
#include <inttypes.h>
#include <array>
#include <vector>
#include <atomic>
#include <cstddef>
#include <cstring>
#include <string>
#include <thread>
#include <inttypes.h>
#include <vector>
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/ur/commander.h"
#include "ur_modern_driver/ur/server.h"
@@ -23,14 +23,12 @@ struct TrajectoryPoint
}
TrajectoryPoint(std::array<double, 6> &pos, std::array<double, 6> &vel, std::chrono::microseconds tfs)
: positions(pos)
, velocities(vel)
, time_from_start(tfs)
: positions(pos), velocities(vel), time_from_start(tfs)
{
}
};
class TrajectoryFollower
class TrajectoryFollower
{
private:
std::atomic<bool> running_;
@@ -49,11 +47,11 @@ private:
return s;
}
bool execute(std::array<double, 6> &positions, bool keep_alive);
double interpolate(double t, double T, double p0_pos, double p1_pos, double p0_vel, double p1_vel);
bool execute(std::array<double, 6> &positions, bool keep_alive);
double interpolate(double t, double T, double p0_pos, double p1_pos, double p0_vel, double p1_vel);
public:
TrajectoryFollower(URCommander &commander, std::string& reverse_ip, int reverse_port, bool version_3);
TrajectoryFollower(URCommander &commander, std::string &reverse_ip, int reverse_port, bool version_3);
bool start();
bool execute(std::array<double, 6> &positions);

25
include/ur_modern_driver/tcp_socket.h
View File

@@ -1,9 +1,9 @@
#pragma once
#pragma once
#include <netdb.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <mutex>
#include <atomic>
#include <mutex>
#include <string>
enum class SocketState
@@ -26,23 +26,28 @@ protected:
return false;
}
virtual void setOptions(int socket_fd);
bool setup(std::string &host, int port);
public:
TCPSocket();
virtual ~TCPSocket();
SocketState getState() { return state_; }
int getSocketFD() { return socket_fd_; }
SocketState getState()
{
return state_;
}
int getSocketFD()
{
return socket_fd_;
}
bool setSocketFD(int socket_fd);
std::string getIP();
bool read(uint8_t* buf, size_t buf_len, size_t &read);
bool write(const uint8_t* buf, size_t buf_len, size_t &written);
bool read(uint8_t *buf, size_t buf_len, size_t &read);
bool write(const uint8_t *buf, size_t buf_len, size_t &written);
void close();
void close();
};

15
include/ur_modern_driver/ur/commander.h
View File

@@ -1,20 +1,20 @@
#pragma once
#include <array>
#include <sstream>
#include <iomanip>
#include <sstream>
#include "ur_modern_driver/ur/stream.h"
class URCommander
{
private:
URStream& stream_;
URStream &stream_;
protected:
bool write(std::string& s);
bool write(std::string &s);
void formatArray(std::ostringstream &out, std::array<double, 6> &values);
public:
URCommander(URStream& stream) : stream_(stream)
URCommander(URStream &stream) : stream_(stream)
{
}
@@ -22,7 +22,7 @@ public:
virtual bool setDigitalOut(uint8_t pin, bool value) = 0;
virtual bool setAnalogOut(uint8_t pin, double value) = 0;
//shared
// shared
bool uploadProg(std::string &s);
bool stopj(double a = 10.0);
bool setToolVoltage(uint8_t voltage);
@@ -33,7 +33,7 @@ public:
class URCommander_V1_X : public URCommander
{
public:
URCommander_V1_X(URStream& stream) : URCommander(stream)
URCommander_V1_X(URStream &stream) : URCommander(stream)
{
}
@@ -42,11 +42,10 @@ public:
virtual bool setAnalogOut(uint8_t pin, double value);
};
class URCommander_V3_X : public URCommander
{
public:
URCommander_V3_X(URStream& stream) : URCommander(stream)
URCommander_V3_X(URStream &stream) : URCommander(stream)
{
}

6
include/ur_modern_driver/ur/factory.h
View File

@@ -76,12 +76,12 @@ public:
return major_version_ == 3;
}
std::unique_ptr<URCommander> getCommander(URStream &stream)
std::unique_ptr<URCommander> getCommander(URStream& stream)
{
if(major_version_ == 1)
if (major_version_ == 1)
return std::unique_ptr<URCommander>(new URCommander_V1_X(stream));
else
return std::unique_ptr<URCommander>(new URCommander_V3_X(stream));
return std::unique_ptr<URCommander>(new URCommander_V3_X(stream));
}
std::unique_ptr<URParser<StatePacket>> getStateParser()

3
include/ur_modern_driver/ur/master_board.h
View File

@@ -1,8 +1,8 @@
#pragma once
#include <inttypes.h>
#include <cstddef>
#include <bitset>
#include <cstddef>
#include "ur_modern_driver/bin_parser.h"
#include "ur_modern_driver/types.h"
#include "ur_modern_driver/ur/state.h"
@@ -42,7 +42,6 @@ public:
virtual bool parseWith(BinParser& bp);
virtual bool consumeWith(URStatePacketConsumer& consumer);
std::bitset<10> digital_input_bits;
std::bitset<10> digital_output_bits;

15
include/ur_modern_driver/ur/producer.h
View File

@@ -35,31 +35,30 @@ public:
// 4KB should be enough to hold any packet received from UR
uint8_t buf[4096];
size_t read = 0;
//expoential backoff reconnects
while(true)
// expoential backoff reconnects
while (true)
{
if(stream_.read(buf, sizeof(buf), read))
if (stream_.read(buf, sizeof(buf), read))
{
//reset sleep amount
// reset sleep amount
timeout_ = std::chrono::seconds(1);
break;
}
if(stream_.closed())
if (stream_.closed())
return false;
LOG_WARN("Failed to read from stream, reconnecting in %ld seconds...", timeout_.count());
std::this_thread::sleep_for(timeout_);
if(stream_.connect())
if (stream_.connect())
continue;
auto next = timeout_ * 2;
if(next <= std::chrono::seconds(120))
if (next <= std::chrono::seconds(120))
timeout_ = next;
}
BinParser bp(buf, read);
return parser_.parse(bp, products);
}

3
include/ur_modern_driver/ur/robot_mode.h
View File

@@ -16,7 +16,7 @@ public:
bool real_robot_enabled;
bool robot_power_on;
bool emergency_stopped;
bool protective_stopped; //AKA security_stopped
bool protective_stopped; // AKA security_stopped
bool program_running;
bool program_paused;
@@ -79,7 +79,6 @@ public:
virtual bool parseWith(BinParser& bp);
virtual bool consumeWith(URStatePacketConsumer& consumer);
robot_mode_V3_X robot_mode;
robot_control_mode_V3_X control_mode;

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

@@ -2,9 +2,9 @@
#include <netdb.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <atomic>
#include <cstdlib>
#include <mutex>
#include <atomic>
#include <string>
#include "ur_modern_driver/tcp_socket.h"
@@ -21,7 +21,6 @@ protected:
}
virtual void setOptions(int socket_fd);
public:
URServer(int port);
~URServer();
@@ -29,5 +28,5 @@ public:
bool bind();
bool accept();
void disconnectClient();
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);
};

8
include/ur_modern_driver/ur/state.h
View File

@@ -32,8 +32,12 @@ class URStatePacketConsumer;
class StatePacket
{
public:
StatePacket() {}
virtual ~StatePacket() {}
StatePacket()
{
}
virtual ~StatePacket()
{
}
virtual bool parseWith(BinParser& bp) = 0;
virtual bool consumeWith(URStatePacketConsumer& consumer) = 0;
};

4
include/ur_modern_driver/ur/state_parser.h
View File

@@ -35,8 +35,8 @@ public:
if (type != message_type::ROBOT_STATE)
{
//quietly ignore the intial version message
if(type != message_type::ROBOT_MESSAGE)
// quietly ignore the intial version message
if (type != message_type::ROBOT_MESSAGE)
{
LOG_WARN("Invalid state message type recieved: %u", static_cast<uint8_t>(type));
}

13
include/ur_modern_driver/ur/stream.h
View File

@@ -2,8 +2,8 @@
#include <netdb.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <mutex>
#include <atomic>
#include <mutex>
#include <string>
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/tcp_socket.h"
@@ -16,7 +16,7 @@ private:
std::mutex write_mutex_, read_mutex_;
protected:
virtual bool open(int socket_fd, struct sockaddr *address, size_t address_len)
virtual bool open(int socket_fd, struct sockaddr* address, size_t address_len)
{
return ::connect(socket_fd, address, address_len) == 0;
}
@@ -36,8 +36,11 @@ public:
TCPSocket::close();
}
bool closed() { return getState() == SocketState::Closed; }
bool closed()
{
return getState() == SocketState::Closed;
}
bool read(uint8_t* buf, size_t buf_len, size_t &read);
bool write(const uint8_t* buf, size_t buf_len, size_t &written);
bool read(uint8_t* buf, size_t buf_len, size_t& read);
bool write(const uint8_t* buf, size_t buf_len, size_t& written);
};

128
src/ros/action_server.cpp
View File

@@ -1,14 +1,9 @@
#include <cmath>
#include "ur_modern_driver/ros/action_server.h"
#include <cmath>
ActionServer::ActionServer(TrajectoryFollower& follower, std::vector<std::string>& joint_names, double max_velocity)
: as_(
nh_,
"follow_joint_trajectory",
boost::bind(&ActionServer::onGoal, this, _1),
boost::bind(&ActionServer::onCancel, this, _1),
false
)
: as_(nh_, "follow_joint_trajectory", boost::bind(&ActionServer::onGoal, this, _1),
boost::bind(&ActionServer::onCancel, this, _1), false)
, joint_names_(joint_names)
, joint_set_(joint_names.begin(), joint_names.end())
, max_velocity_(max_velocity)
@@ -22,7 +17,7 @@ ActionServer::ActionServer(TrajectoryFollower& follower, std::vector<std::string
void ActionServer::start()
{
if(running_)
if (running_)
return;
LOG_INFO("Starting ActionServer");
@@ -35,23 +30,23 @@ void ActionServer::onRobotStateChange(RobotState state)
{
state_ = state;
//don't interrupt if everything is fine
if(state == RobotState::Running)
// don't interrupt if everything is fine
if (state == RobotState::Running)
return;
//don't retry interrupts
if(interrupt_traj_ || !has_goal_)
// don't retry interrupts
if (interrupt_traj_ || !has_goal_)
return;
//on successful lock we're not executing a goal so don't interrupt
if(tj_mutex_.try_lock())
// on successful lock we're not executing a goal so don't interrupt
if (tj_mutex_.try_lock())
{
tj_mutex_.unlock();
return;
}
interrupt_traj_ = true;
//wait for goal to be interrupted and automagically unlock when going out of scope
// wait for goal to be interrupted and automagically unlock when going out of scope
std::lock_guard<std::mutex> lock(tj_mutex_);
Result res;
@@ -60,7 +55,6 @@ void ActionServer::onRobotStateChange(RobotState state)
curr_gh_.setAborted(res, res.error_string);
}
bool ActionServer::updateState(RTShared& data)
{
q_actual_ = data.q_actual;
@@ -68,7 +62,6 @@ bool ActionServer::updateState(RTShared& data)
return true;
}
bool ActionServer::consume(RTState_V1_6__7& state)
{
return updateState(state);
@@ -86,7 +79,6 @@ bool ActionServer::consume(RTState_V3_2__3& state)
return updateState(state);
}
void ActionServer::onGoal(GoalHandle gh)
{
Result res;
@@ -94,7 +86,7 @@ void ActionServer::onGoal(GoalHandle gh)
LOG_INFO("Received new goal");
if(!validate(gh, res) || !try_execute(gh, res))
if (!validate(gh, res) || !try_execute(gh, res))
{
LOG_WARN("Goal error: %s", res.error_string.c_str());
gh.setRejected(res, res.error_string);
@@ -104,7 +96,7 @@ void ActionServer::onGoal(GoalHandle gh)
void ActionServer::onCancel(GoalHandle gh)
{
interrupt_traj_ = true;
//wait for goal to be interrupted
// wait for goal to be interrupted
std::lock_guard<std::mutex> lock(tj_mutex_);
Result res;
@@ -114,22 +106,22 @@ void ActionServer::onCancel(GoalHandle gh)
}
bool ActionServer::validate(GoalHandle& gh, Result& res)
{
{
return validateState(gh, res) && validateJoints(gh, res) && validateTrajectory(gh, res);
}
bool ActionServer::validateState(GoalHandle& gh, Result& res)
{
switch(state_)
switch (state_)
{
case RobotState::EmergencyStopped:
res.error_string = "Robot is emergency stopped";
return false;
case RobotState::ProtectiveStopped:
res.error_string = "Robot is protective stopped";
return false;
case RobotState::Error:
res.error_string = "Robot is not ready, check robot_mode";
return false;
@@ -149,7 +141,7 @@ bool ActionServer::validateJoints(GoalHandle& gh, Result& res)
auto const& joints = goal->trajectory.joint_names;
std::set<std::string> goal_joints(joints.begin(), joints.end());
if(goal_joints == joint_set_)
if (goal_joints == joint_set_)
return true;
res.error_code = Result::INVALID_JOINTS;
@@ -162,42 +154,42 @@ bool ActionServer::validateTrajectory(GoalHandle& gh, Result& res)
auto goal = gh.getGoal();
res.error_code = Result::INVALID_GOAL;
//must at least have one point
if(goal->trajectory.points.size() < 1)
// must at least have one point
if (goal->trajectory.points.size() < 1)
return false;
for(auto const& point : goal->trajectory.points)
for (auto const& point : goal->trajectory.points)
{
if(point.velocities.size() != joint_names_.size())
if (point.velocities.size() != joint_names_.size())
{
res.error_code = Result::INVALID_GOAL;
res.error_string = "Received a goal with an invalid number of velocities";
return false;
}
if(point.positions.size() != joint_names_.size())
if (point.positions.size() != joint_names_.size())
{
res.error_code = Result::INVALID_GOAL;
res.error_string = "Received a goal with an invalid number of positions";
return false;
}
for(auto const& velocity : point.velocities)
for (auto const& velocity : point.velocities)
{
if(!std::isfinite(velocity))
if (!std::isfinite(velocity))
{
res.error_string = "Received a goal with infinities or NaNs in velocity";
return false;
}
if(std::fabs(velocity) > max_velocity_)
if (std::fabs(velocity) > max_velocity_)
{
res.error_string = "Received a goal with velocities that are higher than " + std::to_string(max_velocity_);
return false;
}
}
for(auto const& position : point.positions)
for (auto const& position : point.positions)
{
if(!std::isfinite(position))
if (!std::isfinite(position))
{
res.error_string = "Received a goal with infinities or NaNs in positions";
return false;
@@ -205,34 +197,34 @@ bool ActionServer::validateTrajectory(GoalHandle& gh, Result& res)
}
}
//todo validate start position?
// todo validate start position?
return true;
}
inline std::chrono::microseconds convert(const ros::Duration &dur)
inline std::chrono::microseconds convert(const ros::Duration& dur)
{
return std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::seconds(dur.sec) +
std::chrono::nanoseconds(dur.nsec));
std::chrono::nanoseconds(dur.nsec));
}
bool ActionServer::try_execute(GoalHandle& gh, Result& res)
{
if(!running_)
if (!running_)
{
res.error_string = "Internal error";
return false;
}
if(!tj_mutex_.try_lock())
if (!tj_mutex_.try_lock())
{
interrupt_traj_ = true;
res.error_string = "Received another trajectory";
curr_gh_.setAborted(res, res.error_string);
tj_mutex_.lock();
//todo: make configurable
// todo: make configurable
std::this_thread::sleep_for(std::chrono::milliseconds(250));
}
//locked here
// locked here
curr_gh_ = gh;
interrupt_traj_ = false;
has_goal_ = true;
@@ -244,12 +236,12 @@ bool ActionServer::try_execute(GoalHandle& gh, Result& res)
std::vector<size_t> ActionServer::reorderMap(std::vector<std::string> goal_joints)
{
std::vector<size_t> indecies;
for(auto const& aj : joint_names_)
for (auto const& aj : joint_names_)
{
size_t j = 0;
for(auto const& gj : goal_joints)
for (auto const& gj : goal_joints)
{
if(aj == gj)
if (aj == gj)
break;
j++;
}
@@ -261,40 +253,40 @@ std::vector<size_t> ActionServer::reorderMap(std::vector<std::string> goal_joint
void ActionServer::trajectoryThread()
{
LOG_INFO("Trajectory thread started");
while(running_)
while (running_)
{
std::unique_lock<std::mutex> lk(tj_mutex_);
if(!tj_cv_.wait_for(lk, std::chrono::milliseconds(100), [&]{return running_ && has_goal_;}))
if (!tj_cv_.wait_for(lk, std::chrono::milliseconds(100), [&] { return running_ && has_goal_; }))
continue;
LOG_INFO("Trajectory received and accepted");
curr_gh_.setAccepted();
auto goal = curr_gh_.getGoal();
std::vector<TrajectoryPoint> trajectory;
trajectory.reserve(goal->trajectory.points.size()+1);
trajectory.reserve(goal->trajectory.points.size() + 1);
//joint names of the goal might have a different ordering compared
//to what URScript expects so need to map between the two
// joint names of the goal might have a different ordering compared
// to what URScript expects so need to map between the two
auto mapping = reorderMap(goal->trajectory.joint_names);
LOG_INFO("Translating trajectory");
auto const& fp = goal->trajectory.points[0];
auto fpt = convert(fp.time_from_start);
//make sure we have a proper t0 position
if(fpt > std::chrono::microseconds(0))
// make sure we have a proper t0 position
if (fpt > std::chrono::microseconds(0))
{
LOG_INFO("Trajectory without t0 recieved, inserting t0 at currrent position");
trajectory.push_back(TrajectoryPoint(q_actual_, qd_actual_, std::chrono::microseconds(0)));
}
for(auto const& point : goal->trajectory.points)
for (auto const& point : goal->trajectory.points)
{
std::array<double, 6> pos, vel;
for(size_t i = 0; i < 6; i++)
for (size_t i = 0; i < 6; i++)
{
size_t idx = mapping[i];
pos[idx] = point.positions[i];
@@ -304,17 +296,19 @@ void ActionServer::trajectoryThread()
trajectory.push_back(TrajectoryPoint(pos, vel, t));
}
double t = std::chrono::duration_cast<std::chrono::duration<double>>(trajectory[trajectory.size()-1].time_from_start).count();
double t =
std::chrono::duration_cast<std::chrono::duration<double>>(trajectory[trajectory.size() - 1].time_from_start)
.count();
LOG_INFO("Executing trajectory with %zu points and duration of %4.3fs", trajectory.size(), t);
Result res;
if(follower_.start())
if (follower_.start())
{
if(follower_.execute(trajectory, interrupt_traj_))
if (follower_.execute(trajectory, interrupt_traj_))
{
//interrupted goals must be handled by interrupt trigger
if(!interrupt_traj_)
// interrupted goals must be handled by interrupt trigger
if (!interrupt_traj_)
{
LOG_INFO("Trajectory executed successfully");
res.error_code = Result::SUCCESSFUL;
@@ -327,7 +321,7 @@ void ActionServer::trajectoryThread()
{
LOG_INFO("Trajectory failed");
res.error_code = -100;
res.error_string = "Connection to robot was lost";
res.error_string = "Connection to robot was lost";
curr_gh_.setAborted(res, res.error_string);
}
@@ -337,7 +331,7 @@ void ActionServer::trajectoryThread()
{
LOG_ERROR("Failed to start trajectory follower!");
res.error_code = -100;
res.error_string = "Robot connection could not be established";
res.error_string = "Robot connection could not be established";
curr_gh_.setAborted(res, res.error_string);
}

27
src/ros/controller.cpp
View File

@@ -1,6 +1,7 @@
#include "ur_modern_driver/ros/controller.h"
ROSController::ROSController(URCommander &commander, TrajectoryFollower& follower, std::vector<std::string> &joint_names, double max_vel_change)
ROSController::ROSController(URCommander& commander, TrajectoryFollower& follower,
std::vector<std::string>& joint_names, double max_vel_change)
: controller_(this, nh_)
, joint_interface_(joint_names)
, wrench_interface_()
@@ -18,7 +19,8 @@ void ROSController::setupConsumer()
lastUpdate_ = ros::Time::now();
}
void ROSController::doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list, const std::list<hardware_interface::ControllerInfo>& stop_list)
void ROSController::doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
const std::list<hardware_interface::ControllerInfo>& stop_list)
{
LOG_INFO("Switching hardware interface");
@@ -60,7 +62,7 @@ void ROSController::reset()
{
if (active_interface_ == nullptr)
return;
active_interface_->reset();
}
@@ -70,7 +72,6 @@ void ROSController::read(RTShared& packet)
wrench_interface_.update(packet);
}
bool ROSController::update(RTShared& state)
{
auto time = ros::Time::now();
@@ -80,16 +81,16 @@ bool ROSController::update(RTShared& state)
read(state);
controller_.update(time, diff, !service_enabled_);
//emergency stop and such should not kill the pipeline
//but still prevent writes
if(!service_enabled_)
// emergency stop and such should not kill the pipeline
// but still prevent writes
if (!service_enabled_)
{
reset();
return true;
}
//allow the controller to update x times before allowing writes again
if(service_cooldown_ > 0)
// allow the controller to update x times before allowing writes again
if (service_cooldown_ > 0)
{
service_cooldown_ -= 1;
return true;
@@ -101,9 +102,9 @@ bool ROSController::update(RTShared& state)
void ROSController::onRobotStateChange(RobotState state)
{
bool next = (state == RobotState::Running);
if(next == service_enabled_)
if (next == service_enabled_)
return;
service_enabled_ = next;
service_cooldown_ = 125;
service_cooldown_ = 125;
}

12
src/ros/hardware_interface.cpp
View File

@@ -19,14 +19,15 @@ void JointInterface::update(RTShared &packet)
WrenchInterface::WrenchInterface()
{
registerHandle(hardware_interface::ForceTorqueSensorHandle("wrench", "", tcp_.begin(), tcp_.begin() + 3));
}
}
void WrenchInterface::update(RTShared &packet)
{
tcp_ = packet.tcp_force;
}
VelocityInterface::VelocityInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface, std::vector<std::string> &joint_names, double max_vel_change)
VelocityInterface::VelocityInterface(URCommander &commander, hardware_interface::JointStateInterface &js_interface,
std::vector<std::string> &joint_names, double max_vel_change)
: commander_(commander), max_vel_change_(max_vel_change)
{
for (size_t i = 0; i < 6; i++)
@@ -50,14 +51,15 @@ bool VelocityInterface::write()
void VelocityInterface::reset()
{
for(auto &val : prev_velocity_cmd_)
for (auto &val : prev_velocity_cmd_)
{
val = 0;
}
}
PositionInterface:: PositionInterface(TrajectoryFollower& follower, hardware_interface::JointStateInterface &js_interface, std::vector<std::string> &joint_names)
PositionInterface::PositionInterface(TrajectoryFollower &follower,
hardware_interface::JointStateInterface &js_interface,
std::vector<std::string> &joint_names)
: follower_(follower)
{
for (size_t i = 0; i < 6; i++)

4
src/ros/mb_publisher.cpp
View File

@@ -1,6 +1,6 @@
#include "ur_modern_driver/ros/mb_publisher.h"
inline void appendAnalog(std::vector<ur_msgs::Analog> &vec, double val, uint8_t pin)
inline void appendAnalog(std::vector<ur_msgs::Analog>& vec, double val, uint8_t pin)
{
ur_msgs::Analog ana;
ana.pin = pin;
@@ -8,7 +8,7 @@ inline void appendAnalog(std::vector<ur_msgs::Analog> &vec, double val, uint8_t
vec.push_back(ana);
}
void MBPublisher::publish(ur_msgs::IOStates &io_msg, SharedMasterBoardData& data)
void MBPublisher::publish(ur_msgs::IOStates& io_msg, SharedMasterBoardData& data)
{
appendAnalog(io_msg.analog_in_states, data.analog_input0, 0);
appendAnalog(io_msg.analog_in_states, data.analog_input1, 1);

15
src/ros/service_stopper.cpp
View File

@@ -5,10 +5,9 @@ ServiceStopper::ServiceStopper(std::vector<Service*> services)
, services_(services)
, last_state_(RobotState::Error)
{
//enable_all();
// enable_all();
}
bool ServiceStopper::enableCallback(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& resp)
{
notify_all(RobotState::Running);
@@ -17,10 +16,10 @@ bool ServiceStopper::enableCallback(std_srvs::EmptyRequest& req, std_srvs::Empty
void ServiceStopper::notify_all(RobotState state)
{
if(last_state_ == state)
if (last_state_ == state)
return;
for(auto const service : services_)
for (auto const service : services_)
{
service->onRobotStateChange(state);
}
@@ -30,15 +29,15 @@ void ServiceStopper::notify_all(RobotState state)
bool ServiceStopper::handle(SharedRobotModeData& data, bool error)
{
if(data.emergency_stopped)
if (data.emergency_stopped)
{
notify_all(RobotState::EmergencyStopped);
}
else if(data.protective_stopped)
else if (data.protective_stopped)
{
notify_all(RobotState::ProtectiveStopped);
}
else if(error)
else if (error)
{
notify_all(RobotState::Error);
}

94
src/ros/trajectory_follower.cpp
View File

@@ -1,8 +1,7 @@
#include <cmath>
#include <endian.h>
#include "ur_modern_driver/ros/trajectory_follower.h"
#include <endian.h>
#include <cmath>
static const int32_t MULT_JOINTSTATE_ = 1000000;
static const std::string JOINT_STATE_REPLACE("{{JOINT_STATE_REPLACE}}");
static const std::string SERVO_J_REPLACE("{{SERVO_J_REPLACE}}");
@@ -65,7 +64,8 @@ def driverProg():
end
)";
TrajectoryFollower::TrajectoryFollower(URCommander &commander, std::string& reverse_ip, int reverse_port, bool version_3)
TrajectoryFollower::TrajectoryFollower(URCommander &commander, std::string &reverse_ip, int reverse_port,
bool version_3)
: running_(false)
, commander_(commander)
, server_(reverse_port)
@@ -77,13 +77,12 @@ TrajectoryFollower::TrajectoryFollower(URCommander &commander, std::string& reve
ros::param::get("~servoj_lookahead_time", servoj_lookahead_time_);
ros::param::get("~servoj_gain", servoj_gain_);
std::string res(POSITION_PROGRAM);
res.replace(res.find(JOINT_STATE_REPLACE), JOINT_STATE_REPLACE.length(), std::to_string(MULT_JOINTSTATE_));
std::ostringstream out;
out << "t=" << std::fixed << std::setprecision(4) << servoj_time_;
if(version_3)
if (version_3)
out << ", lookahead_time=" << servoj_lookahead_time_ << ", gain=" << servoj_gain_;
res.replace(res.find(SERVO_J_REPLACE), SERVO_J_REPLACE.length(), out.str());
@@ -91,21 +90,21 @@ TrajectoryFollower::TrajectoryFollower(URCommander &commander, std::string& reve
res.replace(res.find(SERVER_PORT_REPLACE), SERVER_PORT_REPLACE.length(), std::to_string(reverse_port));
program_ = res;
if(!server_.bind())
if (!server_.bind())
{
LOG_ERROR("Failed to bind server, the port %d is likely already in use", reverse_port);
std::exit(-1);
}
}
}
bool TrajectoryFollower::start()
{
if(running_)
return true; //not sure
if (running_)
return true; // not sure
LOG_INFO("Uploading trajectory program to robot");
if(!commander_.uploadProg(program_))
if (!commander_.uploadProg(program_))
{
LOG_ERROR("Program upload failed!");
return false;
@@ -113,29 +112,30 @@ bool TrajectoryFollower::start()
LOG_DEBUG("Awaiting incomming robot connection");
if(!server_.accept())
if (!server_.accept())
{
LOG_ERROR("Failed to accept incomming robot connection");
return false;
}
LOG_DEBUG("Robot successfully connected");
return (running_ = true);
}
bool TrajectoryFollower::execute(std::array<double, 6> &positions, bool keep_alive)
{
if(!running_)
if (!running_)
return false;
// LOG_INFO("servoj([%f,%f,%f,%f,%f,%f])", positions[0], positions[1], positions[2], positions[3], positions[4], positions[5]);
// LOG_INFO("servoj([%f,%f,%f,%f,%f,%f])", positions[0], positions[1], positions[2], positions[3], positions[4],
// positions[5]);
last_positions_ = positions;
uint8_t buf[sizeof(uint32_t)*7];
uint8_t buf[sizeof(uint32_t) * 7];
uint8_t *idx = buf;
for(auto const& pos : positions)
for (auto const &pos : positions)
{
int32_t val = static_cast<int32_t>(pos * MULT_JOINTSTATE_);
val = htobe32(val);
@@ -145,7 +145,7 @@ bool TrajectoryFollower::execute(std::array<double, 6> &positions, bool keep_ali
int32_t val = htobe32(static_cast<int32_t>(keep_alive));
append(idx, val);
size_t written;
size_t written;
return server_.write(buf, sizeof(buf), written);
}
@@ -166,60 +166,54 @@ bool TrajectoryFollower::execute(std::array<double, 6> &positions)
bool TrajectoryFollower::execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt)
{
if(!running_)
if (!running_)
return false;
using namespace std::chrono;
typedef duration<double> double_seconds;
typedef high_resolution_clock Clock;
typedef Clock::time_point Time;
auto& last = trajectory[trajectory.size()-1];
auto& prev = trajectory[0];
auto &last = trajectory[trajectory.size() - 1];
auto &prev = trajectory[0];
Time t0 = Clock::now();
Time latest = t0;
std::array<double, 6> positions;
for(auto const& point : trajectory)
for (auto const &point : trajectory)
{
//skip t0
if(&point == &prev)
// skip t0
if (&point == &prev)
continue;
if(interrupt)
if (interrupt)
break;
auto duration = point.time_from_start - prev.time_from_start;
double d_s = duration_cast<double_seconds>(duration).count();
//interpolation loop
while(!interrupt)
// interpolation loop
while (!interrupt)
{
latest = Clock::now();
auto elapsed = latest - t0;
if(point.time_from_start <= elapsed)
if (point.time_from_start <= elapsed)
break;
if(last.time_from_start <= elapsed)
if (last.time_from_start <= elapsed)
return true;
double elapsed_s = duration_cast<double_seconds>(elapsed - prev.time_from_start).count();
for(size_t j = 0; j < positions.size(); j++)
for (size_t j = 0; j < positions.size(); j++)
{
positions[j] = interpolate(
elapsed_s,
d_s,
prev.positions[j],
point.positions[j],
prev.velocities[j],
point.velocities[j]
);
positions[j] =
interpolate(elapsed_s, d_s, prev.positions[j], point.positions[j], prev.velocities[j], point.velocities[j]);
}
if(!execute(positions, true))
if (!execute(positions, true))
return false;
std::this_thread::sleep_for(std::chrono::milliseconds((int)((servoj_time_ * 1000) / 4.)));
@@ -228,20 +222,20 @@ bool TrajectoryFollower::execute(std::vector<TrajectoryPoint> &trajectory, std::
prev = point;
}
//In theory it's possible the last position won't be sent by
//the interpolation loop above but rather some position between
//t[N-1] and t[N] where N is the number of trajectory points.
//To make sure this does not happen the last position is sent
// In theory it's possible the last position won't be sent by
// the interpolation loop above but rather some position between
// t[N-1] and t[N] where N is the number of trajectory points.
// To make sure this does not happen the last position is sent
return execute(last.positions, true);
}
void TrajectoryFollower::stop()
{
if(!running_)
if (!running_)
return;
//std::array<double, 6> empty;
//execute(empty, false);
// std::array<double, 6> empty;
// execute(empty, false);
server_.disconnectClient();
running_ = false;

32
src/ros_main.cpp
View File

@@ -11,8 +11,8 @@
#include "ur_modern_driver/ros/io_service.h"
#include "ur_modern_driver/ros/mb_publisher.h"
#include "ur_modern_driver/ros/rt_publisher.h"
#include "ur_modern_driver/ros/trajectory_follower.h"
#include "ur_modern_driver/ros/service_stopper.h"
#include "ur_modern_driver/ros/trajectory_follower.h"
#include "ur_modern_driver/ur/commander.h"
#include "ur_modern_driver/ur/factory.h"
#include "ur_modern_driver/ur/messages.h"
@@ -30,14 +30,8 @@ static const std::string BASE_FRAME_ARG("~base_frame");
static const std::string TOOL_FRAME_ARG("~tool_frame");
static const std::string JOINT_NAMES_PARAM("hardware_interface/joints");
static const std::vector<std::string> DEFAULT_JOINTS = {
"shoulder_pan_joint",
"shoulder_lift_joint",
"elbow_joint",
"wrist_1_joint",
"wrist_2_joint",
"wrist_3_joint"
};
static const std::vector<std::string> DEFAULT_JOINTS = { "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
"wrist_1_joint", "wrist_2_joint", "wrist_3_joint" };
static const int UR_SECONDARY_PORT = 30002;
static const int UR_RT_PORT = 30003;
@@ -65,7 +59,7 @@ bool parse_args(ProgArgs &args)
return false;
}
ros::param::param(REVERSE_PORT_ARG, args.reverse_port, int32_t(50001));
ros::param::param(MAX_VEL_CHANGE_ARG, args.max_vel_change, 15.0); // rad/s
ros::param::param(MAX_VEL_CHANGE_ARG, args.max_vel_change, 15.0); // rad/s
ros::param::param(ROS_CONTROL_ARG, args.use_ros_control, false);
ros::param::param(PREFIX_ARG, args.prefix, std::string());
ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link");
@@ -74,7 +68,7 @@ bool parse_args(ProgArgs &args)
return true;
}
std::string getLocalIPAccessibleFromHost(std::string& host)
std::string getLocalIPAccessibleFromHost(std::string &host)
{
URStream stream(host, UR_RT_PORT);
return stream.connect() ? stream.getIP() : std::string();
@@ -91,9 +85,9 @@ int main(int argc, char **argv)
}
std::string local_ip(getLocalIPAccessibleFromHost(args.host));
URFactory factory(args.host);
vector<Service*> services;
vector<Service *> services;
// RT packets
auto rt_parser = factory.getRTParser();
@@ -101,7 +95,7 @@ int main(int argc, char **argv)
URProducer<RTPacket> rt_prod(rt_stream, *rt_parser);
RTPublisher rt_pub(args.prefix, args.base_frame, args.tool_frame, args.use_ros_control);
auto rt_commander = factory.getCommander(rt_stream);
vector<IConsumer<RTPacket> *> rt_vec{&rt_pub};
vector<IConsumer<RTPacket> *> rt_vec{ &rt_pub };
TrajectoryFollower traj_follower(*rt_commander, local_ip, args.reverse_port, factory.isVersion3());
@@ -130,10 +124,10 @@ int main(int argc, char **argv)
URStream state_stream(args.host, UR_SECONDARY_PORT);
URProducer<StatePacket> state_prod(state_stream, *state_parser);
MBPublisher state_pub;
ServiceStopper service_stopper(services);
vector<IConsumer<StatePacket> *> state_vec{&state_pub, &service_stopper};
vector<IConsumer<StatePacket> *> state_vec{ &state_pub, &service_stopper };
MultiConsumer<StatePacket> state_cons(state_vec);
Pipeline<StatePacket> state_pl(state_prod, state_cons);
@@ -145,7 +139,7 @@ int main(int argc, char **argv)
auto state_commander = factory.getCommander(state_stream);
IOService io_service(*state_commander);
if(action_server)
if (action_server)
action_server->start();
ros::spin();
@@ -154,8 +148,8 @@ int main(int argc, char **argv)
rt_pl.stop();
state_pl.stop();
if(controller)
if (controller)
delete controller;
LOG_INFO("Pipelines shutdown complete");

42
src/tcp_socket.cpp
View File

@@ -1,16 +1,14 @@
#include <arpa/inet.h>
#include <endian.h>
#include <netinet/tcp.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <cstring>
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/tcp_socket.h"
TCPSocket::TCPSocket()
: socket_fd_(-1)
, state_(SocketState::Invalid)
{
TCPSocket::TCPSocket() : socket_fd_(-1), state_(SocketState::Invalid)
{
}
TCPSocket::~TCPSocket()
{
@@ -26,7 +24,7 @@ void TCPSocket::setOptions(int socket_fd)
bool TCPSocket::setup(std::string &host, int port)
{
if(state_ == SocketState::Connected)
if (state_ == SocketState::Connected)
return false;
LOG_INFO("Setting up connection: %s:%d", host.c_str(), port);
@@ -51,11 +49,11 @@ bool TCPSocket::setup(std::string &host, int port)
bool connected = false;
// loop through the list of addresses untill we find one that's connectable
for (struct addrinfo* p = result; p != nullptr; p = p->ai_next)
for (struct addrinfo *p = result; p != nullptr; p = p->ai_next)
{
socket_fd_ = ::socket(p->ai_family, p->ai_socktype, p->ai_protocol);
if (socket_fd_ != -1 && open(socket_fd_, p->ai_addr, p->ai_addrlen))
if (socket_fd_ != -1 && open(socket_fd_, p->ai_addr, p->ai_addrlen))
{
connected = true;
break;
@@ -63,8 +61,8 @@ bool TCPSocket::setup(std::string &host, int port)
}
freeaddrinfo(result);
if(!connected)
if (!connected)
{
state_ = SocketState::Invalid;
LOG_ERROR("Connection setup failed for %s:%d", host.c_str(), port);
@@ -80,7 +78,7 @@ bool TCPSocket::setup(std::string &host, int port)
bool TCPSocket::setSocketFD(int socket_fd)
{
if(state_ == SocketState::Connected)
if (state_ == SocketState::Connected)
return false;
socket_fd_ = socket_fd;
state_ = SocketState::Connected;
@@ -89,7 +87,7 @@ bool TCPSocket::setSocketFD(int socket_fd)
void TCPSocket::close()
{
if(state_ != SocketState::Connected)
if (state_ != SocketState::Connected)
return;
state_ = SocketState::Closed;
::shutdown(socket_fd_, SHUT_RDWR);
@@ -100,9 +98,9 @@ std::string TCPSocket::getIP()
{
sockaddr_in name;
socklen_t len = sizeof(name);
int res = ::getsockname(socket_fd_, (sockaddr*)&name, &len);
int res = ::getsockname(socket_fd_, (sockaddr *)&name, &len);
if(res < 0)
if (res < 0)
{
LOG_ERROR("Could not get local IP");
return std::string();
@@ -117,17 +115,17 @@ bool TCPSocket::read(uint8_t *buf, size_t buf_len, size_t &read)
{
read = 0;
if(state_ != SocketState::Connected)
if (state_ != SocketState::Connected)
return false;
ssize_t res = ::recv(socket_fd_, buf, buf_len, 0);
if(res == 0)
if (res == 0)
{
state_ = SocketState::Disconnected;
return false;
}
else if(res < 0)
else if (res < 0)
return false;
read = static_cast<size_t>(res);
@@ -137,8 +135,8 @@ bool TCPSocket::read(uint8_t *buf, size_t buf_len, size_t &read)
bool TCPSocket::write(const uint8_t *buf, size_t buf_len, size_t &written)
{
written = 0;
if(state_ != SocketState::Connected)
if (state_ != SocketState::Connected)
return false;
size_t remaining = buf_len;
@@ -150,10 +148,10 @@ bool TCPSocket::write(const uint8_t *buf, size_t buf_len, size_t &written)
if (sent <= 0)
return false;
written += sent;
remaining -= sent;
}
return true;
}

21
src/ur/commander.cpp
View File

@@ -1,10 +1,10 @@
#include "ur_modern_driver/ur/commander.h"
#include "ur_modern_driver/log.h"
bool URCommander::write(std::string& s)
bool URCommander::write(std::string &s)
{
size_t len = s.size();
const uint8_t* data = reinterpret_cast<const uint8_t*>(s.c_str());
const uint8_t *data = reinterpret_cast<const uint8_t *>(s.c_str());
size_t written;
return stream_.write(data, len, written);
}
@@ -12,10 +12,10 @@ bool URCommander::write(std::string& s)
void URCommander::formatArray(std::ostringstream &out, std::array<double, 6> &values)
{
std::string mod("[");
for(auto const& val : values)
for (auto const &val : values)
{
out << mod << val;
mod = ",";
out << mod << val;
mod = ",";
}
out << "]";
}
@@ -27,7 +27,7 @@ bool URCommander::uploadProg(std::string &s)
bool URCommander::setToolVoltage(uint8_t voltage)
{
if(voltage != 0 || voltage != 12 || voltage != 24)
if (voltage != 0 || voltage != 12 || voltage != 24)
return false;
std::ostringstream out;
@@ -59,7 +59,6 @@ bool URCommander::stopj(double a)
return write(s);
}
bool URCommander_V1_X::speedj(std::array<double, 6> &speeds, double acceleration)
{
std::ostringstream out;
@@ -86,7 +85,6 @@ bool URCommander_V1_X::setDigitalOut(uint8_t pin, bool value)
return write(s);
}
bool URCommander_V3_X::speedj(std::array<double, 6> &speeds, double acceleration)
{
std::ostringstream out;
@@ -110,16 +108,16 @@ bool URCommander_V3_X::setDigitalOut(uint8_t pin, bool value)
std::ostringstream out;
std::string func;
if(pin < 8)
if (pin < 8)
{
func = "set_standard_digital_out";
}
else if(pin < 16)
else if (pin < 16)
{
func = "set_configurable_digital_out";
pin -= 8;
}
else if(pin < 18)
else if (pin < 18)
{
func = "set_tool_digital_out";
pin -= 16;
@@ -127,7 +125,6 @@ bool URCommander_V3_X::setDigitalOut(uint8_t pin, bool value)
else
return false;
out << func << "(" << (int)pin << "," << (value ? "True" : "False") << ")\n";
std::string s(out.str());
return write(s);

2
src/ur/master_board.cpp
View File

@@ -59,7 +59,7 @@ bool MasterBoardData_V3_0__1::parseWith(BinParser& bp)
bp.parse(euromap67_interface_installed);
if (euromap67_interface_installed)
{
{
if (!bp.checkSize(MasterBoardData_V3_0__1::EURO_SIZE))
return false;

33
src/ur/server.cpp
View File

@@ -1,12 +1,11 @@
#include <cstring>
#include <netinet/tcp.h>
#include <arpa/inet.h>
#include <unistd.h>
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/ur/server.h"
#include <arpa/inet.h>
#include <netinet/tcp.h>
#include <unistd.h>
#include <cstring>
#include "ur_modern_driver/log.h"
URServer::URServer(int port)
: port_(port)
URServer::URServer(int port) : port_(port)
{
}
@@ -29,7 +28,7 @@ std::string URServer::getIP()
socklen_t len = sizeof(name);
int res = ::getsockname(getSocketFD(), (sockaddr*)&name, &len);
if(res < 0)
if (res < 0)
{
LOG_ERROR("Could not get local IP");
return std::string();
@@ -44,11 +43,11 @@ bool URServer::bind()
{
std::string empty;
bool res = TCPSocket::setup(empty, port_);
if(!res)
if (!res)
return false;
if(::listen(getSocketFD(), 1) < 0)
if (::listen(getSocketFD(), 1) < 0)
return false;
return true;
@@ -56,14 +55,14 @@ bool URServer::bind()
bool URServer::accept()
{
if(TCPSocket::getState() != SocketState::Connected || client_.getSocketFD() > 0)
if (TCPSocket::getState() != SocketState::Connected || client_.getSocketFD() > 0)
return false;
struct sockaddr addr;
struct sockaddr addr;
socklen_t addr_len;
int client_fd = ::accept(getSocketFD(), &addr, &addr_len);
if(client_fd <= 0)
if (client_fd <= 0)
return false;
setOptions(client_fd);
@@ -73,13 +72,13 @@ bool URServer::accept()
void URServer::disconnectClient()
{
if(client_.getState() != SocketState::Connected)
if (client_.getState() != SocketState::Connected)
return;
client_.close();
}
bool URServer::write(const uint8_t* buf, size_t buf_len, size_t &written)
bool URServer::write(const uint8_t* buf, size_t buf_len, size_t& written)
{
return client_.write(buf, buf_len, written);
}

10
src/ur/stream.cpp
View File

@@ -6,22 +6,22 @@
#include "ur_modern_driver/log.h"
#include "ur_modern_driver/ur/stream.h"
bool URStream::write(const uint8_t* buf, size_t buf_len, size_t &written)
bool URStream::write(const uint8_t* buf, size_t buf_len, size_t& written)
{
std::lock_guard<std::mutex> lock(write_mutex_);
return TCPSocket::write(buf, buf_len, written);
}
bool URStream::read(uint8_t* buf, size_t buf_len, size_t &total)
bool URStream::read(uint8_t* buf, size_t buf_len, size_t& total)
{
std::lock_guard<std::mutex> lock(read_mutex_);
std::lock_guard<std::mutex> lock(read_mutex_);
bool initial = true;
uint8_t* buf_pos = buf;
size_t remainder = sizeof(int32_t);
size_t read = 0;
while(remainder > 0 && TCPSocket::read(buf_pos, remainder, read))
while (remainder > 0 && TCPSocket::read(buf_pos, remainder, read))
{
TCPSocket::setOptions(getSocketFD());
if (initial)
@@ -39,6 +39,6 @@ bool URStream::read(uint8_t* buf, size_t buf_len, size_t &total)
buf_pos += read;
remainder -= read;
}
return remainder == 0;
}