diff --git a/epos_hardware/CMakeLists.txt b/epos_hardware/CMakeLists.txt index b2360b7..337a8a4 100644 --- a/epos_hardware/CMakeLists.txt +++ b/epos_hardware/CMakeLists.txt @@ -8,12 +8,20 @@ find_package(catkin REQUIRED COMPONENTS controller_manager roscpp diagnostic_updater + message_generation + message_runtime +) +add_service_files( + FILES + SetMaxProfileVelocity.srv +) +generate_messages( + DEPENDENCIES ) - 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/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/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 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