From bab5f3aaa7b2be78f5a4b6afddfe1b810016ed0a Mon Sep 17 00:00:00 2001
From: JimmyDaSilva <jimmy.dasilva@isir.upmc.fr>
Date: Mon, 4 Jan 2016 11:49:20 +0100
Subject: [PATCH] Adding option to choose RS232 communication

---
 epos_hardware/include/epos_hardware/epos.h |  1 +
 epos_hardware/launch/example.yaml          |  1 +
 epos_hardware/src/util/epos.cpp            | 43 +++++++++++++++++-----
 3 files changed, 36 insertions(+), 9 deletions(-)

diff --git a/epos_hardware/include/epos_hardware/epos.h b/epos_hardware/include/epos_hardware/epos.h
index 7e29d52..3847f03 100644
--- a/epos_hardware/include/epos_hardware/epos.h
+++ b/epos_hardware/include/epos_hardware/epos.h
@@ -47,6 +47,7 @@ class Epos {
   EposFactory* epos_factory_;
   std::string name_;
   std::string actuator_name_;
+  std::string comm_protocol_;
   uint64_t serial_number_;
   OperationMode operation_mode_;
   NodeHandlePtr node_handle_;
diff --git a/epos_hardware/launch/example.yaml b/epos_hardware/launch/example.yaml
index 07df44a..f73f83a 100644
--- a/epos_hardware/launch/example.yaml
+++ b/epos_hardware/launch/example.yaml
@@ -4,6 +4,7 @@
 my_joint_actuator:
   actuator_name: 'test_joint_actuator'
   serial_number: '662080000026'
+  communication_protocol: 'RS232' # or USB
   operation_mode: 'profile_velocity'
   clear_faults: true
 
diff --git a/epos_hardware/src/util/epos.cpp b/epos_hardware/src/util/epos.cpp
index 26bce29..b60c268 100644
--- a/epos_hardware/src/util/epos.cpp
+++ b/epos_hardware/src/util/epos.cpp
@@ -20,6 +20,11 @@ Epos::Epos(const std::string& name,
     valid_ = false;
   }
 
+  if(!config_nh_.getParam("communication_protocol", comm_protocol_)) {
+    ROS_WARN("You haven't specified which communication protocol you want to use\nWill be USB by default");
+    valid_ = false;
+  }
+
   std::string serial_number_str;
   if(!config_nh_.getParam("serial_number", serial_number_str)) {
     ROS_ERROR("You must specify a serial number");
@@ -139,16 +144,36 @@ bool Epos::init() {
 
   ROS_INFO_STREAM("Initializing: 0x" << std::hex << serial_number_);
   unsigned int error_code;
-  node_handle_ = epos_factory_->CreateNodeHandle("EPOS2", "MAXON SERIAL V2", "USB", serial_number_, &error_code);
-  if(!node_handle_) {
-    ROS_ERROR("Could not find motor");
-    return false;
+  
+  if((comm_protocol_ == "USB") || (comm_protocol_ == "")){
+    node_handle_ = epos_factory_->CreateNodeHandle("EPOS2", "MAXON SERIAL V2", "USB", serial_number_, &error_code);
+    if(!node_handle_) {
+      ROS_ERROR("Could not find motor");
+      return false;
+    }
+    ROS_INFO_STREAM("Found Motor");
+    if(!VCS_SetProtocolStackSettings(node_handle_->device_handle->ptr, 1000000, 500, &error_code)) {
+      ROS_ERROR("Failed to SetProtocolStackSettings");
+      return false;
+    }
   }
-  ROS_INFO_STREAM("Found Motor");
-
-  if(!VCS_SetProtocolStackSettings(node_handle_->device_handle->ptr, 1000000, 500, &error_code)) {
-    ROS_ERROR("Failed to SetProtocolStackSettings");
-    return false;
+  else{
+    if(comm_protocol_ == "RS232"){
+      node_handle_ = epos_factory_->CreateNodeHandle("EPOS2", "MAXON_RS232", "RS232", serial_number_, &error_code);
+      if(!node_handle_) {
+        ROS_ERROR("Could not find motor");
+        return false;
+      }
+      ROS_INFO_STREAM("Found Motor");
+      if(!VCS_SetProtocolStackSettings(node_handle_->device_handle->ptr, 115200, 500, &error_code)) {
+        ROS_ERROR("Failed to SetProtocolStackSettings");
+        return false;
+      }
+    }
+    else{
+      ROS_ERROR_STREAM("Specified communication protocol was : "<<comm_protocol_<<" . Must be either USB or RS232");
+      return false;
+    }
   }
 
   if(!VCS_SetDisableState(node_handle_->device_handle->ptr, node_handle_->node_id, &error_code)) {