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

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

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);