diff --git a/ros_gz_sim_demos/README.md b/ros_gz_sim_demos/README.md index f8409aa3..598f4558 100644 --- a/ros_gz_sim_demos/README.md +++ b/ros_gz_sim_demos/README.md @@ -32,7 +32,7 @@ And Get the current state of a battery. - ros2 launch ros_gz_sim_demos battery.launch.xml + ros2 launch ros_gz_sim_demos battery.launch.py Then send a command so the vehicle moves and drains the battery. diff --git a/ros_gz_sim_demos/config/battery.yaml b/ros_gz_sim_demos/config/battery.yaml index 61ad5ae9..9f6494bb 100644 --- a/ros_gz_sim_demos/config/battery.yaml +++ b/ros_gz_sim_demos/config/battery.yaml @@ -1,18 +1,12 @@ # Battery configuration. -- ros_topic_name: "/model/vehicle_blue/cmd_vel" - gz_topic_name: "/model/vehicle_blue/cmd_vel" +- topic_name: "/model/vehicle_blue/cmd_vel" ros_type_name: "geometry_msgs/msg/Twist" gz_type_name: "gz.msgs.Twist" - subscriber_queue: 5 - publisher_queue: 6 - lazy: false + lazy: true direction: ROS_TO_GZ -- ros_topic_name: "/model/vehicle_blue/battery/linear_battery/state" - gz_topic_name: "/model/vehicle_blue/battery/linear_battery/state" +- topic_name: "/model/vehicle_blue/battery/linear_battery/state" ros_type_name: "sensor_msgs/msg/BatteryState" gz_type_name: "gz.msgs.BatteryState" - subscriber_queue: 5 - publisher_queue: 6 - lazy: false + lazy: true direction: GZ_TO_ROS diff --git a/ros_gz_sim_demos/launch/battery.launch.py b/ros_gz_sim_demos/launch/battery.launch.py new file mode 100644 index 00000000..a0da9fdf --- /dev/null +++ b/ros_gz_sim_demos/launch/battery.launch.py @@ -0,0 +1,64 @@ +# Copyright 2019 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from ros_gz_bridge.actions import RosGzBridge + + +def generate_launch_description(): + + pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim') + pkg_ros_gz_sim_demos = get_package_share_directory('ros_gz_sim_demos') + + # RQt + rqt = Node( + package='rqt_plot', + executable='rqt_plot', + # FIXME: Why isn't the topic being populated on the UI? RQt issue? + arguments=['--force-discover', + '/model/vehicle_blue/battery/linear_battery/state/percentage'], + condition=IfCondition(LaunchConfiguration('rqt')) + ) + + # Gazebo + gz_sim = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')), + launch_arguments={ + 'gz_args': '-r -z 1000000 linear_battery_demo.sdf' + }.items(), + ) + + # Bridge + ros_gz_bridge = RosGzBridge( + bridge_name='ros_gz_bridge', + config_file=os.path.join(pkg_ros_gz_sim_demos, 'config', 'battery.yaml'), + ) + + return LaunchDescription([ + gz_sim, + DeclareLaunchArgument('rqt', default_value='true', + description='Open RQt.'), + ros_gz_bridge, + rqt + ]) diff --git a/ros_gz_sim_demos/launch/battery.launch.xml b/ros_gz_sim_demos/launch/battery.launch.xml deleted file mode 100644 index c93fcf9c..00000000 --- a/ros_gz_sim_demos/launch/battery.launch.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - -