Skip to content

Instantly share code, notes, and snippets.

@soumya997
Created April 1, 2024 20:14
Show Gist options
  • Select an option

  • Save soumya997/29b16cc91dc242fbb201daea2c360c0d to your computer and use it in GitHub Desktop.

Select an option

Save soumya997/29b16cc91dc242fbb201daea2c360c0d to your computer and use it in GitHub Desktop.
import launch
from launch.actions import (
ExecuteProcess,
DeclareLaunchArgument,
RegisterEventHandler,
SetEnvironmentVariable,
)
from launch.conditions import IfCondition
from launch.event_handlers import OnProcessExit
from launch.substitutions import (
Command,
FindExecutable,
LaunchConfiguration,
NotSubstitution,
AndSubstitution,
)
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
import launch_ros
import os
def generate_launch_description():
pkg_share = launch_ros.substitutions.FindPackageShare(
package="sam_bot_nav2_gz"
).find("sam_bot_nav2_gz")
default_model_path = os.path.join(
pkg_share, "src/description/sam_bot_description.urdf"
)
default_rviz_config_path = os.path.join(pkg_share, "rviz/urdf_config.rviz")
world_path = os.path.join(pkg_share, "world/empty.sdf")
gz_models_path = os.path.join(pkg_share, "models")
use_sim_time = LaunchConfiguration("use_sim_time")
use_localization = LaunchConfiguration("use_localization")
use_rviz = LaunchConfiguration("use_rviz")
log_level = LaunchConfiguration("log_level")
gz_verbosity = LaunchConfiguration("gz_verbosity")
run_headless = LaunchConfiguration("run_headless")
robot_state_publisher_node = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
parameters=[
{"robot_description": Command(["xacro ", LaunchConfiguration("model")])}
],
)
rviz_node = Node(
condition=IfCondition(AndSubstitution(NotSubstitution(run_headless), use_rviz)),
package="rviz2",
executable="rviz2",
name="rviz2",
output="screen",
arguments=["-d", LaunchConfiguration("rvizconfig")],
)
# gazebo have to be executed with shell=False, or test_launch won't terminate it
# see: https://github.com/ros2/launch/issues/545
# This code is form taken ros_gz_sim and modified to work with shell=False
# see: https://github.com/gazebosim/ros_gz/blob/ros2/ros_gz_sim/launch/gz_sim.launch.py.in
gz_env = {'GZ_SIM_SYSTEM_PLUGIN_PATH':
':'.join([os.environ.get('GZ_SIM_SYSTEM_PLUGIN_PATH', default=''),
os.environ.get('LD_LIBRARY_PATH', default='')]),
'IGN_GAZEBO_SYSTEM_PLUGIN_PATH': # TODO(CH3): To support pre-garden. Deprecated.
':'.join([os.environ.get('IGN_GAZEBO_SYSTEM_PLUGIN_PATH', default=''),
os.environ.get('LD_LIBRARY_PATH', default='')])}
gazebo = [
ExecuteProcess(
condition=launch.conditions.IfCondition(run_headless),
cmd=['ruby', FindExecutable(name="gz"), 'sim', '-r', '-v', gz_verbosity, '-s', '--headless-rendering', world_path],
output='screen',
additional_env=gz_env, # type: ignore
shell=False,
),
ExecuteProcess(
condition=launch.conditions.UnlessCondition(run_headless),
cmd=['ruby', FindExecutable(name="gz"), 'sim', '-r', '-v', gz_verbosity, world_path],
output='screen',
additional_env=gz_env, # type: ignore
shell=False,
)
]
spawn_entity = Node(
package="ros_gz_sim",
executable="create",
output="screen",
arguments=[
"-name",
"sam_bot",
"-topic",
"robot_description",
"-z",
"1.0",
"-x",
"-2.0",
"--ros-args",
"--log-level",
log_level,
],
parameters=[{"use_sim_time": use_sim_time}],
)
bridge = Node(
package="ros_gz_bridge",
executable="parameter_bridge",
name="gz_bridge",
arguments=[
"/scan@sensor_msgs/msg/LaserScan[gz.msgs.LaserScan",
"/imu@sensor_msgs/msg/Imu[gz.msgs.IMU",
"/sky_cam@sensor_msgs/msg/Image@gz.msgs.Image",
"/robot_cam@sensor_msgs/msg/Image@gz.msgs.Image",
"/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo",
# Clock message is necessary for the diff_drive_controller to accept commands https://github.com/ros-controls/gz_ros2_control/issues/106
"/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock",
],
output="screen",
)
load_joint_state_controller = ExecuteProcess(
name="activate_joint_state_broadcaster",
cmd=[
"ros2",
"control",
"load_controller",
"--set-state",
"active",
"joint_state_broadcaster",
],
shell=False,
output="screen",
)
return launch.LaunchDescription(
[
SetEnvironmentVariable(
name="GZ_SIM_RESOURCE_PATH",
value=gz_models_path,
),
SetEnvironmentVariable(
name="GZ_SIM_MODEL_PATH",
value=gz_models_path,
),
DeclareLaunchArgument(
name="model",
default_value=default_model_path,
description="Absolute path to robot urdf file",
),
DeclareLaunchArgument(
name="use_rviz",
default_value="True",
description="Start RViz",
),
DeclareLaunchArgument(
name="run_headless",
default_value="False",
description="Start GZ in hedless mode and don't start RViz (overrides use_rviz)",
),
DeclareLaunchArgument(
name="rvizconfig",
default_value=default_rviz_config_path,
description="Absolute path to rviz config file",
),
DeclareLaunchArgument(
name="use_sim_time",
default_value="True",
description="Flag to enable use_sim_time",
),
DeclareLaunchArgument(
name="use_localization",
default_value="True",
description="Use EKF to estimagte odom->base_link transform from IMU + wheel odometry",
),
DeclareLaunchArgument(
"gz_verbosity",
default_value="3",
description="Verbosity level for Ignition Gazebo (0~4).",
),
DeclareLaunchArgument(
"gz_args",
default_value="",
description="Extra args for Gazebo (ie. '-s' for running headless)",
),
DeclareLaunchArgument(
name="log_level",
default_value="warn",
description="The level of logging that is applied to all ROS 2 nodes launched by this script.",
),
bridge,
robot_state_publisher_node,
spawn_entity,
# rviz_node,
RegisterEventHandler(
event_handler=OnProcessExit(
target_action=spawn_entity,
on_exit=[load_joint_state_controller],
)
),
] + gazebo
)
@soumya997
Copy link
Author

soumya997 commented Apr 1, 2024

Its copied from,
display.launch.py , made changes in this to make the above code only load the model on gazebo and run it on rviz. it is written for gazebo ignition. So, I made changes to make it run on gazebo sim. But for some reason the world is opening but the model is not showing up in the gazebo.

and shows below error,

@soumya997
Copy link
Author

Unable to load the model on rviz, if activate_joint_state_broadcaster not running

when not running activate_joint_state_broadcaster

when running activate_joint_state_broadcaster

I dont know thats the need of this(activate_joint_state_broadcaster) package, seems like a ros control package.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment