added the service for changing the velocity fixes #15

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
 )
 
 ###########

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_;

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;

epos_hardware/package.xml

@@ -13,12 +13,15 @@
   <author email="mwills@wpi.edu">Mitchell Wills</author>
 
   <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>message_runtime</build_depend>
+  <build_depend>message_generation</build_depend>
   <build_depend>epos_library</build_depend>
   <build_depend>hardware_interface</build_depend>
   <build_depend>transmission_interface</build_depend>
   <build_depend>controller_manager</build_depend>
   <build_depend>roscpp</build_depend>
   <build_depend>diagnostic_updater</build_depend>
+  <run_depend>message_runtime</run_depend>
   <run_depend>epos_library</run_depend>
   <run_depend>hardware_interface</run_depend>
   <run_depend>transmission_interface</run_depend>

epos_hardware/src/nodes/epos_hardware_node.cpp

@@ -3,6 +3,7 @@
 #include "epos_hardware/epos_hardware.h"
 #include <controller_manager/controller_manager.h>
 #include <vector>
+#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()) {

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) {

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<Epos> found_epos;
+  BOOST_FOREACH(const boost::shared_ptr<Epos>& 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;
+}
 }

epos_hardware/srv/SetMaxProfileVelocity.srv

@@ -0,0 +1,4 @@
+string motor_name
+uint64 max_profile_velocity
+---
+bool success