1
0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-10 18:10:47 +02:00

fix naming conventions

This commit is contained in:
Felix Mauch
2019-06-06 10:45:05 +02:00
committed by Tristan Schnell
parent 9788504f93
commit e250128d7e
7 changed files with 22 additions and 22 deletions

View File

@@ -58,7 +58,7 @@ private:
std::thread script_thread_; std::thread script_thread_;
std::string program_; std::string program_;
const std::string PROGRAM_REQUEST = std::string("request_program\n"); const std::string PROGRAM_REQUEST_ = std::string("request_program\n");
void runScriptSender() void runScriptSender()
{ {
@@ -86,7 +86,7 @@ private:
if (read_successful) if (read_successful)
{ {
if (std::string(buffer) == PROGRAM_REQUEST) if (std::string(buffer) == PROGRAM_REQUEST_)
{ {
return true; return true;
} }

View File

@@ -70,7 +70,7 @@ public:
RobotStateType type; RobotStateType type;
sbp.parse(type); sbp.parse(type);
std::unique_ptr<PrimaryPackage> packet(state_from_type(type)); std::unique_ptr<PrimaryPackage> packet(stateFromType(type));
if (packet == nullptr) if (packet == nullptr)
{ {
@@ -109,7 +109,7 @@ public:
bp.parse(source); bp.parse(source);
bp.parse(message_type); bp.parse(message_type);
std::unique_ptr<PrimaryPackage> packet(message_from_type(message_type, timestamp, source)); std::unique_ptr<PrimaryPackage> packet(messageFromType(message_type, timestamp, source));
if (!packet->parseWith(bp)) if (!packet->parseWith(bp))
{ {
LOG_ERROR("Package parsing of type %d failed!", static_cast<int>(message_type)); LOG_ERROR("Package parsing of type %d failed!", static_cast<int>(message_type));
@@ -132,7 +132,7 @@ public:
} }
private: private:
RobotState* state_from_type(RobotStateType type) RobotState* stateFromType(RobotStateType type)
{ {
switch (type) switch (type)
{ {
@@ -149,7 +149,7 @@ private:
} }
} }
RobotMessage* message_from_type(RobotMessagePackageType type, uint64_t timestamp, uint8_t source) RobotMessage* messageFromType(RobotMessagePackageType type, uint64_t timestamp, uint8_t source)
{ {
switch (type) switch (type)
{ {

View File

@@ -109,7 +109,7 @@ public:
private: private:
// Const would be better here // Const would be better here
static std::unordered_map<std::string, _rtde_type_variant> type_list_; static std::unordered_map<std::string, _rtde_type_variant> g_type_list;
uint8_t recipe_id_; uint8_t recipe_id_;
std::unordered_map<std::string, _rtde_type_variant> data_; std::unordered_map<std::string, _rtde_type_variant> data_;
std::vector<std::string> recipe_; std::vector<std::string> recipe_;

View File

@@ -75,7 +75,7 @@ public:
} }
default: default:
{ {
std::unique_ptr<RTDEPackage> package(package_from_type(type)); std::unique_ptr<RTDEPackage> package(packageFromType(type));
if (!package->parseWith(bp)) if (!package->parseWith(bp))
{ {
LOG_ERROR("Package parsing of type %d failed!", static_cast<int>(type)); LOG_ERROR("Package parsing of type %d failed!", static_cast<int>(type));
@@ -98,7 +98,7 @@ public:
private: private:
std::vector<std::string> recipe_; std::vector<std::string> recipe_;
RTDEPackage* package_from_type(PackageType type) RTDEPackage* packageFromType(PackageType type)
{ {
switch (type) switch (type)
{ {

View File

@@ -30,13 +30,13 @@
#include <csignal> #include <csignal>
#include <ur_rtde_driver/ros/hardware_interface.h> #include <ur_rtde_driver/ros/hardware_interface.h>
std::unique_ptr<ur_driver::HardwareInterface> hw_interface; std::unique_ptr<ur_driver::HardwareInterface> g_hw_interface;
void signalHandler(int signum) void signalHandler(int signum)
{ {
std::cout << "Interrupt signal (" << signum << ") received.\n"; std::cout << "Interrupt signal (" << signum << ") received.\n";
hw_interface.reset(); g_hw_interface.reset();
// cleanup and close up stuff here // cleanup and close up stuff here
// terminate program // terminate program
@@ -62,15 +62,15 @@ int main(int argc, char** argv)
auto stopwatch_last = std::chrono::steady_clock::now(); auto stopwatch_last = std::chrono::steady_clock::now();
auto stopwatch_now = stopwatch_last; auto stopwatch_now = stopwatch_last;
hw_interface.reset(new ur_driver::HardwareInterface); g_hw_interface.reset(new ur_driver::HardwareInterface);
if (!hw_interface->init(nh, nh_priv)) if (!g_hw_interface->init(nh, nh_priv))
{ {
ROS_ERROR_STREAM("Could not correctly initialize robot. Exiting"); ROS_ERROR_STREAM("Could not correctly initialize robot. Exiting");
exit(1); exit(1);
} }
ROS_INFO_STREAM("initialized hw interface"); ROS_INFO_STREAM("initialized hw interface");
controller_manager::ControllerManager cm(hw_interface.get(), nh); controller_manager::ControllerManager cm(g_hw_interface.get(), nh);
ROS_INFO_STREAM("started controller manager"); ROS_INFO_STREAM("started controller manager");
// Get current time and elapsed time since last read // Get current time and elapsed time since last read
@@ -128,13 +128,13 @@ int main(int argc, char** argv)
} }
} }
double expected_cycle_time = 1.0 / (static_cast<double>(hw_interface->getControlFrequency())); double expected_cycle_time = 1.0 / (static_cast<double>(g_hw_interface->getControlFrequency()));
// Run as fast as possible // Run as fast as possible
while (ros::ok()) while (ros::ok())
{ {
// Receive current state from robot // Receive current state from robot
hw_interface->read(timestamp, period); g_hw_interface->read(timestamp, period);
// Get current time and elapsed time since last read // Get current time and elapsed time since last read
timestamp = ros::Time::now(); timestamp = ros::Time::now();
@@ -144,7 +144,7 @@ int main(int argc, char** argv)
cm.update(timestamp, period); cm.update(timestamp, period);
hw_interface->write(timestamp, period); g_hw_interface->write(timestamp, period);
// if (!control_rate.sleep()) // if (!control_rate.sleep())
if (period.toSec() > expected_cycle_time) if (period.toSec() > expected_cycle_time)
{ {

View File

@@ -30,7 +30,7 @@ namespace ur_driver
{ {
namespace rtde_interface namespace rtde_interface
{ {
std::unordered_map<std::string, DataPackage::_rtde_type_variant> DataPackage::type_list_{ std::unordered_map<std::string, DataPackage::_rtde_type_variant> DataPackage::g_type_list{
{ "timestamp", double() }, { "timestamp", double() },
{ "target_q", vector6d_t() }, { "target_q", vector6d_t() },
{ "target_qd", vector6d_t() }, { "target_qd", vector6d_t() },
@@ -193,9 +193,9 @@ bool rtde_interface::DataPackage ::parseWith(comm::BinParser& bp)
bp.parse(recipe_id_); bp.parse(recipe_id_);
for (auto& item : recipe_) for (auto& item : recipe_)
{ {
if (type_list_.find(item) != type_list_.end()) if (g_type_list.find(item) != g_type_list.end())
{ {
_rtde_type_variant entry = type_list_[item]; _rtde_type_variant entry = g_type_list[item];
auto bound_visitor = std::bind(ParseVisitor(), std::placeholders::_1, bp); auto bound_visitor = std::bind(ParseVisitor(), std::placeholders::_1, bp);
boost::apply_visitor(bound_visitor, entry); boost::apply_visitor(bound_visitor, entry);
data_[item] = entry; data_[item] = entry;

View File

@@ -36,7 +36,7 @@
namespace ur_driver namespace ur_driver
{ {
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}}");
static const std::string SERVO_J_REPLACE("{{SERVO_J_REPLACE}}"); static const std::string SERVO_J_REPLACE("{{SERVO_J_REPLACE}}");
static const std::string SERVER_IP_REPLACE("{{SERVER_IP_REPLACE}}"); static const std::string SERVER_IP_REPLACE("{{SERVER_IP_REPLACE}}");
@@ -67,7 +67,7 @@ ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& sc
uint32_t script_sender_port = 50002; // TODO: Make this a parameter uint32_t script_sender_port = 50002; // TODO: Make this a parameter
std::string prog = readScriptFile(script_file); std::string prog = readScriptFile(script_file);
prog.replace(prog.find(JOINT_STATE_REPLACE), JOINT_STATE_REPLACE.length(), std::to_string(MULT_JOINTSTATE_)); prog.replace(prog.find(JOINT_STATE_REPLACE), JOINT_STATE_REPLACE.length(), std::to_string(MULT_JOINTSTATE));
std::ostringstream out; std::ostringstream out;
out << "lookahead_time=" << servoj_lookahead_time_ << ", gain=" << servoj_gain_; out << "lookahead_time=" << servoj_lookahead_time_ << ", gain=" << servoj_gain_;
prog.replace(prog.find(SERVO_J_REPLACE), SERVO_J_REPLACE.length(), out.str()); prog.replace(prog.find(SERVO_J_REPLACE), SERVO_J_REPLACE.length(), out.str());