Merge pull request #42 from UBC-Snowbots/rowzaw/cleanup

cleaning up armjoy and sample node stuff

Author: Roozki

src/arm_control/include/joy_arm_control.h

@@ -12,100 +12,34 @@
 
 class ArmJoy : public rclcpp::Node {
 public:
-    ArmJoy() : Node("arm_joy_control") {
-        auto qos = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local();
-        //command_publisher_ = this->create_publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>("/control/command/control_cmd", qos);
-        arm_publisher = this->create_publisher<rover_msgs::msg::ArmCommand>("/arm/command", qos);
-
-        joy_vibrator = this->create_publisher<sensor_msgs::msg::JoyFeedback>("/joy/feedback", qos);
-
-        // timer_ = this->create_wall_timer(
-        // std::chrono::duration<double>(period),std::bind(&ManualControlNode::test_send, this));
-        ps4_subscriber = this->create_subscription<sensor_msgs::msg::Joy>(
-            "/jorsnyuorasniertsnieirsnyuoy", 10, std::bind(&ArmJoy::joy_callback, this, std::placeholders::_1));
-        arm_subscriber = this->create_subscription<rover_msgs::msg::ArmCommand>(
-            "/arm/feedback", 10, std::bind(&ArmJoy::arm_callback, this, std::placeholders::_1));
-    }
-
-    void send_command(float steering_angle, float speed, float acceleration, float jerk) {
-   
-    }
-
-    void send_gear_command(int gear){
-      
-    }
-
-    
-
-    // void test_send(){
-    //     send_command(0.5, 1.0, 1.0, 0.5);
-    //     // rclcpp::logger
-
-    // }
+    ArmJoy();
 
 private:
-    
-    int current_gear = 0;
-    int prev_paddleR = 0;
-    int prev_paddleL = 0;
-
-    struct Axis{
-        float position = 00.00;
-        float velocity = 00.00;
-        bool homed = 0;
-    };
-    Axis axes[NUM_JOINTS];
 
-    // rclcpp::Publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr command_publisher_;
-    rclcpp::Publisher<rover_msgs::msg::ArmCommand>::SharedPtr arm_publisher;
+// Pubs
+rclcpp::Publisher<rover_msgs::msg::ArmCommand>::SharedPtr arm_publisher;
+rclcpp::Publisher<sensor_msgs::msg::JoyFeedback>::SharedPtr joy_vibrator;
 
-    rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr ps4_subscriber;
-    rclcpp::Subscription<rover_msgs::msg::ArmCommand>::SharedPtr arm_subscriber;
+// Subs
+rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr ps4_subscriber;
+rclcpp::Subscription<rover_msgs::msg::ArmCommand>::SharedPtr arm_subscriber;
 
-    rclcpp::Publisher<sensor_msgs::msg::JoyFeedback>::SharedPtr joy_vibrator;
+// Callbacks
+void joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg);
+void arm_callback(const rover_msgs::msg::ArmCommand::SharedPtr msg);
 
+// Timers
+rclcpp::TimerBase::SharedPtr timer_;
 
-    rclcpp::TimerBase::SharedPtr timer_;
 
-    void joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg){
-        rover_msgs::msg::ArmCommand target;
-        target.positions.resize(NUM_JOINTS);
-        target.velocities.resize(NUM_JOINTS);
-        float target_positions[6];
-        float inputs[6];
-
-        inputs[2] = msg->axes[0];
-        inputs[1] = msg->axes[1];
-        inputs[0] = msg->axes[5] - msg->axes[2];
-        inputs[3] = msg->axes[3];
-        inputs[4] = msg->axes[4];
-        inputs[5] = msg->axes[6];
-        
-        if(CONTROL_MODE == POSITION_CONTROL){
-            target.cmd_type = 'P';
-
-        for (int i = 0; i < NUM_JOINTS; i++){
-            target.positions[i] = axes[i].position + inputs[i] * 10;
-        }
-        }else if(CONTROL_MODE == VELOCITY_CONTROL){
-            target.cmd_type = 'V';
-        for (int i = 0; i < NUM_JOINTS; i++){
-            target.velocities[i] = inputs[i] * 100;
-        }
-        }
-
-         arm_publisher->publish(target);
-
-    }
-
-    void arm_callback(const rover_msgs::msg::ArmCommand::SharedPtr msg){
-
-        for (int i = 0; i < NUM_JOINTS; i++){
-            axes[i].position = msg->positions[i];
-            // axes[i].velocity = msg->velocities
-
-        }
-    }
-        
+int current_gear = 0;
+int prev_paddleR = 0;
+int prev_paddleL = 0;
 
+struct Axis{
+    float position = 00.00;
+    float velocity = 00.00;
+    bool homed = 0;
+};
+Axis axes[NUM_JOINTS];
 };

src/arm_control/src/joy_arm_control.cpp

@@ -1,17 +1,73 @@
-//this is a sample node, with a joy input
 #include "joy_arm_control.h"
 
+// Constructor
+ArmJoy::ArmJoy() : 
+Node("arm_joy_control") 
+{
+    auto qos = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local();
+    arm_publisher = this->create_publisher<rover_msgs::msg::ArmCommand>("/arm/command", qos);
 
+    joy_vibrator = this->create_publisher<sensor_msgs::msg::JoyFeedback>("/joy/feedback", qos);
 
+    // timer_ = this->create_wall_timer(
+    // std::chrono::duration<double>(period),std::bind(&ManualControlNode::test_send, this));
+    ps4_subscriber = this->create_subscription<sensor_msgs::msg::Joy>(
+        "/jorsnyuorasniertsnieirsnyuoy", 10, std::bind(&ArmJoy::joy_callback, this, std::placeholders::_1));
+    arm_subscriber = this->create_subscription<rover_msgs::msg::ArmCommand>(
+        "/arm/feedback", 10, std::bind(&ArmJoy::arm_callback, this, std::placeholders::_1));
+}
+
+// Program Entry Point
 int main(int argc, char *argv[]) {
     rclcpp::init(argc, argv);
     auto node = std::make_shared<ArmJoy>();
-
-    // Example: send a command with a steering angle of 0.5 rad and speed of 1.0 m/s
-
 
     rclcpp::spin(node);
 
     rclcpp::shutdown();
     return 0;
+}
+
+
+// Member Functions
+
+void ArmJoy::joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg){
+    rover_msgs::msg::ArmCommand target;
+    target.positions.resize(NUM_JOINTS);
+    target.velocities.resize(NUM_JOINTS);
+    float target_positions[6];
+    float inputs[6];
+
+    inputs[2] = msg->axes[0];
+    inputs[1] = msg->axes[1];
+    inputs[0] = msg->axes[5] - msg->axes[2];
+    inputs[3] = msg->axes[3];
+    inputs[4] = msg->axes[4];
+    inputs[5] = msg->axes[6];
+    
+    if(CONTROL_MODE == POSITION_CONTROL){
+        target.cmd_type = 'P';
+
+    for (int i = 0; i < NUM_JOINTS; i++){
+        target.positions[i] = axes[i].position + inputs[i] * 10;
+    }
+    }else if(CONTROL_MODE == VELOCITY_CONTROL){
+        target.cmd_type = 'V';
+    for (int i = 0; i < NUM_JOINTS; i++){
+        target.velocities[i] = inputs[i] * 100;
+    }
+    }
+
+        arm_publisher->publish(target);
+
+}
+
+
+void ArmJoy::arm_callback(const rover_msgs::msg::ArmCommand::SharedPtr msg){
+
+    for (int i = 0; i < NUM_JOINTS; i++){
+        axes[i].position = msg->positions[i];
+        // axes[i].velocity = msg->velocities
+
+    }
 }

src/rover_samples/rv_sample_package/CMakeLists.txt

@@ -10,14 +10,15 @@ find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(joy REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(rover_utils REQUIRED)
 # uncomment the following section in order to fill in
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
 include_directories(include/)
 #add_executable adds a 'target' for CMake to go in and do it's magic
 #The target's name here is 'sample_node', and while the naming doesn't need to match the source files, its good practice to keep them the same as this is what you use with ros2 run
-add_executable(sample_node src/sample_node.cpp include/sample_node.h)
-ament_target_dependencies(sample_node rclcpp joy)
+add_executable(sample_node src/sample_node.cpp include/sample_node.h include/sample_node_defines.h)
+ament_target_dependencies(sample_node rclcpp joy rover_utils)
 
 
 

src/rover_samples/rv_sample_package/include/sample_node.h

@@ -1,6 +1,58 @@
-#define CONTROL_RATE 60.0
-#define GEAR_REVERSE 20
-#define GEAR_PARKING 22
-#define GEAR_NEUTRAL 1
-#define GEAR_1 2 //or drive
-#define GEAR_2 3 //guessing this is how autoware deals with manual cars
+// A little blurb at the top of your header or .cpp is always welcome!!
+//? Best to keep this blurb in header? probably?
+
+// Includes
+// If file is not in this directory, use <>
+// If file path is reative to this directory, use "". 
+#include <rover_utils/include/roverCommon.h> // roverCommon.h will get you set up with basic ros2 includes as well as our utility libraries. Some parts of the repo
+#include "sample_node_defines.h"
+// Other ros2 includes seperated by a line
+#include "sensor_msgs/msg/joy.hpp" 
+// cpp specific or other also seperated by a line
+#include <atomic>
+
+
+// Defines go after includes, but before class definitions
+// If you need a lot of defines, create a seperate defines file (see sample_node_defines.h for more info)
+#define CONSTANT_ONE 45
+#define MACRO(x) 45*x
+
+
+// Class definition
+class SampleNode : public rclcpp::Node {
+public:
+    SampleNode(); // Constructor is usually defined in .cpp. Its okay if you want to define it here instead. 
+
+    void send_command(float steering_angle, float speed, float acceleration, float jerk) {
+        //params can be refreshed at runtime
+        std::vector<std::string> expected_nodes = this->get_parameter("expected_nodes").as_string_array();
+
+    }
+
+    void send_gear_command(int gear){
+      
+    }
+
+
+private:
+    int current_gear = 0;
+    int prev_paddleR = 0;
+    int prev_paddleL = 0;
+
+    // rclcpp::Publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr command_publisher_;
+    // rclcpp::Publisher<autoware_auto_vehicle_msgs::msg::GearCommand>::SharedPtr gear_publisher_;
+    rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr g29_subscriber_;
+
+    rclcpp::TimerBase::SharedPtr timer_;
+
+    void joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg){
+        float steering_angle = msg->axes[0];
+        float speed = 99.0 - (msg->axes[3] + 1)*100;
+        float acceleration = (msg->axes[2] + 1)*20 - (msg->axes[3] + 1)*20;
+        float jerk = (msg->axes[2] + 1)*20 - (msg->axes[3] + 1)*20;
+        int paddleR = msg->buttons[4];
+        int paddleL = msg->buttons[5];
+        
+    
+    }
+};

src/rover_samples/rv_sample_package/include/sample_node_defines.h

@@ -0,0 +1,55 @@
+// Once your node needs to define a buncha stuff (more than 5 or 6 defines, but use your judgement) move the defines to a seperate file, <main header name>_defines.h
+#define CONTROL_RATE_HZ 60.0
+#define YOUR_MOMS_MASS_KG 3000 // Defines should have units in the name.
+
+
+
+//Sometimes, you want to define a constant with a type. in a #define, the preprocessor is what replaces the defines with the value.
+// For a constant to be type enforced by the compiler, the compiler needs to see it. 
+static constexpr bool ON = true;
+static constexpr bool OFF = false;
+
+
+// Now, we should also talk about enums:
+
+// Classic enum we've had since the 90s
+// Defaults to int type, but is not scoped by a class. Usage: REVERSE
+enum Gears {
+    REVERSE,
+    NEUTRAL,
+    ONE,
+    TWO
+}; 
+
+
+enum class GearsScoped { // Now, to use youd need the scope operator. Usage: GearsScoped::REVERSE
+    REVERSE,
+    NEUTRAL,
+    ONE,
+    TWO
+};
+
+// Can also enforce integer types:
+enum class GearsScoped_ : uint8_t { 
+    REVERSE = 0,
+    NEUTRAL = 1,
+    ONE, // would = 2
+    TWO
+};
+
+
+// Now, on the border of enums and defines, you can to scoped static constexprs using namespaces:
+namespace CarStuff {
+
+    static constexpr char SERIAL_PORT_PATH[] = "/dev/ttyUSB0";
+    static constexpr uint8_t NUM_WHEELS = 3;
+
+    // Nested is fine
+    namespace Gears {
+        static constexpr int REVERSE = -1;
+        static constexpr int NEUTRAL = 0;
+        static constexpr int ONE = 1;
+        static constexpr int TWO = 2;
+    }
+
+}