mirror of
https://gitlab.com/obbart/universal_robots_ros_driver.git
synced 2026-04-10 01:50:46 +02:00
Format sources with clang-format
This commit is contained in:
@@ -110,7 +110,6 @@ public:
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
template <typename T>
|
||||
class Pipeline
|
||||
{
|
||||
@@ -140,7 +139,7 @@ private:
|
||||
{
|
||||
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_)
|
||||
return;
|
||||
|
||||
LOG_DEBUG("Stopping pipeline! <%s>",name_.c_str());
|
||||
LOG_DEBUG("Stopping pipeline! <%s>", name_.c_str());
|
||||
|
||||
consumer_.stopConsumer();
|
||||
producer_.stopProducer();
|
||||
|
||||
@@ -28,5 +28,5 @@ public:
|
||||
virtual bool start() = 0;
|
||||
virtual bool execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt) = 0;
|
||||
virtual void stop() = 0;
|
||||
virtual ~ActionTrajectoryFollowerInterface() {};
|
||||
virtual ~ActionTrajectoryFollowerInterface(){};
|
||||
};
|
||||
|
||||
@@ -9,11 +9,11 @@
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
#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/server.h"
|
||||
#include "ur_modern_driver/ros/action_trajectory_follower_interface.h"
|
||||
|
||||
class LowBandwidthTrajectoryFollower: public ActionTrajectoryFollowerInterface
|
||||
class LowBandwidthTrajectoryFollower : public ActionTrajectoryFollowerInterface
|
||||
{
|
||||
private:
|
||||
std::atomic<bool> running_;
|
||||
@@ -21,14 +21,13 @@ private:
|
||||
URCommander &commander_;
|
||||
URServer server_;
|
||||
|
||||
double time_interval_, servoj_time_, servoj_time_waiting_, max_waiting_time_, \
|
||||
servoj_gain_, servoj_lookahead_time_, max_joint_difference_;
|
||||
double time_interval_, servoj_time_, servoj_time_waiting_, max_waiting_time_, servoj_gain_, servoj_lookahead_time_,
|
||||
max_joint_difference_;
|
||||
|
||||
std::string program_;
|
||||
|
||||
bool execute(const std::array<double, 6> &positions,
|
||||
const std::array<double, 6> &velocities,
|
||||
double sample_number, double time_in_seconds);
|
||||
bool execute(const std::array<double, 6> &positions, const std::array<double, 6> &velocities, double sample_number,
|
||||
double time_in_seconds);
|
||||
|
||||
public:
|
||||
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);
|
||||
void stop();
|
||||
|
||||
virtual ~LowBandwidthTrajectoryFollower() {};
|
||||
virtual ~LowBandwidthTrajectoryFollower(){};
|
||||
};
|
||||
|
||||
@@ -35,7 +35,8 @@ private:
|
||||
void publishRobotStatus(const RobotModeData_V3_0__1& data) const;
|
||||
|
||||
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))
|
||||
{
|
||||
}
|
||||
|
||||
@@ -9,9 +9,9 @@
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
#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/server.h"
|
||||
#include "ur_modern_driver/ros/action_trajectory_follower_interface.h"
|
||||
|
||||
class TrajectoryFollower : public ActionTrajectoryFollowerInterface
|
||||
{
|
||||
@@ -43,5 +43,5 @@ public:
|
||||
bool execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt);
|
||||
void stop();
|
||||
|
||||
virtual ~TrajectoryFollower() {};
|
||||
virtual ~TrajectoryFollower(){};
|
||||
};
|
||||
|
||||
@@ -8,10 +8,9 @@
|
||||
#include "ur_modern_driver/ros/service_stopper.h"
|
||||
#include "ur_modern_driver/ur/commander.h"
|
||||
|
||||
class URScriptHandler: public Service
|
||||
class URScriptHandler : public Service
|
||||
{
|
||||
private:
|
||||
|
||||
ros::NodeHandle nh_;
|
||||
URCommander &commander_;
|
||||
ros::Subscriber urscript_sub_;
|
||||
@@ -19,6 +18,6 @@ private:
|
||||
|
||||
public:
|
||||
URScriptHandler(URCommander &commander);
|
||||
void urscriptInterface(const std_msgs::String::ConstPtr& msg);
|
||||
void urscriptInterface(const std_msgs::String::ConstPtr &msg);
|
||||
void onRobotStateChange(RobotState state);
|
||||
};
|
||||
|
||||
@@ -1,11 +1,11 @@
|
||||
#pragma once
|
||||
#include <inttypes.h>
|
||||
#include <algorithm>
|
||||
#include <bitset>
|
||||
#include <climits>
|
||||
#include <cstddef>
|
||||
#include <functional>
|
||||
#include <random>
|
||||
#include <bitset>
|
||||
#include "ur_modern_driver/bin_parser.h"
|
||||
|
||||
class RandomDataTest
|
||||
@@ -52,7 +52,7 @@ public:
|
||||
template <typename T>
|
||||
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)
|
||||
|
||||
@@ -26,6 +26,6 @@ public:
|
||||
bool bind();
|
||||
bool accept();
|
||||
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);
|
||||
};
|
||||
|
||||
@@ -1,7 +1,8 @@
|
||||
#include "ur_modern_driver/ros/action_server.h"
|
||||
#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),
|
||||
boost::bind(&ActionServer::onCancel, this, _1), false)
|
||||
, joint_names_(joint_names)
|
||||
@@ -147,9 +148,10 @@ bool ActionServer::validateJoints(GoalHandle& gh, Result& res)
|
||||
res.error_code = Result::INVALID_JOINTS;
|
||||
res.error_string = "Invalid joint names for goal\n";
|
||||
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: ";
|
||||
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;
|
||||
}
|
||||
|
||||
@@ -187,7 +189,8 @@ bool ActionServer::validateTrajectory(GoalHandle& gh, Result& res)
|
||||
}
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -58,7 +58,7 @@ void ROSController::doSwitch(const std::list<hardware_interface::ControllerInfo>
|
||||
return;
|
||||
}
|
||||
|
||||
if(start_list.size() > 0)
|
||||
if (start_list.size() > 0)
|
||||
LOG_WARN("Failed to start interface!");
|
||||
}
|
||||
|
||||
|
||||
@@ -1,9 +1,9 @@
|
||||
#include "ur_modern_driver/ros/lowbandwidth_trajectory_follower.h"
|
||||
#include <endian.h>
|
||||
#include <cmath>
|
||||
#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 SERVOJ_TIME("{{SERVOJ_TIME}}");
|
||||
@@ -233,8 +233,8 @@ end
|
||||
|
||||
)";
|
||||
|
||||
LowBandwidthTrajectoryFollower::LowBandwidthTrajectoryFollower(URCommander &commander, std::string &reverse_ip, int reverse_port,
|
||||
bool version_3)
|
||||
LowBandwidthTrajectoryFollower::LowBandwidthTrajectoryFollower(URCommander &commander, std::string &reverse_ip,
|
||||
int reverse_port, bool version_3)
|
||||
: running_(false)
|
||||
, commander_(commander)
|
||||
, server_(reverse_port)
|
||||
@@ -256,7 +256,8 @@ LowBandwidthTrajectoryFollower::LowBandwidthTrajectoryFollower(URCommander &comm
|
||||
|
||||
std::string res(POSITION_PROGRAM);
|
||||
std::ostringstream out;
|
||||
if (!version_3) {
|
||||
if (!version_3)
|
||||
{
|
||||
LOG_ERROR("Low Bandwidth Trajectory Follower only works for interface version > 3");
|
||||
std::exit(-1);
|
||||
}
|
||||
@@ -307,8 +308,8 @@ bool LowBandwidthTrajectoryFollower::start()
|
||||
}
|
||||
|
||||
bool LowBandwidthTrajectoryFollower::execute(const std::array<double, 6> &positions,
|
||||
const std::array<double, 6> &velocities,
|
||||
double sample_number, double time_in_seconds)
|
||||
const std::array<double, 6> &velocities, double sample_number,
|
||||
double time_in_seconds)
|
||||
{
|
||||
if (!running_)
|
||||
return false;
|
||||
@@ -317,11 +318,11 @@ bool LowBandwidthTrajectoryFollower::execute(const std::array<double, 6> &positi
|
||||
|
||||
out << "(";
|
||||
out << sample_number << ",";
|
||||
for (auto const &pos: positions)
|
||||
for (auto const &pos : positions)
|
||||
{
|
||||
out << pos << ",";
|
||||
}
|
||||
for (auto const &vel: velocities)
|
||||
for (auto const &vel : velocities)
|
||||
{
|
||||
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
|
||||
const std::string tmp = out.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;
|
||||
LOG_DEBUG("Sending message %s", formatted_message);
|
||||
@@ -346,7 +347,7 @@ bool LowBandwidthTrajectoryFollower::execute(std::vector<TrajectoryPoint> &traje
|
||||
|
||||
bool finished = false;
|
||||
|
||||
char* line[MAX_SERVER_BUF_LEN];
|
||||
char *line[MAX_SERVER_BUF_LEN];
|
||||
|
||||
bool res = true;
|
||||
|
||||
@@ -358,13 +359,14 @@ bool LowBandwidthTrajectoryFollower::execute(std::vector<TrajectoryPoint> &traje
|
||||
finished = true;
|
||||
break;
|
||||
}
|
||||
unsigned int message_num=atoi((const char *) line);
|
||||
unsigned int message_num = atoi((const char *)line);
|
||||
LOG_DEBUG("Received request %i", message_num);
|
||||
if (message_num < trajectory.size())
|
||||
{
|
||||
res = execute(trajectory[message_num].positions, trajectory[message_num].velocities,
|
||||
message_num, trajectory[message_num].time_from_start.count() / 1e6);
|
||||
} else
|
||||
res = execute(trajectory[message_num].positions, trajectory[message_num].velocities, message_num,
|
||||
trajectory[message_num].time_from_start.count() / 1e6);
|
||||
}
|
||||
else
|
||||
{
|
||||
res = execute(EMPTY_VALUES, EMPTY_VALUES, message_num, 0.0);
|
||||
}
|
||||
|
||||
@@ -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
|
||||
{
|
||||
//note that this is true as soon as the drives are powered,
|
||||
//even if the breakes are still closed
|
||||
//which is in slight contrast to the comments in the
|
||||
//message definition
|
||||
// note that this is true as soon as the drives are powered,
|
||||
// even if the breakes are still closed
|
||||
// which is in slight contrast to the comments in the
|
||||
// message definition
|
||||
status.drives_powered.val = data.robot_power_on;
|
||||
|
||||
status.e_stopped.val = data.emergency_stopped;
|
||||
|
||||
//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
|
||||
//but it would also be true when another programm is running that might not
|
||||
//in fact contain any movement commands
|
||||
// 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
|
||||
// but it would also be true when another programm is running that might not
|
||||
// in fact contain any movement commands
|
||||
status.in_motion.val = industrial_msgs::TriState::UNKNOWN;
|
||||
|
||||
//the error code, if any, is not transmitted by this protocol
|
||||
//it can and should be fetched seperately
|
||||
// the error code, if any, is not transmitted by this protocol
|
||||
// it can and should be fetched seperately
|
||||
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_pub_.publish(status);
|
||||
@@ -53,9 +53,9 @@ void MBPublisher::publishRobotStatus(const RobotModeData_V1_X& data) const
|
||||
else
|
||||
msg.mode.val = industrial_msgs::RobotMode::AUTO;
|
||||
|
||||
//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)
|
||||
? industrial_msgs::TriState::ON : industrial_msgs::TriState::OFF;
|
||||
// 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) ? industrial_msgs::TriState::ON :
|
||||
industrial_msgs::TriState::OFF;
|
||||
|
||||
publishRobotStatus(msg, data);
|
||||
}
|
||||
@@ -64,8 +64,8 @@ void MBPublisher::publishRobotStatus(const RobotModeData_V3_0__1& data) const
|
||||
{
|
||||
industrial_msgs::RobotStatus msg;
|
||||
|
||||
msg.motion_possible.val = (data.robot_mode == robot_mode_V3_X::RUNNING)
|
||||
? industrial_msgs::TriState::ON : industrial_msgs::TriState::OFF;
|
||||
msg.motion_possible.val =
|
||||
(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)
|
||||
msg.mode.val = industrial_msgs::RobotMode::MANUAL;
|
||||
|
||||
@@ -20,7 +20,8 @@ ServiceStopper::ServiceStopper(std::vector<Service*> services)
|
||||
{
|
||||
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";
|
||||
}
|
||||
notify_all(RobotState::Running);
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
#include "ur_modern_driver/ros/trajectory_follower.h"
|
||||
#include <endian.h>
|
||||
#include <cmath>
|
||||
#include <ros/ros.h>
|
||||
#include <cmath>
|
||||
|
||||
static const int32_t MULT_JOINTSTATE_ = 1000000;
|
||||
static const std::string JOINT_STATE_REPLACE("{{JOINT_STATE_REPLACE}}");
|
||||
|
||||
@@ -1,9 +1,7 @@
|
||||
#include "ur_modern_driver/ros/urscript_handler.h"
|
||||
#include "ur_modern_driver/log.h"
|
||||
|
||||
URScriptHandler::URScriptHandler(URCommander &commander)
|
||||
: commander_(commander)
|
||||
, state_(RobotState::Error)
|
||||
URScriptHandler::URScriptHandler(URCommander& commander) : commander_(commander), state_(RobotState::Error)
|
||||
{
|
||||
LOG_INFO("Initializing ur_driver/URScript subscriber");
|
||||
urscript_sub_ = nh_.subscribe("ur_driver/URScript", 1, &URScriptHandler::urscriptInterface, this);
|
||||
|
||||
@@ -9,11 +9,11 @@
|
||||
#include "ur_modern_driver/ros/action_server.h"
|
||||
#include "ur_modern_driver/ros/controller.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/rt_publisher.h"
|
||||
#include "ur_modern_driver/ros/service_stopper.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/ur/commander.h"
|
||||
#include "ur_modern_driver/ur/factory.h"
|
||||
@@ -62,10 +62,12 @@ public:
|
||||
class IgnorePipelineStoppedNotifier : public INotifier
|
||||
{
|
||||
public:
|
||||
void started(std::string name){
|
||||
void started(std::string name)
|
||||
{
|
||||
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());
|
||||
}
|
||||
};
|
||||
@@ -73,17 +75,18 @@ public:
|
||||
class ShutdownOnPipelineStoppedNotifier : public INotifier
|
||||
{
|
||||
public:
|
||||
void started(std::string name){
|
||||
void started(std::string name)
|
||||
{
|
||||
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());
|
||||
ros::shutdown();
|
||||
exit(1);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
bool parse_args(ProgArgs &args)
|
||||
{
|
||||
if (!ros::param::get(IP_ADDR_ARG, args.host))
|
||||
@@ -121,9 +124,9 @@ int main(int argc, char **argv)
|
||||
return EXIT_FAILURE;
|
||||
}
|
||||
|
||||
//Add prefix to joint names
|
||||
std::transform (args.joint_names.begin(), args.joint_names.end(), args.joint_names.begin(),
|
||||
[&args](std::string name){return args.prefix + name;});
|
||||
// Add prefix to joint names
|
||||
std::transform(args.joint_names.begin(), args.joint_names.end(), args.joint_names.begin(),
|
||||
[&args](std::string name) { return args.prefix + name; });
|
||||
|
||||
std::string local_ip(getLocalIPAccessibleFromHost(args.host));
|
||||
|
||||
@@ -144,8 +147,8 @@ int main(int argc, char **argv)
|
||||
if (args.use_ros_control)
|
||||
{
|
||||
LOG_INFO("ROS control enabled");
|
||||
TrajectoryFollower *traj_follower = new TrajectoryFollower(
|
||||
*rt_commander, local_ip, args.reverse_port, factory.isVersion3());
|
||||
TrajectoryFollower *traj_follower =
|
||||
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);
|
||||
rt_vec.push_back(controller);
|
||||
services.push_back(controller);
|
||||
@@ -157,8 +160,8 @@ int main(int argc, char **argv)
|
||||
if (args.use_lowbandwidth_trajectory_follower)
|
||||
{
|
||||
LOG_INFO("Use low bandwidth trajectory follower");
|
||||
traj_follower = new LowBandwidthTrajectoryFollower(*rt_commander,
|
||||
local_ip, args.reverse_port,factory.isVersion3());
|
||||
traj_follower =
|
||||
new LowBandwidthTrajectoryFollower(*rt_commander, local_ip, args.reverse_port, factory.isVersion3());
|
||||
}
|
||||
else
|
||||
{
|
||||
|
||||
@@ -117,10 +117,9 @@ bool TCPSocket::read(char *character)
|
||||
// It's inefficient, but in our case we read very small messages
|
||||
// and the overhead connected with reading character by character is
|
||||
// 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)
|
||||
{
|
||||
read = 0;
|
||||
|
||||
@@ -22,7 +22,7 @@ void URCommander::formatArray(std::ostringstream &out, std::array<double, 6> &va
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
@@ -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)
|
||||
{
|
||||
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());
|
||||
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)
|
||||
{
|
||||
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());
|
||||
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)
|
||||
{
|
||||
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());
|
||||
return write(s);
|
||||
}
|
||||
@@ -116,7 +122,9 @@ bool URCommander_V3_X::setDigitalOut(uint8_t pin, bool value)
|
||||
else
|
||||
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());
|
||||
return write(s);
|
||||
}
|
||||
|
||||
@@ -31,8 +31,7 @@ std::string URServer::getIP()
|
||||
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;
|
||||
setsockopt(socket_fd, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(int));
|
||||
@@ -63,9 +62,10 @@ bool URServer::accept()
|
||||
int client_fd = -1;
|
||||
|
||||
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);
|
||||
if(retry++ >= 5)
|
||||
if (retry++ >= 5)
|
||||
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)
|
||||
{
|
||||
char *current_pointer = buffer;
|
||||
char* current_pointer = buffer;
|
||||
char ch;
|
||||
size_t total_read;
|
||||
|
||||
if (buf_len <= 0 || buffer == NULL) {
|
||||
if (buf_len <= 0 || buffer == NULL)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
total_read = 0;
|
||||
for (;;) {
|
||||
for (;;)
|
||||
{
|
||||
if (client_.read(&ch))
|
||||
{
|
||||
if (total_read < buf_len - 1) // just in case ...
|
||||
{
|
||||
total_read ++;
|
||||
total_read++;
|
||||
*current_pointer++ = ch;
|
||||
}
|
||||
if (ch == '\n')
|
||||
|
||||
@@ -9,7 +9,7 @@
|
||||
TEST(MasterBoardData_V1_X, testRandomDataParsing)
|
||||
{
|
||||
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();
|
||||
MasterBoardData_V1_X state;
|
||||
|
||||
@@ -43,7 +43,7 @@ TEST(MasterBoardData_V1_X, testRandomDataParsing)
|
||||
TEST(MasterBoardData_V3_0__1, testRandomDataParsing)
|
||||
{
|
||||
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();
|
||||
MasterBoardData_V3_0__1 state;
|
||||
ASSERT_TRUE(state.parseWith(bp)) << "parse() returned false";
|
||||
@@ -78,7 +78,7 @@ TEST(MasterBoardData_V3_0__1, testRandomDataParsing)
|
||||
TEST(MasterBoardData_V3_2, testRandomDataParsing)
|
||||
{
|
||||
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();
|
||||
MasterBoardData_V3_2 state;
|
||||
ASSERT_TRUE(state.parseWith(bp)) << "parse() returned false";
|
||||
|
||||
Reference in New Issue
Block a user