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