diff --git a/software/ros_packages/ground_station/src/Framework/InputSystems/ControllerControlSender.py b/software/ros_packages/ground_station/src/Framework/InputSystems/ControllerControlSender.py index c3c7705..57be7c3 100644 --- a/software/ros_packages/ground_station/src/Framework/InputSystems/ControllerControlSender.py +++ b/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, - "y_axis": 512, - "z_axis": 128, - "throttle_axis": 128, + "left_x_axis": 0, + "left_y_axis": 0, + "left_stick_button": 0, + + "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, - "thumb_pressed": 0, - "three_pressed": 0, - "four_pressed": 0, - "five_pressed": 0, - "six_pressed": 0, + "back_button": 0, + "start_button": 0, + "xbox_button": 0, - "seven_pressed": 0, - "eight_pressed": 0, - "nine_pressed": 0, - "ten_pressed": 0, - "eleven_pressed": 0, - "twelve_pressed": 0, + "x_button": 0, + "a_button": 0, + "b_button": 0, + "y_button": 0 } self.raw_mapping_to_class_mapping = { - "ABS_X": "x_axis", - "ABS_Y": "y_axis", - "ABS_RZ": "z_axis", - "ABS_THROTTLE": "throttle_axis", + "ABS_X": "left_x_axis", + "ABS_Y": "left_y_axis", + "BTN_THUMBL": "left_stick_button", + + "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_THUMB": "thumb_pressed", - "BTN_THUMB2": "three_pressed", - "BTN_TOP": "four_pressed", - "BTN_TOP2": "five_pressed", - "BTN_PINKIE": "six_pressed", + "BTN_SELECT": "back_button", + "BTN_START": "start_button", + "BTN_MODE": "xbox_button", - "BTN_BASE": "seven_pressed", - "BTN_BASE2": "eight_pressed", - "BTN_BASE3": "nine_pressed", - "BTN_BASE4": "ten_pressed", - "BTN_BASE5": "eleven_pressed", - "BTN_BASE6": "twelve_pressed" + "BTN_NORTH": "x_button", + "BTN_SOUTH": "a_button", + "BTN_EAST": "b_button", + "BTN_WEST": "y_button" } 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)) diff --git a/software/ros_packages/ground_station/src/Framework/MiscSystems/SSHConsoleCore.py b/software/ros_packages/ground_station/src/Framework/MiscSystems/BashConsoleCore.py similarity index 57% rename from software/ros_packages/ground_station/src/Framework/MiscSystems/SSHConsoleCore.py rename to software/ros_packages/ground_station/src/Framework/MiscSystems/BashConsoleCore.py index 952f715..45974f9 100644 --- a/software/ros_packages/ground_station/src/Framework/MiscSystems/SSHConsoleCore.py +++ b/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. -ACCESS_POINT_USER = "ubnt" -ACCESS_POINT_PASSWORD = "rover4lyfe^" # We don't care about this password, don't freak out. Wifi is open anyways... +THREAD_HERTZ = 5 ##################################### # 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.ubiquiti_channel_apply_button = self.left_screen.ubiquiti_channel_apply_button # type: QtWidgets.QPushButton + self.ssh_widget = self.left_screen.ssh_console_widget # type: QtWidgets.QSpinBox # ########## 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 \ No newline at end of file + 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 diff --git a/software/ros_packages/ground_station/src/ground_station.py b/software/ros_packages/ground_station/src/ground_station.py index b1facf9..5556771 100644 --- a/software/ros_packages/ground_station/src/ground_station.py +++ b/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() diff --git a/software/ros_packages/rover_arm/CMakeLists.txt b/software/ros_packages/rover_arm/CMakeLists.txt index 506f96b..8ed758a 100644 --- a/software/ros_packages/rover_arm/CMakeLists.txt +++ b/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( -# FILES -#) +add_message_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( -# DEPENDENCIES -# std_msgs -#) +generate_messages( + DEPENDENCIES + 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 ) ############# diff --git a/software/ros_packages/rover_arm/msg/ArmControlMessage.msg b/software/ros_packages/rover_arm/msg/ArmControlMessage.msg new file mode 100644 index 0000000..8323c22 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/software/ros_packages/rover_arm/msg/ArmStatusMessage.msg b/software/ros_packages/rover_arm/msg/ArmStatusMessage.msg new file mode 100644 index 0000000..e7c7417 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/software/ros_packages/rover_arm/package.xml b/software/ros_packages/rover_arm/package.xml index d6fd2cd..78fecd3 100644 --- a/software/ros_packages/rover_arm/package.xml +++ b/software/ros_packages/rover_arm/package.xml @@ -51,10 +51,16 @@ catkin roscpp std_msgs + ros_msgs + message_generation roscpp std_msgs + ros_msgs roscpp std_msgs + ros_msgs + message_generation + message_runtime diff --git a/software/ros_packages/rover_arm/src/rover_arm.cpp b/software/ros_packages/rover_arm/src/rover_arm.cpp index 5b83132..53ca02e 100644 --- a/software/ros_packages/rover_arm/src/rover_arm.cpp +++ b/software/ros_packages/rover_arm/src/rover_arm.cpp @@ -1,11 +1,14 @@ #include #include #include +#include #include #include #include "simplemotion/simplemotion.h" +#include +#include 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 smint32 axis1_max_count = 100000; -float axis1_max_degrees = 180.0; +const string default_absolute_position_control_topic = "control/absolute"; +const string default_relative_position_control_topic = "control/relative"; +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(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); - 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); + if(!arm_sucessfully_connected){ return; } - smint32 status = 0; - smint32 status1 = 0; - smint32 status2 = 0; - - 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); - } + clear_faults(); + get_joint_statuses(); + set_joint_positions(); + reset_controllers(); 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(); diff --git a/software/ros_packages/rover_arm/src/simplemotion/CMakeLists.txt b/software/ros_packages/rover_arm/src/simplemotion/CMakeLists.txt index 56b849a..c3a25f1 100644 --- a/software/ros_packages/rover_arm/src/simplemotion/CMakeLists.txt +++ b/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}) \ No newline at end of file +target_include_directories (simplemotion2 PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) \ No newline at end of file diff --git a/software/rover_setup.sh b/software/rover_setup.sh index 2209c80..37a87dd 100755 --- a/software/rover_setup.sh +++ b/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