Magnezijum-Robotics/magrob
2
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

Added launch_files

Browse Source
This commit is contained in:
Pimpest
2025-05-08 23:01:51 +02:00
parent bd47553b86
commit ca069cd664
4 changed files with 226 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files

35
mg_bringup/launch/launch-external.py Normal file
View File

@ -0,0 +1,35 @@
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction
from launch.conditions import UnlessCondition
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
def generate_launch_description():
is_local_test = DeclareLaunchArgument(
'local_test',
default_value="False",
description='Launch with simulated components'
)
return LaunchDescription([
is_local_test,
IncludeLaunchDescription(
PathJoinSubstitution([
FindPackageShare("mg_bt"),
'launch',
'launch.py'
]),
),
Node(
package="mg_planner",
executable="mg_planner",
name="mg_planner",
emulate_tty=True,
output='screen',
)
])

26
mg_bringup/launch/launch.py
View File

@ -1,6 +1,6 @@
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction
from launch.conditions import UnlessCondition
from launch.conditions import UnlessCondition, IfCondition
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
@ -26,6 +26,14 @@ def generate_launch_description():
'local_test': LaunchConfiguration('local_test')
}.items()
),
IncludeLaunchDescription(
PathJoinSubstitution([
FindPackageShare("mg_lidar"),
'launch',
'launch.py'
]),
condition=UnlessCondition(LaunchConfiguration('local_test')),
),
Node(
package="mg_odometry",
executable="mg_odom_publisher",
@ -39,6 +47,22 @@ def generate_launch_description():
emulate_tty=True,
output='screen'
),
IncludeLaunchDescription(
PathJoinSubstitution([
FindPackageShare("mg_bt"),
'launch',
'launch.py'
]),
condition=IfCondition(LaunchConfiguration('local_test')),
),
Node(
package="mg_planner",
executable="mg_planner",
name="mg_planner",
emulate_tty=True,
condition=IfCondition(LaunchConfiguration('local_test')),
output='screen',
),
Node(
package="mg_navigation",
executable="mg_nav_server",

133
mg_bt/behavior_trees/calib_bt.xml
View File

@ -3,29 +3,52 @@
<BehaviorTree ID="calib2_bt">
<Sequence>
<ZeroNode service_name="/zero"/>
<MovePoint action_name="/MovePoint"
tolerance="0.02"
<MovePoint x_goal="0.7"
y_goal="0.0"
max_wheel_speed="3.000000"
max_angular="0.1"
x_goal="0.7"
y_goal="0.0"/>
max_vel="2.000000"
ornt_mult="4.000000"
tolerance="0.02"
action_name="/MovePoint"/>
<RotateNode angle="-2"
max_wheel_speed="3.000000"
max_angular="3.140000"
tolerance="0.001000"
action_name="/Rotate"/>
<RotateNode angle="-3.14"
max_wheel_speed="3.000000"
max_angular="3.140000"
tolerance="0.001000"
action_name="/Rotate"/>
<MovePoint action_name="/MovePoint"
<MovePoint x_goal="0.3"
y_goal="0.0"
max_wheel_speed="3.000000"
max_angular="3.140000"
max_vel="2.000000"
ornt_mult="4.000000"
tolerance="0.02"
x_goal="0.3"
y_goal="0.0"/>
action_name="/MovePoint"/>
<RotateNode angle="-2"
max_wheel_speed="3.000000"
max_angular="3.140000"
tolerance="0.001000"
action_name="/Rotate"/>
<RotateNode angle="0"
max_wheel_speed="3.000000"
max_angular="3.140000"
tolerance="0.001000"
action_name="/Rotate"/>
<Fallback>
<Timeout msec="5000">
<MovePoint action_name="/MovePoint"
<MovePoint x_goal="-0.1"
y_goal="0.0"
max_wheel_speed="3.000000"
max_angular="0.1"
x_goal="-0.1"
y_goal="0.0"/>
max_vel="2.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
</Timeout>
<AlwaysSuccess/>
</Fallback>
@ -41,23 +64,39 @@
<Repeat num_cycles="5">
<Sequence>
<ZeroNode service_name="/zero"/>
<MovePoint action_name="/MovePoint"
tolerance="0.04"
<MovePoint x_goal="0.7"
y_goal="0.0"
max_wheel_speed="3.000000"
max_angular="0.1"
x_goal="0.7"
y_goal="0.0"/>
max_vel="2.000000"
ornt_mult="4.000000"
tolerance="0.04"
action_name="/MovePoint"/>
<RotateNode angle="2"
max_wheel_speed="3.000000"
max_angular="3.140000"
tolerance="0.001000"
action_name="/Rotate"/>
<RotateNode angle="-2"
max_wheel_speed="3.000000"
max_angular="3.140000"
tolerance="0.001000"
action_name="/Rotate"/>
<RotateNode angle="0"
max_wheel_speed="3.000000"
max_angular="3.140000"
tolerance="0.001000"
action_name="/Rotate"/>
<Fallback>
<Timeout msec="20000">
<MovePoint action_name="/MovePoint"
<MovePoint x_goal="-0.1"
y_goal="0.0"
max_wheel_speed="3.000000"
max_angular="0.1"
x_goal="-0.1"
y_goal="0.0"/>
max_vel="2.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
</Timeout>
<AlwaysSuccess/>
</Fallback>
@ -85,59 +124,39 @@
type="std::string">Service name</input_port>
</Action>
<Action ID="MovePoint">
<input_port name="action_name"
type="std::string">Action server name</input_port>
<input_port name="tolerance"
type="double"/>
<input_port name="obst_mult_a"
type="double"/>
<input_port name="ornt_mult"
type="double"/>
<input_port name="pos_mult"
type="double"/>
<input_port name="t_ornt_mult"
type="double"/>
<input_port name="obst_mult_c"
type="double"/>
<input_port name="obst_mult_b"
type="double"/>
<input_port name="max_vel"
type="double"/>
<input_port name="max_angular"
type="double"/>
<input_port name="IgnoreList"
type="std::string"/>
<input_port name="max_wheel_speed"
type="double"/>
<input_port name="x_goal"
type="double"/>
<input_port name="y_goal"
type="double"/>
<input_port name="max_wheel_speed"
default="3.000000"
type="double"/>
<input_port name="max_angular"
default="3.140000"
type="double"/>
<input_port name="max_vel"
default="2.000000"
type="double"/>
<input_port name="ornt_mult"
default="4.000000"
type="double"/>
<input_port name="tolerance"
default="0.001000"
type="double"/>
<input_port name="action_name"
type="std::string">Action server name</input_port>
</Action>
<Action ID="RotateNode">
<input_port name="angle"
type="double"/>
<input_port name="pos_mult"
type="double"/>
<input_port name="max_wheel_speed"
default="3.000000"
type="double"/>
<input_port name="IgnoreList"
type="std::string"/>
<input_port name="max_angular"
type="double"/>
<input_port name="max_vel"
type="double"/>
<input_port name="obst_mult_b"
type="double"/>
<input_port name="obst_mult_c"
type="double"/>
<input_port name="ornt_mult"
type="double"/>
<input_port name="t_ornt_mult"
type="double"/>
<input_port name="obst_mult_a"
default="3.140000"
type="double"/>
<input_port name="tolerance"
default="0.001000"
type="double"/>
<input_port name="action_name"
type="std::string">Action server name</input_port>

87
mg_bt/behavior_trees/tactics.xml
View File

@ -257,7 +257,7 @@
<BehaviorTree ID="tactic_base">
<Sequence>
<I2CSignal Address="66"
<I2CSignal Address="65"
Data="1"
Result="{tactic_id}"
action_name="/i2c_action"/>
@ -275,11 +275,79 @@
</BehaviorTree>
<BehaviorTree ID="tactic_default_yellow">
<AlwaysSuccess/>
<Sequence>
<I2CSignal Address="65"
Data="1"
Result="{i2c_res}"
action_name="/i2c_action"/>
<SendPose x="1.0 + 16.5"
y="0.45 - 14"
angle="90"
isDegree="true"
service_name="/set_pose"/>
<SideObstaclePub tactic="1"
topic_name="/side"/>
<Timeout msec="100000">
<Sequence>
<MoveLocal x_goal="0.20"
y_goal="0"
max_wheel_speed="3.000000"
max_angular="3.140000"
max_vel="2.000000"
pos_mult="15.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
<MoveLocal x_goal="-0.20"
y_goal="0"
max_wheel_speed="3.000000"
max_angular="3.140000"
max_vel="2.000000"
pos_mult="15.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
</Sequence>
</Timeout>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="tatic_default_blue">
<AlwaysFailure/>
<Sequence>
<I2CSignal Address="65"
Data="1"
Result="{i2c_res}"
action_name="/i2c_action"/>
<SendPose x="0.31"
y="1.71"
angle="90"
isDegree="true"
service_name="/set_pose"/>
<SideObstaclePub tactic="2"
topic_name="/side"/>
<Timeout msec="100000">
<Sequence>
<MoveLocal x_goal="0.20"
y_goal="0"
max_wheel_speed="3.000000"
max_angular="3.140000"
max_vel="2.000000"
pos_mult="15.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
<MoveLocal x_goal="-0.20"
y_goal="0"
max_wheel_speed="3.000000"
max_angular="3.140000"
max_vel="2.000000"
pos_mult="15.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
</Sequence>
</Timeout>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="testbed">
@ -439,6 +507,19 @@
<input_port name="action_name"
type="std::string">Action server name</input_port>
</Action>
<Action ID="SendPose">
<input_port name="x"
type="double"/>
<input_port name="y"
type="double"/>
<input_port name="angle"
type="double"/>
<input_port name="isDegree"
default="true"
type="bool"/>
<input_port name="service_name"
type="std::string">Service name</input_port>
</Action>
<Condition ID="SideObstaclePub">
<input_port name="tactic"
type="int"/>