From 658670b642b86d694d99e98b0317c4ae72c86bf7 Mon Sep 17 00:00:00 2001 From: Giuseppe Barbieri Date: Fri, 19 May 2017 10:41:48 +0000 Subject: [PATCH 1/9] Add motor homing --- epos_hardware/CMakeLists.txt | 12 +++ epos_hardware/include/epos_hardware/epos.h | 4 +- .../include/epos_hardware/epos_hardware.h | 1 + .../include/epos_hardware/epos_manager.h | 1 + epos_hardware/package.xml | 2 + .../src/nodes/epos_hardware_node.cpp | 16 +++- epos_hardware/src/util/epos.cpp | 85 +++++++++++++++++-- epos_hardware/src/util/epos_hardware.cpp | 3 + epos_hardware/src/util/epos_manager.cpp | 11 +++ 9 files changed, 126 insertions(+), 9 deletions(-) diff --git a/epos_hardware/CMakeLists.txt b/epos_hardware/CMakeLists.txt index b2360b7..6dcfc79 100644 --- a/epos_hardware/CMakeLists.txt +++ b/epos_hardware/CMakeLists.txt @@ -8,6 +8,18 @@ find_package(catkin REQUIRED COMPONENTS controller_manager roscpp diagnostic_updater + std_msgs + message_generation +) + +add_service_files( + FILES + StopHoming.srv +) + +generate_messages( + DEPENDENCIES + std_msgs ) catkin_package( diff --git a/epos_hardware/include/epos_hardware/epos.h b/epos_hardware/include/epos_hardware/epos.h index 7e29d52..2b7e256 100644 --- a/epos_hardware/include/epos_hardware/epos.h +++ b/epos_hardware/include/epos_hardware/epos.h @@ -25,7 +25,8 @@ class Epos { public: typedef enum { PROFILE_POSITION_MODE = 1, - PROFILE_VELOCITY_MODE = 3 + PROFILE_VELOCITY_MODE = 3, + HOMING_MODE = 6 } OperationMode; Epos(const std::string& name, @@ -38,6 +39,7 @@ class Epos { bool init(); void read(); void write(); + bool stop_homing(); std::string name() { return name_; } std::string actuator_name() { return actuator_name_; } void update_diagnostics(); diff --git a/epos_hardware/include/epos_hardware/epos_hardware.h b/epos_hardware/include/epos_hardware/epos_hardware.h index 6a91dc3..3b6cd6b 100644 --- a/epos_hardware/include/epos_hardware/epos_hardware.h +++ b/epos_hardware/include/epos_hardware/epos_hardware.h @@ -22,6 +22,7 @@ class EposHardware : public hardware_interface::RobotHW { bool init(); void read(); void write(); + bool stop_homing(); void update_diagnostics(); private: hardware_interface::ActuatorStateInterface asi; diff --git a/epos_hardware/include/epos_hardware/epos_manager.h b/epos_hardware/include/epos_hardware/epos_manager.h index f402fa1..aff1ee0 100644 --- a/epos_hardware/include/epos_hardware/epos_manager.h +++ b/epos_hardware/include/epos_hardware/epos_manager.h @@ -24,6 +24,7 @@ class EposManager { bool init(); void read(); void write(); + bool stop_homing(); void update_diagnostics(); std::vector > motors() { return motors_; }; private: diff --git a/epos_hardware/package.xml b/epos_hardware/package.xml index 5c04c55..646e85a 100644 --- a/epos_hardware/package.xml +++ b/epos_hardware/package.xml @@ -19,12 +19,14 @@ controller_manager roscpp diagnostic_updater + message_generation epos_library hardware_interface transmission_interface controller_manager roscpp diagnostic_updater + message_runtime diff --git a/epos_hardware/src/nodes/epos_hardware_node.cpp b/epos_hardware/src/nodes/epos_hardware_node.cpp index 18f1ec0..73bcdb6 100644 --- a/epos_hardware/src/nodes/epos_hardware_node.cpp +++ b/epos_hardware/src/nodes/epos_hardware_node.cpp @@ -3,6 +3,15 @@ #include "epos_hardware/epos_hardware.h" #include #include +#include "epos_hardware/StopHoming.h" + +bool stopHoming(epos_hardware::StopHoming::Request &req, + epos_hardware::StopHoming::Response &res, epos_hardware::EposHardware* robot) +{ + res.stopped = robot->stop_homing(); + if(res.stopped == true) + return true; +} int main(int argc, char** argv) { ros::init(argc, argv, "epos_velocity_hardware"); @@ -16,9 +25,14 @@ int main(int argc, char** argv) { epos_hardware::EposHardware robot(nh, pnh, motor_names); controller_manager::ControllerManager cm(&robot, nh); - ros::AsyncSpinner spinner(1); + ros::AsyncSpinner spinner(2); spinner.start(); + boost::function + stop_homing_cb = boost::bind(&stopHoming, _1, _2, &robot); + + ros::ServiceServer stop_motor_homing = nh.advertiseService("stop_motor_homing", stop_homing_cb); + ROS_INFO("Initializing Motors"); if(!robot.init()) { ROS_FATAL("Failed to initialize motors"); diff --git a/epos_hardware/src/util/epos.cpp b/epos_hardware/src/util/epos.cpp index 26bce29..3c8ce57 100644 --- a/epos_hardware/src/util/epos.cpp +++ b/epos_hardware/src/util/epos.cpp @@ -156,7 +156,7 @@ bool Epos::init() { return false; } - VCS(SetOperationMode, operation_mode_); + //VCS(SetOperationMode, operation_mode_); std::string fault_reaction_str; #define SET_FAULT_REACTION_OPTION(val) \ @@ -465,7 +465,70 @@ bool Epos::init() { } } + ROS_INFO_STREAM("Enabling Motor"); + if(!VCS_SetEnableState(node_handle_->device_handle->ptr, node_handle_->node_id, &error_code)) + return false; + + { + ROS_INFO("Configuring homing mode"); + VCS(SetOperationMode, HOMING_MODE); + + ROS_INFO("Getting Homing Parameters"); + ros::NodeHandle homing_mode_nh(config_nh_, "homing"); + { + + bool homing; + int homing_method; + int timeout; + int homing_acceleration; + int speed_switch; + int speed_index; + int home_offset; + int current_threshold; + int home_position; + + if(!ParameterSetLoader(homing_mode_nh) + .param("homing_acceleration", homing_acceleration) + .param("speed_switch", speed_switch) + .param("speed_index", speed_index) + .param("home_offset", home_offset) + .param("current_threshold", current_threshold) + .param("home_position", home_position) + .param("homing_method", homing_method) + .param("timeout", timeout) + .all_or_none(homing)) + return false; + + int position_raw; + VCS_GetPositionIs(node_handle_->device_handle->ptr, node_handle_->node_id, &position_raw, &error_code); + std::cout << "position_raw: " << position_raw << std::endl; + if(homing == 1 && position_raw == 1){ + VCS(SetHomingParameter, + homing_acceleration, + speed_switch, + speed_index, + home_offset, + current_threshold, + home_position); + + ROS_INFO("Start Homing"); + if(!VCS_FindHome(node_handle_->device_handle->ptr, node_handle_->node_id, homing_method, &error_code)) + return false; + + ROS_INFO("Waiting for homing attained"); + if(!VCS_WaitForHomingAttained(node_handle_->device_handle->ptr, node_handle_->node_id, timeout, &error_code)){ + VCS_StopHoming(node_handle_->device_handle->ptr, node_handle_->node_id, &error_code); + return false; + } + } + else{ + ROS_INFO("No homing needed, proceeding with normal startup"); + } + } + } + + VCS(SetOperationMode, operation_mode_); ROS_INFO("Querying Faults"); unsigned char num_errors; @@ -510,13 +573,14 @@ bool Epos::init() { torque_constant_ = 1.0; } - ROS_INFO_STREAM("Enabling Motor"); - if(!VCS_SetEnableState(node_handle_->device_handle->ptr, node_handle_->node_id, &error_code)) - return false; + //ROS_INFO_STREAM("Enabling Motor"); + //if(!VCS_SetEnableState(node_handle_->device_handle->ptr, node_handle_->node_id, &error_code)) + // return false; - has_init_ = true; - return true; -} + + has_init_ = true; + return true; + } void Epos::read() { if(!has_init_) @@ -689,5 +753,12 @@ void Epos::buildMotorOutputStatus(diagnostic_updater::DiagnosticStatusWrapper &s } } +bool Epos::stop_homing(){ + unsigned int error_code; + if(!VCS_StopHoming(node_handle_->device_handle->ptr, node_handle_->node_id, &error_code)) + return false; + else + return true; } +} diff --git a/epos_hardware/src/util/epos_hardware.cpp b/epos_hardware/src/util/epos_hardware.cpp index 08e93ac..fad70d8 100644 --- a/epos_hardware/src/util/epos_hardware.cpp +++ b/epos_hardware/src/util/epos_hardware.cpp @@ -97,4 +97,7 @@ void EposHardware::write() { epos_manager_.write(); } +bool EposHardware::stop_homing() { + return epos_manager_.stop_homing(); +} } diff --git a/epos_hardware/src/util/epos_manager.cpp b/epos_hardware/src/util/epos_manager.cpp index d3c1a29..0e5e0e2 100644 --- a/epos_hardware/src/util/epos_manager.cpp +++ b/epos_hardware/src/util/epos_manager.cpp @@ -47,5 +47,16 @@ void EposManager::write() { } } +bool EposManager::stop_homing(){ + bool success = true; + BOOST_FOREACH(const boost::shared_ptr& motor, motors_) { + if(!motor->stop_homing()){ + ROS_ERROR_STREAM("Could not stop motor: " << motor->name()); + success = false; + } + } + return success; +} + } From 5553b69244ae9b329b08aaf60af78a6a18b6bf17 Mon Sep 17 00:00:00 2001 From: Giuseppe Barbieri Date: Fri, 19 May 2017 10:56:31 +0000 Subject: [PATCH 2/9] Remove comments --- epos_hardware/src/util/epos.cpp | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/epos_hardware/src/util/epos.cpp b/epos_hardware/src/util/epos.cpp index 3c8ce57..dbc0b7d 100644 --- a/epos_hardware/src/util/epos.cpp +++ b/epos_hardware/src/util/epos.cpp @@ -156,8 +156,6 @@ bool Epos::init() { return false; } - //VCS(SetOperationMode, operation_mode_); - std::string fault_reaction_str; #define SET_FAULT_REACTION_OPTION(val) \ do { \ @@ -502,7 +500,7 @@ bool Epos::init() { int position_raw; VCS_GetPositionIs(node_handle_->device_handle->ptr, node_handle_->node_id, &position_raw, &error_code); - std::cout << "position_raw: " << position_raw << std::endl; + if(homing == 1 && position_raw == 1){ VCS(SetHomingParameter, homing_acceleration, @@ -573,11 +571,6 @@ bool Epos::init() { torque_constant_ = 1.0; } - //ROS_INFO_STREAM("Enabling Motor"); - //if(!VCS_SetEnableState(node_handle_->device_handle->ptr, node_handle_->node_id, &error_code)) - // return false; - - has_init_ = true; return true; } From b2b32db9e8da06d5488b051af56b5af7067f8f97 Mon Sep 17 00:00:00 2001 From: Giuseppe Barbieri Date: Fri, 19 May 2017 11:01:31 +0000 Subject: [PATCH 3/9] Add srv --- epos_hardware/srv/StopHoming.srv | 3 +++ 1 file changed, 3 insertions(+) create mode 100644 epos_hardware/srv/StopHoming.srv diff --git a/epos_hardware/srv/StopHoming.srv b/epos_hardware/srv/StopHoming.srv new file mode 100644 index 0000000..50c3053 --- /dev/null +++ b/epos_hardware/srv/StopHoming.srv @@ -0,0 +1,3 @@ +bool stop +--- +bool stopped \ No newline at end of file From 7ebecb18d9f59d6783039a0c9d466c50acfe59c7 Mon Sep 17 00:00:00 2001 From: Giuseppe Barbieri Date: Fri, 19 May 2017 11:11:48 +0000 Subject: [PATCH 4/9] Tiny fixes --- epos_hardware/src/util/epos.cpp | 4 ++-- epos_hardware/srv/StopHoming.srv | 3 +-- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/epos_hardware/src/util/epos.cpp b/epos_hardware/src/util/epos.cpp index dbc0b7d..afc2a94 100644 --- a/epos_hardware/src/util/epos.cpp +++ b/epos_hardware/src/util/epos.cpp @@ -156,6 +156,7 @@ bool Epos::init() { return false; } + std::string fault_reaction_str; #define SET_FAULT_REACTION_OPTION(val) \ do { \ @@ -500,8 +501,7 @@ bool Epos::init() { int position_raw; VCS_GetPositionIs(node_handle_->device_handle->ptr, node_handle_->node_id, &position_raw, &error_code); - - if(homing == 1 && position_raw == 1){ + if(homing == 1 && position_raw == 0){ VCS(SetHomingParameter, homing_acceleration, speed_switch, diff --git a/epos_hardware/srv/StopHoming.srv b/epos_hardware/srv/StopHoming.srv index 50c3053..91a705f 100644 --- a/epos_hardware/srv/StopHoming.srv +++ b/epos_hardware/srv/StopHoming.srv @@ -1,3 +1,2 @@ -bool stop --- -bool stopped \ No newline at end of file +bool stopped From 1da6bb1025fdd2fad3aef572eeb3c05a2710b60d Mon Sep 17 00:00:00 2001 From: Giuseppe Barbieri Date: Sun, 21 May 2017 17:31:02 +0000 Subject: [PATCH 5/9] FIx --- epos_hardware/src/util/epos.cpp | 20 +++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/epos_hardware/src/util/epos.cpp b/epos_hardware/src/util/epos.cpp index afc2a94..10fa250 100644 --- a/epos_hardware/src/util/epos.cpp +++ b/epos_hardware/src/util/epos.cpp @@ -464,10 +464,6 @@ bool Epos::init() { } } - ROS_INFO_STREAM("Enabling Motor"); - if(!VCS_SetEnableState(node_handle_->device_handle->ptr, node_handle_->node_id, &error_code)) - return false; - { ROS_INFO("Configuring homing mode"); @@ -499,9 +495,15 @@ bool Epos::init() { .all_or_none(homing)) return false; - int position_raw; + int position_raw_init; VCS_GetPositionIs(node_handle_->device_handle->ptr, node_handle_->node_id, &position_raw, &error_code); - if(homing == 1 && position_raw == 0){ + + ROS_INFO_STREAM("Enabling Motor"); + if(!VCS_SetEnableState(node_handle_->device_handle->ptr, node_handle_->node_id, &error_code)) + return false; + + std::cout<<"current pos: "<< position_raw <device_handle->ptr, node_handle_->node_id, homing_method, &error_code)) + if(!VCS_FindHome(node_handle_->device_handle->ptr, node_handle_->node_id, homing_method, &error_code)){ + ROS_INFO("Found, Stop Homing"); + VCS_StopHoming(node_handle_->device_handle->ptr, node_handle_->node_id, &error_code); return false; + } + ROS_INFO("Waiting for homing attained"); if(!VCS_WaitForHomingAttained(node_handle_->device_handle->ptr, node_handle_->node_id, timeout, &error_code)){ From da2eca0dd5edf17eb354590838a5f31fcbb97f2e Mon Sep 17 00:00:00 2001 From: dg-shadow Date: Mon, 22 May 2017 13:26:54 +0100 Subject: [PATCH 6/9] Fix wrong variable --- epos_hardware/src/util/epos.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/epos_hardware/src/util/epos.cpp b/epos_hardware/src/util/epos.cpp index 10fa250..7447594 100644 --- a/epos_hardware/src/util/epos.cpp +++ b/epos_hardware/src/util/epos.cpp @@ -495,7 +495,7 @@ bool Epos::init() { .all_or_none(homing)) return false; - int position_raw_init; + int position_raw; VCS_GetPositionIs(node_handle_->device_handle->ptr, node_handle_->node_id, &position_raw, &error_code); ROS_INFO_STREAM("Enabling Motor"); @@ -503,7 +503,7 @@ bool Epos::init() { return false; std::cout<<"current pos: "<< position_raw < Date: Tue, 23 May 2017 10:32:45 +0100 Subject: [PATCH 7/9] Add start service --- epos_hardware/CMakeLists.txt | 1 + epos_hardware/include/epos_hardware/epos.h | 1 + .../include/epos_hardware/epos_hardware.h | 1 + .../include/epos_hardware/epos_manager.h | 1 + .../src/nodes/epos_hardware_node.cpp | 15 +- epos_hardware/src/util/epos.cpp | 139 +++++++++--------- epos_hardware/src/util/epos_hardware.cpp | 4 + epos_hardware/src/util/epos_manager.cpp | 12 +- epos_hardware/srv/StartHoming.srv | 2 + 9 files changed, 106 insertions(+), 70 deletions(-) create mode 100644 epos_hardware/srv/StartHoming.srv diff --git a/epos_hardware/CMakeLists.txt b/epos_hardware/CMakeLists.txt index 6dcfc79..12dbc89 100644 --- a/epos_hardware/CMakeLists.txt +++ b/epos_hardware/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(catkin REQUIRED COMPONENTS add_service_files( FILES StopHoming.srv + StartHoming.srv ) generate_messages( diff --git a/epos_hardware/include/epos_hardware/epos.h b/epos_hardware/include/epos_hardware/epos.h index 2b7e256..e346a7c 100644 --- a/epos_hardware/include/epos_hardware/epos.h +++ b/epos_hardware/include/epos_hardware/epos.h @@ -40,6 +40,7 @@ class Epos { void read(); void write(); bool stop_homing(); + bool start_homing(); std::string name() { return name_; } std::string actuator_name() { return actuator_name_; } void update_diagnostics(); diff --git a/epos_hardware/include/epos_hardware/epos_hardware.h b/epos_hardware/include/epos_hardware/epos_hardware.h index 3b6cd6b..70d3d90 100644 --- a/epos_hardware/include/epos_hardware/epos_hardware.h +++ b/epos_hardware/include/epos_hardware/epos_hardware.h @@ -23,6 +23,7 @@ class EposHardware : public hardware_interface::RobotHW { void read(); void write(); bool stop_homing(); + bool start_homing(); void update_diagnostics(); private: hardware_interface::ActuatorStateInterface asi; diff --git a/epos_hardware/include/epos_hardware/epos_manager.h b/epos_hardware/include/epos_hardware/epos_manager.h index aff1ee0..b90acac 100644 --- a/epos_hardware/include/epos_hardware/epos_manager.h +++ b/epos_hardware/include/epos_hardware/epos_manager.h @@ -25,6 +25,7 @@ class EposManager { void read(); void write(); bool stop_homing(); + bool start_homing(); void update_diagnostics(); std::vector > motors() { return motors_; }; private: diff --git a/epos_hardware/src/nodes/epos_hardware_node.cpp b/epos_hardware/src/nodes/epos_hardware_node.cpp index 73bcdb6..8d26822 100644 --- a/epos_hardware/src/nodes/epos_hardware_node.cpp +++ b/epos_hardware/src/nodes/epos_hardware_node.cpp @@ -4,6 +4,7 @@ #include #include #include "epos_hardware/StopHoming.h" +#include "epos_hardware/StartHoming.h" bool stopHoming(epos_hardware::StopHoming::Request &req, epos_hardware::StopHoming::Response &res, epos_hardware::EposHardware* robot) @@ -13,6 +14,15 @@ bool stopHoming(epos_hardware::StopHoming::Request &req, return true; } +bool startHoming(epos_hardware::StartHoming::Request &req, + epos_hardware::StartHoming::Response &res, epos_hardware::EposHardware* robot) +{ + res.started = robot->start_homing(); + if(res.started == true) + return true; + +} + int main(int argc, char** argv) { ros::init(argc, argv, "epos_velocity_hardware"); ros::NodeHandle nh; @@ -25,13 +35,16 @@ int main(int argc, char** argv) { epos_hardware::EposHardware robot(nh, pnh, motor_names); controller_manager::ControllerManager cm(&robot, nh); - ros::AsyncSpinner spinner(2); + ros::AsyncSpinner spinner(3); spinner.start(); boost::function stop_homing_cb = boost::bind(&stopHoming, _1, _2, &robot); + boost::function + start_homing_cb = boost::bind(&startHoming, _1, _2, &robot); ros::ServiceServer stop_motor_homing = nh.advertiseService("stop_motor_homing", stop_homing_cb); + ros::ServiceServer start_motor_homing = nh.advertiseService("start_motor_homing", start_homing_cb); ROS_INFO("Initializing Motors"); if(!robot.init()) { diff --git a/epos_hardware/src/util/epos.cpp b/epos_hardware/src/util/epos.cpp index 7447594..243567d 100644 --- a/epos_hardware/src/util/epos.cpp +++ b/epos_hardware/src/util/epos.cpp @@ -464,74 +464,6 @@ bool Epos::init() { } } - { - ROS_INFO("Configuring homing mode"); - - VCS(SetOperationMode, HOMING_MODE); - - ROS_INFO("Getting Homing Parameters"); - ros::NodeHandle homing_mode_nh(config_nh_, "homing"); - { - - bool homing; - int homing_method; - int timeout; - int homing_acceleration; - int speed_switch; - int speed_index; - int home_offset; - int current_threshold; - int home_position; - - if(!ParameterSetLoader(homing_mode_nh) - .param("homing_acceleration", homing_acceleration) - .param("speed_switch", speed_switch) - .param("speed_index", speed_index) - .param("home_offset", home_offset) - .param("current_threshold", current_threshold) - .param("home_position", home_position) - .param("homing_method", homing_method) - .param("timeout", timeout) - .all_or_none(homing)) - return false; - - int position_raw; - VCS_GetPositionIs(node_handle_->device_handle->ptr, node_handle_->node_id, &position_raw, &error_code); - - ROS_INFO_STREAM("Enabling Motor"); - if(!VCS_SetEnableState(node_handle_->device_handle->ptr, node_handle_->node_id, &error_code)) - return false; - - std::cout<<"current pos: "<< position_raw <device_handle->ptr, node_handle_->node_id, homing_method, &error_code)){ - ROS_INFO("Found, Stop Homing"); - VCS_StopHoming(node_handle_->device_handle->ptr, node_handle_->node_id, &error_code); - return false; - } - - - ROS_INFO("Waiting for homing attained"); - if(!VCS_WaitForHomingAttained(node_handle_->device_handle->ptr, node_handle_->node_id, timeout, &error_code)){ - VCS_StopHoming(node_handle_->device_handle->ptr, node_handle_->node_id, &error_code); - return false; - } - } - else{ - ROS_INFO("No homing needed, proceeding with normal startup"); - } - } - } - VCS(SetOperationMode, operation_mode_); ROS_INFO("Querying Faults"); @@ -577,6 +509,10 @@ bool Epos::init() { torque_constant_ = 1.0; } + ROS_INFO_STREAM("Enabling Motor"); + if(!VCS_SetEnableState(node_handle_->device_handle->ptr, node_handle_->node_id, &error_code)) + return false; + has_init_ = true; return true; } @@ -759,5 +695,72 @@ bool Epos::stop_homing(){ else return true; +} + +bool Epos::start_homing(){ + unsigned int error_code; + ROS_INFO("Configuring homing mode"); + + VCS(SetOperationMode, HOMING_MODE); + + ROS_INFO("Getting Homing Parameters"); + ros::NodeHandle homing_mode_nh(config_nh_, "homing"); + { + bool homing; + int homing_method; + int timeout; + int homing_acceleration; + int speed_switch; + int speed_index; + int home_offset; + int current_threshold; + int home_position; + + if(!ParameterSetLoader(homing_mode_nh) + .param("homing_acceleration", homing_acceleration) + .param("speed_switch", speed_switch) + .param("speed_index", speed_index) + .param("home_offset", home_offset) + .param("current_threshold", current_threshold) + .param("home_position", home_position) + .param("homing_method", homing_method) + .param("timeout", timeout) + .all_or_none(homing)) + return false; + + int position_raw; + VCS_GetPositionIs(node_handle_->device_handle->ptr, node_handle_->node_id, &position_raw, &error_code); + + std::cout<<"current pos: "<< position_raw <device_handle->ptr, node_handle_->node_id, homing_method, &error_code)){ + ROS_INFO("Found, Stop Homing"); + VCS_StopHoming(node_handle_->device_handle->ptr, node_handle_->node_id, &error_code); + return false; + } + + ROS_INFO("Waiting for homing attained"); + if(!VCS_WaitForHomingAttained(node_handle_->device_handle->ptr, node_handle_->node_id, timeout, &error_code)){ + ROS_INFO("Timed out"); + VCS_StopHoming(node_handle_->device_handle->ptr, node_handle_->node_id, &error_code); + return false; + } + + } + else{ + ROS_INFO("No homing needed, proceeding with normal startup"); + VCS(SetOperationMode, PROFILE_POSITION_MODE); + } + } + } } diff --git a/epos_hardware/src/util/epos_hardware.cpp b/epos_hardware/src/util/epos_hardware.cpp index fad70d8..be240e8 100644 --- a/epos_hardware/src/util/epos_hardware.cpp +++ b/epos_hardware/src/util/epos_hardware.cpp @@ -100,4 +100,8 @@ void EposHardware::write() { bool EposHardware::stop_homing() { return epos_manager_.stop_homing(); } + +bool EposHardware::start_homing() { + return epos_manager_.start_homing(); +} } diff --git a/epos_hardware/src/util/epos_manager.cpp b/epos_hardware/src/util/epos_manager.cpp index 0e5e0e2..08b2f2f 100644 --- a/epos_hardware/src/util/epos_manager.cpp +++ b/epos_hardware/src/util/epos_manager.cpp @@ -51,12 +51,22 @@ bool EposManager::stop_homing(){ bool success = true; BOOST_FOREACH(const boost::shared_ptr& motor, motors_) { if(!motor->stop_homing()){ - ROS_ERROR_STREAM("Could not stop motor: " << motor->name()); + ROS_ERROR_STREAM("Could not stop homing motor: " << motor->name()); success = false; } } return success; } +bool EposManager::start_homing(){ + bool success = true; + BOOST_FOREACH(const boost::shared_ptr& motor, motors_) { + if(!motor->start_homing()){ + ROS_ERROR_STREAM("Could not start homing motor: " << motor->name()); + success = false; + } + } + return success; +} } diff --git a/epos_hardware/srv/StartHoming.srv b/epos_hardware/srv/StartHoming.srv new file mode 100644 index 0000000..1e75a78 --- /dev/null +++ b/epos_hardware/srv/StartHoming.srv @@ -0,0 +1,2 @@ +--- +bool started From 81a15b03247e80cbc6a139af9ec6cd14ab421a55 Mon Sep 17 00:00:00 2001 From: Giuseppe Barbieri Date: Wed, 24 May 2017 11:01:30 +0000 Subject: [PATCH 8/9] fix change of state --- epos_hardware/src/util/epos.cpp | 23 ++++++++++++++++------- 1 file changed, 16 insertions(+), 7 deletions(-) diff --git a/epos_hardware/src/util/epos.cpp b/epos_hardware/src/util/epos.cpp index 243567d..b7d9ac2 100644 --- a/epos_hardware/src/util/epos.cpp +++ b/epos_hardware/src/util/epos.cpp @@ -729,6 +729,9 @@ bool Epos::start_homing(){ return false; int position_raw; + int homing_attained; + int homing_error; + VCS_GetPositionIs(node_handle_->device_handle->ptr, node_handle_->node_id, &position_raw, &error_code); std::cout<<"current pos: "<< position_raw <device_handle->ptr, node_handle_->node_id, homing_method, &error_code)){ - ROS_INFO("Found, Stop Homing"); - VCS_StopHoming(node_handle_->device_handle->ptr, node_handle_->node_id, &error_code); return false; } ROS_INFO("Waiting for homing attained"); if(!VCS_WaitForHomingAttained(node_handle_->device_handle->ptr, node_handle_->node_id, timeout, &error_code)){ - ROS_INFO("Timed out"); - VCS_StopHoming(node_handle_->device_handle->ptr, node_handle_->node_id, &error_code); - return false; + VCS_GetHomingState(node_handle_->device_handle->ptr, node_handle_->node_id, &homing_attained, &homing_error, &error_code); + if(homing_attained == 1){ + ROS_INFO("Done, Stop Homing"); + VCS_StopHoming(node_handle_->device_handle->ptr, node_handle_->node_id, &error_code); + VCS(SetOperationMode, operation_mode_); + } + else{ + ROS_INFO("Timed_out"); + VCS_StopHoming(node_handle_->device_handle->ptr, node_handle_->node_id, &error_code); + ROS_INFO("Stop Homing"); + return false; + } } - } else{ ROS_INFO("No homing needed, proceeding with normal startup"); - VCS(SetOperationMode, PROFILE_POSITION_MODE); + VCS(SetOperationMode, operation_mode_); } } From bc96b4b6fa04c5dee4a3580a386b01b9d3b83627 Mon Sep 17 00:00:00 2001 From: giusebar Date: Thu, 8 Jun 2017 15:32:13 +0000 Subject: [PATCH 9/9] Implement services directly in the class --- .../include/epos_hardware/epos_hardware.h | 12 +++++++-- .../src/nodes/epos_hardware_node.cpp | 26 ------------------- epos_hardware/src/util/epos_hardware.cpp | 20 +++++++++++--- 3 files changed, 26 insertions(+), 32 deletions(-) diff --git a/epos_hardware/include/epos_hardware/epos_hardware.h b/epos_hardware/include/epos_hardware/epos_hardware.h index 70d3d90..6b749dd 100644 --- a/epos_hardware/include/epos_hardware/epos_hardware.h +++ b/epos_hardware/include/epos_hardware/epos_hardware.h @@ -12,6 +12,8 @@ #include "epos_hardware/utils.h" #include "epos_hardware/epos.h" #include "epos_hardware/epos_manager.h" +#include "epos_hardware/StopHoming.h" +#include "epos_hardware/StartHoming.h" namespace epos_hardware { @@ -22,8 +24,6 @@ class EposHardware : public hardware_interface::RobotHW { bool init(); void read(); void write(); - bool stop_homing(); - bool start_homing(); void update_diagnostics(); private: hardware_interface::ActuatorStateInterface asi; @@ -34,6 +34,14 @@ class EposHardware : public hardware_interface::RobotHW { transmission_interface::RobotTransmissions robot_transmissions; boost::scoped_ptr transmission_loader; + + bool stopHomingSrv(epos_hardware::StopHoming::Request &req, + epos_hardware::StopHoming::Response &res); + + bool startHomingSrv(epos_hardware::StartHoming::Request &req, + epos_hardware::StartHoming::Response &res); + + ros::ServiceServer stop_motor_homing, start_motor_homing; }; } diff --git a/epos_hardware/src/nodes/epos_hardware_node.cpp b/epos_hardware/src/nodes/epos_hardware_node.cpp index 8d26822..b1540e1 100644 --- a/epos_hardware/src/nodes/epos_hardware_node.cpp +++ b/epos_hardware/src/nodes/epos_hardware_node.cpp @@ -3,25 +3,7 @@ #include "epos_hardware/epos_hardware.h" #include #include -#include "epos_hardware/StopHoming.h" -#include "epos_hardware/StartHoming.h" -bool stopHoming(epos_hardware::StopHoming::Request &req, - epos_hardware::StopHoming::Response &res, epos_hardware::EposHardware* robot) -{ - res.stopped = robot->stop_homing(); - if(res.stopped == true) - return true; -} - -bool startHoming(epos_hardware::StartHoming::Request &req, - epos_hardware::StartHoming::Response &res, epos_hardware::EposHardware* robot) -{ - res.started = robot->start_homing(); - if(res.started == true) - return true; - -} int main(int argc, char** argv) { ros::init(argc, argv, "epos_velocity_hardware"); @@ -38,14 +20,6 @@ int main(int argc, char** argv) { ros::AsyncSpinner spinner(3); spinner.start(); - boost::function - stop_homing_cb = boost::bind(&stopHoming, _1, _2, &robot); - boost::function - start_homing_cb = boost::bind(&startHoming, _1, _2, &robot); - - ros::ServiceServer stop_motor_homing = nh.advertiseService("stop_motor_homing", stop_homing_cb); - ros::ServiceServer start_motor_homing = nh.advertiseService("start_motor_homing", start_homing_cb); - ROS_INFO("Initializing Motors"); if(!robot.init()) { ROS_FATAL("Failed to initialize motors"); diff --git a/epos_hardware/src/util/epos_hardware.cpp b/epos_hardware/src/util/epos_hardware.cpp index be240e8..94622be 100644 --- a/epos_hardware/src/util/epos_hardware.cpp +++ b/epos_hardware/src/util/epos_hardware.cpp @@ -73,6 +73,10 @@ EposHardware::EposHardware(ros::NodeHandle& nh, ros::NodeHandle& pnh, const std: } } + // Advertise services + stop_motor_homing = nh.advertiseService("stop_motor_homing", &EposHardware::stopHomingSrv, this); + start_motor_homing = nh.advertiseService("start_motor_homing", &EposHardware::startHomingSrv, this); + } bool EposHardware::init() { @@ -97,11 +101,19 @@ void EposHardware::write() { epos_manager_.write(); } -bool EposHardware::stop_homing() { - return epos_manager_.stop_homing(); +bool EposHardware::stopHomingSrv(epos_hardware::StopHoming::Request &req, + epos_hardware::StopHoming::Response &res) +{ + res.stopped = epos_manager_.stop_homing(); + if(res.stopped == true) + return true; } -bool EposHardware::start_homing() { - return epos_manager_.start_homing(); +bool EposHardware::startHomingSrv(epos_hardware::StartHoming::Request &req, + epos_hardware::StartHoming::Response &res) +{ + res.started = epos_manager_.start_homing(); + if(res.started == true) + return true; } }