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

last up 2025

Browse Source
This commit is contained in:
acki
2025-10-04 11:20:56 +02:00
parent 8c9e6e67ad
commit 037e3c841a
17 changed files with 47 additions and 411 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
build.zsh Normal file → Executable file
View File

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

30
simulated_pcb/odo.py
View File

@@ -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")

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

@@ -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<std::string>("serial_port", "/dev/USB_ACTION");
declare_parameter<int>("baudrate", 115200);
declare_parameter<std::string>("name", "pcb_action");
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->bauds = Config::get<int>("config.usb.pcb.pcb_action.baudrate", 115200);
request->serial_port = Config::get<std::string>("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<modelec_interfaces::srv::AddSerialListener>("add_serial_listener");
while (!client->wait_for_service(std::chrono::seconds(1)))

2
src/modelec_com/src/pcb_alim_interface.cpp
View File

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

8
src/modelec_com/src/pcb_odo_interface.cpp
View File

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

74
src/modelec_core/launch/launch_sim.launch.py
View File

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

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

@@ -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 []

41
src/modelec_core/launch/modelec_gamecontroller.launch.yml
View File

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

11
src/modelec_core/launch/modelec_gamecontroller_remote_controller.launch.yml
View File

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

31
src/modelec_core/launch/modelec_gamecontroller_remote_server.launch.yml
View File

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

41
src/modelec_core/launch/rsp.launch.py
View File

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

111
src/modelec_core/launch/test.modelec.launch.py
View File

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

36
src/modelec_core/launch/test.no_gui.modelec.launch.yml
View File

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

36
src/modelec_core/launch/test.no_strat.modelec.launch.yml
View File

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

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

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

1
src/modelec_utils/include/modelec_utils/config.hpp
View File

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

2
src/modelec_utils/include/modelec_utils/point.hpp
View File

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