Merging message-rollback

Author: alyashour

CMakeLists.txt

@@ -38,6 +38,11 @@ install(TARGETS
   DESTINATION lib/${PROJECT_NAME}
 )
 
+# copy the config dir over as well
+install(DIRECTORY config
+  DESTINATION share/${PROJECT_NAME}
+)
+
 if(BUILD_TESTING)
   # no auto linting for now
 endif()

control_node_cfg.csv config/control_node_cfg.csvcontrol_node_cfg.csv renamed to config/control_node_cfg.csv

@@ -13,6 +13,11 @@
 
 #include "ap1_msgs/msg/speed_profile_stamped.hpp"
 #include "ap1_msgs/msg/target_path_stamped.hpp"
+<<<<<<< HEAD
+=======
+#include "ap1_msgs/msg/turn_angle_stamped.hpp"
+#include "ap1_msgs/msg/vehicle_speed_stamped.hpp"
+>>>>>>> message-rollback
 #include "ap1_msgs/msg/float_stamped.hpp"
 
 #include "ap1/control/ackermann_controller.hpp"
@@ -34,8 +39,6 @@ class ControlNode : public rclcpp::Node
     AckermannController ackermann_controller_;
 
     // Memory
-    // half these types are very unnecessary, we should just have stampedfloat or
-    // stamped double or something
     ap1_msgs::msg::SpeedProfileStamped speed_profile_;
     ap1_msgs::msg::TargetPathStamped target_path_;
     ap1_msgs::msg::FloatStamped vehicle_speed_;

include/ap1/control/control_node.hpp

@@ -11,6 +11,8 @@
 
 #include "ap1_msgs/msg/speed_profile_stamped.hpp"
 #include "ap1_msgs/msg/target_path_stamped.hpp"
+#include "ap1_msgs/msg/turn_angle_stamped.hpp"
+#include "ap1_msgs/msg/vehicle_speed_stamped.hpp"
 #include "ap1_msgs/msg/float_stamped.hpp"
 
 #include "ap1/control/control_node.hpp"
@@ -23,6 +25,7 @@ using namespace ap1::control;
 
 void ControlNode::on_speed_profile(const SpeedProfileStamped speed_profile)
 {
+
     speed_profile_ = speed_profile;
 }
 
@@ -31,20 +34,20 @@ void ControlNode::on_path(const TargetPathStamped target_path)
     target_path_ = target_path;
 }
 
-void ControlNode::on_speed(const FloatStamped & msg)
+void ControlNode::on_speed(const FloatStamped speed)
 {
     vehicle_speed_ = msg.value;
 }
 
-void ControlNode::on_turn_angle(const FloatStamped & msg)
+void ControlNode::on_turn_angle(const FloatStamped turn_angle)
 {
     vehicle_turn_angle = msg.value;
 }
 
 void ControlNode::control_loop_callback()
 {
     // the car's current velocity. we only support moving forward atp
-    const vec3f velocity(this->vehicle_speed_, 0, 0); // +x is always forward on the car
+    const vec3f velocity(this->vehicle_speed_.value, 0, 0); // +x is always forward on the car
 
     const bool PATH_IS_STALE = false, SPEED_PROFILE_IS_STALE = false; // TEMP
 
@@ -66,17 +69,13 @@ void ControlNode::control_loop_callback()
         velocity, vec2f(next_waypoint.x, next_waypoint.y), speed_profile_.speeds.at(0));
 
     // log
-    std::string s = "ACC: " + std::to_string(acc.x) + ", " + std::to_string(acc.y) + ", " +
-                    std::to_string(acc.z);
-    RCLCPP_INFO(this->get_logger(), s.c_str());
+    // RCLCPP_INFO(this->get_logger(), "ACC: %.2f, %.2f, %.2f", acc.x, acc.y, acc.z);
 
     // compute acc and throttle using ackermann controller
     AckermannController::Command cmd = ackermann_controller_.compute_command(acc, velocity);
 
     // log
-    s = "CMD: {throttle:" + std::to_string(cmd.throttle) +
-        ", steering:" + std::to_string(cmd.steering) + "}";
-    RCLCPP_INFO(this->get_logger(), s.c_str());
+    // RCLCPP_INFO(this->get_logger(), "CMD: {throttle: %.2f, steering: %.2f}", cmd.throttle, cmd.steering);
 
     // pack the turn angle into a message
     FloatStamped turn_msg;
@@ -89,7 +88,6 @@ void ControlNode::control_loop_callback()
     pwr_msg.header.stamp = this->now();
     pwr_msg.header.frame_id = "base_link";
     pwr_msg.value = cmd.throttle; // [-1, 1]
-    // pwr_msg.power = 1.0f;
 
     // send both messages out
     turning_angle_pub_->publish(turn_msg);