F add homing start service

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(

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();

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_interface::TransmissionInterfaceLoader> 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;
 };
 
 }

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<boost::shared_ptr<Epos> > motors() { return motors_; };
 private:

epos_hardware/package.xml

@@ -19,12 +19,14 @@
   <build_depend>controller_manager</build_depend>
   <build_depend>roscpp</build_depend>
   <build_depend>diagnostic_updater</build_depend>
+  <build_depend>message_generation</build_depend>
   <run_depend>epos_library</run_depend>
   <run_depend>hardware_interface</run_depend>
   <run_depend>transmission_interface</run_depend>
   <run_depend>controller_manager</run_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>diagnostic_updater</run_depend>
+  <run_depend>message_runtime</run_depend>
 
   <export>
   </export>

epos_hardware/src/nodes/epos_hardware_node.cpp

@@ -4,6 +4,7 @@
 #include <controller_manager/controller_manager.h>
 #include <vector>
 
+
 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");

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 <<std::endl;
+        if(homing == 1){
+            VCS(SetHomingParameter,
+	            homing_acceleration,
+	            speed_switch,
+	            speed_index,
+	            home_offset,
+	            current_threshold,
+	            home_position);
+
+            ROS_INFO("Start Homing");
+            if(!VCS_FindHome(node_handle_->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_);
+        }
+    }
+
+}
 }

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;
+}
 }

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<Epos>& 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<Epos>& motor, motors_) {
+        if(!motor->start_homing()){
+          ROS_ERROR_STREAM("Could not start homing motor: " << motor->name());
+          success = false;
+        }
+    }
+    return success;
+}
 
 }

epos_hardware/srv/StartHoming.srv

@@ -0,0 +1,2 @@
+---
+bool started

epos_hardware/srv/StopHoming.srv

@@ -0,0 +1,2 @@
+---
+bool stopped