From 17c450f008fb98fa000d1966d188514eab66d602 Mon Sep 17 00:00:00 2001
From: Max Polzin <max.polzin@epfl.ch>
Date: Wed, 23 Feb 2022 10:09:37 +0000
Subject: [PATCH 1/9] Rearrange hardware_interface.

---
 electra_hardware_interface/CMakeLists.txt     |  6 +-
 .../config/controllers.yaml                   | 60 +++++++++----------
 .../electra_hardware_interface.h              |  8 +--
 .../launch/electra_hardware_interface.launch  | 26 ++++----
 ...ode.cpp => electra_hardware_interface.cpp} | 35 ++++-------
 .../src/electra_hardware_interface_node.cpp   | 12 ++++
 ...ra.urdf.xacro => electra_model.urdf.xacro} |  0
 electra_launch/launch/electra.launch          |  2 +
 8 files changed, 78 insertions(+), 71 deletions(-)
 rename electra_hardware_interface/src/{hardware_interface_node.cpp => electra_hardware_interface.cpp} (61%)
 create mode 100644 electra_hardware_interface/src/electra_hardware_interface_node.cpp
 rename electra_hardware_interface/urdf/{electra.urdf.xacro => electra_model.urdf.xacro} (100%)

diff --git a/electra_hardware_interface/CMakeLists.txt b/electra_hardware_interface/CMakeLists.txt
index cdf5aa0..5364682 100644
--- a/electra_hardware_interface/CMakeLists.txt
+++ b/electra_hardware_interface/CMakeLists.txt
@@ -20,6 +20,6 @@ include_directories(
   ${catkin_INCLUDE_DIRS}
 )
 
-add_executable(electra_hardware_interface src/hardware_interface_node.cpp)
-add_dependencies(electra_hardware_interface ${catkin_EXPORTED_TARGETS})
-target_link_libraries(electra_hardware_interface ${catkin_LIBRARIES})
+add_executable(electra_hardware_interface_node src/electra_hardware_interface_node.cpp src/electra_hardware_interface.cpp)
+add_dependencies(electra_hardware_interface_node ${catkin_EXPORTED_TARGETS})
+target_link_libraries(electra_hardware_interface_node ${catkin_LIBRARIES})
diff --git a/electra_hardware_interface/config/controllers.yaml b/electra_hardware_interface/config/controllers.yaml
index 30a7154..61fb490 100644
--- a/electra_hardware_interface/config/controllers.yaml
+++ b/electra_hardware_interface/config/controllers.yaml
@@ -1,35 +1,35 @@
 electra:
-    # Publish all joint states -----------------------------------
-    joints_update:
-      type: joint_state_controller/JointStateController
-      publish_rate: 50
+  # Publish all joint states -----------------------------------
+  joints_update:
+    type: joint_state_controller/JointStateController
+    publish_rate: 50
+
+  # # Position Controllers ---------------------------------------
+  # reel_position_controller:
+  #   type: effort_controllers/JointPositionController
+  #   joint: reel
+  #   pid: {p: 1300.0, i: 0.0, d: 5, i_clamp_min: -130.0, i_clamp_max: 130, antiwindup: True}
     
-    # # Position Controllers ---------------------------------------
-    # reel_position_controller:
-    #   type: effort_controllers/JointPositionController
-    #   joint: reel
-    #   pid: {p: 1300.0, i: 0.0, d: 5, i_clamp_min: -130.0, i_clamp_max: 130, antiwindup: True}
-      
-    # Velocity Controllers ---------------------------------------
-    velocity_controller:
-      type: effort_controllers/JointVelocityController
-      joint: electra
-      # pid: {p: 7.0, i: 15.0, d: 0.5, i_clamp_min: -10000.0, i_clamp_max: 88.0, antiwindup: True}
-      # pid: {p: 20.0, i: 0.0, d: 0.0, i_clamp_min: 255.0, i_clamp_max: 254.0, antiwindup: True}
-      # pid: {p: 0.0, i: 5.0, d: 0.0, i_clamp_min: -10000.0, i_clamp_max: 90.0, antiwindup: True}
-      pid: {p: 7.0, i: 15.0, d: 0.5, i_clamp_min: -100.0, i_clamp_max: 100.0, antiwindup: True}
+  # Velocity Controllers ---------------------------------------
+  velocity_controller:
+    type: effort_controllers/JointVelocityController
+    joint: electra
+    # pid: {p: 7.0, i: 15.0, d: 0.5, i_clamp_min: -10000.0, i_clamp_max: 88.0, antiwindup: True}
+    # pid: {p: 20.0, i: 0.0, d: 0.0, i_clamp_min: 255.0, i_clamp_max: 254.0, antiwindup: True}
+    # pid: {p: 0.0, i: 5.0, d: 0.0, i_clamp_min: -10000.0, i_clamp_max: 90.0, antiwindup: True}
+    pid: {p: 7.0, i: 15.0, d: 0.5, i_clamp_min: -100.0, i_clamp_max: 100.0, antiwindup: True}
 
-    force_controller:
-      type: electra_controllers/ForceController
-      joint: electra
-      pid: {p: 50.0, i: 0.0, d: 0.0, i_clamp_min: 0.0, i_clamp_max: 0.0, antiwindup: True}
+  # force_controller:
+  #   type: electra_controllers/ForceController
+  #   joint: electra
+  #   pid: {p: 50.0, i: 0.0, d: 0.0, i_clamp_min: 0.0, i_clamp_max: 0.0, antiwindup: True}
 
-    reel_tether_controller:
-      type: electra_controllers/TetherController
-      joint: electra
+  # reel_tether_controller:
+  #   type: electra_controllers/TetherController
+  #   joint: electra
 
-    # # Command forward controller
-    # reel_effort_controller:
-    #   type: effort_controllers/JointEffortController
-    #   joint: reel
-    #   pid: {p: 1.0, i: 0.0, d: 0.0, i_clamp_min: -10000.0, i_clamp_max: 10000.0, antiwindup: True}
+  # Command forward controller
+  effort_controller:
+    type: effort_controllers/JointEffortController
+    joint: electra
+    pid: {p: 1.0, i: 0.0, d: 0.0, i_clamp_min: -10000.0, i_clamp_max: 10000.0, antiwindup: True}
diff --git a/electra_hardware_interface/include/electra_hardware_interface/electra_hardware_interface.h b/electra_hardware_interface/include/electra_hardware_interface/electra_hardware_interface.h
index 001852c..643dd0a 100644
--- a/electra_hardware_interface/include/electra_hardware_interface/electra_hardware_interface.h
+++ b/electra_hardware_interface/include/electra_hardware_interface/electra_hardware_interface.h
@@ -8,16 +8,16 @@
 #include <controller_manager/controller_manager.h>
 #include <boost/scoped_ptr.hpp>
 #include <ros/ros.h>
-#include <std_msgs/Int16.h>
+#include <std_msgs/Int32.h>
 #include <std_msgs/Float32.h>
 #include <electra_msgs/State.h>
 #include <angles/angles.h>
 
-class ROBOTHardwareInterface : public hardware_interface::RobotHW 
+class ElectraHardwareInterface : public hardware_interface::RobotHW 
 {
 	public:
-        ROBOTHardwareInterface(ros::NodeHandle& nh);
-        ~ROBOTHardwareInterface();
+        ElectraHardwareInterface(ros::NodeHandle& nh);
+        ~ElectraHardwareInterface(){};
         void init();
         void update(const ros::TimerEvent& e);
         void write(ros::Duration elapsed_time);
diff --git a/electra_hardware_interface/launch/electra_hardware_interface.launch b/electra_hardware_interface/launch/electra_hardware_interface.launch
index c1bdab5..94c7d3c 100644
--- a/electra_hardware_interface/launch/electra_hardware_interface.launch
+++ b/electra_hardware_interface/launch/electra_hardware_interface.launch
@@ -1,24 +1,30 @@
 <launch>
 
+    <!-- <group ns="electra"> -->
 
     <rosparam file="$(find electra_hardware_interface)/config/controllers.yaml" command="load" />
     <rosparam file="$(find electra_hardware_interface)/config/joint_limits.yaml" command="load" />
 
-    <arg name="model" default="$(find electra_hardware_interface)/urdf/electra.urdf.xacro" />
+    <arg name="model" default="$(find electra_hardware_interface)/urdf/electra_model.urdf.xacro" />
     <param name="robot_description" command="$(find xacro)/xacro $(arg model)" />
 
-    <node name="electra_hardware_interface" pkg="electra_hardware_interface" type="electra_hardware_interface" output="screen" />
+    <node name="hardware_interface_node" pkg="electra_hardware_interface" type="electra_hardware_interface_node" output="screen" />
     <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"></node>
 
-    <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="
-              /electra/joints_update
-              /electra/velocity_controller
-          " />
+    <!-- <remap from="controller_manager/load_controller" to="electra/controller_manager/load_controller"/>
+        <remap from="controller_manager/switch_controller" to="electra/controller_manager/switch_controller"/>
+        <remap from="controller_manager/unload_controller" to="electra/controller_manager/unload_controller"/> -->
 
-    <!-- <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="
-          /electra/joints_update
-          /electra/force_controller
-      " /> -->
+    <!-- </group> -->
+
+
+    <!-- <arg name="controller" value="
+        /electra/joints_update
+        /electra/effort_controller" /> -->
+
+    <arg name="controller" value="/electra/velocity_controller /electra/joints_update" />
+
+    <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="$(arg controller)" />
 
 
 </launch>
\ No newline at end of file
diff --git a/electra_hardware_interface/src/hardware_interface_node.cpp b/electra_hardware_interface/src/electra_hardware_interface.cpp
similarity index 61%
rename from electra_hardware_interface/src/hardware_interface_node.cpp
rename to electra_hardware_interface/src/electra_hardware_interface.cpp
index 2093e83..4425d39 100644
--- a/electra_hardware_interface/src/hardware_interface_node.cpp
+++ b/electra_hardware_interface/src/electra_hardware_interface.cpp
@@ -1,31 +1,28 @@
 #include <electra_hardware_interface/electra_hardware_interface.h>
 
-ROBOTHardwareInterface::ROBOTHardwareInterface(ros::NodeHandle& nh) : nh_(nh) {
+ElectraHardwareInterface::ElectraHardwareInterface(ros::NodeHandle& nh) : nh_(nh) {
     init();
     controller_manager_.reset(new controller_manager::ControllerManager(this, nh_));
     loop_hz_= 15;
     ros::Duration update_freq = ros::Duration(1.0/loop_hz_);
 	
-	command_publisher = nh_.advertise<std_msgs::Int16>("/electra/command/raw",10);
-    encoder_subscriber = nh_.subscribe("/electra/encoder/raw", 1, &ROBOTHardwareInterface::encoder_callback, this);
-    loadcell_subscriber = nh_.subscribe("/electra/loadcell/raw", 1, &ROBOTHardwareInterface::loadcell_callback, this);
+	command_publisher = nh_.advertise<std_msgs::Int32>("/electra/motor/command",10);
+    encoder_subscriber = nh_.subscribe("/electra/encoder/raw", 1, &ElectraHardwareInterface::encoder_callback, this);
+    loadcell_subscriber = nh_.subscribe("/electra/loadcell/raw", 1, &ElectraHardwareInterface::loadcell_callback, this);
 
-    non_realtime_loop_ = nh_.createTimer(update_freq, &ROBOTHardwareInterface::update, this);
+    non_realtime_loop_ = nh_.createTimer(update_freq, &ElectraHardwareInterface::update, this);
 }
 
-ROBOTHardwareInterface::~ROBOTHardwareInterface() {
-}
-
-void ROBOTHardwareInterface::encoder_callback(const electra_msgs::StateConstPtr& msg){
+void ElectraHardwareInterface::encoder_callback(const electra_msgs::StateConstPtr& msg){
     joint_position_ = msg->position;
     joint_velocity_ = msg->velocity;
 }
 
-void ROBOTHardwareInterface::loadcell_callback(const std_msgs::Float32ConstPtr& msg){
+void ElectraHardwareInterface::loadcell_callback(const std_msgs::Float32ConstPtr& msg){
     joint_effort_ = msg->data;
 }
 
-void ROBOTHardwareInterface::init() {   
+void ElectraHardwareInterface::init() {   
 	joint_name_="electra";
     
     // Create joint state interface
@@ -49,27 +46,17 @@ void ROBOTHardwareInterface::init() {
     registerInterface(&effortJointSaturationInterface);
 }
 
-void ROBOTHardwareInterface::update(const ros::TimerEvent& e) {
+void ElectraHardwareInterface::update(const ros::TimerEvent& e) {
     elapsed_time_ = ros::Duration(e.current_real - e.last_real);
     controller_manager_->update(ros::Time::now(), elapsed_time_);
     write(elapsed_time_);
 }
 
 
-void ROBOTHardwareInterface::write(ros::Duration elapsed_time) {
+void ElectraHardwareInterface::write(ros::Duration elapsed_time) {
     effortJointSaturationInterface.enforceLimits(elapsed_time);
-    std_msgs::Int16 motorCommand;
+    std_msgs::Int32 motorCommand;
     motorCommand.data = (int) joint_effort_command_;	
 	ROS_INFO_THROTTLE(1,"PWM Cmd: %d",motorCommand.data);
 	command_publisher.publish(motorCommand);		
 }
-
-
-int main(int argc, char** argv) {
-    ros::init(argc, argv, "electra_hardware_interface");
-    ros::NodeHandle nh;
-    ros::MultiThreadedSpinner spinner(2); // Multiple threads for controller service callback and for the Service client callback used to get the feedback from ardiuno
-    ROBOTHardwareInterface ROBOT(nh);
-    spinner.spin();
-    return 0;
-}
diff --git a/electra_hardware_interface/src/electra_hardware_interface_node.cpp b/electra_hardware_interface/src/electra_hardware_interface_node.cpp
new file mode 100644
index 0000000..e641598
--- /dev/null
+++ b/electra_hardware_interface/src/electra_hardware_interface_node.cpp
@@ -0,0 +1,12 @@
+#include <electra_hardware_interface/electra_hardware_interface.h>
+
+int main(int argc, char** argv) {
+    ros::init(argc, argv, "electra_hardware_interface");
+    ros::NodeHandle nh;
+    // ros::MultiThreadedSpinner spinner(2); 
+    ElectraHardwareInterface electra(nh);
+    // spinner.spin();
+    ros::spin();
+
+    return 0;
+}
diff --git a/electra_hardware_interface/urdf/electra.urdf.xacro b/electra_hardware_interface/urdf/electra_model.urdf.xacro
similarity index 100%
rename from electra_hardware_interface/urdf/electra.urdf.xacro
rename to electra_hardware_interface/urdf/electra_model.urdf.xacro
diff --git a/electra_launch/launch/electra.launch b/electra_launch/launch/electra.launch
index f106fd2..22a6d98 100644
--- a/electra_launch/launch/electra.launch
+++ b/electra_launch/launch/electra.launch
@@ -7,5 +7,7 @@
     <include file="$(find electra_motor)/launch/electra_motor.launch" />
     <include file="$(find electra_encoder)/launch/electra_encoder.launch" />
 
+    <include file="$(find electra_hardware_interface)/launch/electra_hardware_interface.launch" />
+
 
 </launch>
\ No newline at end of file
-- 
GitLab


From 939f3deee3c36250c556e598d51886004e8b0f1d Mon Sep 17 00:00:00 2001
From: Max Polzin <max.polzin@epfl.ch>
Date: Wed, 23 Feb 2022 10:27:04 +0000
Subject: [PATCH 2/9] Like this?

---
 .../launch/electra_hardware_interface.launch  | 26 +++++++++----------
 1 file changed, 13 insertions(+), 13 deletions(-)

diff --git a/electra_hardware_interface/launch/electra_hardware_interface.launch b/electra_hardware_interface/launch/electra_hardware_interface.launch
index 94c7d3c..a22e44c 100644
--- a/electra_hardware_interface/launch/electra_hardware_interface.launch
+++ b/electra_hardware_interface/launch/electra_hardware_interface.launch
@@ -1,30 +1,30 @@
 <launch>
 
-    <!-- <group ns="electra"> -->
+    <group ns="electra">
 
-    <rosparam file="$(find electra_hardware_interface)/config/controllers.yaml" command="load" />
-    <rosparam file="$(find electra_hardware_interface)/config/joint_limits.yaml" command="load" />
+        <rosparam file="$(find electra_hardware_interface)/config/controllers.yaml" command="load" />
+        <rosparam file="$(find electra_hardware_interface)/config/joint_limits.yaml" command="load" />
 
-    <arg name="model" default="$(find electra_hardware_interface)/urdf/electra_model.urdf.xacro" />
-    <param name="robot_description" command="$(find xacro)/xacro $(arg model)" />
+        <arg name="model" default="$(find electra_hardware_interface)/urdf/electra_model.urdf.xacro" />
+        <param name="robot_description" command="$(find xacro)/xacro $(arg model)" />
 
-    <node name="hardware_interface_node" pkg="electra_hardware_interface" type="electra_hardware_interface_node" output="screen" />
-    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"></node>
+        <node name="hardware_interface_node" pkg="electra_hardware_interface" type="electra_hardware_interface_node" output="screen" />
+        <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"></node>
 
-    <!-- <remap from="controller_manager/load_controller" to="electra/controller_manager/load_controller"/>
+        <!-- <remap from="controller_manager/load_controller" to="electra/controller_manager/load_controller"/>
         <remap from="controller_manager/switch_controller" to="electra/controller_manager/switch_controller"/>
         <remap from="controller_manager/unload_controller" to="electra/controller_manager/unload_controller"/> -->
 
-    <!-- </group> -->
+        <arg name="controller" value="electra/velocity_controller electra/joints_update" />
+
+        <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="$(arg controller)" />
+
+    </group>
 
 
     <!-- <arg name="controller" value="
         /electra/joints_update
         /electra/effort_controller" /> -->
 
-    <arg name="controller" value="/electra/velocity_controller /electra/joints_update" />
-
-    <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="$(arg controller)" />
-
 
 </launch>
\ No newline at end of file
-- 
GitLab


From 6649a56d9338032d41f8418df46529c42acf00b3 Mon Sep 17 00:00:00 2001
From: Max Polzin <max.polzin@epfl.ch>
Date: Wed, 23 Feb 2022 10:46:46 +0000
Subject: [PATCH 3/9] Add ns.

---
 .../launch/electra_hardware_interface.launch                | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/electra_hardware_interface/launch/electra_hardware_interface.launch b/electra_hardware_interface/launch/electra_hardware_interface.launch
index a22e44c..40c8689 100644
--- a/electra_hardware_interface/launch/electra_hardware_interface.launch
+++ b/electra_hardware_interface/launch/electra_hardware_interface.launch
@@ -15,9 +15,11 @@
         <remap from="controller_manager/switch_controller" to="electra/controller_manager/switch_controller"/>
         <remap from="controller_manager/unload_controller" to="electra/controller_manager/unload_controller"/> -->
 
-        <arg name="controller" value="electra/velocity_controller electra/joints_update" />
+        <arg name="controller" value="
+        electra/velocity_controller 
+        electra/joints_update" />
 
-        <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="$(arg controller)" />
+        <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="--namespace=/electra $(arg controller)" />
 
     </group>
 
-- 
GitLab


From 1bf27f7959ed6d5c826fdf3252b23e7ec2aabf9b Mon Sep 17 00:00:00 2001
From: Max Polzin <max.polzin@epfl.ch>
Date: Wed, 23 Feb 2022 11:02:54 +0000
Subject: [PATCH 4/9] Works namespaced now.

---
 .../config/controllers.yaml                   | 62 +++++++++----------
 .../launch/electra_hardware_interface.launch  | 28 ++++-----
 2 files changed, 42 insertions(+), 48 deletions(-)

diff --git a/electra_hardware_interface/config/controllers.yaml b/electra_hardware_interface/config/controllers.yaml
index 61fb490..ebf5e01 100644
--- a/electra_hardware_interface/config/controllers.yaml
+++ b/electra_hardware_interface/config/controllers.yaml
@@ -1,35 +1,35 @@
-electra:
-  # Publish all joint states -----------------------------------
-  joints_update:
-    type: joint_state_controller/JointStateController
-    publish_rate: 50
-
-  # # Position Controllers ---------------------------------------
-  # reel_position_controller:
-  #   type: effort_controllers/JointPositionController
-  #   joint: reel
-  #   pid: {p: 1300.0, i: 0.0, d: 5, i_clamp_min: -130.0, i_clamp_max: 130, antiwindup: True}
+# electra_ns:
+    # Publish all joint states -----------------------------------
+    joints_update:
+      type: joint_state_controller/JointStateController
+      publish_rate: 50
     
-  # Velocity Controllers ---------------------------------------
-  velocity_controller:
-    type: effort_controllers/JointVelocityController
-    joint: electra
-    # pid: {p: 7.0, i: 15.0, d: 0.5, i_clamp_min: -10000.0, i_clamp_max: 88.0, antiwindup: True}
-    # pid: {p: 20.0, i: 0.0, d: 0.0, i_clamp_min: 255.0, i_clamp_max: 254.0, antiwindup: True}
-    # pid: {p: 0.0, i: 5.0, d: 0.0, i_clamp_min: -10000.0, i_clamp_max: 90.0, antiwindup: True}
-    pid: {p: 7.0, i: 15.0, d: 0.5, i_clamp_min: -100.0, i_clamp_max: 100.0, antiwindup: True}
+    # # Position Controllers ---------------------------------------
+    # reel_position_controller:
+    #   type: effort_controllers/JointPositionController
+    #   joint: reel
+    #   pid: {p: 1300.0, i: 0.0, d: 5, i_clamp_min: -130.0, i_clamp_max: 130, antiwindup: True}
+      
+    # Velocity Controllers ---------------------------------------
+    velocity_controller:
+      type: effort_controllers/JointVelocityController
+      joint: electra
+      # pid: {p: 7.0, i: 15.0, d: 0.5, i_clamp_min: -10000.0, i_clamp_max: 88.0, antiwindup: True}
+      # pid: {p: 20.0, i: 0.0, d: 0.0, i_clamp_min: 255.0, i_clamp_max: 254.0, antiwindup: True}
+      # pid: {p: 0.0, i: 5.0, d: 0.0, i_clamp_min: -10000.0, i_clamp_max: 90.0, antiwindup: True}
+      pid: {p: 7.0, i: 15.0, d: 0.5, i_clamp_min: -100.0, i_clamp_max: 100.0, antiwindup: True}
 
-  # force_controller:
-  #   type: electra_controllers/ForceController
-  #   joint: electra
-  #   pid: {p: 50.0, i: 0.0, d: 0.0, i_clamp_min: 0.0, i_clamp_max: 0.0, antiwindup: True}
+    force_controller:
+      type: electra_controllers/ForceController
+      joint: electra
+      pid: {p: 50.0, i: 0.0, d: 0.0, i_clamp_min: 0.0, i_clamp_max: 0.0, antiwindup: True}
 
-  # reel_tether_controller:
-  #   type: electra_controllers/TetherController
-  #   joint: electra
+    reel_tether_controller:
+      type: electra_controllers/TetherController
+      joint: electra
 
-  # Command forward controller
-  effort_controller:
-    type: effort_controllers/JointEffortController
-    joint: electra
-    pid: {p: 1.0, i: 0.0, d: 0.0, i_clamp_min: -10000.0, i_clamp_max: 10000.0, antiwindup: True}
+    # # Command forward controller
+    # reel_effort_controller:
+    #   type: effort_controllers/JointEffortController
+    #   joint: reel
+    #   pid: {p: 1.0, i: 0.0, d: 0.0, i_clamp_min: -10000.0, i_clamp_max: 10000.0, antiwindup: True}
diff --git a/electra_hardware_interface/launch/electra_hardware_interface.launch b/electra_hardware_interface/launch/electra_hardware_interface.launch
index 40c8689..ff0b704 100644
--- a/electra_hardware_interface/launch/electra_hardware_interface.launch
+++ b/electra_hardware_interface/launch/electra_hardware_interface.launch
@@ -1,6 +1,6 @@
 <launch>
 
-    <group ns="electra">
+    <group ns="/electra">
 
         <rosparam file="$(find electra_hardware_interface)/config/controllers.yaml" command="load" />
         <rosparam file="$(find electra_hardware_interface)/config/joint_limits.yaml" command="load" />
@@ -8,25 +8,19 @@
         <arg name="model" default="$(find electra_hardware_interface)/urdf/electra_model.urdf.xacro" />
         <param name="robot_description" command="$(find xacro)/xacro $(arg model)" />
 
-        <node name="hardware_interface_node" pkg="electra_hardware_interface" type="electra_hardware_interface_node" output="screen" />
+        <node name="electra_hardware_interface" pkg="electra_hardware_interface" type="electra_hardware_interface" output="screen" />
         <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"></node>
 
-        <!-- <remap from="controller_manager/load_controller" to="electra/controller_manager/load_controller"/>
-        <remap from="controller_manager/switch_controller" to="electra/controller_manager/switch_controller"/>
-        <remap from="controller_manager/unload_controller" to="electra/controller_manager/unload_controller"/> -->
-
-        <arg name="controller" value="
-        electra/velocity_controller 
-        electra/joints_update" />
-
-        <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="--namespace=/electra $(arg controller)" />
+        <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="
+              --namespace=/electra  
+              /electra/joints_update
+              /electra/velocity_controller
+          " />
 
+        <!-- <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="
+          /electra/joints_update
+          /electra/force_controller
+      " /> -->
     </group>
 
-
-    <!-- <arg name="controller" value="
-        /electra/joints_update
-        /electra/effort_controller" /> -->
-
-
 </launch>
\ No newline at end of file
-- 
GitLab


From f5ba9073a91111bc020df09312f0910fcfa06800 Mon Sep 17 00:00:00 2001
From: Max Polzin <max.polzin@epfl.ch>
Date: Wed, 23 Feb 2022 11:13:02 +0000
Subject: [PATCH 5/9] Reformat file.

---
 .../config/controllers.yaml                   | 63 +++++++++----------
 1 file changed, 31 insertions(+), 32 deletions(-)

diff --git a/electra_hardware_interface/config/controllers.yaml b/electra_hardware_interface/config/controllers.yaml
index ebf5e01..cc1f8f6 100644
--- a/electra_hardware_interface/config/controllers.yaml
+++ b/electra_hardware_interface/config/controllers.yaml
@@ -1,35 +1,34 @@
-# electra_ns:
-    # Publish all joint states -----------------------------------
-    joints_update:
-      type: joint_state_controller/JointStateController
-      publish_rate: 50
-    
-    # # Position Controllers ---------------------------------------
-    # reel_position_controller:
-    #   type: effort_controllers/JointPositionController
-    #   joint: reel
-    #   pid: {p: 1300.0, i: 0.0, d: 5, i_clamp_min: -130.0, i_clamp_max: 130, antiwindup: True}
-      
-    # Velocity Controllers ---------------------------------------
-    velocity_controller:
-      type: effort_controllers/JointVelocityController
-      joint: electra
-      # pid: {p: 7.0, i: 15.0, d: 0.5, i_clamp_min: -10000.0, i_clamp_max: 88.0, antiwindup: True}
-      # pid: {p: 20.0, i: 0.0, d: 0.0, i_clamp_min: 255.0, i_clamp_max: 254.0, antiwindup: True}
-      # pid: {p: 0.0, i: 5.0, d: 0.0, i_clamp_min: -10000.0, i_clamp_max: 90.0, antiwindup: True}
-      pid: {p: 7.0, i: 15.0, d: 0.5, i_clamp_min: -100.0, i_clamp_max: 100.0, antiwindup: True}
+# Publish all joint states -----------------------------------
+joints_update:
+  type: joint_state_controller/JointStateController
+  publish_rate: 50
 
-    force_controller:
-      type: electra_controllers/ForceController
-      joint: electra
-      pid: {p: 50.0, i: 0.0, d: 0.0, i_clamp_min: 0.0, i_clamp_max: 0.0, antiwindup: True}
+# # Position Controllers ---------------------------------------
+# reel_position_controller:
+#   type: effort_controllers/JointPositionController
+#   joint: reel
+#   pid: {p: 1300.0, i: 0.0, d: 5, i_clamp_min: -130.0, i_clamp_max: 130, antiwindup: True}
+  
+# Velocity Controllers ---------------------------------------
+velocity_controller:
+  type: effort_controllers/JointVelocityController
+  joint: electra
+  # pid: {p: 7.0, i: 15.0, d: 0.5, i_clamp_min: -10000.0, i_clamp_max: 88.0, antiwindup: True}
+  # pid: {p: 20.0, i: 0.0, d: 0.0, i_clamp_min: 255.0, i_clamp_max: 254.0, antiwindup: True}
+  # pid: {p: 0.0, i: 5.0, d: 0.0, i_clamp_min: -10000.0, i_clamp_max: 90.0, antiwindup: True}
+  pid: {p: 7.0, i: 15.0, d: 0.5, i_clamp_min: -100.0, i_clamp_max: 100.0, antiwindup: True}
 
-    reel_tether_controller:
-      type: electra_controllers/TetherController
-      joint: electra
+force_controller:
+  type: electra_controllers/ForceController
+  joint: electra
+  pid: {p: 50.0, i: 0.0, d: 0.0, i_clamp_min: 0.0, i_clamp_max: 0.0, antiwindup: True}
 
-    # # Command forward controller
-    # reel_effort_controller:
-    #   type: effort_controllers/JointEffortController
-    #   joint: reel
-    #   pid: {p: 1.0, i: 0.0, d: 0.0, i_clamp_min: -10000.0, i_clamp_max: 10000.0, antiwindup: True}
+reel_tether_controller:
+  type: electra_controllers/TetherController
+  joint: electra
+
+# # Command forward controller
+# reel_effort_controller:
+#   type: effort_controllers/JointEffortController
+#   joint: reel
+#   pid: {p: 1.0, i: 0.0, d: 0.0, i_clamp_min: -10000.0, i_clamp_max: 10000.0, antiwindup: True}
-- 
GitLab


From a4aa31dfa49c8429b8eda51da55c4fa918fd9386 Mon Sep 17 00:00:00 2001
From: Max Polzin <max.polzin@epfl.ch>
Date: Wed, 23 Feb 2022 11:13:14 +0000
Subject: [PATCH 6/9] Multispinner to prevent blocking.

---
 .../src/electra_hardware_interface_node.cpp                 | 6 ++----
 1 file changed, 2 insertions(+), 4 deletions(-)

diff --git a/electra_hardware_interface/src/electra_hardware_interface_node.cpp b/electra_hardware_interface/src/electra_hardware_interface_node.cpp
index e641598..6881e80 100644
--- a/electra_hardware_interface/src/electra_hardware_interface_node.cpp
+++ b/electra_hardware_interface/src/electra_hardware_interface_node.cpp
@@ -3,10 +3,8 @@
 int main(int argc, char** argv) {
     ros::init(argc, argv, "electra_hardware_interface");
     ros::NodeHandle nh;
-    // ros::MultiThreadedSpinner spinner(2); 
+    ros::MultiThreadedSpinner spinner(2); 
     ElectraHardwareInterface electra(nh);
-    // spinner.spin();
-    ros::spin();
-
+    spinner.spin();
     return 0;
 }
-- 
GitLab


From 80c07eef3ce0f3341453da929f4b0562467d60a8 Mon Sep 17 00:00:00 2001
From: Max Polzin <max.polzin@epfl.ch>
Date: Wed, 23 Feb 2022 11:13:21 +0000
Subject: [PATCH 7/9] Launch _node.

---
 .../launch/electra_hardware_interface.launch  | 21 +++++++++++--------
 1 file changed, 12 insertions(+), 9 deletions(-)

diff --git a/electra_hardware_interface/launch/electra_hardware_interface.launch b/electra_hardware_interface/launch/electra_hardware_interface.launch
index ff0b704..e8dddcd 100644
--- a/electra_hardware_interface/launch/electra_hardware_interface.launch
+++ b/electra_hardware_interface/launch/electra_hardware_interface.launch
@@ -1,5 +1,4 @@
 <launch>
-
     <group ns="/electra">
 
         <rosparam file="$(find electra_hardware_interface)/config/controllers.yaml" command="load" />
@@ -8,19 +7,23 @@
         <arg name="model" default="$(find electra_hardware_interface)/urdf/electra_model.urdf.xacro" />
         <param name="robot_description" command="$(find xacro)/xacro $(arg model)" />
 
-        <node name="electra_hardware_interface" pkg="electra_hardware_interface" type="electra_hardware_interface" output="screen" />
+        <node name="electra_hardware_interface" pkg="electra_hardware_interface" type="electra_hardware_interface_node" output="screen" />
         <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"></node>
 
+        <arg name="controller" value="
+        /electra/joints_update
+        /electra/velocity_controller
+        " />
+
+        <!-- <arg name="controller" value="
+        /electra/joints_update
+        /electra/force_controller
+        " /> -->
+
         <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="
               --namespace=/electra  
-              /electra/joints_update
-              /electra/velocity_controller
+              $(arg controller)
           " />
 
-        <!-- <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="
-          /electra/joints_update
-          /electra/force_controller
-      " /> -->
     </group>
-
 </launch>
\ No newline at end of file
-- 
GitLab


From 29986a20ec25ae718368a828c646d58c6d167d16 Mon Sep 17 00:00:00 2001
From: Max Polzin <max.polzin@epfl.ch>
Date: Wed, 23 Feb 2022 15:26:34 +0000
Subject: [PATCH 8/9] Update controllers.

---
 .../config/controllers.yaml                   |  5 +---
 .../config/joint_limits.yaml                  |  2 +-
 .../src/electra_hardware_interface.cpp        |  2 +-
 .../firmware/ElectraMotor/ElectraMotor.ino    | 27 +++++++++----------
 4 files changed, 16 insertions(+), 20 deletions(-)

diff --git a/electra_hardware_interface/config/controllers.yaml b/electra_hardware_interface/config/controllers.yaml
index cc1f8f6..849a100 100644
--- a/electra_hardware_interface/config/controllers.yaml
+++ b/electra_hardware_interface/config/controllers.yaml
@@ -13,10 +13,7 @@ joints_update:
 velocity_controller:
   type: effort_controllers/JointVelocityController
   joint: electra
-  # pid: {p: 7.0, i: 15.0, d: 0.5, i_clamp_min: -10000.0, i_clamp_max: 88.0, antiwindup: True}
-  # pid: {p: 20.0, i: 0.0, d: 0.0, i_clamp_min: 255.0, i_clamp_max: 254.0, antiwindup: True}
-  # pid: {p: 0.0, i: 5.0, d: 0.0, i_clamp_min: -10000.0, i_clamp_max: 90.0, antiwindup: True}
-  pid: {p: 7.0, i: 15.0, d: 0.5, i_clamp_min: -100.0, i_clamp_max: 100.0, antiwindup: True}
+  pid: {p: 20.0, i: 30.0, d: 1.0, i_clamp_min: -1000.0, i_clamp_max: 1000.0, antiwindup: True}
 
 force_controller:
   type: electra_controllers/ForceController
diff --git a/electra_hardware_interface/config/joint_limits.yaml b/electra_hardware_interface/config/joint_limits.yaml
index c61c761..7fe2c01 100644
--- a/electra_hardware_interface/config/joint_limits.yaml
+++ b/electra_hardware_interface/config/joint_limits.yaml
@@ -10,6 +10,6 @@ joint_limits:
       has_jerk_limits: false
       max_jerk: 0
       has_effort_limits: true
-      max_effort: 91
+      max_effort: 255
       min_effort: -255
 
diff --git a/electra_hardware_interface/src/electra_hardware_interface.cpp b/electra_hardware_interface/src/electra_hardware_interface.cpp
index 4425d39..3359ff3 100644
--- a/electra_hardware_interface/src/electra_hardware_interface.cpp
+++ b/electra_hardware_interface/src/electra_hardware_interface.cpp
@@ -3,7 +3,7 @@
 ElectraHardwareInterface::ElectraHardwareInterface(ros::NodeHandle& nh) : nh_(nh) {
     init();
     controller_manager_.reset(new controller_manager::ControllerManager(this, nh_));
-    loop_hz_= 15;
+    loop_hz_= 15.0;
     ros::Duration update_freq = ros::Duration(1.0/loop_hz_);
 	
 	command_publisher = nh_.advertise<std_msgs::Int32>("/electra/motor/command",10);
diff --git a/electra_motor/firmware/ElectraMotor/ElectraMotor.ino b/electra_motor/firmware/ElectraMotor/ElectraMotor.ino
index 96250e1..8b5f780 100644
--- a/electra_motor/firmware/ElectraMotor/ElectraMotor.ino
+++ b/electra_motor/firmware/ElectraMotor/ElectraMotor.ino
@@ -11,6 +11,8 @@ const uint8_t BRAKEGND = 3;
 const int inAPin = 7;
 const int inBPin = 8;
 const int pwmPin = 5;
+
+const int PWM_DEADBAND = 20;
 const int PWM_MAX = 255;
 
 ros::NodeHandle  nh;
@@ -37,21 +39,18 @@ void rosSetup(){
 }
 
 void rosCommandCallback( const std_msgs::Int32& command){
-  if (command.data < -PWM_MAX){
-    nh.logerror("Invalid command: <-255");
-  }
-  else if (-PWM_MAX <= command.data && command.data < 0){
-    motorGo(CW, command.data);
-  }
-  else if (command.data == 0 ){
-    motorOff();  
-  }
-  else if (0 < command.data && command.data <= PWM_MAX){
-    motorGo(CCW, command.data);
-  }
-  else if (PWM_MAX < command.data){
-    nh.logerror("Invalid command: >255");
+  uint8_t command_direction = command.data > 0 ? CW : CCW;
+  uint8_t command_magnitude = abs(command.data);
+
+  if (PWM_DEADBAND <= command_magnitude && command_magnitude <= PWM_MAX){
+    motorGo(command_direction, command_magnitude);
   }
+  else if (command_magnitude <= PWM_DEADBAND){
+    motorGo(command_direction, 0);
+  } 
+  else if (PWM_MAX <= command_magnitude){
+    nh.logerror("Invalid command: >|255|");
+  } 
   else{
     nh.logerror("Invalid command");
   }
-- 
GitLab


From 3db1505fa237307f474492c4a9bf17fdec1bd308 Mon Sep 17 00:00:00 2001
From: Max Polzin <max.polzin@epfl.ch>
Date: Wed, 23 Feb 2022 15:31:06 +0000
Subject: [PATCH 9/9] Update sketch.

---
 electra_motor/firmware/ElectraMotor/ElectraMotor.ino | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/electra_motor/firmware/ElectraMotor/ElectraMotor.ino b/electra_motor/firmware/ElectraMotor/ElectraMotor.ino
index 8b5f780..d7f8822 100644
--- a/electra_motor/firmware/ElectraMotor/ElectraMotor.ino
+++ b/electra_motor/firmware/ElectraMotor/ElectraMotor.ino
@@ -49,7 +49,7 @@ void rosCommandCallback( const std_msgs::Int32& command){
     motorGo(command_direction, 0);
   } 
   else if (PWM_MAX <= command_magnitude){
-    nh.logerror("Invalid command: >|255|");
+    nh.logerror("Invalid command: |255|<|command|");
   } 
   else{
     nh.logerror("Invalid command");
-- 
GitLab