From 167a25b32ec89458405d6673b651c27fd5cc7423 Mon Sep 17 00:00:00 2001 From: Vahid Aminzadeh Date: Tue, 20 Jun 2017 13:10:50 +0100 Subject: [PATCH 1/2] added the service for changing the velocity --- epos_hardware/CMakeLists.txt | 9 ++++- epos_hardware/include/epos_hardware/epos.h | 1 + .../include/epos_hardware/epos_hardware.h | 2 ++ .../src/nodes/epos_hardware_node.cpp | 2 ++ epos_hardware/src/util/epos.cpp | 34 +++++++++++++++++++ epos_hardware/src/util/epos_hardware.cpp | 28 +++++++++++++++ epos_hardware/srv/SetMaxProfileVelocity.srv | 4 +++ 7 files changed, 79 insertions(+), 1 deletion(-) create mode 100644 epos_hardware/srv/SetMaxProfileVelocity.srv diff --git a/epos_hardware/CMakeLists.txt b/epos_hardware/CMakeLists.txt index b2360b7..ea13173 100644 --- a/epos_hardware/CMakeLists.txt +++ b/epos_hardware/CMakeLists.txt @@ -8,8 +8,15 @@ find_package(catkin REQUIRED COMPONENTS controller_manager roscpp diagnostic_updater + message_generation +) +add_service_files( + FILES + SetMaxProfileVelocity.srv +) +generate_messages( + DEPENDENCIES ) - catkin_package( INCLUDE_DIRS include LIBRARIES epos_library_utils epos_manager epos_hardware diff --git a/epos_hardware/include/epos_hardware/epos.h b/epos_hardware/include/epos_hardware/epos.h index 7e29d52..748e976 100644 --- a/epos_hardware/include/epos_hardware/epos.h +++ b/epos_hardware/include/epos_hardware/epos.h @@ -41,6 +41,7 @@ class Epos { std::string name() { return name_; } std::string actuator_name() { return actuator_name_; } void update_diagnostics(); + bool update_position_profile_velocity(unsigned int new_velocity); private: ros::NodeHandle config_nh_; diagnostic_updater::Updater diagnostic_updater_; diff --git a/epos_hardware/include/epos_hardware/epos_hardware.h b/epos_hardware/include/epos_hardware/epos_hardware.h index 41b9b09..c08e40c 100644 --- a/epos_hardware/include/epos_hardware/epos_hardware.h +++ b/epos_hardware/include/epos_hardware/epos_hardware.h @@ -10,6 +10,7 @@ #include "epos_hardware/utils.h" #include "epos_hardware/epos.h" #include "epos_hardware/epos_manager.h" +#include "epos_hardware/SetMaxProfileVelocity.h" namespace epos_hardware { @@ -21,6 +22,7 @@ class EposHardware : public hardware_interface::RobotHW { void read(); void write(); void update_diagnostics(); + bool change_profile_velocity(epos_hardware::SetMaxProfileVelocityRequest &request, epos_hardware::SetMaxProfileVelocityResponse &response); private: hardware_interface::ActuatorStateInterface asi; hardware_interface::VelocityActuatorInterface avi; diff --git a/epos_hardware/src/nodes/epos_hardware_node.cpp b/epos_hardware/src/nodes/epos_hardware_node.cpp index 18f1ec0..9d50048 100644 --- a/epos_hardware/src/nodes/epos_hardware_node.cpp +++ b/epos_hardware/src/nodes/epos_hardware_node.cpp @@ -3,6 +3,7 @@ #include "epos_hardware/epos_hardware.h" #include #include +#include "epos_hardware/SetMaxProfileVelocity.h" int main(int argc, char** argv) { ros::init(argc, argv, "epos_velocity_hardware"); @@ -26,6 +27,7 @@ int main(int argc, char** argv) { } ROS_INFO("Motors Initialized"); + ros::ServiceServer change_profile_velocity_service = nh.advertiseService("change_profile_velocity", &epos_hardware::EposHardware::change_profile_velocity, &robot); ros::Rate controller_rate(50); ros::Time last = ros::Time::now(); while (ros::ok()) { diff --git a/epos_hardware/src/util/epos.cpp b/epos_hardware/src/util/epos.cpp index 26bce29..9bd5455 100644 --- a/epos_hardware/src/util/epos.cpp +++ b/epos_hardware/src/util/epos.cpp @@ -646,6 +646,40 @@ void Epos::buildMotorStatus(diagnostic_updater::DiagnosticStatusWrapper &stat) { } } +bool Epos::update_position_profile_velocity(unsigned int new_velocity) +{ + unsigned int error_code; + ROS_INFO("Updating Position Profile"); + ros::NodeHandle position_profile_nh(config_nh_, "position_profile"); + { + bool position_profile; + int velocity, acceleration, deceleration; + if(!ParameterSetLoader(position_profile_nh) + .param("acceleration", acceleration) + .param("deceleration", deceleration) + .all_or_none(position_profile)) + { + return false; + } + if (new_velocity > max_profile_velocity_) + { + ROS_WARN_STREAM("New velocity value: " << new_velocity << " is higher than max velocity value: " << max_profile_velocity_); + ROS_WARN_STREAM("For safety, velocity is not updated"); + return false; + } + if(position_profile){ + VCS(SetPositionProfile, new_velocity, acceleration, deceleration); + } + else + { + ROS_WARN_STREAM("Attempted to change profile velocity of a non position profile motor"); + return false; + } + } + return true; +} + + void Epos::buildMotorOutputStatus(diagnostic_updater::DiagnosticStatusWrapper &stat) { std::string operation_mode_str; if(operation_mode_ == PROFILE_POSITION_MODE) { diff --git a/epos_hardware/src/util/epos_hardware.cpp b/epos_hardware/src/util/epos_hardware.cpp index 08e93ac..94f51e4 100644 --- a/epos_hardware/src/util/epos_hardware.cpp +++ b/epos_hardware/src/util/epos_hardware.cpp @@ -97,4 +97,32 @@ void EposHardware::write() { epos_manager_.write(); } +bool EposHardware::change_profile_velocity(epos_hardware::SetMaxProfileVelocityRequest &request, epos_hardware::SetMaxProfileVelocityResponse &response) +{ + bool found = false; + epos_manager_.motors(); + boost::shared_ptr found_epos; + BOOST_FOREACH(const boost::shared_ptr& epos, epos_manager_.motors()) + { + epos->name(); + if (epos->name() == request.motor_name) + { + found_epos = epos; + found = true; + break; + } + } + if (!found) + { + ROS_WARN_STREAM("Could not find motor name " << request.motor_name << std::endl); + response.success = false; + return false; + } + if (found_epos->update_position_profile_velocity(request.max_profile_velocity)) + { + response.success = true; + return true; + } + return false; +} } diff --git a/epos_hardware/srv/SetMaxProfileVelocity.srv b/epos_hardware/srv/SetMaxProfileVelocity.srv new file mode 100644 index 0000000..99eae4b --- /dev/null +++ b/epos_hardware/srv/SetMaxProfileVelocity.srv @@ -0,0 +1,4 @@ +string motor_name +uint64 max_profile_velocity +--- +bool success From 7d021f774d91aa9426aa9cce38b8ff2eb652f658 Mon Sep 17 00:00:00 2001 From: Vahid Aminzadeh Date: Wed, 21 Jun 2017 15:05:22 +0100 Subject: [PATCH 2/2] fixed catkin lint errors --- epos_hardware/CMakeLists.txt | 3 ++- epos_hardware/package.xml | 3 +++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/epos_hardware/CMakeLists.txt b/epos_hardware/CMakeLists.txt index ea13173..337a8a4 100644 --- a/epos_hardware/CMakeLists.txt +++ b/epos_hardware/CMakeLists.txt @@ -9,6 +9,7 @@ find_package(catkin REQUIRED COMPONENTS roscpp diagnostic_updater message_generation + message_runtime ) add_service_files( FILES @@ -20,7 +21,7 @@ generate_messages( catkin_package( INCLUDE_DIRS include LIBRARIES epos_library_utils epos_manager epos_hardware - CATKIN_DEPENDS epos_library hardware_interface roscpp controller_manager diagnostic_updater + CATKIN_DEPENDS epos_library hardware_interface roscpp controller_manager diagnostic_updater message_runtime ) ########### diff --git a/epos_hardware/package.xml b/epos_hardware/package.xml index 5c04c55..6e09da0 100644 --- a/epos_hardware/package.xml +++ b/epos_hardware/package.xml @@ -13,12 +13,15 @@ Mitchell Wills catkin + message_runtime + message_generation epos_library hardware_interface transmission_interface controller_manager roscpp diagnostic_updater + message_runtime epos_library hardware_interface transmission_interface