Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Standalone launch file for rviz #161

Merged
merged 9 commits into from
Nov 16, 2023
Merged

Standalone launch file for rviz #161

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
3ddaa11
add arguments to configure publishing pointcloud and depth_registered…
jschornak-bdai Oct 5, 2023
3ef4e7c
fix line lengths
jschornak-bdai Oct 5, 2023
a4c5e77
Standalone launch file for rviz
jbarry-bdai Oct 12, 2023
34d0d27
Update spot_driver/launch/spot_driver.launch.py
jbarry-bdai Oct 23, 2023
844fbdd
has_arm is no longer a launch argument
abaker-bdai Nov 7, 2023
e183496
pre-commit
abaker-bdai Nov 7, 2023
85c3940
pre-commit one more time
abaker-bdai Nov 8, 2023
37c0311
Merge branch 'main' into jlb/rviz_launch
abaker-bdai Nov 13, 2023
46c1656
Merge branch 'main' into jlb/rviz_launch
abaker-bdai Nov 16, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
81 changes: 81 additions & 0 deletions spot_driver/launch/rviz.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,81 @@
# Copyright [2023] Boston Dynamics AI Institute, Inc.
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The Quality of Service needs to be set to "best effort" in Rviz if you want to visualize the PointCloud2 under Reliability Policy. Is there a way to set that automatically in this config?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

There is a way to have that default to the right setting, but I think you need to provide that in the config YAML file. Here's the full entry for a PointCloud display, for example. We set Topic -> Reliability Policy to Best Effort instead of Reliable.

    - Alpha: 1
      Autocompute Intensity Bounds: true
      Autocompute Value Bounds:
        Max Value: 3.0432052612304688
        Min Value: -0.07985210418701172
        Value: true
      Axis: Z
      Channel Name: intensity
      Class: rviz_default_plugins/PointCloud2
      Color: 255; 255; 255
      Color Transformer: AxisColor
      Decay Time: 0
      Enabled: true
      Invert Rainbow: false
      Max Color: 255; 255; 255
      Max Intensity: 4096
      Min Color: 0; 0; 0
      Min Intensity: 0
      Name: PointCloud2
      Position Transformer: XYZ
      Selectable: true
      Size (Pixels): 3
      Size (m): 0.009999999776482582
      Style: Flat Squares
      Topic:
        Depth: 5
        Durability Policy: Volatile
        History Policy: Keep Last
        Reliability Policy: Best Effort
        Value: /points_back
      Use Fixed Frame: true
      Use rainbow: true
      Value: true


import os

import launch
import launch_ros
import yaml
from ament_index_python.packages import get_package_share_directory
from launch import LaunchContext, LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction
from launch.substitutions import (
LaunchConfiguration,
PathJoinSubstitution,
)
from launch_ros.substitutions import FindPackageShare

THIS_PACKAGE = "spot_driver"


def create_rviz_config(robot_name: str) -> None:
"""Writes a configuration file for rviz to visualize a single spot robot"""

RVIZ_TEMPLATE_FILENAME = os.path.join(get_package_share_directory(THIS_PACKAGE), "rviz", "spot_template.yaml")
RVIZ_OUTPUT_FILENAME = os.path.join(get_package_share_directory(THIS_PACKAGE), "rviz", "spot.rviz")

with open(RVIZ_TEMPLATE_FILENAME, "r") as template_file:
config = yaml.safe_load(template_file)

if robot_name:
# replace fixed frame with robot body frame
config["Visualization Manager"]["Global Options"]["Fixed Frame"] = f"{robot_name}/vision"
# Add robot models for each robot
for display in config["Visualization Manager"]["Displays"]:
if "RobotModel" in display["Class"]:
display["Description Topic"]["Value"] = f"/{robot_name}/robot_description"

if "Image" in display["Class"]:
topic_name = display["Topic"]["Value"]
display["Topic"]["Value"] = f"/{robot_name}{topic_name}"

with open(RVIZ_OUTPUT_FILENAME, "w") as out_file:
yaml.dump(config, out_file)


def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None:
rviz_config_file = LaunchConfiguration("rviz_config_file").perform(context)
spot_name = LaunchConfiguration("spot_name").perform(context)

# It looks like passing an optional of value "None" gets converted to a string of value "None"
if not rviz_config_file or rviz_config_file == "None":
create_rviz_config(spot_name)
rviz_config_file = PathJoinSubstitution([FindPackageShare(THIS_PACKAGE), "rviz", "spot.rviz"]).perform(context)

rviz = launch_ros.actions.Node(
package="rviz2",
executable="rviz2",
name="rviz2",
arguments=["-d", rviz_config_file],
output="screen",
)

ld.add_action(rviz)


def generate_launch_description() -> launch.LaunchDescription:
launch_args = []

launch_args.append(
DeclareLaunchArgument(
"rviz_config_file",
default_value="",
description="RViz config file",
)
)
launch_args.append(DeclareLaunchArgument("spot_name", default_value="", description="Name of Spot"))

ld = launch.LaunchDescription(launch_args)

ld.add_action(OpaqueFunction(function=launch_setup, args=[ld]))

return ld
47 changes: 8 additions & 39 deletions spot_driver/launch/spot_driver.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,11 +7,10 @@

import launch
import launch_ros
import yaml
from ament_index_python.packages import get_package_share_directory
from launch import LaunchContext, LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution, TextSubstitution
from launch_ros.substitutions import FindPackageShare

Expand Down Expand Up @@ -105,31 +104,6 @@ def create_point_cloud_nodelets(
return composable_node_descriptions


def create_rviz_config(robot_name: str) -> None:
"""Writes a configuration file for rviz to visualize a single spot robot"""

RVIZ_TEMPLATE_FILENAME = os.path.join(get_package_share_directory(THIS_PACKAGE), "rviz", "spot_template.yaml")
RVIZ_OUTPUT_FILENAME = os.path.join(get_package_share_directory(THIS_PACKAGE), "rviz", "spot.rviz")

with open(RVIZ_TEMPLATE_FILENAME, "r") as template_file:
config = yaml.safe_load(template_file)

if robot_name:
# replace fixed frame with robot body frame
config["Visualization Manager"]["Global Options"]["Fixed Frame"] = f"{robot_name}/vision"
# Add robot models for each robot
for display in config["Visualization Manager"]["Displays"]:
if "RobotModel" in display["Class"]:
display["Description Topic"]["Value"] = f"/{robot_name}/robot_description"

if "Image" in display["Class"]:
topic_name = display["Topic"]["Value"]
display["Topic"]["Value"] = f"/{robot_name}{topic_name}"

with open(RVIZ_OUTPUT_FILENAME, "w") as out_file:
yaml.dump(config, out_file)


def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None:
logger = logging.getLogger("spot_driver_launch")

Expand Down Expand Up @@ -240,17 +214,12 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None:
)
ld.add_action(robot_state_publisher)

# It looks like passing an optional of value "None" gets converted to a string of value "None"
if not rviz_config_file or rviz_config_file == "None":
create_rviz_config(spot_name)
rviz_config_file = PathJoinSubstitution([FindPackageShare(THIS_PACKAGE), "rviz", "spot.rviz"]).perform(context)

rviz = launch_ros.actions.Node(
package="rviz2",
executable="rviz2",
name="rviz2",
arguments=["-d", rviz_config_file],
output="screen",
rviz = IncludeLaunchDescription(
PythonLaunchDescriptionSource([FindPackageShare(THIS_PACKAGE), "/launch", "/rviz.launch.py"]),
launch_arguments={
"spot_name": spot_name,
"rviz_config_file": rviz_config_file,
}.items(),
condition=IfCondition(launch_rviz),
)

Expand Down
Loading