added desired heading callback

Author: lross03

src/network_systems/projects/can_transceiver/src/can_frame_parser.cpp

@@ -747,7 +747,8 @@ std::string DesiredHeading::debugStr() const
 std::string DesiredHeading::toString() const
 {
     std::stringstream ss;
-    ss << "[DESIRED HEADING] Heading: " << heading_;
+    ss << "[DESIRED HEADING] Heading: " << heading_ << "\n" 
+       << "Steering bit: " << steering_ << "\n";
     return ss.str();
 }
 

src/network_systems/projects/can_transceiver/src/can_transceiver_ros_intf.cpp

@@ -89,6 +89,9 @@ class CanTransceiverIntf : public NetNode
 
             sail_cmd_sub_ = this->create_subscription<msg::SailCmd>(
               ros_topics::SAIL_CMD, QUEUE_SIZE, [this](msg::SailCmd sail_cmd_) { subSailCmdCb(sail_cmd_); });
+            desired_heading_sub_ = this->create_subscription<msg::DesiredHeading>(
+              ros_topics::DESIRED_HEADING, QUEUE_SIZE,
+              [this](msg::DesiredHeading desired_heading_) { subDesiredHeading(desired_heading_); });
 
             if (mode == SYSTEM_MODE::DEV) {  // Initialize the CAN Sim Intf
                 mock_ais_sub_ = this->create_subscription<msg::AISShips>(
@@ -129,6 +132,7 @@ class CanTransceiverIntf : public NetNode
     rclcpp::Publisher<msg::GenericSensors>::SharedPtr    generic_sensors_pub_;
     msg::GenericSensors                                  generic_sensors_;
     rclcpp::Subscription<msg::DesiredHeading>::SharedPtr desired_heading_sub_;
+    msg::DesiredHeading                                  desired_heading_;
     rclcpp::Subscription<msg::SailCmd>::SharedPtr        sail_cmd_sub_;
     msg::SailCmd                                         sail_cmd_;
 
@@ -213,7 +217,7 @@ class CanTransceiverIntf : public NetNode
      * @brief Publish a GPS frame
      *        Intended to be registered as a callback with the CAN Tranceiver instance
      *
-     * @param gps_frame gps CAN rfame read from the CAN bus
+     * @param gps_frame gps CAN frame read from the CAN bus
      */
     void publishGPS(const CanFrame & gps_frame)
     {
@@ -307,6 +311,23 @@ class CanTransceiverIntf : public NetNode
         RCLCPP_INFO(this->get_logger(), "%s %s", getCurrentTimeString().c_str(), ss.str().c_str());
     }
 
+    /**
+     * @brief Subscribe to a MAIN_HEADING frame
+     *        Intended to be registered as a callback with the CAN Tranceiver instance
+     *
+     * @param heading_frame main_heading CAN frame read from the CAN bus
+     */
+    void subDesiredHeading(msg::DesiredHeading & desired_heading_input)
+    {
+        msg::HelperHeading helper_msg;
+        helper_msg = desired_heading_input.heading;
+        desired_heading_.set__heading(helper_msg);
+        desired_heading_.set__steering(desired_heading_input.steering);
+        auto main_heading_frame = CAN_FP::DesiredHeading(desired_heading_, CanId::MAIN_HEADING);
+        can_trns_->send(main_heading_frame.toLinuxCan());
+        RCLCPP_INFO(this->get_logger(), "%s %s", getCurrentTimeString().c_str(), main_heading_frame.toString().c_str());
+    }
+
     // SIMULATION CALLBACKS //
 
     /**