Merge pull request #43 from UBC-Snowbots/rowzaw/dev

sample packge updates

Author: Roozki

src/rover_samples/rv_sample_package/include/sample_node.h

@@ -7,52 +7,47 @@
 #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" 
+#include <sensor_msgs/msg/joy.hpp> 
+#include <geometry_msgs/msg/twist_stamped.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 TIMER_HZ 5
 #define MACRO(x) 45*x
+#define TIMER_PERIOD_MS (1.0/TIMER_HZ)
 
 
 // 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:
+    // Most member structures and functions are private in a custom ros2 node. 
+
     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_;
 
+    // SUBS AND PUBS
+    rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr cmd_vel_pub;
+
+    rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_subscriber;
+    void joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg); // Sub callback goes underneath subscriber definition
+
+    // OTHER ROS2 OBJECTS (Timers)
     rclcpp::TimerBase::SharedPtr timer_;
+    void timer_callback(); // Timer callbacks should go below timer def.
+
+    void send_generic_command(float steering_angle, float speed, float acceleration, float jerk);
+
+    void send_gear_command(int gear);
+
+      
 
-    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/src/sample_node.cpp

@@ -2,16 +2,19 @@
 #include "sample_node.h"
 
 // Constructor
-SampleNode::SampleNode() : Node("sample_node") { // We create a class using rclcpp::Node as a base class. You can still use another base class if you need, albeit sometimes with difficulties in passing args..
+SampleNode::SampleNode() : Node("sample_node") 
+{ // We create a class using rclcpp::Node as a base class. You can still use another base class if you need, albeit sometimes with difficulties in passing args..
+   
     // Quality of service example
     auto qos = rclcpp::QoS(rclcpp::KeepLast(1)).transient_local();
 
-    double period = 1.0/CONTROL_RATE_HZ;
-    // timer_ = this->create_wall_timer(
-    // std::chrono::duration<double>(period),std::bind(&ManualControlNode::test_send, this));
-    g29_subscriber_ = this->create_subscription<sensor_msgs::msg::Joy>(
+    joy_subscriber = this->create_subscription<sensor_msgs::msg::Joy>(
         "/joy", qos /* or an int for quick qos*/, std::bind(&SampleNode::joy_callback, this, std::placeholders::_1));
-
+    
+    cmd_vel_pub = this->create_publisher<geometry_msgs::msg::TwistStamped>("/cmd_vel", qos);
+        
+        timer_ = this->create_wall_timer(
+        std::chrono::duration<double>(TIMER_PERIOD_MS),std::bind(&SampleNode::timer_callback, this));
 
     // parameters:
     this->declare_parameter("param_name", std::vector<std::string>({"Watchdog"}));
@@ -20,7 +23,8 @@ SampleNode::SampleNode() : Node("sample_node") { // We create a class using rclc
 
 
 // Main function (entry point of node)
-int main(int argc, char *argv[]) {
+int main(int argc, char *argv[]) 
+{
     // Init ros2 with args. 
     rclcpp::init(argc, argv);
 
@@ -33,4 +37,45 @@ int main(int argc, char *argv[]) {
     // Code only reaches here if we crash or shutdown the node
     rclcpp::shutdown();
     return 0;
-}
+}
+
+// Member function implementations (sub and timer callbacks go first)
+
+void SampleNode::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];
+    send_generic_command(steering_angle, speed, acceleration, jerk);
+    
+}
+
+void SampleNode::timer_callback()
+{
+    std::cout << "Node is running (std cout)" << std::endl; // Standard C++ is always available
+    RCLCPP_INFO(this->get_logger(), "Node is running (ros2 info)"); // ROS2 (rclcpp) also has functions/macros that can replace some of the standard c++ workflow.
+    RCLCPP_WARN(this->get_logger(), "Node is running (ros2 warn)"); // ROS2 (rclcpp) also has functions that can replace some of the standard c++ workflow.
+    RCLCPP_ERROR(this->get_logger(), "Node is running (ros2 error)"); // ROS2 (rclcpp) also has functions that can replace some of the standard c++ workflow.
+}
+
+
+void SampleNode::send_generic_command(float steering_angle, float speed, float acceleration, float jerk) 
+{
+    // msg.steering angle = steering angle;
+    // etc...
+    // some_publisher.pub(msg); 
+}
+
+
+
+
+
+
+
+// Other examples/tips
+
+//params can be refreshed at runtime
+// std::vector<std::string> expected_nodes = this->get_parameter("expected_nodes").as_string_array();