Merge branch 'main' into fix/cone-ground-plane-intersection

docs/debugging.md

@@ -22,7 +22,7 @@ The last thing to do is to actually create a configuration to connect to the ser
 1. Create `Remote debug` configuration
 2. Use `Bundled GDB`
 3. `target args`: `:1234` (or any other port you set before)
-4. `symbol file`: the executable of your node, e.g. `.../DUT22_SASSy/install/fake_sensors/lib/fake_sensors/fake_sensors`
+4. `symbol file`: the executable of your node, e.g. `.../DUT22-AVA/install/fake_sensors/lib/fake_sensors/fake_sensors`
 
 Then start your launch file and your debugger and everything should work!
 

src/ava/ava_fake_trackdrive/CMakeLists.txt

@@ -0,0 +1,25 @@
+cmake_minimum_required(VERSION 3.14)
+project(ava_fake_trackdrive)
+
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+    add_compile_options(-Wall -Wextra -Wpedantic)
+endif ()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(embedded_msgs REQUIRED)
+
+include_directories(include)
+
+add_executable(ava_fake_trackdrive_node src/trackdrive.cpp)
+ament_target_dependencies(ava_fake_trackdrive_node rclcpp embedded_msgs)
+
+install(TARGETS
+        ava_fake_trackdrive_node
+        DESTINATION lib/${PROJECT_NAME})
+
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
+
+ament_package()

src/ava/ava_fake_trackdrive/include/trackdrive.hpp

@@ -0,0 +1,18 @@
+#ifndef AVA_PLANNING_NODE_HPP
+#define AVA_PLANNING_NODE_HPP
+
+#include "embedded_msgs/msg/trajectory_setpoints.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+class AvaPlanningNode : public rclcpp::Node {
+   public:
+    AvaPlanningNode();
+
+    void on_tick();
+
+   private:
+    rclcpp::TimerBase::SharedPtr run_timer;
+    rclcpp::Publisher<embedded_msgs::msg::TrajectorySetpoints>::SharedPtr trajectory_publisher;
+};
+
+#endif

src/ava/ava_fake_trackdrive/launch/trackdrive.launch.xml

@@ -0,0 +1,5 @@
+<?xml version="1.0"?>
+<launch>
+    <node pkg="ava_fake_trackdrive" exec="ava_fake_trackdrive_node" name="ava_fake_trackdrive_node" respawn="true"
+          output="screen"/>
+</launch>

src/ava/ava_fake_trackdrive/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+    <name>ava_fake_trackdrive</name>
+    <version>0.0.0</version>
+    <description>>ava_fake_trackdrive</description>
+
+    <maintainer email="dut@dut.dut">DUT</maintainer>
+    <license>Apache License 2.0</license>
+
+    <buildtool_depend>ament_cmake</buildtool_depend>
+
+    <depend>rclcpp</depend>
+    <depend>embedded_msgs</depend>
+
+    <test_depend>ament_lint_auto</test_depend>
+    <test_depend>ament_lint_common</test_depend>
+
+    <export>
+        <build_type>ament_cmake</build_type>
+    </export>
+</package>

src/ava/ava_fake_trackdrive/src/trackdrive.cpp

@@ -0,0 +1,31 @@
+#include "trackdrive.hpp"
+
+#include <chrono>
+
+using namespace std::chrono_literals;
+
+AvaPlanningNode::AvaPlanningNode() : Node("ava_planning_node") {
+    run_timer = rclcpp::create_timer(this, get_clock(), 100ms, std::bind(&AvaPlanningNode::on_tick, this));
+    trajectory_publisher = create_publisher<embedded_msgs::msg::TrajectorySetpoints>("/embedded/to/TrajectorySetpoints", 1);
+}
+
+void AvaPlanningNode::on_tick() {
+    embedded_msgs::msg::TrajectorySetpoints setpoints;
+    setpoints.header.stamp = now();
+    setpoints.acceleration = 15;
+    setpoints.steer_angle = 0.5f;
+    trajectory_publisher->publish(setpoints);
+}
+
+int main(int argc, char **argv) {
+    rclcpp::init(argc, argv);
+    auto ava_planning_node = std::make_shared<AvaPlanningNode>();
+    RCLCPP_INFO(ava_planning_node->get_logger(), "Ava Node Main Setup");
+
+    rclcpp::Rate loop_rate(50.0);
+    while (rclcpp::ok()) {
+        rclcpp::spin_some(ava_planning_node);
+        loop_rate.sleep();
+    }
+    return 0;
+}

src/mission_control/launch/trackdrive.launch.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0"?>
+<launch>
+    <group>
+        <include file="$(find-pkg-share ava_fake_trackdrive)/launch/trackdrive.launch.xml"/>
+    </group>
+</launch>

src/mission_control/mission_control/mission_control.py

@@ -54,10 +54,6 @@ def mission_callback(self, msg):
             self.get_logger().info("[MissionControl] Launching Inspection Test")
             self.launch("inspection")
 
-        if msg.mission == 101:
-            self.get_logger().info("[MissionControl] Launching Sassy")
-            self.launch("sassy")
-
 
 def main(args=None):
     rclpy.init(args=args)

src/simulation/fake_embedded/fake_embedded/fake_asp.py

@@ -36,7 +36,7 @@ def __init__(self):
         super().__init__("state_machine")
 
         self._mission = self.declare_parameter("mission_name", "acceleration").get_parameter_value().string_value
-        self._mission_map = {"acceleration": 1, "skidpad": 2, "autocross": 3, "trackdrive": 4, "ebstest": 5, "inspection": 6, "idle": 8, "sassy": 101}
+        self._mission_map = {"acceleration": 1, "skidpad": 2, "autocross": 3, "trackdrive": 4, "ebstest": 5, "inspection": 6, "idle": 8}
 
         # Less states than in the car since when sim is launched we go to driving immediately
         self._state_map = {"AS_Off": 28, "AS_Ready": 29, "AS_Driving": 30, "AS_Finished": 31, "AS_Emergency": 32, "Manual_Driving": 33}

src/simulation/simulator/CMakeLists.txt

@@ -24,7 +24,8 @@ find_package(fake_dynamics REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(sim_bags REQUIRED)
-find_package(sassy_fake_planning REQUIRED)
+
+find_package(ava_fake_trackdrive REQUIRED)
 
 include_directories(include)
 

src/simulation/simulator/launch/simulation.launch.xml

@@ -5,7 +5,7 @@
 
     <!-- mission_name has to be one of the missions on the dashboard (competition events currently). This impacts the track that will be layed out. -->
     <arg name="mission_name"
-         default="sassy"/> <!-- acceleration, skidpad, autocross, trackdrive, ebstest, inspection, idle -->
+         default="trackdrive"/> <!-- acceleration, skidpad, autocross, trackdrive, ebstest, inspection, idle -->
 
     <!-- Do you want to show the simulation in Rviz? -->
     <arg name="rviz" default="true"/>
@@ -83,10 +83,6 @@
         <include file="$(find-pkg-share fake_sensors)/launch/fake_sensors.launch.xml"/>
     </group>
 
-    <group>
-        <include file="$(find-pkg-share fake_lidar)/launch/fake_lidar.launch.xml"/>
-    </group>
-
     <!-- Simulate if we are not playing bag from a bag -->
     <group if="$(eval '\'$(var playback_bag)\' == \'\'')">
         <include file="$(find-pkg-share fake_perception)/launch/fake_perception.launch.xml">
@@ -124,10 +120,6 @@
     <!-- this is simulating the launch sequence on the car partially (fake Luke). Until there is an idle mode to launch in, this node will have to be killed or the file will have to be changed so that it allows users to test open-loop tests, joystick, etc-->
     <node pkg="mission_control" name="mission_control" exec="mission_control" output="screen"/>
 
-    <!--    <group>-->
-    <!--        <include file="$(find-pkg-share sassy_perception)/launch/sassy_perception.launch.xml"/>-->
-    <!--    </group>-->
-
     <group>
         <group if="$(var rviz)">
             <include file="$(find-pkg-share simulator)/launch/viz.launch.xml">

src/simulation/simulator/package.xml

@@ -26,9 +26,9 @@
     <build_depend>fake_sensors</build_depend>
     <build_depend>fake_embedded</build_depend>
     <build_depend>fake_perception</build_depend>
-    <build_depend>fake_lidar</build_depend>
 
-    <build_depend>sassy_fake_planning</build_depend>
+    <build_depend>ava_fake_trackdrive</build_depend>
+
     <test_depend>ament_lint_auto</test_depend>
     <test_depend>ament_lint_common</test_depend>
     <test_depend>ament_cmake_gtest</test_depend>

src/simulation/simulator/simulator/sim_master.py

@@ -71,7 +71,7 @@ def __init__(self):
 
     def set_track(self, dashboard_mission):
         # Mission is converted to a track_name we want the track service to load
-        if dashboard_mission == "autocross" or dashboard_mission == "trackdrive" or dashboard_mission == "sassy":
+        if dashboard_mission == "autocross" or dashboard_mission == "trackdrive":
             # The name of the yaml files containing cones, time-keeping equipment
             if self._track_requested == "":
                 TrackGenerator()

src/simulation/simulator/src/sim_viz.cpp

@@ -292,12 +292,12 @@ void SimViz::addConeMarker(const rclcpp::Time &time, const sim_msgs::msg::TrackO
 
     } else if (track_object.object_type == sim_msgs::msg::TrackObject::ORANGE) {
         marker.color.r = 1.0;
-        marker.color.g = static_cast<float>(165.0 / 255.0);
+        marker.color.g = 0.647;
         marker.color.b = 0.0;
         marker.mesh_resource = "package://simulator/meshes/track/cone_orange.dae";
     } else if (track_object.object_type == sim_msgs::msg::TrackObject::BIG_ORANGE) {
         marker.color.r = 1.0;
-        marker.color.g = static_cast<float>(165.0 / 255.0);
+        marker.color.g = 0.647;
         marker.color.b = 0.0;
         marker.mesh_resource = "package://simulator/meshes/track/cone_orange_big.dae";
     } else if (track_object.object_type == sim_msgs::msg::TrackObject::TK_DEVICE) {
@@ -340,8 +340,12 @@ void SimViz::addWorldStateMarker(const cv_msgs::msg::Cone &cone, u_int32_t id, v
         marker.color.r = 1.0;
         marker.color.g = 1.0;
         marker.color.b = 0.0;
-    } else {
+    } else if (cone_type == cv_msgs::msg::Cone::ORANGE) {
         marker.color.r = 1.0;
+        marker.color.g = 0.647;
+        marker.color.b = 0.0;
+    } else {
+        marker.color.r = 0.0;
         marker.color.g = 0.0;
         marker.color.b = 0.0;
     }