From a2af5c091606cdca742327d6c5180e92099bce05 Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 15 May 2025 20:39:22 -0400 Subject: [PATCH] action v1 --- simulated_pcb/action.py | 17 +++--- src/modelec_com/src/pcb_action_interface.cpp | 7 +-- src/modelec_core/launch/modelec.launch.py | 61 +++++++++++-------- src/modelec_strat/data/config.xml | 6 +- src/modelec_strat/src/action_executor.cpp | 13 +--- .../src/missions/banner_mission.cpp | 2 +- src/modelec_strat/src/navigation_helper.cpp | 4 ++ 7 files changed, 57 insertions(+), 53 deletions(-) diff --git a/simulated_pcb/action.py b/simulated_pcb/action.py index 3e4df76..80e157e 100644 --- a/simulated_pcb/action.py +++ b/simulated_pcb/action.py @@ -12,10 +12,10 @@ class SimulatedPCB: self.last_update = time.time() # État simulé - self.servos = {} # {id: pos_index} - self.servo_angles = {} # {id: {pos_index: angle}} - self.relais = {1: 0, 2: 0, 3: 0} - self.ascenseur_pos = "low" # 'low', 'climb', 'high', 'desc' + self.servos = {0: 0, 1: 0, 2: 0, 3: 0, 4: 0, 5: 0, } # {id: pos_index} + self.servo_angles = {0: {0: 0, 1: 0, 3: 0}, 1: {0: 0, 1: 0, 3: 0}, 2: {0: 0, 1: 0, 3: 0}, 3: {0: 0, 1: 0, 3: 0}, 4: {0: 0, 1: 0, 3: 0}, 5: {0: 0, 1: 0, 3: 0}, } # {id: {pos_index: angle}} + self.relais = {1: 0, 2: 0} + self.ascenseur_pos = 0 self.fin_course = {} # à implémenter si besoin self.tirette_arm = False @@ -73,7 +73,7 @@ class SimulatedPCB: self.servo_angles[sid][pos_index] = angle self.send_response(f"OK;{category};{data};{val}") - elif category == "ASC" and data in ("HIGH", "LOW"): + elif category == "ASC": self.send_response(f"OK;ASC;{data};{val}") else: self.send_response(f"KO;{category};{data};{val}") @@ -85,11 +85,8 @@ class SimulatedPCB: category, data = parts[1], parts[2] if category == "ASC": - if data in ("HIGH", "LOW"): - self.ascenseur_pos = data.lower() - self.send_response(f"OK;{category};{data}") - else: - self.send_response(f"KO;{category};{data}") + self.ascenseur_pos = data + self.send_response(f"OK;{category};{data}") elif category.startswith("SERVO") and data.startswith("POS"): sid = int(category[5:]) pos_index = int(data[3:]) diff --git a/src/modelec_com/src/pcb_action_interface.cpp b/src/modelec_com/src/pcb_action_interface.cpp index e7c355e..607a703 100644 --- a/src/modelec_com/src/pcb_action_interface.cpp +++ b/src/modelec_com/src/pcb_action_interface.cpp @@ -128,7 +128,6 @@ namespace Modelec "action/move/asc", 10, [this](const modelec_interfaces::msg::ActionAscPos::SharedPtr msg) { - RCLCPP_INFO(this->get_logger(), "ASC move: %d", msg->pos); SendMove("ASC", {std::to_string(msg->pos)}); }); @@ -147,7 +146,7 @@ namespace Modelec }); asc_move_res_pub_ = this->create_publisher( - "action/mode/asc/res", 10); + "action/move/asc/res", 10); servo_move_res_pub_ = this->create_publisher( "action/move/servo/res", 10); @@ -331,7 +330,7 @@ namespace Modelec void PCBActionInterface::SendToPCB(const std::string& data) const { - RCLCPP_INFO(this->get_logger(), "Sending to PCB: '%s'", data.c_str()); + RCLCPP_DEBUG(this->get_logger(), "Sending to PCB: '%s'", data.c_str()); auto message = std_msgs::msg::String(); message.data = data; pcb_publisher_->publish(message); @@ -362,7 +361,7 @@ namespace Modelec void PCBActionInterface::SendMove(const std::string& elem, const std::vector& data) const { - SendToPCB("MOVE", elem, data); + SendToPCB("MOV", elem, data); } } diff --git a/src/modelec_core/launch/modelec.launch.py b/src/modelec_core/launch/modelec.launch.py index c39d3b9..9e63735 100644 --- a/src/modelec_core/launch/modelec.launch.py +++ b/src/modelec_core/launch/modelec.launch.py @@ -1,32 +1,28 @@ from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction, Shutdown +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction, Shutdown, RegisterEventHandler from launch.conditions import IfCondition from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -from launch.actions import RegisterEventHandler 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='Whether to launch the GUI' - ) - with_rplidar_arg = DeclareLaunchArgument( - 'with_rplidar', - default_value='true', - description='Whether to launch the RPLIDAR' - ) + 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 launch (conditionally included) + # Conditionally include RPLIDAR launch rplidar_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join( @@ -38,7 +34,7 @@ def generate_launch_description(): condition=IfCondition(with_rplidar) ) - # Function to create GUI node and its shutdown handler + # Function to launch GUI and shutdown handler def launch_gui(context, *args, **kwargs): if context.launch_configurations.get('with_gui') == 'true': gui = Node( @@ -55,20 +51,35 @@ def generate_launch_description(): 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_alim_interface', name='pcb_alim_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, + rplidar_launch, - - # Core nodes - 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_alim_interface', name='pcb_alim_interface'), - Node(package='modelec_com', executable='pcb_action_interface', name='pcb_action_interface'), - 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'), - - # GUI (conditionally included with its shutdown) OpaqueFunction(function=launch_gui), + OpaqueFunction(function=launch_com), + OpaqueFunction(function=launch_strat), ]) diff --git a/src/modelec_strat/data/config.xml b/src/modelec_strat/data/config.xml index 6f8ed1e..37e459d 100644 --- a/src/modelec_strat/data/config.xml +++ b/src/modelec_strat/data/config.xml @@ -43,17 +43,17 @@ pcb_odo - /dev/pts/9 + /dev/pts/20 115200 pcb_alim - /dev/pts/12 + /dev/pts/22 115200 pcb_action - /dev/pts/16 + /dev/pts/24 115200 diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index 08e84c1..0166e57 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -78,11 +78,8 @@ namespace Modelec void ActionExecutor::Update() { - RCLCPP_INFO(node_->get_logger(), "ActionExecutor::Update()"); if (step_.empty()) { - RCLCPP_INFO(node_->get_logger(), "action finished"); - action_ = NONE; action_done_ = true; return; @@ -90,16 +87,10 @@ namespace Modelec if (step_running_ == 0) { - RCLCPP_INFO(node_->get_logger(), "Running new action"); - - step_.pop(); - switch (step_.front()) { case DEPLOY_BANNER_STEP: { - RCLCPP_INFO(node_->get_logger(), "Deploy banner step"); - modelec_interfaces::msg::ActionServoPos msg; msg.id = 5; // TODO : to define msg.pos = 1; @@ -281,8 +272,10 @@ namespace Modelec break; default: - break; + return; } + + step_.pop(); } } diff --git a/src/modelec_strat/src/missions/banner_mission.cpp b/src/modelec_strat/src/missions/banner_mission.cpp index aea943f..b0b80ff 100644 --- a/src/modelec_strat/src/missions/banner_mission.cpp +++ b/src/modelec_strat/src/missions/banner_mission.cpp @@ -39,7 +39,7 @@ namespace Modelec { nav_->GoTo(spawn_.x, (nav_->GetPathfinding()->robot_length_mm_ / 2) + 5, M_PI_2, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE); - step_ = DEPLOY_BANNER; + step_ = GO_TO_FRONT; } break; diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp index 7f4074b..b794d8d 100644 --- a/src/modelec_strat/src/navigation_helper.cpp +++ b/src/modelec_strat/src/navigation_helper.cpp @@ -201,6 +201,10 @@ namespace Modelec bool NavigationHelper::HasArrived() const { + if (waypoints_.empty()) + { + return true; + } return waypoints_.back().reached; }