Merge pull request #307 from QUT-Motorsport/composition+msgs

Composition overhaul

src/common/driverless_common/driverless_common/marker.py

@@ -1,7 +1,7 @@
 from math import sqrt
 
 from builtin_interfaces.msg import Duration
-from driverless_msgs.msg import Cone, ConeDetectionStamped, ConeWithCovariance, TrackDetectionStamped
+from driverless_msgs.msg import Cone, ConeDetectionStamped, ConeWithCovariance
 from geometry_msgs.msg import Point, Pose, Quaternion, Vector3
 from std_msgs.msg import ColorRGBA, Header
 from visualization_msgs.msg import Marker, MarkerArray
@@ -13,7 +13,7 @@
 MAX_NUM_CONES = 50
 
 
-def marker_array_from_map(detection: TrackDetectionStamped, ground_truth: bool = False) -> MarkerArray:
+def marker_array_from_map(detection: ConeDetectionStamped, ground_truth: bool = False) -> MarkerArray:
     cones: List[ConeWithCovariance] = detection.cones
 
     if ground_truth:  # ground truth markers out of the sim are translucent

src/common/driverless_msgs/CMakeLists.txt

@@ -18,26 +18,21 @@ find_package(geometry_msgs REQUIRED)
 
 # messages
 set(msg_files
-  "msg/BrickData.msg"
+  "msg/Can.msg"
   "msg/Cone.msg"
   "msg/ConeDetectionStamped.msg"
-  "msg/CarStatus.msg"
-  "msg/MotorRPM.msg"
+  "msg/ConeWithCovariance.msg"
+  "msg/DebugMsg.msg"
+  "msg/DoubleMatrix.msg"
+  "msg/DrivingDynamics1.msg"
+  "msg/Float32Stamped.msg"
+  "msg/Int32Stamped.msg"
   "msg/PathPoint.msg"
   "msg/PathStamped.msg"
-  "msg/Can.msg"
   "msg/RES.msg"
-  "msg/Reset.msg"
   "msg/Shutdown.msg"
   "msg/State.msg"
-  "msg/ConeWithCovariance.msg"
-  "msg/TrackDetectionStamped.msg"
-  "msg/SteeringReading.msg"
   "msg/SystemStatus.msg"
-  "msg/DrivingDynamics1.msg"
-  "msg/WSSVelocity.msg"
-  "msg/DebugMsg.msg"
-  "msg/DoubleMatrix.msg"
 )
 
 rosidl_generate_interfaces(${PROJECT_NAME}

src/common/driverless_msgs/msg/WSSVelocity.msg → src/common/driverless_msgs/msg/Float32Stamped.msg

@@ -1,2 +1,2 @@
 std_msgs/Header header
-float32 velocity
+float32 data

src/common/driverless_msgs/msg/Int32Stamped.msg

@@ -0,0 +1,2 @@
+std_msgs/Header header
+int32 data

src/control/velocity_controller/CMakeLists.txt

@@ -12,23 +12,40 @@ find_package(ackermann_msgs REQUIRED)
 find_package(driverless_msgs REQUIRED)
 find_package(rcl_interfaces REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_components REQUIRED)
 
-include_directories(include)
+include_directories(
+  include
+  ${CMAKE_SOURCE_DIR}/../../common/driverless_common/include
+)
 
 SET_SOURCE_FILES_PROPERTIES(${SOURCES} PROPERTIES LANGUAGE CXX )
 
-add_executable(velocity_controller_node src/node_velocity_controller.cpp ${SOURCES})
-target_include_directories(velocity_controller_node PUBLIC ${CMAKE_SOURCE_DIR}/../../common/driverless_common/include)
-target_include_directories(velocity_controller_node PUBLIC
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include>)
+# ROS 2 components
+add_library(velocity_controller_component SHARED src/component_velocity_controller.cpp)
+ament_target_dependencies(velocity_controller_component
+  "rclcpp"
+  "rclcpp_components"
+  "rcl_interfaces"
+  "std_msgs"
+  "nav_msgs"
+  "ackermann_msgs"
+  "driverless_msgs"
+)
+rclcpp_components_register_nodes(velocity_controller_component "velocity_controller::VelocityController")
 
+# ROS 2 nodes
+add_executable(velocity_controller_node src/node_velocity_controller.cpp)
+target_link_libraries(velocity_controller_node velocity_controller_component)
 ament_target_dependencies(velocity_controller_node
-  rclcpp
-  rcl_interfaces
-  nav_msgs
-  ackermann_msgs
-  driverless_msgs)
+  "rclcpp"
+)
+
+install(TARGETS
+  velocity_controller_component
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin)
 
 install(TARGETS velocity_controller_node
   DESTINATION lib/${PROJECT_NAME})

src/control/velocity_controller/config/velocity_controller.yaml

@@ -1,6 +1,6 @@
 velocity_controller_node:
   ros__parameters:
-      Kp: 0.05
+      Kp: 0.15
       Ki: 0.0
       max_integral_torque: 0.0
       histerisis_kickin_ms: 0.0

src/control/velocity_controller/include/component_velocity_controller.hpp

@@ -0,0 +1,66 @@
+#ifndef VELOCITY_CONTROLLER__COMPONENT_VELOCITY_CONTROLLER_HPP_
+#define VELOCITY_CONTROLLER__COMPONENT_VELOCITY_CONTROLLER_HPP_
+
+#include <iostream>
+
+#include "ackermann_msgs/msg/ackermann_drive_stamped.hpp"
+#include "driverless_common/common.hpp"
+#include "driverless_msgs/msg/float32_stamped.hpp"
+#include "driverless_msgs/msg/state.hpp"
+#include "rcl_interfaces/msg/set_parameters_result.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+using std::placeholders::_1;
+
+namespace velocity_controller {
+
+class VelocityController : public rclcpp::Node {
+   private:
+    float Kp_ = 0;
+    float Ki_ = 0;
+    float max_integral_torque_ = 0;
+    float histerisis_kickin_ms_ = 0;
+    float histerisis_reset_ms_ = 0;
+    float min_time_to_max_accel_sec_ = 0;
+    float max_accel_per_tick_ = 0;
+
+    float integral_error_ = 0;
+
+    rclcpp::TimerBase::SharedPtr controller_timer_;
+
+    rclcpp::Publisher<ackermann_msgs::msg::AckermannDriveStamped>::SharedPtr accel_pub_;
+    rclcpp::Subscription<ackermann_msgs::msg::AckermannDriveStamped>::SharedPtr ackermann_sub_;
+    rclcpp::Subscription<driverless_msgs::msg::State>::SharedPtr state_sub_;
+    rclcpp::Subscription<driverless_msgs::msg::Float32Stamped>::SharedPtr velocity_sub_;
+
+    std::shared_ptr<rclcpp::ParameterEventHandler> param_event_handler_;
+    std::shared_ptr<rclcpp::ParameterEventCallbackHandle> param_cb_handle_;
+
+    const int loop_ms_ = 10;
+
+    // Enable motors logic
+    driverless_msgs::msg::State::SharedPtr state_ = std::make_shared<driverless_msgs::msg::State>();
+    bool motors_enabled_ = false;
+    bool received_velocity_ = false;
+    bool received_ackermann_ = false;
+
+    float avg_velocity_;
+    ackermann_msgs::msg::AckermannDriveStamped::SharedPtr target_ackermann_ =
+        std::make_shared<ackermann_msgs::msg::AckermannDriveStamped>();
+    float prev_accel_ = 0;
+
+   public:
+    VelocityController(const rclcpp::NodeOptions& options);
+
+    void update_parameters(const rcl_interfaces::msg::ParameterEvent& event);
+
+    void ackermann_callback(const ackermann_msgs::msg::AckermannDriveStamped::SharedPtr msg);
+    void velocity_callback(const driverless_msgs::msg::Float32Stamped::SharedPtr msg);
+    void state_callback(const driverless_msgs::msg::State::SharedPtr msg);
+
+    void controller_callback();
+};
+
+}  // namespace velocity_controller
+
+#endif  // VELOCITY_CONTROLLER__COMPONENT_VELOCITY_CONTROLLER_HPP_