modelec/modelec-marcel-ROS
1
0
Fork 0
mirror of https://github.com/modelec/modelec-marcel-ROS.git synced 2026-01-18 16:47:43 +01:00
Code Issues Packages Projects Releases Wiki Activity

action v1

Browse Source
This commit is contained in:
acki
2025-05-15 20:39:22 -04:00
parent 394a90b3f9
commit a2af5c0916
7 changed files with 57 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
simulated_pcb/action.py
View File

@@ -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:])

7
src/modelec_com/src/pcb_action_interface.cpp
View File

@@ -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<modelec_interfaces::msg::ActionAscPos>(
"action/mode/asc/res", 10);
"action/move/asc/res", 10);
servo_move_res_pub_ = this->create_publisher<modelec_interfaces::msg::ActionServoPos>(
"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<std::string>& data) const
{
SendToPCB("MOVE", elem, data);
SendToPCB("MOV", elem, data);
}
}

61
src/modelec_core/launch/modelec.launch.py
View File

@@ -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),
])

6
src/modelec_strat/data/config.xml
View File

@@ -43,17 +43,17 @@
<pcb>
<pcb_odo>
<name>pcb_odo</name>
<port>/dev/pts/9</port>
<port>/dev/pts/20</port>
<baud_rate>115200</baud_rate>
</pcb_odo>
<pcb_alim>
<name>pcb_alim</name>
<port>/dev/pts/12</port>
<port>/dev/pts/22</port>
<baud_rate>115200</baud_rate>
</pcb_alim>
<pcb_action>
<name>pcb_action</name>
<port>/dev/pts/16</port>
<port>/dev/pts/24</port>
<baud_rate>115200</baud_rate>
</pcb_action>
</pcb>

13
src/modelec_strat/src/action_executor.cpp
View File

@@ -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();
}
}

2
src/modelec_strat/src/missions/banner_mission.cpp
View File

@@ -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;

4
src/modelec_strat/src/navigation_helper.cpp
View File

@@ -201,6 +201,10 @@ namespace Modelec
bool NavigationHelper::HasArrived() const
{
if (waypoints_.empty())
{
return true;
}
return waypoints_.back().reached;
}