Rover arm now moves one axis via topics. Close to having the rest moving tomorrow.

software/ros_packages/ground_station/src/Framework/InputSystems/ControllerControlSender.py

@@ -35,51 +35,59 @@ class XBOXController(QtCore.QThread):
         self.gamepad = None  # type: GamePad
 
         self.controller_states = {
-            "x_axis": 512,
+            "left_x_axis": 0,
-            "y_axis": 512,
+            "left_y_axis": 0,
-            "z_axis": 128,
+            "left_stick_button": 0,
-            "throttle_axis": 128,
+
+            "right_x_axis": 0,
+            "right_y_axis": 0,
+            "right_stick_button": 0,
+
+            "left_trigger": 0,
+            "left_bumper": 0,
+
+            "right_trigger": 0,
+            "right_bumper": 0,
 
             "hat_x_axis": 0,
             "hat_y_axis": 0,
 
-            "trigger_pressed": 0,
+            "back_button": 0,
-            "thumb_pressed": 0,
+            "start_button": 0,
-            "three_pressed": 0,
+            "xbox_button": 0,
-            "four_pressed": 0,
-            "five_pressed": 0,
-            "six_pressed": 0,
 
-            "seven_pressed": 0,
+            "x_button": 0,
-            "eight_pressed": 0,
+            "a_button": 0,
-            "nine_pressed": 0,
+            "b_button": 0,
-            "ten_pressed": 0,
+            "y_button": 0
-            "eleven_pressed": 0,
-            "twelve_pressed": 0,
         }
 
         self.raw_mapping_to_class_mapping = {
-            "ABS_X": "x_axis",
+            "ABS_X": "left_x_axis",
-            "ABS_Y": "y_axis",
+            "ABS_Y": "left_y_axis",
-            "ABS_RZ": "z_axis",
+            "BTN_THUMBL": "left_stick_button",
-            "ABS_THROTTLE": "throttle_axis",
+
+            "ABS_RX": "right_x_axis",
+            "ABS_RY": "right_y_axis",
+            "BTN_THUMBR": "right_stick_button",
+
+            "ABS_Z": "left_trigger",
+            "BTN_TL": "left_bumper",
+
+            "ABS_RZ": "right_trigger",
+            "BTN_TR": "right_bumper",
 
             "ABS_HAT0X": "hat_x_axis",
             "ABS_HAT0Y": "hat_y_axis",
 
-            "BTN_TRIGGER": "trigger_pressed",
+            "BTN_SELECT": "back_button",
-            "BTN_THUMB": "thumb_pressed",
+            "BTN_START": "start_button",
-            "BTN_THUMB2": "three_pressed",
+            "BTN_MODE": "xbox_button",
-            "BTN_TOP": "four_pressed",
-            "BTN_TOP2": "five_pressed",
-            "BTN_PINKIE": "six_pressed",
 
-            "BTN_BASE": "seven_pressed",
+            "BTN_NORTH": "x_button",
-            "BTN_BASE2": "eight_pressed",
+            "BTN_SOUTH": "a_button",
-            "BTN_BASE3": "nine_pressed",
+            "BTN_EAST": "b_button",
-            "BTN_BASE4": "ten_pressed",
+            "BTN_WEST": "y_button"
-            "BTN_BASE5": "eleven_pressed",
-            "BTN_BASE6": "twelve_pressed"
         }
 
         self.ready = False
@@ -109,8 +117,12 @@ class XBOXController(QtCore.QThread):
             events = self.gamepad.read()
 
             for event in events:
-                print event.code
+                # For seeing codes you haven't added yet...
+                # if event.code not in self.raw_mapping_to_class_mapping and event.code != "SYN_REPORT":
+                #     print event.code, ":", event.state
+
                 if event.code in self.raw_mapping_to_class_mapping:
+                    # print event.code, ":", event.state
                     self.controller_states[self.raw_mapping_to_class_mapping[event.code]] = event.state
 
             self.ready = True
@@ -136,7 +148,7 @@ class ControllerControlSender(QtCore.QThread):
         self.logger = logging.getLogger("groundstation")
 
         # ########## Thread Flags ##########
-        self.run_thread_flag = False
+        self.run_thread_flag = True
 
         self.controller = XBOXController()
 
@@ -150,6 +162,8 @@ class ControllerControlSender(QtCore.QThread):
         while self.run_thread_flag:
             start_time = time()
 
+            # print self.controller.controller_states
+
             time_diff = time() - start_time
 
             self.msleep(max(int(self.wait_time - time_diff), 0))

software/ros_packages/ground_station/src/Framework/MiscSystems/BashConsoleCore.py

@@ -6,29 +6,26 @@
 from PyQt5 import QtCore, QtWidgets
 import logging
 from time import time
-import paramiko
+
 
 #####################################
 # Global Variables
 #####################################
-ACCESS_POINT_IP = "192.168.1.20"  # The channel only has to be set on the access point. The staion will adjust.
+THREAD_HERTZ = 5
-ACCESS_POINT_USER = "ubnt"
-ACCESS_POINT_PASSWORD = "rover4lyfe^"  # We don't care about this password, don't freak out. Wifi is open anyways...
 
 
 #####################################
 # UbiquitiRadioSettings Class Definition
 #####################################
-class SSHConsole(QtCore.QThread):
+class BashConsole(QtCore.QThread):
     def __init__(self, shared_objects):
-        super(SSHConsole, self).__init__()
+        super(BashConsole, self).__init__()
 
         # ########## Reference to class init variables ##########
         self.shared_objects = shared_objects
         self.left_screen = self.shared_objects["screens"]["left_screen"]
 
-        self.ubiquiti_channel_spin_box = self.left_screen.ssh_console_widget  # type: QtWidgets.QSpinBox
+        self.ssh_widget = self.left_screen.ssh_console_widget  # type: QtWidgets.QSpinBox
-        self.ubiquiti_channel_apply_button = self.left_screen.ubiquiti_channel_apply_button  # type: QtWidgets.QPushButton
 
         # ########## Get the settings instance ##########
         self.settings = QtCore.QSettings()
@@ -40,8 +37,23 @@ class SSHConsole(QtCore.QThread):
         self.run_thread_flag = True
 
         # ########## Class Variables ##########
+        self.wait_time = 1.0 / THREAD_HERTZ
 
-        self.connect_signals_and_slots()
+    def run(self):
+        while self.run_thread_flag:
+            start_time = time()
+
+            time_diff = time() - start_time
+
+            self.msleep(max(int(self.wait_time - time_diff), 0))
 
     def connect_signals_and_slots(self):
         pass
+
+    def setup_signals(self, start_signal, signals_and_slots_signal, kill_signal):
+        start_signal.connect(self.start)
+        signals_and_slots_signal.connect(self.connect_signals_and_slots)
+        kill_signal.connect(self.on_kill_threads_requested__slot)
+
+    def on_kill_threads_requested__slot(self):
+        self.run_thread_flag = False

software/ros_packages/ground_station/src/ground_station.py

@@ -25,6 +25,7 @@ import Framework.StatusSystems.UbiquitiStatusCore as UbiquitiStatusCore
 import Framework.SettingsSystems.UbiquitiRadioSettings as UbiquitiRadioSettings
 import Framework.InputSystems.SpaceNavControlSender as SpaceNavControlSender
 import Framework.MiscSystems.MiningCore as MiningCore
+import Framework.MiscSystems.BashConsoleCore as BashConsoleCore
 
 #####################################
 # Global Variables
@@ -117,6 +118,7 @@ class GroundStation(QtCore.QObject):
         self.__add_thread("Ubiquiti Radio Settings", UbiquitiRadioSettings.UbiquitiRadioSettings(self.shared_objects))
         self.__add_thread("Waypoints Coordinator", WaypointsCoordinator.WaypointsCoordinator(self.shared_objects))
         self.__add_thread("Spacenav Sender", SpaceNavControlSender.SpaceNavControlSender(self.shared_objects))
+        self.__add_thread("Bash Console", BashConsoleCore.BashConsole(self.shared_objects))
 
         self.connect_signals_and_slots_signal.emit()
         self.__connect_signals_to_slots()

software/ros_packages/rover_arm/CMakeLists.txt

@@ -9,8 +9,9 @@ add_compile_options(-std=c++11)
 ## is used, also find other catkin packages
 find_package(catkin REQUIRED COMPONENTS
   roscpp
-  #message_generation
   std_msgs
+  std_msgs
+  message_generation
 )
 
 ## System dependencies are found with CMake's conventions
@@ -47,9 +48,11 @@ find_package(catkin REQUIRED COMPONENTS
 ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 
 ## Generate messages in the 'msg' folder
-#add_message_files(
+add_message_files(
-#    FILES
+    FILES
-#)
+    ArmControlMessage.msg
+    ArmStatusMessage.msg
+)
 
 ## Generate services in the 'srv' folder
 # add_service_files(
@@ -66,10 +69,10 @@ find_package(catkin REQUIRED COMPONENTS
 # )
 
 ## Generate added messages and services with any dependencies listed here
-#generate_messages(
+generate_messages(
-#    DEPENDENCIES
+    DEPENDENCIES
-#    std_msgs
+    std_msgs
-#)
+)
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##
@@ -103,7 +106,7 @@ find_package(catkin REQUIRED COMPONENTS
 catkin_package(
 #  INCLUDE_DIRS include
 #  LIBRARIES rover_arm
-  CATKIN_DEPENDS roscpp std_msgs
+  CATKIN_DEPENDS roscpp std_msgs message_generation
 #  DEPENDS system_lib
 )
 
@@ -150,7 +153,7 @@ add_executable(${PROJECT_NAME} src/rover_arm.cpp )
  target_link_libraries(
          ${PROJECT_NAME}
          ${catkin_LIBRARIES}
-         simplemotion
+         simplemotion2
  )
 
 #############

software/ros_packages/rover_arm/msg/ArmControlMessage.msg

@@ -0,0 +1,9 @@
+float64 base
+float64 shoulder
+float64 elbow
+float64 roll
+float64 wrist_pitch
+float64 wrist_roll
+
+bool clear_faults
+bool reset_controllers

software/ros_packages/rover_arm/msg/ArmStatusMessage.msg

@@ -0,0 +1,27 @@
+float64 base
+float64 shoulder
+float64 elbow
+float64 roll
+float64 wrist_pitch
+float64 wrist_roll
+
+bool base_present
+bool shoulder_present
+bool elbow_present
+bool roll_present
+bool wrist_pitch_present
+bool wrist_roll_present
+
+int32 base_status
+int32 shoulder_status
+int32 elbow_status
+int32 roll_status
+int32 wrist_pitch_status
+int32 wrist_roll_status
+
+int32 base_faults
+int32 shoulder_faults
+int32 elbow_faults
+int32 roll_faults
+int32 wrist_pitch_faults
+int32 wrist_roll_faults

software/ros_packages/rover_arm/package.xml

@@ -51,10 +51,16 @@
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>roscpp</build_depend>
   <build_depend>std_msgs</build_depend>
+  <build_depend>ros_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
   <build_export_depend>roscpp</build_export_depend>
   <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>ros_msgs</build_export_depend>
   <exec_depend>roscpp</exec_depend>
   <exec_depend>std_msgs</exec_depend>
+  <exec_depend>ros_msgs</exec_depend>
+  <exec_depend>message_generation</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->

software/ros_packages/rover_arm/src/rover_arm.cpp

@@ -1,11 +1,14 @@
 #include <string>
 #include <iostream>
 #include <unistd.h>
+#include <algorithm>
 
 #include <ros/ros.h>
 #include <ros/console.h>
 
 #include "simplemotion/simplemotion.h"
+#include <rover_arm/ArmControlMessage.h>
+#include <rover_arm/ArmStatusMessage.h>
 
 
 using namespace std;
@@ -14,21 +17,46 @@ int device_address = 1;
 
 ////////// Global Variables //////////
 // ROS Parameter Defaults
-const string default_port = "/dev/rover/ttyARM";
+const string default_port = "/dev/ttyUSB0";
 
-// Axis Defaults
+const string default_absolute_position_control_topic = "control/absolute";
-const smint32 axis1_max_count = 100000;
+const string default_relative_position_control_topic = "control/relative";
-float axis1_max_degrees = 180.0;
+const string default_arm_status_topic = "status";
 
-const smint32 axis2_max_count = 100000;
+////////// Axis limits and conversions //////////
+// Base
+const smuint8 base_address = 1;
+const smint32 base_counts_per_rev = 5725807;
+const double base_min = -0.5;
+const double base_max_rev = 0.5;
 
-const smint32 axis3_max_count = 100000;
+// Shoulder
+const smuint8 shoulder_address = 2;
+const smint32 shoulder_counts_per_rev = 5725807; //FIXME
+const double shoulder_min_rev = -0.25;
+const double shoulder_max_rev = 0.25;
 
-const smint32 axis4_max_count = 100000;
+//Elbow
+const smuint8 elbow_address = 3;
+const smint32 elbow_counts_per_rev = 5725807;  //FIXME
+const double elbow_min_rev = -0.5;
+const double elbow_max_rev = 0.5;
 
-const smint32 axis5_max_count = 320000;
+//Roll
+const smuint8 roll_address = 4;
+const smint32 roll_counts_per_rev = 5725807; //FIXME
+const double roll_min_rev = -0.25;
+const double roll_max_rev = 0.25;
 
-const smint32 axis6_max_count = 100000;
+//Wrist Pitch
+const smuint8 wrist_pitch_address = 5;
+const smint32 wrist_pitch_counts_per_rev = 5725807; //FIXME
+const double wrist_pitch_min_rev = -0.25;
+const double wrist_pitch_max_rev = 0.25;
+
+//Wrist Roll
+const smuint8 wrist_roll_address = 6;
+const smint32 wrist_roll_counts_per_rev = 5725807; //FIXME
 
 
 class RoverArm{
@@ -40,84 +68,274 @@ public:
 
         node_handle->param("port", arm_port, default_port);
 
-
-        // Connect to arm, exit if failed
         arm_bus_handle = smOpenBus(arm_port.c_str());
 
         if(arm_bus_handle < 0){
             ROS_ERROR("Could not connect to arm");
+            return;
+        }else{
+            arm_sucessfully_connected = true;
         }
 
-        ROS_INFO("Connected to smBus");
+        absolute_position_control_subscriber = node_handle->subscribe(default_absolute_position_control_topic, 1, &RoverArm::absolute_position_callback, this);
+        relative_position_control_subscriber = node_handle->subscribe(default_relative_position_control_topic, 1, &RoverArm::relative_position_callback, this);
+
+        status_publisher = node_handle->advertise<rover_arm::ArmStatusMessage>(default_arm_status_topic, 1);
+
     }
 
     void run(){
         char dir = 0;
 
-        smSetParameter(arm_bus_handle, device_address, SMP_ABSOLUTE_SETPOINT, 0);
+
-        smSetParameter(arm_bus_handle, 2, SMP_ABSOLUTE_SETPOINT, 0);
-        smSetParameter(arm_bus_handle, 3, SMP_ABSOLUTE_SETPOINT, 0);
 
         while(ros::ok()){
-            smSetParameter(arm_bus_handle, device_address, SMP_FAULTS, 0);
+            if(!arm_sucessfully_connected){ return; }
-            smSetParameter(arm_bus_handle, device_address, SMP_CONTROL_BITS1, SMP_CB1_ENABLE);
-            smSetParameter(arm_bus_handle, 2, SMP_FAULTS, 0);
-            smSetParameter(arm_bus_handle, 2, SMP_CONTROL_BITS1, SMP_CB1_ENABLE);
-            smSetParameter(arm_bus_handle, 3, SMP_FAULTS, 0);
-            smSetParameter(arm_bus_handle, 3, SMP_CONTROL_BITS1, SMP_CB1_ENABLE);
 
-            smint32 status = 0;
+            clear_faults();
-            smint32 status1 = 0;
+            get_joint_statuses();
-            smint32 status2 = 0;
+            set_joint_positions();
-
+            reset_controllers();
-            smint32 position = 0;
-            smint32 position1 = 0;
-            smint32 position2 = 0;
-
-            smint32 read_bus_voltage;
-
-            while(!(status & STAT_TARGET_REACHED) || !(status1 & STAT_TARGET_REACHED) || !(status2 & STAT_TARGET_REACHED)){
-                smRead2Parameters(arm_bus_handle, device_address, SMP_STATUS, &status, SMP_ACTUAL_POSITION_FB, &position);
-                smRead2Parameters(arm_bus_handle, 2, SMP_STATUS, &status1, SMP_ACTUAL_POSITION_FB, &position1);
-                smRead2Parameters(arm_bus_handle, 3, SMP_STATUS, &status2, SMP_ACTUAL_POSITION_FB, &position2);
-                smRead1Parameter(arm_bus_handle, device_address, SMP_ACTUAL_BUS_VOLTAGE, &read_bus_voltage);
-
-                ROS_INFO("Dir: %u\tVoltage: %ld\tAxis 1: %ld\tAxis 2: %ld\tAxis 3: %ld", dir, read_bus_voltage, position, position1, position2);
-            }
-
-            dir = !dir;
-
-            if(dir){
-                smSetParameter(arm_bus_handle, device_address, SMP_ABSOLUTE_SETPOINT, -50000);
-                smSetParameter(arm_bus_handle, 2, SMP_ABSOLUTE_SETPOINT, -50000);
-                smSetParameter(arm_bus_handle, 3, SMP_ABSOLUTE_SETPOINT, -50000);
-            }else{
-                smSetParameter(arm_bus_handle, device_address, SMP_ABSOLUTE_SETPOINT, 50000);
-                smSetParameter(arm_bus_handle, 2, SMP_ABSOLUTE_SETPOINT, 50000);
-                smSetParameter(arm_bus_handle, 3, SMP_ABSOLUTE_SETPOINT, 50000);
-            }
 
             ros::spinOnce();
+            sleep(1);
         }
 
-        smSetParameter(arm_bus_handle, device_address, SMP_ABSOLUTE_SETPOINT, 0);
-        smSetParameter(arm_bus_handle, 2, SMP_ABSOLUTE_SETPOINT, 0);
-        smSetParameter(arm_bus_handle, 3, SMP_ABSOLUTE_SETPOINT, 0);
 
         ROS_ERROR("Shutting down.");
     }
+
+    void reset_controllers(){
+        // Check if should reset
+        // Reset all, then kill thread
+    }
+
+    void clear_faults(){
+        if(should_clear_faults) {
+            smSetParameter(arm_bus_handle, base_address, SMP_FAULTS, 0);
+            smSetParameter(arm_bus_handle, base_address, SMP_CONTROL_BITS1, SMP_CB1_ENABLE);
+
+            should_clear_faults = false;
+        }
+
+    }
+
+    void get_joint_statuses(){
+
+        // Check if joint states valid
+        // SMP_STATUS
+        // SMP_FAULTS
+
+        // SMP_ACTUAL_POSITION
+
+        if(!received_first_joint_position_update) {
+            base_comm_state = smRead3Parameters(arm_bus_handle, base_address, SMP_STATUS, &base_status, SMP_FAULTS,
+                                                &base_faults, SMP_ACTUAL_POSITION_FB, &base_position);
+        }
+
+        smint32 base_current_position;
+        smRead3Parameters(arm_bus_handle, base_address, SMP_STATUS, &base_status, SMP_FAULTS,
+                                                       &base_faults, SMP_ACTUAL_POSITION_FB, &base_current_position);
+
+        smint32 shoulder_current_position;
+        smRead3Parameters(arm_bus_handle, base_address, SMP_STATUS, &base_status, SMP_FAULTS,
+                          &base_faults, SMP_ACTUAL_POSITION_FB, &shoulder_current_position);
+//        shoulder_comm_state = smRead3Parameters(arm_bus_handle, shoulder_address, SMP_STATUS, &shoulder_status, SMP_FAULTS, &shoulder_faults, SMP_ACTUAL_POSITION_FB, &shoulder_position);
+//        elbow_comm_state = smRead3Parameters(arm_bus_handle, elbow_address, SMP_STATUS, &elbow_status, SMP_FAULTS, &elbow_faults, SMP_ACTUAL_POSITION_FB, &elbow_position);
+//        roll_comm_state = smRead3Parameters(arm_bus_handle, roll_address, SMP_STATUS, &roll_status, SMP_FAULTS, &roll_faults, SMP_ACTUAL_POSITION_FB, &roll_position);
+//        wrist_pitch_comm_state = smRead3Parameters(arm_bus_handle, wrist_pitch_address, SMP_STATUS, &wrist_pitch_status, SMP_FAULTS, &wrist_pitch_faults, SMP_ACTUAL_POSITION_FB, &wrist_pitch_position);
+//        wrist_roll_comm_state = smRead3Parameters(arm_bus_handle, wrist_roll_address, SMP_STATUS, &wrist_roll_status, SMP_FAULTS, &wrist_roll_faults, SMP_ACTUAL_POSITION_FB, &wrist_roll_position);
+        ROS_INFO("Base position: %ld", base_position);
+        // If so, broadcast
+        status_message.base = base_current_position / base_counts_per_rev;
+        status_message.shoulder = shoulder_current_position / shoulder_counts_per_rev;
+        status_message.elbow = elbow_position / elbow_counts_per_rev;
+        status_message.roll = roll_position / roll_counts_per_rev;
+        status_message.wrist_pitch = wrist_pitch_position / wrist_pitch_counts_per_rev;
+        status_message.wrist_roll = wrist_roll_position / wrist_roll_counts_per_rev;
+
+//        ROS_INFO("BASE:%d\tSHOULDER: %d\tELBOW: %d\tROLL: %d\tW_PITCH: %d\tW_ROLL: %d\t", base_comm_state, shoulder_comm_state, elbow_comm_state, roll_comm_state, wrist_pitch_comm_state, wrist_roll_comm_state );
+
+        //float64 base
+        //float64 shoulder
+        //float64 elbow
+        //float64 roll
+        //float64 wrist_pitch
+        //float64 wrist_roll
+        //
+        //bool base_present
+        //bool shoulder_present
+        //bool elbow_present
+        //bool roll_present
+        //bool wrist_pitch_present
+        //bool wrist_roll_present
+        //
+        //int32 base_status
+        //int32 shoulder_status
+        //int32 elbow_status
+        //int32 roll_status
+        //int32 wrist_pitch_status
+        //int32 wrist_roll_status
+        //
+        //int32 base_faults
+        //int32 shoulder_faults
+        //int32 elbow_faults
+        //int32 roll_faults
+        //int32 wrist_pitch_faults
+        //int32 wrist_roll_faults
+
+        status_publisher.publish(status_message);
+
+        // If first run, set received first message
+        if(!received_first_joint_position_update){ received_first_joint_position_update = true; }
+
+    }
+
+    void set_joint_positions(){
+        if(new_positions_received){
+            set_base_position();
+            set_shoulder_position();
+            set_elbow_position();
+            set_roll_position();
+            set_wrist_positions();
+
+         new_positions_received = false ;
+        }
+    }
+
+    void set_base_position(){
+        smSetParameter(arm_bus_handle, base_address, SMP_FAULTS, 0);
+        smSetParameter(arm_bus_handle, base_address, SMP_CONTROL_BITS1, SMP_CB1_ENABLE);
+        smSetParameter(arm_bus_handle, base_address, SMP_ABSOLUTE_SETPOINT, base_position);
+        ROS_INFO("SENT TO BASE: %ld", base_position);
+    }
+
+    void set_shoulder_position(){
+//        smSetParameter(arm_bus_handle, shoulder_address, SMP_FAULTS, 0);
+//        smSetParameter(arm_bus_handle, shoulder_address, SMP_CONTROL_BITS1, SMP_CB1_ENABLE);
+//        smSetParameter(arm_bus_handle, shoulder_address, SMP_ABSOLUTE_SETPOINT, shoulder_position);
+
+    }
+
+    void set_elbow_position(){
+
+    }
+
+    void set_roll_position(){
+
+    }
+
+    void set_wrist_positions(){
+
+    }
+
+    void absolute_position_callback(const rover_arm::ArmControlMessage::ConstPtr& msg)
+    {
+        if(!received_first_joint_position_update){ return; }
+
+
+
+//        base_position = smint32(min(max(msg->base, base_min), base_max_rev) * base_counts_per_rev);
+//        shoulder_position = smint32(min(max(msg->base, base_min), base_max_rev) * base_counts_per_rev);;
+//        elbow_position = smint32(min(max(msg->elbow, elbow_min_rev), elbow_max_rev) * elbow_counts_per_rev);;
+//        roll_position = smint32(min(max(msg->roll, roll_min_rev), roll_max_rev) * roll_counts_per_rev);;
+//        wrist_pitch_position = smint32(min(max(msg->wrist_pitch, wrist_pitch_min_rev), wrist_pitch_max_rev) * wrist_pitch_counts_per_rev);
+//        wrist_roll_position = smint32(msg->wrist_roll * wrist_roll_counts_per_rev);
+
+        base_position = msg->base * base_counts_per_rev;
+        shoulder_position = msg->base * base_counts_per_rev;
+
+//        shoulder_position = smint32(min(max(msg->base* base_counts_per_rev);;
+//        elbow_position = smint32(min(max(msg->elbow, elbow_min_rev), elbow_max_rev) * elbow_counts_per_rev);;
+//        roll_position = smint32(min(max(msg->roll, roll_min_rev), roll_max_rev) * roll_counts_per_rev);;
+//        wrist_pitch_position = smint32(min(max(msg->wrist_pitch, wrist_pitch_min_rev), wrist_pitch_max_rev) * wrist_pitch_counts_per_rev);
+//        wrist_roll_position = smint32(msg->wrist_roll * wrist_roll_counts_per_rev);
+
+        should_clear_faults = (msg->clear_faults);
+        should_reset = (msg->reset_controllers);
+
+        new_positions_received = true;
+
+        ROS_INFO("BASE:%d\tSHOULDER: %d\tELBOW: %d\tROLL: %d\tW_PITCH: %d\tW_ROLL: %d\t", base_position, shoulder_position, elbow_position, roll_position, wrist_pitch_position, wrist_roll_position );
+//        ROS_INFO("ABS::::Base: %f\tshoulder: %f", msg->base, msg->shoulder);
+    }
+
+    void relative_position_callback(const rover_arm::ArmControlMessage::ConstPtr& msg)
+    {
+        if(!received_first_joint_position_update){ return; }
+
+        base_position += msg->base;
+        shoulder_position += msg->shoulder;
+        elbow_position += msg->elbow;
+        roll_position += msg->roll;
+        wrist_pitch_position += msg->wrist_pitch;
+        wrist_roll_position += msg->wrist_roll;
+
+        should_clear_faults += msg->clear_faults;
+        should_reset += msg->reset_controllers;
+
+        new_positions_received = true;
+
+        ROS_INFO("REL::::Base: %f\tshoulder: %f", msg->base, msg->shoulder);
+    }
+
 private:
     ros::NodeHandle *node_handle;
 
     string arm_port;
     smbus arm_bus_handle;
+    bool arm_sucessfully_connected = false;
+
+    ros::Publisher status_publisher;
+    rover_arm::ArmStatusMessage status_message;
+
+    ros::Subscriber absolute_position_control_subscriber;
+    ros::Subscriber relative_position_control_subscriber;
+
+    bool received_first_joint_position_update = false;
+
+    bool new_positions_received = false;
+
+    smint32 base_position = 0;
+    smint32 base_comm_state = 0;
+    smint32 base_status = 0;
+    smint32 base_faults = 0;
+
+    smint32 shoulder_position = 0;
+    smint32 shoulder_comm_state = 0;
+    smint32 shoulder_status = 0;
+    smint32 shoulder_faults = 0;
+
+    smint32 elbow_position = 0;
+    smint32 elbow_comm_state = 0;
+    smint32 elbow_status = 0;
+    smint32 elbow_faults = 0;
+
+    smint32 roll_position = 0;
+    smint32 roll_comm_state = 0;
+    smint32 roll_status = 0;
+    smint32 roll_faults = 0;
+
+    smint32 wrist_pitch_position = 0;
+    smint32 wrist_pitch_comm_state = 0;
+    smint32 wrist_pitch_status = 0;
+    smint32 wrist_pitch_faults = 0;
+
+    smint32 wrist_roll_position = 0;
+    smint32 wrist_roll_comm_state = 0;
+    smint32 wrist_roll_status = 0;
+    smint32 wrist_roll_faults = 0;
+
+
+
+    bool should_clear_faults = true;
+    bool should_reset = false;
 
 
 
 };
 
 
-
 int main(int argc, char** argv){
     RoverArm rover_arm(argc, argv);
     rover_arm.run();

software/ros_packages/rover_arm/src/simplemotion/CMakeLists.txt

@@ -1,3 +1,3 @@
-add_library(simplemotion rs232.c simplemotion.c busdevice.c sm_consts.c)
+add_library(simplemotion2 rs232.c simplemotion.c busdevice.c sm_consts.c)
 
-target_include_directories (simplemotion PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+target_include_directories (simplemotion2 PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

software/rover_setup.sh

@@ -15,6 +15,10 @@ folders_to_link=(
     nimbro_topic_transport
     rover_main
     rover_odometry
+    mr1718-arm-urdf_export
+    rover_arm_moveit_config
+    gazebo_ros_demos
+    ros_control_boilerplate
 )
 
 # Print heading