From 037e3c841a939e99c3381e13bf9440bb3fbd6020 Mon Sep 17 00:00:00 2001 From: acki Date: Sat, 4 Oct 2025 11:20:56 +0200 Subject: [PATCH] last up 2025 --- build.zsh | 2 +- simulated_pcb/odo.py | 30 +++-- src/modelec_com/src/pcb_action_interface.cpp | 14 +-- src/modelec_com/src/pcb_alim_interface.cpp | 2 +- src/modelec_com/src/pcb_odo_interface.cpp | 8 +- src/modelec_core/launch/launch_sim.launch.py | 74 ------------ src/modelec_core/launch/modelec.launch.py | 14 ++- .../launch/modelec_gamecontroller.launch.yml | 41 ------- ...amecontroller_remote_controller.launch.yml | 11 -- ...ec_gamecontroller_remote_server.launch.yml | 31 ----- src/modelec_core/launch/rsp.launch.py | 41 ------- .../launch/test.modelec.launch.py | 111 ------------------ .../launch/test.no_gui.modelec.launch.yml | 36 ------ .../launch/test.no_strat.modelec.launch.yml | 36 ------ src/modelec_strat/src/strat_fms.cpp | 4 - .../include/modelec_utils/config.hpp | 1 + .../include/modelec_utils/point.hpp | 2 +- 17 files changed, 47 insertions(+), 411 deletions(-) mode change 100644 => 100755 build.zsh delete mode 100644 src/modelec_core/launch/launch_sim.launch.py delete mode 100644 src/modelec_core/launch/modelec_gamecontroller.launch.yml delete mode 100644 src/modelec_core/launch/modelec_gamecontroller_remote_controller.launch.yml delete mode 100644 src/modelec_core/launch/modelec_gamecontroller_remote_server.launch.yml delete mode 100644 src/modelec_core/launch/rsp.launch.py delete mode 100644 src/modelec_core/launch/test.modelec.launch.py delete mode 100644 src/modelec_core/launch/test.no_gui.modelec.launch.yml delete mode 100644 src/modelec_core/launch/test.no_strat.modelec.launch.yml diff --git a/build.zsh b/build.zsh old mode 100644 new mode 100755 index 0a4c491..f783a7b --- a/build.zsh +++ b/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" diff --git a/simulated_pcb/odo.py b/simulated_pcb/odo.py index a9bec9a..f0145a4 100644 --- a/simulated_pcb/odo.py +++ b/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: - # self.ser.write(f'SET;POS;{int(self.x)};{int(self.y)};{self.theta:.5f}\n'.encode()) - self.last_send = now + if now - self.last_send > 0.1: + # print(f'[TX] SET;POS;{int(self.x)};{int(self.y)};{self.theta:.5f}') + 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") \ No newline at end of file diff --git a/src/modelec_com/src/pcb_action_interface.cpp b/src/modelec_com/src/pcb_action_interface.cpp index efd0b39..f39cdce 100644 --- a/src/modelec_com/src/pcb_action_interface.cpp +++ b/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"; - if (!Config::load(config_path)) - { - RCLCPP_ERROR(get_logger(), "Failed to load config file: %s", config_path.c_str()); - } + declare_parameter("serial_port", "/dev/USB_ACTION"); + declare_parameter("baudrate", 115200); + declare_parameter("name", "pcb_action"); auto request = std::make_shared(); - request->name = Config::get("config.usb.pcb.pcb_action.name", "pcb_action"); - request->bauds = Config::get("config.usb.pcb.pcb_action.baudrate", 115200); - request->serial_port = Config::get("config.usb.pcb.pcb_action.port", "/dev/ttyUSB0"); + request->name = get_parameter("name").as_string(); + request->bauds = get_parameter("baudrate").as_int(); + request->serial_port = get_parameter("serial_port").as_string(); auto client = this->create_client("add_serial_listener"); while (!client->wait_for_service(std::chrono::seconds(1))) diff --git a/src/modelec_com/src/pcb_alim_interface.cpp b/src/modelec_com/src/pcb_alim_interface.cpp index 0c325d4..d461e36 100644 --- a/src/modelec_com/src/pcb_alim_interface.cpp +++ b/src/modelec_com/src/pcb_alim_interface.cpp @@ -10,7 +10,7 @@ namespace Modelec // Service to create a new serial listener declare_parameter("serial_port", "/dev/ttyUSB0"); declare_parameter("baudrate", 115200); - declare_parameter("name", "pcb_action"); + declare_parameter("name", "pcb_alim"); auto request = std::make_shared(); request->name = get_parameter("name").as_string(); diff --git a/src/modelec_com/src/pcb_odo_interface.cpp b/src/modelec_com/src/pcb_odo_interface.cpp index bbaec9c..68c2716 100644 --- a/src/modelec_com/src/pcb_odo_interface.cpp +++ b/src/modelec_com/src/pcb_odo_interface.cpp @@ -108,7 +108,7 @@ namespace Modelec }); odo_add_waypoints_subscriber_ = this->create_subscription( - "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 data(msg->waypoints.size() * 5); + std::vector 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); } diff --git a/src/modelec_core/launch/launch_sim.launch.py b/src/modelec_core/launch/launch_sim.launch.py deleted file mode 100644 index cf68ac2..0000000 --- a/src/modelec_core/launch/launch_sim.launch.py +++ /dev/null @@ -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, - ]) \ No newline at end of file diff --git a/src/modelec_core/launch/modelec.launch.py b/src/modelec_core/launch/modelec.launch.py index 733ea4c..c525cd0 100644 --- a/src/modelec_core/launch/modelec.launch.py +++ b/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_action_interface', name='pcb_action_interface'), + Node(package='modelec_com', executable='pcb_odo_interface', name='pcb_odo_interface', parameters=[{ + '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 [] diff --git a/src/modelec_core/launch/modelec_gamecontroller.launch.yml b/src/modelec_core/launch/modelec_gamecontroller.launch.yml deleted file mode 100644 index 778cb12..0000000 --- a/src/modelec_core/launch/modelec_gamecontroller.launch.yml +++ /dev/null @@ -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' diff --git a/src/modelec_core/launch/modelec_gamecontroller_remote_controller.launch.yml b/src/modelec_core/launch/modelec_gamecontroller_remote_controller.launch.yml deleted file mode 100644 index cb24ac0..0000000 --- a/src/modelec_core/launch/modelec_gamecontroller_remote_controller.launch.yml +++ /dev/null @@ -1,11 +0,0 @@ -launch: - -- node: - pkg: 'joy' - exec: 'joy_node' - name: 'joy_node' - -- node: - pkg: 'modelec' - exec: 'gamecontroller_listener' - name: 'gamecontroller_listener' diff --git a/src/modelec_core/launch/modelec_gamecontroller_remote_server.launch.yml b/src/modelec_core/launch/modelec_gamecontroller_remote_server.launch.yml deleted file mode 100644 index eb508d8..0000000 --- a/src/modelec_core/launch/modelec_gamecontroller_remote_server.launch.yml +++ /dev/null @@ -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' diff --git a/src/modelec_core/launch/rsp.launch.py b/src/modelec_core/launch/rsp.launch.py deleted file mode 100644 index 5e02d68..0000000 --- a/src/modelec_core/launch/rsp.launch.py +++ /dev/null @@ -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 - ]) \ No newline at end of file diff --git a/src/modelec_core/launch/test.modelec.launch.py b/src/modelec_core/launch/test.modelec.launch.py deleted file mode 100644 index d623b75..0000000 --- a/src/modelec_core/launch/test.modelec.launch.py +++ /dev/null @@ -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), - ]) diff --git a/src/modelec_core/launch/test.no_gui.modelec.launch.yml b/src/modelec_core/launch/test.no_gui.modelec.launch.yml deleted file mode 100644 index 73d2156..0000000 --- a/src/modelec_core/launch/test.no_gui.modelec.launch.yml +++ /dev/null @@ -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' diff --git a/src/modelec_core/launch/test.no_strat.modelec.launch.yml b/src/modelec_core/launch/test.no_strat.modelec.launch.yml deleted file mode 100644 index c159f85..0000000 --- a/src/modelec_core/launch/test.no_strat.modelec.launch.yml +++ /dev/null @@ -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' diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index 920e45d..631c9aa 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -30,10 +30,6 @@ namespace Modelec spawn_id_sub_ = create_subscription( "/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_); diff --git a/src/modelec_utils/include/modelec_utils/config.hpp b/src/modelec_utils/include/modelec_utils/config.hpp index 62e7a59..8aba547 100644 --- a/src/modelec_utils/include/modelec_utils/config.hpp +++ b/src/modelec_utils/include/modelec_utils/config.hpp @@ -3,6 +3,7 @@ #include #include #include +#include namespace Modelec { diff --git a/src/modelec_utils/include/modelec_utils/point.hpp b/src/modelec_utils/include/modelec_utils/point.hpp index ebbe2c9..aa6b17c 100644 --- a/src/modelec_utils/include/modelec_utils/point.hpp +++ b/src/modelec_utils/include/modelec_utils/point.hpp @@ -19,4 +19,4 @@ namespace Modelec [[nodiscard]] Point GetTakeBasePosition() const; [[nodiscard]] Point GetTakeClosePosition() const; }; -} +} \ No newline at end of file