Skip to content

Commit

Permalink
format
Browse files Browse the repository at this point in the history
  • Loading branch information
@lukicdarkoo
lukicdarkoo committed Dec 16, 2024
1 parent 8a225d9 commit c282fb3
Show file tree
Hide file tree
Showing 5 changed files with 554 additions and 449 deletions.
182 changes: 98 additions & 84 deletions webots_ros2_epuck/launch/robot_launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,138 +27,152 @@
from ament_index_python.packages import get_package_share_directory
from webots_ros2_driver.webots_launcher import WebotsLauncher
from webots_ros2_driver.webots_controller import WebotsController
from webots_ros2_driver.wait_for_controller_connection import WaitForControllerConnection
from webots_ros2_driver.wait_for_controller_connection import (
WaitForControllerConnection,
)


def generate_launch_description():
package_dir = get_package_share_directory('webots_ros2_epuck')
world = LaunchConfiguration('world')
use_nav = LaunchConfiguration('nav', default=False)
use_rviz = LaunchConfiguration('rviz', default=False)
use_mapper = LaunchConfiguration('mapper', default=False)
fill_map = LaunchConfiguration('fill_map', default=True)
map_filename = LaunchConfiguration('map', default=os.path.join(package_dir, 'resource', 'epuck_world_map.yaml'))
use_sim_time = LaunchConfiguration('use_sim_time', default=True)
package_dir = get_package_share_directory("webots_ros2_epuck")
world = LaunchConfiguration("world")
use_nav = LaunchConfiguration("nav", default=False)
use_rviz = LaunchConfiguration("rviz", default=False)
use_mapper = LaunchConfiguration("mapper", default=False)
fill_map = LaunchConfiguration("fill_map", default=True)
map_filename = LaunchConfiguration(
"map", default=os.path.join(package_dir, "resource", "epuck_world_map.yaml")
)
use_sim_time = LaunchConfiguration("use_sim_time", default=True)

robot_description_path = os.path.join(package_dir, 'resource', 'epuck_webots.urdf')
ros2_control_params = os.path.join(package_dir, 'resource', 'ros2_control.yml')
robot_description_path = os.path.join(package_dir, "resource", "epuck_webots.urdf")
ros2_control_params = os.path.join(package_dir, "resource", "ros2_control.yml")

webots = WebotsLauncher(
world=PathJoinSubstitution([package_dir, 'worlds', world]),
ros2_supervisor=True
world=PathJoinSubstitution([package_dir, "worlds", world]), ros2_supervisor=True
)

robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
parameters=[{
'robot_description': '<robot name=""><link name=""/></robot>'
}],
package="robot_state_publisher",
executable="robot_state_publisher",
output="screen",
parameters=[{"robot_description": '<robot name=""><link name=""/></robot>'}],
)

footprint_publisher = Node(
package='tf2_ros',
executable='static_transform_publisher',
output='screen',
arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_footprint'],
package="tf2_ros",
executable="static_transform_publisher",
output="screen",
arguments=["0", "0", "0", "0", "0", "0", "base_link", "base_footprint"],
)

# ROS control spawners
controller_manager_timeout = ['--controller-manager-timeout', '50']
controller_manager_prefix = 'python.exe' if os.name == 'nt' else ''
controller_manager_timeout = ["--controller-manager-timeout", "50"]
controller_manager_prefix = "python.exe" if os.name == "nt" else ""
diffdrive_controller_spawner = Node(
package='controller_manager',
executable='spawner',
output='screen',
package="controller_manager",
executable="spawner",
output="screen",
prefix=controller_manager_prefix,
arguments=['diffdrive_controller'] + controller_manager_timeout + ['--param-file', ros2_control_params],
arguments=["diffdrive_controller"]
+ controller_manager_timeout
+ ["--param-file", ros2_control_params],
parameters=[
{'use_sim_time': use_sim_time},
{"use_sim_time": use_sim_time},
],
)
joint_state_broadcaster_spawner = Node(
package='controller_manager',
executable='spawner',
output='screen',
package="controller_manager",
executable="spawner",
output="screen",
prefix=controller_manager_prefix,
arguments=['joint_state_broadcaster'] + controller_manager_timeout + ['--param-file', ros2_control_params],
arguments=["joint_state_broadcaster"]
+ controller_manager_timeout
+ ["--param-file", ros2_control_params],
parameters=[
{'use_sim_time': use_sim_time},
{"use_sim_time": use_sim_time},
],
)
ros_control_spawners = [diffdrive_controller_spawner, joint_state_broadcaster_spawner]
ros_control_spawners = [
diffdrive_controller_spawner,
joint_state_broadcaster_spawner,
]

use_twist_stamped = 'ROS_DISTRO' in os.environ and (os.environ['ROS_DISTRO'] in ['rolling', 'jazzy'])
use_twist_stamped = "ROS_DISTRO" in os.environ and (
os.environ["ROS_DISTRO"] in ["rolling", "jazzy"]
)
if use_twist_stamped:
mappings = [('/diffdrive_controller/cmd_vel', '/cmd_vel'), ('/diffdrive_controller/odom', '/odom')]
mappings = [
("/diffdrive_controller/cmd_vel", "/cmd_vel"),
("/diffdrive_controller/odom", "/odom"),
]
else:
mappings = [('/diffdrive_controller/cmd_vel_unstamped', '/cmd_vel'), ('/diffdrive_controller/odom', '/odom')]
mappings = [
("/diffdrive_controller/cmd_vel_unstamped", "/cmd_vel"),
("/diffdrive_controller/odom", "/odom"),
]
epuck_driver = WebotsController(
robot_name='e-puck',
robot_name="e-puck",
parameters=[
{'robot_description': robot_description_path,
'use_sim_time': use_sim_time,
'set_robot_state_publisher': True},
ros2_control_params
{
"robot_description": robot_description_path,
"use_sim_time": use_sim_time,
"set_robot_state_publisher": True,
},
ros2_control_params,
],
remappings=mappings,
respawn=True
respawn=True,
)

epuck_process = Node(
package='webots_ros2_epuck',
executable='epuck_node',
output='screen',
package="webots_ros2_epuck",
executable="epuck_node",
output="screen",
parameters=[
{'use_sim_time': use_sim_time},
{"use_sim_time": use_sim_time},
],
)

# Tools
tool_nodes = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(package_dir, 'launch', 'robot_tools_launch.py')
os.path.join(package_dir, "launch", "robot_tools_launch.py")
),
launch_arguments={
'fill_map': fill_map,
'mapper': use_mapper,
'map': map_filename,
'nav': use_nav,
'rviz': use_rviz,
'use_sim_time': use_sim_time,
"fill_map": fill_map,
"mapper": use_mapper,
"map": map_filename,
"nav": use_nav,
"rviz": use_rviz,
"use_sim_time": use_sim_time,
}.items(),
)

# Wait for the simulation to be ready to start the tools and spawners
waiting_nodes = WaitForControllerConnection(
target_driver=epuck_driver,
nodes_to_start=[tool_nodes] + ros_control_spawners
target_driver=epuck_driver, nodes_to_start=[tool_nodes] + ros_control_spawners
)

return LaunchDescription([
DeclareLaunchArgument(
'world',
default_value='epuck_world.wbt',
description='Choose one of the world files from `/webots_ros2_epuck/world` directory'
),
webots,
webots._supervisor,

robot_state_publisher,
footprint_publisher,

epuck_driver,
epuck_process,
waiting_nodes,

# This action will kill all nodes once the Webots simulation has exited
launch.actions.RegisterEventHandler(
event_handler=launch.event_handlers.OnProcessExit(
target_action=webots,
on_exit=[
launch.actions.EmitEvent(event=launch.events.Shutdown())
],
)
)
])
return LaunchDescription(
[
DeclareLaunchArgument(
"world",
default_value="epuck_world.wbt",
description="Choose one of the world files from `/webots_ros2_epuck/world` directory",
),
webots,
webots._supervisor,
robot_state_publisher,
footprint_publisher,
epuck_driver,
epuck_process,
waiting_nodes,
# This action will kill all nodes once the Webots simulation has exited
launch.actions.RegisterEventHandler(
event_handler=launch.event_handlers.OnProcessExit(
target_action=webots,
on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())],
)
),
]
)
Loading

0 comments on commit c282fb3

Please sign in to comment.