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

145
mg_bt/behavior_trees/calib_bt.xml
View File

@ -3,32 +3,55 @@
<BehaviorTree ID="calib2_bt"> <BehaviorTree ID="calib2_bt">
<Sequence> <Sequence>
<ZeroNode service_name="/zero"/> <ZeroNode service_name="/zero"/>
<MovePoint action_name="/MovePoint" <MovePoint x_goal="0.7"
tolerance="0.02" y_goal="0.0"
max_wheel_speed="3.000000"
max_angular="0.1" max_angular="0.1"
x_goal="0.7" max_vel="2.000000"
y_goal="0.0"/> ornt_mult="4.000000"
tolerance="0.02"
action_name="/MovePoint"/>
<RotateNode angle="-2" <RotateNode angle="-2"
max_wheel_speed="3.000000"
max_angular="3.140000"
tolerance="0.001000"
action_name="/Rotate"/> action_name="/Rotate"/>
<RotateNode angle="-3.14" <RotateNode angle="-3.14"
max_wheel_speed="3.000000"
max_angular="3.140000"
tolerance="0.001000"
action_name="/Rotate"/> 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" tolerance="0.02"
x_goal="0.3" action_name="/MovePoint"/>
y_goal="0.0"/>
<RotateNode angle="-2" <RotateNode angle="-2"
max_wheel_speed="3.000000"
max_angular="3.140000"
tolerance="0.001000"
action_name="/Rotate"/> action_name="/Rotate"/>
<RotateNode angle="0" <RotateNode angle="0"
max_wheel_speed="3.000000"
max_angular="3.140000"
tolerance="0.001000"
action_name="/Rotate"/> action_name="/Rotate"/>
<Fallback> <Fallback>
<Timeout msec="5000"> <Timeout msec="5000">
<MovePoint action_name="/MovePoint" <MovePoint x_goal="-0.1"
max_angular="0.1" y_goal="0.0"
x_goal="-0.1" max_wheel_speed="3.000000"
y_goal="0.0"/> max_angular="0.1"
</Timeout> max_vel="2.000000"
<AlwaysSuccess/> ornt_mult="4.000000"
</Fallback> tolerance="0.001000"
action_name="/MovePoint"/>
</Timeout>
<AlwaysSuccess/>
</Fallback>
<ZeroNode service_name="/endCalib"/> <ZeroNode service_name="/endCalib"/>
</Sequence> </Sequence>
</BehaviorTree> </BehaviorTree>
@ -41,23 +64,39 @@
<Repeat num_cycles="5"> <Repeat num_cycles="5">
<Sequence> <Sequence>
<ZeroNode service_name="/zero"/> <ZeroNode service_name="/zero"/>
<MovePoint action_name="/MovePoint" <MovePoint x_goal="0.7"
tolerance="0.04" y_goal="0.0"
max_wheel_speed="3.000000"
max_angular="0.1" max_angular="0.1"
x_goal="0.7" max_vel="2.000000"
y_goal="0.0"/> ornt_mult="4.000000"
tolerance="0.04"
action_name="/MovePoint"/>
<RotateNode angle="2" <RotateNode angle="2"
max_wheel_speed="3.000000"
max_angular="3.140000"
tolerance="0.001000"
action_name="/Rotate"/> action_name="/Rotate"/>
<RotateNode angle="-2" <RotateNode angle="-2"
max_wheel_speed="3.000000"
max_angular="3.140000"
tolerance="0.001000"
action_name="/Rotate"/> action_name="/Rotate"/>
<RotateNode angle="0" <RotateNode angle="0"
max_wheel_speed="3.000000"
max_angular="3.140000"
tolerance="0.001000"
action_name="/Rotate"/> action_name="/Rotate"/>
<Fallback> <Fallback>
<Timeout msec="20000"> <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" max_angular="0.1"
x_goal="-0.1" max_vel="2.000000"
y_goal="0.0"/> ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
</Timeout> </Timeout>
<AlwaysSuccess/> <AlwaysSuccess/>
</Fallback> </Fallback>
@ -85,59 +124,39 @@
type="std::string">Service name</input_port> type="std::string">Service name</input_port>
</Action> </Action>
<Action ID="MovePoint"> <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" <input_port name="x_goal"
type="double"/> type="double"/>
<input_port name="y_goal" <input_port name="y_goal"
type="double"/> 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>
<Action ID="RotateNode"> <Action ID="RotateNode">
<input_port name="angle" <input_port name="angle"
type="double"/> type="double"/>
<input_port name="pos_mult"
type="double"/>
<input_port name="max_wheel_speed" <input_port name="max_wheel_speed"
default="3.000000"
type="double"/> type="double"/>
<input_port name="IgnoreList"
type="std::string"/>
<input_port name="max_angular" <input_port name="max_angular"
type="double"/> default="3.140000"
<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"
type="double"/> type="double"/>
<input_port name="tolerance" <input_port name="tolerance"
default="0.001000"
type="double"/> type="double"/>
<input_port name="action_name" <input_port name="action_name"
type="std::string">Action server name</input_port> 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"> <BehaviorTree ID="tactic_base">
<Sequence> <Sequence>
<I2CSignal Address="66" <I2CSignal Address="65"
Data="1" Data="1"
Result="{tactic_id}" Result="{tactic_id}"
action_name="/i2c_action"/> action_name="/i2c_action"/>
@ -275,11 +275,79 @@
</BehaviorTree> </BehaviorTree>
<BehaviorTree ID="tactic_default_yellow"> <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>
<BehaviorTree ID="tatic_default_blue"> <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>
<BehaviorTree ID="testbed"> <BehaviorTree ID="testbed">
@ -439,6 +507,19 @@
<input_port name="action_name" <input_port name="action_name"
type="std::string">Action server name</input_port> type="std::string">Action server name</input_port>
</Action> </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"> <Condition ID="SideObstaclePub">
<input_port name="tactic" <input_port name="tactic"
type="int"/> type="int"/>