diff --git a/epos_hardware/CMakeLists.txt b/epos_hardware/CMakeLists.txt index b2360b7..12dbc89 100644 --- a/epos_hardware/CMakeLists.txt +++ b/epos_hardware/CMakeLists.txt @@ -8,6 +8,19 @@ find_package(catkin REQUIRED COMPONENTS controller_manager roscpp diagnostic_updater + std_msgs + message_generation +) + +add_service_files( + FILES + StopHoming.srv + StartHoming.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..e346a7c 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,8 @@ class Epos { bool init(); 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 6a91dc3..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 { @@ -32,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/include/epos_hardware/epos_manager.h b/epos_hardware/include/epos_hardware/epos_manager.h index f402fa1..b90acac 100644 --- a/epos_hardware/include/epos_hardware/epos_manager.h +++ b/epos_hardware/include/epos_hardware/epos_manager.h @@ -24,6 +24,8 @@ class EposManager { bool init(); void read(); void write(); + bool stop_homing(); + bool start_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..b1540e1 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 + int main(int argc, char** argv) { ros::init(argc, argv, "epos_velocity_hardware"); ros::NodeHandle nh; @@ -16,7 +17,7 @@ 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(3); spinner.start(); ROS_INFO("Initializing Motors"); diff --git a/epos_hardware/src/util/epos.cpp b/epos_hardware/src/util/epos.cpp index 26bce29..b7d9ac2 100644 --- a/epos_hardware/src/util/epos.cpp +++ b/epos_hardware/src/util/epos.cpp @@ -156,7 +156,6 @@ bool Epos::init() { return false; } - VCS(SetOperationMode, operation_mode_); std::string fault_reaction_str; #define SET_FAULT_REACTION_OPTION(val) \ @@ -465,7 +464,7 @@ bool Epos::init() { } } - + VCS(SetOperationMode, operation_mode_); ROS_INFO("Querying Faults"); unsigned char num_errors; @@ -510,13 +509,13 @@ 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 +688,88 @@ 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; + +} + +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; + 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)){ + return false; + } + + ROS_INFO("Waiting for homing attained"); + if(!VCS_WaitForHomingAttained(node_handle_->device_handle->ptr, node_handle_->node_id, timeout, &error_code)){ + 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, operation_mode_); + } + } + +} } diff --git a/epos_hardware/src/util/epos_hardware.cpp b/epos_hardware/src/util/epos_hardware.cpp index 08e93ac..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,4 +101,19 @@ void EposHardware::write() { epos_manager_.write(); } +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::startHomingSrv(epos_hardware::StartHoming::Request &req, + epos_hardware::StartHoming::Response &res) +{ + res.started = epos_manager_.start_homing(); + if(res.started == true) + return true; +} } diff --git a/epos_hardware/src/util/epos_manager.cpp b/epos_hardware/src/util/epos_manager.cpp index d3c1a29..08b2f2f 100644 --- a/epos_hardware/src/util/epos_manager.cpp +++ b/epos_hardware/src/util/epos_manager.cpp @@ -47,5 +47,26 @@ 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 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 diff --git a/epos_hardware/srv/StopHoming.srv b/epos_hardware/srv/StopHoming.srv new file mode 100644 index 0000000..91a705f --- /dev/null +++ b/epos_hardware/srv/StopHoming.srv @@ -0,0 +1,2 @@ +--- +bool stopped