caperren/osurc_mars_rover_2017_2018
1
0
Fork 0
mirror of https://github.com/OSURoboticsClub/Rover_2017_2018.git synced 2025-11-08 10:11:14 +00:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Corwin Perren
2018-08-04 01:28:21 -07:00
parent d62fdd45b3
commit 7f80f44b96
10 changed files with 406 additions and 111 deletions
Download Patch File Download Diff File Expand all files Collapse all files

82
software/ros_packages/ground_station/src/Framework/InputSystems/ControllerControlSender.py
View File

@@ -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))

32
software/ros_packages/ground_station/src/Framework/MiscSystems/SSHConsoleCore.py → software/ros_packages/ground_station/src/Framework/MiscSystems/BashConsoleCore.py
View File

@@ -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
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

2
software/ros_packages/ground_station/src/ground_station.py
View File

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

23
software/ros_packages/rover_arm/CMakeLists.txt
View File

@@ -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
)
#############

9
software/ros_packages/rover_arm/msg/ArmControlMessage.msg Normal file
View File

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

27
software/ros_packages/rover_arm/msg/ArmStatusMessage.msg Normal file
View File

@@ -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

6
software/ros_packages/rover_arm/package.xml
View File

@@ -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 -->

328
software/ros_packages/rover_arm/src/rover_arm.cpp
View File

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

4
software/ros_packages/rover_arm/src/simplemotion/CMakeLists.txt
View File

@@ -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})

4
software/rover_setup.sh
View File

@@ -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