Pimpest/toid
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

wip-behaviors #3

Merged
Pimpest merged 86 commits from wip-behaviors into main 2026-05-28 06:21:26 +00:00
Conversation 0 Commits 86 Files Changed 129 +2 -251
3 changed files with 2 additions and 251 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 5777a0e804 - Show all commits

93
toid_navigation/launch/mock_control_launch.py
View File

@@ -1,93 +0,0 @@
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.conditions import IfCondition, UnlessCondition
from launch.substitutions import Command, LaunchConfiguration
from launch_ros.actions import Node, LifecycleNode
from launch_ros.substitutions import FindPackageShare
import os
def generate_launch_description():
pkg_share = FindPackageShare("").find('toid_navigation')
params = os.path.join(pkg_share, 'params', 'toid_general_params.yaml')
map = os.path.join(pkg_share, 'maps', 'mapb2_5cm.yaml')
default_rviz_config_path = os.path.join(pkg_share, 'rviz', 'ros_control.rviz')
description_pkg_share = FindPackageShare("").find('toid_bot_description')
default_model_path = os.path.join(
description_pkg_share,
'src',
'toid_bot_description.urdf'
)
visualize = LaunchConfiguration("visualize")
visualize_arg = DeclareLaunchArgument(
'visualize',
default_value='True',
description="Whether to launch rviz2"
)
odom_broadcast = Node(
package='tf2_ros',
executable='static_transform_publisher',
name='map_to_odom_broadcaster',
arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'],
condition=IfCondition(LaunchConfiguration('visualize'))
)
robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
output='screen',
parameters=[{'robot_description': Command(['xacro ', default_model_path])}]
)
controller_manager = Node(
package='controller_manager',
executable='ros2_control_node',
output='screen',
parameters=[params]
)
joint_state_broadcaster = Node(
package='controller_manager',
executable='spawner',
output='screen',
arguments=["joint_state_broadcaster"]
)
diffbot_base_controller = Node(
package='controller_manager',
executable='spawner',
output='both',
arguments=[
"diffbot_base_controller",
"-p",
params,
"--controller-ros-args",
"-r diffbot_base_controller/cmd_vel:=/cmd_vel",
"--controller-ros-args",
"-r diffbot_base_controller/odom:=/odom"
]
)
rviz_node = Node(
package='rviz2',
executable='rviz2',
name='rviz2',
output='screen',
arguments=['-d', default_rviz_config_path],
condition=IfCondition(visualize)
)
return LaunchDescription([
visualize_arg,
odom_broadcast,
robot_state_publisher,
controller_manager,
joint_state_broadcaster,
diffbot_base_controller,
rviz_node
])

116
toid_navigation/launch/navigation_launch.py
View File

@@ -1,116 +0,0 @@
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.conditions import IfCondition, UnlessCondition
from launch.substitutions import LaunchConfiguration
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
import os
def generate_launch_description():
pkg_share = FindPackageShare("").find('toid_navigation')
params = os.path.join(pkg_share, 'params', 'toid_general_params.yaml')
map = os.path.join(pkg_share, 'maps', 'mapb2_5cm.yaml')
launch_dir = os.path.join(pkg_share, 'launch')
default_rviz_config_path = os.path.join(pkg_share, 'rviz', 'nav2.rviz')
bt_path = os.path.join(pkg_share, "behaviors", "navigate_to_pose_w_backtracking.xml")
lattice_path = os.path.join(pkg_share, "params", "output.json")
position = "0.756 0.225 0 0 0 0".split(' ')
#position = "0 0 0 0 0 0".split(' ')
visualize = LaunchConfiguration("visualize")
visualize_arg = DeclareLaunchArgument(
'visualize',
default_value='True',
description="Whether to launch rviz2"
)
toid_control = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(launch_dir, 'mock_control_launch.py')
),
launch_arguments={
'visualize': 'False'
}.items()
)
set_position = Node(
package='tf2_ros',
executable='static_transform_publisher',
name='map_to_odom_broadcaster',
arguments=position + ['map', 'odom'],
condition=IfCondition(visualize)
)
map_server = Node(
package='nav2_map_server',
executable='map_server',
name='map_server',
output='screen',
parameters=[{'yaml_filename': map}]
)
planner_server = Node(
package='nav2_planner',
executable='planner_server',
name='planner_server',
output='screen',
parameters=[{'GridBased':{'lattice_filepath': lattice_path}}, params]
)
controller_server = Node(
package='nav2_controller',
executable='controller_server',
name='controller_server',
output='screen',
parameters=[params]
)
bt_navigator = Node(
package='nav2_bt_navigator',
executable='bt_navigator',
name='bt_navigator',
output='screen',
parameters=[{'default_nav_to_pose_bt_xml': bt_path}, params]
)
# default_nav_to_pose_bt_xml: "$(find-pkg-share toid_navigation)/behaviors/navigate_to_pose_w_backtracking.xml"
behavior_server = Node(
package='nav2_behaviors',
executable='behavior_server',
name='behavior_server',
output='screen',
parameters=[params]
)
lifecycle_manager_node = Node(
package='nav2_lifecycle_manager',
executable='lifecycle_manager',
name='lifecycle_manager',
output='screen',
parameters=[params]
)
rviz_node = Node(
package='rviz2',
executable='rviz2',
name='rviz2',
output='screen',
arguments=['-d', default_rviz_config_path],
condition=IfCondition(visualize)
)
return LaunchDescription([
visualize_arg,
set_position,
rviz_node,
map_server,
bt_navigator,
behavior_server,
planner_server,
controller_server,
lifecycle_manager_node,
toid_control
])

42
toid_navigation/params/toid_general_params.yaml
View File

@@ -22,17 +22,11 @@ behavior_server:
local_footprint_topic: local_costmap/published_footprint
global_footprint_topic: global_costmap/published_footprint
cycle_frequency: 50.0
behavior_plugins: ["backup", "spin", "drive_on_heading", "assisted_teleop", "wait", "rotate", "translateX", "moveCoords"]
behavior_plugins: ["backup", "spin", "rotate", "translateX", "moveCoords"]
spin:
plugin: "nav2_behaviors::Spin"
backup:
plugin: "nav2_behaviors::BackUp"
drive_on_heading:
plugin: "nav2_behaviors::DriveOnHeading"
wait:
plugin: "nav2_behaviors::Wait"
assisted_teleop:
plugin: "nav2_behaviors::AssistedTeleop"
rotate:
plugin: "toid::SimpleRotateBehavior"
max_angular_accel: 4.0
@@ -195,37 +189,3 @@ lifecycle_manager:
ros__parameters:
autostart: true
node_names: ['controller_server','planner_server','behavior_server','map_server','bt_navigator' ]
controller_manager:
ros__parameters:
update_rate: 20 # Hz
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
diffbot_base_controller:
ros__parameters:
type: diff_drive_controller/DiffDriveController
left_wheel_names: ["drivewhl_l_joint"]
right_wheel_names: ["drivewhl_r_joint"]
wheel_separation: 0.29
wheel_radius: 0.04
use_stamped_vel: false
wheel_separation_multiplier: 1.0
left_wheel_radius_multiplier: 1.0
right_wheel_radius_multiplier: 1.0
publish_rate: 50.0
odom_frame_id: odom
base_frame_id: base_footprint
open_loop: true
enable_odom_tf: true
publish_limited_velocity: true
cmd_vel_timeout: 1.0