115 lines
4.5 KiB
Python
115 lines
4.5 KiB
Python
import os
|
|
from launch import LaunchDescription
|
|
from launch_ros.actions import Node
|
|
from launch.actions import ExecuteProcess, IncludeLaunchDescription
|
|
from launch_ros.substitutions import FindPackageShare
|
|
from launch.substitutions import Command
|
|
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
|
|
|
def generate_launch_description():
|
|
robot_name = "iisy"
|
|
package_name = "gaz_simulation"
|
|
pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros')
|
|
path = FindPackageShare(package=package_name).find(package_name)
|
|
world_path = os.path.join(path, "world/test2.xml")
|
|
urdf_path = os.path.join(path, "urdf/iisy.urdf")
|
|
return LaunchDescription([
|
|
#IncludeLaunchDescription(
|
|
# PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')),
|
|
# launch_arguments={'world': world_path}.items()
|
|
#),
|
|
|
|
ExecuteProcess(
|
|
cmd=[[
|
|
'LIBGL_ALWAYS_SOFTWARE=true ros2 launch gazebo_ros gzserver.launch.py "world:='+world_path+'"'
|
|
]],
|
|
shell=True
|
|
),
|
|
|
|
|
|
# Start Gazebo client
|
|
IncludeLaunchDescription(
|
|
PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py'))
|
|
),
|
|
Node(
|
|
package='tf2_ros',
|
|
namespace='gaz_simulation',
|
|
executable='static_transform_publisher',
|
|
name='lidar_1_broadcaster',
|
|
arguments=["1", "-1.9", "1", "1.5707963267949", "0", "0", "map", "lidar_1_link"]
|
|
),
|
|
Node(
|
|
package='tf2_ros',
|
|
namespace='gaz_simulation',
|
|
executable='static_transform_publisher',
|
|
name='lidar_2_broadcaster',
|
|
arguments=["-1.9", "1", "1", "0", "0", "0", "map", "lidar_2_link"]
|
|
),
|
|
Node(
|
|
package='gazebo_ros',
|
|
executable='spawn_entity.py',
|
|
arguments=['-entity', robot_name,
|
|
'-topic', 'robot_description',
|
|
'-x', '-1',
|
|
'-y', '-1',
|
|
'-z', '1',
|
|
'-Y', '0'], # Yaw
|
|
output='screen'
|
|
),
|
|
# Subscribe to the joint states of the robot, and publish the 3D pose of each link.
|
|
Node(
|
|
package='robot_state_publisher',
|
|
executable='robot_state_publisher',
|
|
parameters=[{'robot_description': Command(['xacro ', urdf_path])}]
|
|
),
|
|
|
|
# Publish the joint states of the robot
|
|
Node(
|
|
package='joint_state_publisher',
|
|
executable='joint_state_publisher',
|
|
name='joint_state_publisher'
|
|
)
|
|
|
|
# ,
|
|
# Node(
|
|
# namespace='turtlesim1',
|
|
# executable='gazebo',
|
|
# name='gazebo',
|
|
# arguments=["test.xml"]
|
|
# )
|
|
])
|
|
|
|
|
|
<?xml version="1.0"?>
|
|
<launch>
|
|
<arg name="paused" default="false"/>
|
|
<arg name="gazebo_gui" default="true"/>
|
|
<arg name="initial_joint_positions" doc="Initial joint configuration of the robot"
|
|
default=" -J base_link1_joint 0 -J base_rot 0 -J link1_link2_joint 0 -J link2_link3_joint 0 -J link2_rot 0 -J link3_rot 0"/>
|
|
|
|
<!-- startup simulated world -->
|
|
<include file="$(find gazebo_ros)/launch/empty_world.launch">
|
|
<let name="world_name" default="worlds/empty.world"/>
|
|
<let name="paused" value="true"/>
|
|
<let name="gui" value="$(arg gazebo_gui)"/>
|
|
</include>
|
|
|
|
<!-- send robot urdf to param server -->
|
|
<param name="robot_description" textfile="$(find gaz_simulation)/urdf/iisy.urdf" />
|
|
|
|
<!-- unpause only after loading robot model -->
|
|
<let name="unpause" value="$(eval '' if arg('paused') else '-unpause')" />
|
|
<!-- push robot_description to factory and spawn robot in gazebo at the origin, change x,y,z arguments to spawn in a different position -->
|
|
<arg name="world_pose" value="-x 0 -y 0 -z 0" />
|
|
<node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot $(arg unpause) $(arg world_pose) $(arg initial_joint_positions)"
|
|
respawn="false" output="screen" />
|
|
|
|
<!-- Load joint controller parameters for Gazebo -->
|
|
<param from="$(find iisy_config)/config/gazebo_controllers.yaml" />
|
|
<!-- Spawn Gazebo ROS controllers -->
|
|
<node name="gazebo_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller" />
|
|
<!-- Load ROS controllers -->
|
|
<include file="$(dirname)/ros_controllers.launch"/>
|
|
|
|
</launch>
|