last up 2025

build.zsh

@@ -1,3 +1,3 @@
-colcon build --symlink-install --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON
+colcon build --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON
 
 source "install/setup.zsh"

simulated_pcb/odo.py

@@ -56,12 +56,12 @@ class SimulatedPCB:
                         self.waypoint_order.pop(0)
                         del self.waypoints[wp['id']]
                     else:
-                        speed = min(300.0, distance * 2)
+                        speed = min(100.0, distance / 2)
                         self.vx = speed * math.cos(angle)
                         self.vy = speed * math.sin(angle)
                         self.vtheta = max(-9.0, min(9.0, angle_diff * 2))
                 else:
-                    speed = min(400.0, distance * 3)
+                    speed = min(150.0, distance / 2)
                     self.vx = speed * math.cos(angle)
                     self.vy = speed * math.sin(angle)
                     self.vtheta = 0.0
@@ -80,9 +80,10 @@ class SimulatedPCB:
             self.theta += self.vtheta * dt
             self.theta = self.normalize_angle(self.theta)
 
-            if now - self.last_send > 0.1:
+        if now - self.last_send > 0.1:
-                # self.ser.write(f'SET;POS;{int(self.x)};{int(self.y)};{self.theta:.5f}\n'.encode())
+            # print(f'[TX] SET;POS;{int(self.x)};{int(self.y)};{self.theta:.5f}')
-                self.last_send = now
+            self.ser.write(f'SET;POS;{int(self.x)};{int(self.y)};{self.theta:.5f}\n'.encode())
+            self.last_send = now
 
     def run(self):
         while self.running:
@@ -119,11 +120,26 @@ class SimulatedPCB:
             print(f'[TX] OK;PID;1')
             self.ser.write(f'OK;PID;1\n'.encode())
 
-        elif msg.startswith("SET;START"):
+        elif msg.startswith("SET;START;"):
             self.start = msg.split(';')[2] == '1'
             print(f'[TX] OK;START;1')
             self.ser.write(f'OK;START;1\n'.encode())
 
+        elif msg.startswith("SET;WAYPOINT;"):
+            parts = msg.split(';')
+            if len(parts) >= 7:
+                wp = {
+                    'id': int(parts[2]),
+                    'type': int(parts[3]),
+                    'x': float(parts[4]),
+                    'y': float(parts[5]),
+                    'theta': float(parts[6])
+                }
+                self.waypoints[wp['id']] = wp
+                if wp['id'] not in self.waypoint_order:
+                    self.waypoint_order.append(wp['id'])
+                self.ser.write(f'OK;WAYPOINT;{wp["id"]}\n'.encode())
+
         elif msg.startswith("SET;WAYPOINTS;"):
             parts = msg.split(';')
             if len(parts) / 7 >= 1:
@@ -168,4 +184,4 @@ if __name__ == '__main__':
     except KeyboardInterrupt:
         if sim:
             sim.stop()
-        print("Simulation stopped")
+        print("Simulation stopped")

src/modelec_com/src/pcb_action_interface.cpp

@@ -8,16 +8,14 @@ namespace Modelec
     PCBActionInterface::PCBActionInterface() : Node("pcb_action_interface")
     {
         // Service to create a new serial listener
-        std::string config_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/config.xml";
+        declare_parameter<std::string>("serial_port", "/dev/USB_ACTION");
-        if (!Config::load(config_path))
+        declare_parameter<int>("baudrate", 115200);
-        {
+        declare_parameter<std::string>("name", "pcb_action");
-            RCLCPP_ERROR(get_logger(), "Failed to load config file: %s", config_path.c_str());
-        }
 
         auto request = std::make_shared<modelec_interfaces::srv::AddSerialListener::Request>();
-        request->name = Config::get<std::string>("config.usb.pcb.pcb_action.name", "pcb_action");
+        request->name = get_parameter("name").as_string();
-        request->bauds = Config::get<int>("config.usb.pcb.pcb_action.baudrate", 115200);
+        request->bauds = get_parameter("baudrate").as_int();
-        request->serial_port = Config::get<std::string>("config.usb.pcb.pcb_action.port", "/dev/ttyUSB0");
+        request->serial_port = get_parameter("serial_port").as_string();
 
         auto client = this->create_client<modelec_interfaces::srv::AddSerialListener>("add_serial_listener");
         while (!client->wait_for_service(std::chrono::seconds(1)))

src/modelec_com/src/pcb_alim_interface.cpp

@@ -10,7 +10,7 @@ namespace Modelec
         // Service to create a new serial listener
         declare_parameter<std::string>("serial_port", "/dev/ttyUSB0");
         declare_parameter<int>("baudrate", 115200);
-        declare_parameter<std::string>("name", "pcb_action");
+        declare_parameter<std::string>("name", "pcb_alim");
 
         auto request = std::make_shared<modelec_interfaces::srv::AddSerialListener::Request>();
         request->name = get_parameter("name").as_string();

src/modelec_com/src/pcb_odo_interface.cpp

@@ -108,7 +108,7 @@ namespace Modelec
             });
 
         odo_add_waypoints_subscriber_ = this->create_subscription<modelec_interfaces::msg::OdometryWaypoints>(
-            "odometry/add_waypoint", 30,
+            "odometry/add_waypoints", 30,
             [this](const modelec_interfaces::msg::OdometryWaypoints::SharedPtr msg)
             {
                 AddWaypointsCallback(msg);
@@ -134,7 +134,6 @@ namespace Modelec
             {
                 if (msg->data != start_odo_)
                 {
-                    RCLCPP_INFO(this->get_logger(), "Start Odo: %s", std::to_string(msg->data).c_str());
                     start_odo_ = msg->data;
                     SendOrder("START", {std::to_string(msg->data)});
                 }
@@ -235,7 +234,6 @@ namespace Modelec
             else if (tokens[1] == "WAYPOINT")
             {
             }
-
             else if (tokens[1] == "PID")
             {
             }
@@ -358,7 +356,7 @@ namespace Modelec
             start_odo_ = true;
         }
 
-        std::vector<std::string> data(msg->waypoints.size() * 5);
+        std::vector<std::string> data;
 
         for (const auto& wp : msg->waypoints)
         {
@@ -367,7 +365,7 @@ namespace Modelec
             data.push_back(std::to_string(wp.x));
             data.push_back(std::to_string(wp.y));
             data.push_back(std::to_string(wp.theta));
-        }
+       }
 
         SendOrder("WAYPOINT", data);
     }

src/modelec_core/launch/launch_sim.launch.py

@@ -1,74 +0,0 @@
-import os
-
-from ament_index_python.packages import get_package_share_directory
-
-
-from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration
-
-from launch_ros.actions import Node
-
-
-
-def generate_launch_description():
-
-
-    # Include the robot_state_publisher launch file, provided by our own package. Force sim time to be enabled
-    # !!! MAKE SURE YOU SET THE PACKAGE NAME CORRECTLY !!!
-
-    package_name='modelec' #<--- CHANGE ME
-
-    rsp = IncludeLaunchDescription(
-                PythonLaunchDescriptionSource([os.path.join(
-                    get_package_share_directory(package_name),'launch','rsp.launch.py'
-                )]), launch_arguments={'use_sim_time': 'true'}.items()
-    )
-
-    default_world = os.path.join(
-        get_package_share_directory(package_name),
-        'worlds',
-        'empty.world'
-        )    
-    
-    world = LaunchConfiguration('world')
-    world_arg = DeclareLaunchArgument(
-        'world',
-        default_value=default_world,
-        description='World to load'
-        )
-
-    # Include the Gazebo launch file, provided by the gazebo_ros package
-    gazebo = IncludeLaunchDescription(
-                PythonLaunchDescriptionSource([os.path.join(
-                    get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py')]),
-                    launch_arguments={'gz_args': ['-r -v4 ', world], 'on_exit_shutdown': 'true'}.items()
-            )
-
-    # Run the spawner node from the gazebo_ros package. The entity name doesn't really matter if you only have a single robot.
-    spawn_entity = Node(package='ros_gz_sim', executable='create',
-                        arguments=['-topic', 'robot_description',
-                                    '-name', 'my_bot',
-                                    '-z', '0.1'],
-                        output='screen')
-
-    bridge_params = os.path.join(get_package_share_directory(package_name),'config','gz_bridge.yaml')
-    ros_gz_bridge = Node(
-        package="ros_gz_bridge",
-        executable="parameter_bridge",
-        arguments=[
-            '--ros-args',
-            '-p',
-            f'config_file:={bridge_params}',
-        ]
-    )
-
-    # Launch them all!
-    return LaunchDescription([
-        rsp,
-        world_arg,
-        gazebo,
-        spawn_entity,
-        ros_gz_bridge,
-    ])

src/modelec_core/launch/modelec.launch.py

@@ -56,7 +56,7 @@ def generate_launch_description():
                     on_exit=[
                         TimerAction(
                             period=8.0,
-                            actions=[create_rplidar_node()]  # ✅ créer un NOUVEAU Node
+                            actions=[create_rplidar_node()]
                         )
                     ]
                 )
@@ -88,8 +88,16 @@ def generate_launch_description():
         if context.launch_configurations.get('with_com') == 'true':
             return [
                 Node(package='modelec_com', executable='serial_listener', name='serial_listener'),
-                Node(package='modelec_com', executable='pcb_odo_interface', name='pcb_odo_interface'),
+                Node(package='modelec_com', executable='pcb_odo_interface', name='pcb_odo_interface', parameters=[{
-                Node(package='modelec_com', executable='pcb_action_interface', name='pcb_action_interface'),
+                    'serial_port': "/tmp/ODO",
+                    'baudrate': 115200,
+                    'name': "pcb_odo",
+                }]),
+                Node(package='modelec_com', executable='pcb_action_interface', name='pcb_action_interface', parameters=[{
+                    'serial_port': "/tmp/ACTION",
+                    'baudrate': 115200,
+                    'name': "pcb_action",
+                }]),
             ]
         return []
 

src/modelec_core/launch/modelec_gamecontroller.launch.yml

@@ -1,41 +0,0 @@
-launch:
-
-- node:
-    pkg: 'modelec_com'
-    exec: "serial_listener"
-    name: "serial_listener"
-
-- node:
-      pkg: 'modelec_com'
-      exec: 'pcb_odo_interface'
-      name: 'pcb_odo_interface'
-
-- node:
-    pkg: 'modelec_com'
-    exec: 'pcb_alim_interface'
-    name: 'pcb_alim_interface'
-
-- node:
-    pkg: 'modelec_com'
-    exec: 'pca9685_controller'
-    name: 'pca9685_controller'
-
-- node:
-    pkg: 'modelec'
-    exec: 'solenoid_controller'
-    name: 'solenoid_controller'
-
-- node:
-    pkg: 'modelec'
-    exec: 'arm_controller'
-    name: 'arm_controller'
-
-- node:
-    pkg: 'joy'
-    exec: 'joy_node'
-    name: 'joy_node'
-
-- node:
-    pkg: 'modelec'
-    exec: 'gamecontroller_listener'
-    name: 'gamecontroller_listener'

src/modelec_core/launch/modelec_gamecontroller_remote_controller.launch.yml

@@ -1,11 +0,0 @@
-launch:
-
-- node:
-    pkg: 'joy'
-    exec: 'joy_node'
-    name: 'joy_node'
-
-- node:
-    pkg: 'modelec'
-    exec: 'gamecontroller_listener'
-    name: 'gamecontroller_listener'

src/modelec_core/launch/modelec_gamecontroller_remote_server.launch.yml

@@ -1,31 +0,0 @@
-launch:
-
-- node:
-    pkg: 'modelec_com'
-    exec: "serial_listener"
-    name: "serial_listener"
-
-- node:
-      pkg: 'modelec_com'
-      exec: 'pcb_odo_interface'
-      name: 'pcb_odo_interface'
-
-- node:
-    pkg: 'modelec_com'
-    exec: 'pcb_alim_interface'
-    name: 'pcb_alim_interface'
-
-- node:
-    pkg: 'modelec_com'
-    exec: 'pca9685_controller'
-    name: 'pca9685_controller'
-
-- node:
-    pkg: 'modelec'
-    exec: 'solenoid_controller'
-    name: 'solenoid_controller'
-
-- node:
-    pkg: 'modelec'
-    exec: 'arm_controller'
-    name: 'arm_controller'

src/modelec_core/launch/rsp.launch.py

@@ -1,41 +0,0 @@
-import os
-
-from ament_index_python.packages import get_package_share_directory
-
-from launch import LaunchDescription
-from launch.substitutions import LaunchConfiguration
-from launch.actions import DeclareLaunchArgument
-from launch_ros.actions import Node
-
-import xacro
-
-
-def generate_launch_description():
-
-    # Check if we're told to use sim time
-    use_sim_time = LaunchConfiguration('use_sim_time')
-
-    # Process the URDF file
-    pkg_path = os.path.join(get_package_share_directory('modelec'))
-    xacro_file = os.path.join(pkg_path,'description','robot.urdf.xacro')
-    robot_description_config = xacro.process_file(xacro_file)
-    
-    # Create a robot_state_publisher node
-    params = {'robot_description': robot_description_config.toxml(), 'use_sim_time': use_sim_time}
-    node_robot_state_publisher = Node(
-        package='robot_state_publisher',
-        executable='robot_state_publisher',
-        output='screen',
-        parameters=[params]
-    )
-
-
-    # Launch!
-    return LaunchDescription([
-        DeclareLaunchArgument(
-            'use_sim_time',
-            default_value='false',
-            description='Use sim time if true'),
-
-        node_robot_state_publisher
-    ])

src/modelec_core/launch/test.modelec.launch.py

@@ -1,111 +0,0 @@
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, OpaqueFunction, TimerAction, RegisterEventHandler, Shutdown
-from launch.event_handlers import OnProcessExit
-from launch_ros.actions import Node
-from launch.substitutions import LaunchConfiguration
-from ament_index_python.packages import get_package_share_directory
-import os
-
-def generate_launch_description():
-    # Declare launch arguments
-    with_gui_arg = DeclareLaunchArgument('with_gui', default_value='true', description='Launch GUI?')
-    with_rplidar_arg = DeclareLaunchArgument('with_rplidar', default_value='true', description='Launch RPLIDAR?')
-    with_com_arg = DeclareLaunchArgument('with_com', default_value='true', description='Launch COM nodes?')
-    with_strat_arg = DeclareLaunchArgument('with_strat', default_value='true', description='Launch strategy nodes?')
-
-    # Get launch configs
-    with_gui = LaunchConfiguration('with_gui')
-    with_rplidar = LaunchConfiguration('with_rplidar')
-    with_com = LaunchConfiguration('with_com')
-    with_strat = LaunchConfiguration('with_strat')
-
-    # RPLIDAR Node parameters
-    rplidar_serial_port = LaunchConfiguration('serial_port', default='/dev/ttyUSB0')
-    rplidar_serial_baudrate = LaunchConfiguration('serial_baudrate', default='115200')
-    rplidar_frame_id = LaunchConfiguration('frame_id', default='laser')
-    rplidar_inverted = LaunchConfiguration('inverted', default='false')
-    rplidar_angle_compensate = LaunchConfiguration('angle_compensate', default='true')
-    rplidar_scan_mode = LaunchConfiguration('scan_mode', default='Sensitivity')
-
-    # Function to launch RPLIDAR Node directly
-    def launch_rplidar_node(context, *args, **kwargs):
-        if context.launch_configurations.get('with_rplidar') == 'true':
-            rplidar_node = Node(
-                package='rplidar_ros',
-                executable='rplidar_node',
-                name='rplidar_node',
-                parameters=[{
-                    'serial_port': rplidar_serial_port,
-                    'serial_baudrate': rplidar_serial_baudrate,
-                    'frame_id': rplidar_frame_id,
-                    'inverted': rplidar_inverted,
-                    'angle_compensate': rplidar_angle_compensate,
-                    'scan_mode': rplidar_scan_mode
-                }],
-                output='screen'
-            )
-
-            # Register event handler to restart the node if it crashes
-            restart_handler = RegisterEventHandler(
-                OnProcessExit(
-                    target_action=rplidar_node,
-                    on_exit=[
-                        TimerAction(
-                            period=5.0,  # Delay before restarting the node (in seconds)
-                            actions=[rplidar_node]  # Restart the RPLIDAR node
-                        )
-                    ]
-                )
-            )
-
-            return [rplidar_node, restart_handler]
-        return []
-
-    # Function to launch GUI and shutdown handler
-    def launch_gui(context, *args, **kwargs):
-        if context.launch_configurations.get('with_gui') == 'true':
-            gui = Node(
-                package='modelec_gui',
-                executable='modelec_gui',
-                name='modelec_gui'
-            )
-            shutdown = RegisterEventHandler(
-                OnProcessExit(
-                    target_action=gui,
-                    on_exit=[Shutdown()]
-                )
-            )
-            return [gui, shutdown]
-        return []
-
-    # Function to launch COM nodes
-    def launch_com(context, *args, **kwargs):
-        if context.launch_configurations.get('with_com') == 'true':
-            return [
-                Node(package='modelec_com', executable='serial_listener', name='serial_listener'),
-                Node(package='modelec_com', executable='pcb_odo_interface', name='pcb_odo_interface'),
-                Node(package='modelec_com', executable='pcb_action_interface', name='pcb_action_interface'),
-            ]
-        return []
-
-    # Function to launch strategy nodes
-    def launch_strat(context, *args, **kwargs):
-        if context.launch_configurations.get('with_strat') == 'true':
-            return [
-                Node(package='modelec_strat', executable='strat_fsm', name='strat_fsm'),
-                Node(package='modelec_strat', executable='pami_manager', name='pami_manager'),
-                Node(package='modelec_strat', executable='enemy_manager', name='enemy_manager'),
-            ]
-        return []
-
-    return LaunchDescription([
-        with_gui_arg,
-        with_rplidar_arg,
-        with_com_arg,
-        with_strat_arg,
-
-        OpaqueFunction(function=launch_rplidar_node),
-        OpaqueFunction(function=launch_gui),
-        OpaqueFunction(function=launch_com),
-        OpaqueFunction(function=launch_strat),
-    ])

src/modelec_core/launch/test.no_gui.modelec.launch.yml

@@ -1,36 +0,0 @@
-launch:
-
-  - node:
-      pkg: 'modelec_com'
-      exec: "serial_listener"
-      name: "serial_listener"
-
-  - node:
-      pkg: 'modelec_com'
-      exec: 'pcb_odo_interface'
-      name: 'pcb_odo_interface'
-
-  - node:
-      pkg: 'modelec_com'
-      exec: 'pcb_alim_interface'
-      name: 'pcb_alim_interface'
-
-  - node:
-      pkg: 'modelec_com'
-      exec: 'pcb_action_interface'
-      name: 'pcb_action_interface'
-
-  - node:
-      pkg: 'modelec_core'
-      exec: 'speed_result'
-      name: 'speed_result'
-
-  - node:
-      pkg: 'modelec_strat'
-      exec: 'strat_fsm'
-      name: 'strat_fsm'
-
-  - node:
-      pkg: 'modelec_strat'
-      exec: 'pami_manager'
-      name: 'pami_manager'

src/modelec_core/launch/test.no_strat.modelec.launch.yml

@@ -1,36 +0,0 @@
-launch:
-
-  - node:
-      pkg: 'modelec_com'
-      exec: "serial_listener"
-      name: "serial_listener"
-
-  - node:
-      pkg: 'modelec_com'
-      exec: 'pcb_odo_interface'
-      name: 'pcb_odo_interface'
-
-  - node:
-      pkg: 'modelec_com'
-      exec: 'pcb_alim_interface'
-      name: 'pcb_alim_interface'
-
-  - node:
-      pkg: 'modelec_com'
-      exec: 'pcb_action_interface'
-      name: 'pcb_action_interface'
-
-  - node:
-      pkg: 'modelec_gui'
-      exec: "modelec_gui"
-      name: "modelec_gui"
-
-  - node:
-      pkg: 'modelec_core'
-      exec: 'speed_result'
-      name: 'speed_result'
-
-  - node:
-      pkg: 'modelec_strat'
-      exec: 'pami_manager'
-      name: 'pami_manager'

src/modelec_strat/src/strat_fms.cpp

@@ -30,10 +30,6 @@ namespace Modelec
         spawn_id_sub_ = create_subscription<modelec_interfaces::msg::Spawn>(
             "/strat/spawn", 10, [this](const modelec_interfaces::msg::Spawn::SharedPtr msg)
             {
-                // std_msgs::msg::Bool tir_msg;
-                // tir_msg.data = true;
-                // tir_arm_set_pub_->publish(tir_msg);
-
                 team_selected_ = true;
                 team_id_ = msg->team_id;
                 nav_->SetTeamId(team_id_);

src/modelec_utils/include/modelec_utils/config.hpp

@@ -3,6 +3,7 @@
 #include <string>
 #include <tinyxml2.h>
 #include <unordered_map>
+#include <sstream>
 
 namespace Modelec
 {

src/modelec_utils/include/modelec_utils/point.hpp

@@ -19,4 +19,4 @@ namespace Modelec
         [[nodiscard]] Point GetTakeBasePosition() const;
         [[nodiscard]] Point GetTakeClosePosition() const;
     };
-}
+}