can someone help me with ros2 gazebo? i tried to bridge ros2 topic with gazebo topic to publish std_msgs/Int32 msg to a gz.msgs.Int32 topic with joystick button presses to make the cmd_vel output respond so that the robot included in gazebo world respond so that robot moves in a certain direction. the gazebo topic viewer can see my ros2 topic but the triggered publisher does not respond. the code and sdf file and the cmake file is here below:
# this is a launch file to learn to publish a gazebo world with gazebo and publish ros2 joystick msgs to gazebo
# so the comments can be messy
import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, Command, TextSubstitution
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
from launch_ros.substitutions import FindPackageShare
def generate_launch_description():
pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim')
gazebo_basics_pkg = get_package_share_directory('gz_train1')
default_rviz_config_path = PathJoinSubstitution([gazebo_basics_pkg, 'rviz', 'urdf.rviz'])
# Show joint state publisher GUI for joints
gui_arg = DeclareLaunchArgument(name='gui', default_value='true', choices=['true', 'false'],
description='Flag to enable joint_state_publisher_gui')
# RViz config file path
rviz_arg = DeclareLaunchArgument(name='rvizconfig', default_value=default_rviz_config_path,
description='Absolute path to rviz config file')
# URDF model path to spawn urdf file in gazebo
model_arg = DeclareLaunchArgument(
'model', default_value=os.path.join(gazebo_basics_pkg,'urdf','07-physics.urdf'),
description='Name of the URDF description to load'
)
# Use built-in ROS2 URDF launch package with our own arguments
urdf_rviz = IncludeLaunchDescription(
PathJoinSubstitution([FindPackageShare('urdf_launch'), 'launch', 'display.launch.py']),
launch_arguments={
'urdf_package': 'gz_train1',
'urdf_package_path': PathJoinSubstitution([gazebo_basics_pkg,'urdf', '08-macroed.urdf.xacro']),
'rviz_config': LaunchConfiguration('rvizconfig'),
'jsp_gui': LaunchConfiguration('gui')}.items()
)
# Define the path to your URDF or Xacro file
urdf_file_path = PathJoinSubstitution([os.path.join(
gazebo_basics_pkg, # Replace with your package name
"urdf","07-physics.urdf")
])
gazebo_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py'),
),
launch_arguments={'gz_args': [PathJoinSubstitution([
gazebo_basics_pkg,
'worlds',
'camjoylidar.sdf'
]),
#TextSubstitution(text=' -r -v -v1 --render-engine ogre')],
TextSubstitution(text=' -r -v -v4')],
'on_exit_shutdown': 'false'}.items()
)
joy = Node(package='gz_train1',
namespace='joy_bridge'
,executable='joy1',
remappings=[('/joy1', '/joy')],
name='pub3',
output='screen')
# node that supposed to send ros2 msgs to the sdf world
bridge = Node(
package='ros_gz_bridge',
executable='parameter_bridge',
name="joy_bridge",
namespace='joy_trigger',
arguments=['/joy1@std_msgs/msg/Int32@gz.msgs.Int32'],
remappings=[('/joy1', '/joy')],
output='screen'
,
parameters=[
{'use_sim_time': True},
]
)
launchDescriptionObject = LaunchDescription()
launchDescriptionObject.add_action(gazebo_launch)
launchDescriptionObject.add_action(bridge)
launchDescriptionObject.add_action(joy)
launchDescriptionObject.add_action(gui_arg)
launchDescriptionObject.add_action(rviz_arg)
#launchDescriptionObject.add_action(model_arg)
#launchDescriptionObject.add_action(urdf_rviz)
#launchDescriptionObject.add_action(robot_state_publisher_node)
#launchDescriptionObject.add_action(joint_state_publisher_gui_node)
return launchDescriptionObject
// This is a node to publish msgs with joystick button presses (this pkg is only used with the xbox one controller)
// Below are the standard headers
#include <chrono>
#include <string>
#include <stdio.h>
#include <unistd.h>
#include <stdint.h>
#include <fcntl.h>
#include <iostream>
// Below are the standard headers for ros2
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/int32.hpp"
// Below is the header to get joystick input from bluetooth communication.
#include <linux/joystick.h>
// Below are the standard namespaces to shorten the code.
using namespace std;
using namespace std::chrono_literals;
// node msg is created.
// below is the struct for the joystick values create
// This node only uses the button values for this node but you can use more if you want to
//but i recommend to look for this repo as a example: https://github.com/t-kiyozumi/joystick_on_linux.git
typedef struct
{
uint16_t X;
uint16_t Y;
uint16_t A;
uint16_t B;
uint16_t LB;
uint16_t LT;
uint16_t RB;
uint16_t RT;
uint16_t start;
uint16_t back;
int16_t axes1_x;
int16_t axes1_y;
int16_t axes0_x;
int16_t axes0_y;
} controler_state;
void write_controler_state(controler_state *controler, js_event event) // this fuction writes the controller state and publish the node
{
switch (event.type)
{
case JS_EVENT_BUTTON:
auto node = rclcpp::Node::make_shared("topic");
auto publisher = node->create_publisher<std_msgs::msg::Int32>("joy1", 10);
// below are the button commands to publish the message data.
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(node);
auto message = std_msgs::msg::Int32();
message.data = 0;
publisher->publish(message);
RCLCPP_INFO(node->get_logger(), "Publishing joystick button b:'%i'", message.data);
if (event.number == 1)
{
controler->B = event.value;
message.data = 20;
publisher->publish(message);
RCLCPP_INFO(node->get_logger(), "Publishing joystick button b:'%i'", message.data);
}
if (event.number == 0)
{
controler->A = event.value;
message.data = 10;
publisher->publish(message);
RCLCPP_INFO(node->get_logger(), "Publishing joystick button a: x'%i'", message.data);
}
if (event.number == 3)
{
controler->X = event.value;
message.data = 30;
publisher->publish(message);
RCLCPP_INFO(node->get_logger(), "Publishing joystick button a: y'%i'", message.data);
}
if (event.number == 4)
{
controler->Y = event.value;
message.data = 40;
publisher->publish(message);
RCLCPP_INFO(node->get_logger(), "Publishing joystick button y:'%i'", message.data);
}
if (event.number == 6)
{
controler->LB = event.value;
message.data = 0;
publisher->publish(message);
RCLCPP_INFO(node->get_logger(), "Publishing joystick value:'%i'", message.data);
}
if (event.number == 7)
{
controler->RB = event.value;
message.data = 7;
publisher->publish(message);
RCLCPP_INFO(node->get_logger(), "Publishing joystick button rb:'%i'", message.data);
rclcpp::shutdown();
}
}
}
int main(int argc, char * argv[])
{ rclcpp::init(argc, argv); // Initialise rclcpp
int fd = open("/dev/input/js0", O_RDONLY);
struct js_event event;
controler_state *controler;
controler = (controler_state *)malloc(sizeof(controler_state));
while (1) // now the code publish msgs created by a button presses in a loop.
{
read(fd, &event, sizeof(event));
write_controler_state(controler, event);
usleep(1000);
}
return 0;
}
<!--
this is a world were you can drive a diffrential driver
with telop plugin,gz.msgs.Int32(maybe ros2 topic in the future)
-->
<sdf version="1.10">
<world name="visualize_lidar_world">
<physics name="1ms" type="ignored">
<max_step_size>0.001</max_step_size>
<real_time_factor>1.0</real_time_factor>
</physics>
<plugin
filename="gz-sim-physics-system"
name="gz::sim::systems::Physics">
</plugin>
<plugin
filename="gz-sim-sensors-system"
name="gz::sim::systems::Sensors">
<render_engine>ogre2</render_engine>
</plugin>
<plugin
filename="gz-sim-scene-broadcaster-system"
name="gz::sim::systems::SceneBroadcaster">
</plugin>
<gui fullscreen="0">
<!-- 3D scene -->
<plugin filename="MinimalScene" name="3D View">
<gz-gui>
<title>3D View</title>
<property type="bool" key="showTitleBar">false</property>
<property type="string" key="state">docked</property>
</gz-gui>
<engine>ogre2</engine>
<scene>scene</scene>
<ambient_light>0.4 0.4 0.4</ambient_light>
<background_color>0.8 0.8 0.8</background_color>
<camera_pose>-6 0 6 0 0.5 0</camera_pose>
</plugin>
<!-- Plugins that add functionality to the scene -->
<plugin filename="EntityContextMenuPlugin" name="Entity context menu">
<gz-gui>
<property key="state" type="string">floating</property>
<property key="width" type="double">5</property>
<property key="height" type="double">5</property>
<property key="showTitleBar" type="bool">false</property>
</gz-gui>
</plugin>
<plugin filename="GzSceneManager" name="Scene Manager">
<gz-gui>
<property key="resizable" type="bool">false</property>
<property key="width" type="double">5</property>
<property key="height" type="double">5</property>
<property key="state" type="string">floating</property>
<property key="showTitleBar" type="bool">false</property>
</gz-gui>
</plugin>
<plugin filename="InteractiveViewControl" name="Interactive view control">
<gz-gui>
<property key="resizable" type="bool">false</property>
<property key="width" type="double">5</property>
<property key="height" type="double">5</property>
<property key="state" type="string">floating</property>
<property key="showTitleBar" type="bool">false</property>
</gz-gui>
</plugin>
<plugin filename="CameraTracking" name="Camera Tracking">
<gz-gui>
<property key="resizable" type="bool">false</property>
<property key="width" type="double">5</property>
<property key="height" type="double">5</property>
<property key="state" type="string">floating</property>
<property key="showTitleBar" type="bool">false</property>
</gz-gui>
</plugin>
<!-- World control -->
<plugin filename="WorldControl" name="World control">
<gz-gui>
<title>World control</title>
<property type="bool" key="showTitleBar">false</property>
<property type="bool" key="resizable">false</property>
<property type="double" key="height">72</property>
<property type="double" key="z">1</property>
<property type="string" key="state">floating</property>
<anchors target="3D View">
<line own="left" target="left"/>
<line own="bottom" target="bottom"/>
</anchors>
</gz-gui>
<play_pause>true</play_pause>
<step>true</step>
<start_paused>true</start_paused>
<use_event>true</use_event>
</plugin>
<!-- World statistics -->
<plugin filename="WorldStats" name="World stats">
<gz-gui>
<title>World stats</title>
<property type="bool" key="showTitleBar">false</property>
<property type="bool" key="resizable">false</property>
<property type="double" key="height">110</property>
<property type="double" key="width">290</property>
<property type="double" key="z">1</property>
<property type="string" key="state">floating</property>
<anchors target="3D View">
<line own="right" target="right"/>
<line own="bottom" target="bottom"/>
</anchors>
</gz-gui>
<sim_time>true</sim_time>
<real_time>true</real_time>
<real_time_factor>true</real_time_factor>
<iterations>true</iterations>
</plugin>
<plugin filename="VisualizeLidar" name="Visualize Lidar">
</plugin>
<!-- Inspector -->
<plugin filename="ComponentInspector" name="Component inspector">
<gz-gui>
<property type="string" key="state">docked</property>
</gz-gui>
</plugin>
<!-- Entity tree -->
<plugin filename="EntityTree" name="Entity tree">
<gz-gui>
<property type="string" key="state">docked</property>
</gz-gui>
</plugin>
</gui>
<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>
<model name="ground_plane">
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<box>
<size>20 20 0.1</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>20 20 0.1</size>
</box>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
</model>
<model name="box">
<pose>0 -1 0.5 0 0 0</pose>
<link name="box_link">
<inertial>
<inertia>
<ixx>1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1</iyy>
<iyz>0</iyz>
<izz>1</izz>
</inertia>
<mass>1.0</mass>
</inertial>
<collision name="box_collision">
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
</collision>
<visual name="box_visual">
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
<material>
<ambient>1 0 0 1</ambient>
<diffuse>1 0 0 1</diffuse>
<specular>1 0 0 1</specular>
</material>
</visual>
</link>
</model>
<model name="model_with_lidar">
<pose>4 0 0.5 0 0.0 3.14</pose>
<link name="link">
<pose>0.05 0.05 0.05 0 0 0</pose>
<inertial>
<mass>0.1</mass>
<inertia>
<ixx>0.000166667</ixx>
<iyy>0.000166667</iyy>
<izz>0.000166667</izz>
</inertia>
</inertial>
<collision name="collision">
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
</visual>
<sensor name='gpu_lidar' type='gpu_lidar'>
<pose relative_to='lidar_sensor_joint' > 4 0 0.5 0 0.0 3.14</pose>
<topic>lidar</topic>
<update_rate>10</update_rate>
<lidar>
<scan>
<horizontal>
<samples>640</samples>
<resolution>1</resolution>
<min_angle>-1.396263</min_angle>
<max_angle>1.396263</max_angle>
</horizontal>
<vertical>
<samples>16</samples>
<resolution>1</resolution>
<min_angle>-0.261799</min_angle>
<max_angle>0.261799</max_angle>
</vertical>
</scan>
<range>
<min>0.08</min>
<max>10.0</max>
<resolution>0.01</resolution>
</range>
</lidar>
<visualize>true</visualize>
</sensor>
</link>
<static>true</static>
</model>
<model name='vehicle_blue'>
<pose>-4 0 0.325 0 0 0.0</pose>
<link name='chassis'>
<pose>-0.151427 -0 0.175 0 -0 0</pose>
<inertial>
<mass>1.14395</mass>
<inertia>
<ixx>0.126164</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.416519</iyy>
<iyz>0</iyz>
<izz>0.481014</izz>
</inertia>
</inertial>
<visual name='visual'>
<geometry>
<box>
<size>2.01142 1 0.568726</size>
</box>
</geometry>
<material>
<ambient>0.5 0.5 1.0 1</ambient>
<diffuse>0.5 0.5 1.0 1</diffuse>
<specular>0.0 0.0 1.0 1</specular>
</material>
</visual>
<collision name='collision'>
<geometry>
<box>
<size>2.01142 1 0.568726</size>
</box>
</geometry>
</collision>
</link>
<link name="lidar_link">
<pose>0 0 0.5 0 0 0</pose>
<inertial>
<mass>0.1</mass>
<inertia>
<ixx>0.000166667</ixx>
<iyy>0.000166667</iyy>
<izz>0.000166667</izz>
</inertia>
</inertial>
<collision name="collision">
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
</visual>
<sensor name="boundingbox_camera" type="boundingbox_camera">
<topic>img</topic>
<pose relative_to='lidar_link' > 0 0 0.5 0 0.0 0 </pose>
<camera>
<box_type>2d</box_type>
<horizontal_fov>1.047</horizontal_fov>
<image>
<width>800</width>
<height>600</height>
</image>
<clip>
<near>0.1</near>
<far>10</far>
</clip>
</camera>
<always_on>1</always_on>
<update_rate>30</update_rate>
<visualize>true</visualize>
</sensor>
</link>
<link name='left_wheel'>
<pose>0.554283 0.625029 -0.025 -1.5707 0 0</pose>
<inertial>
<mass>2</mass>
<inertia>
<ixx>0.145833</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.145833</iyy>
<iyz>0</iyz>
<izz>0.125</izz>
</inertia>
</inertial>
<visual name='visual'>
<geometry>
<sphere>
<radius>0.3</radius>
</sphere>
</geometry>
<material>
<ambient>0.2 0.2 0.2 1</ambient>
<diffuse>0.2 0.2 0.2 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
</material>
</visual>
<collision name='collision'>
<geometry>
<sphere>
<radius>0.3</radius>
</sphere>
</geometry>
</collision>
</link>
<link name='right_wheel'>
<pose>0.554282 -0.625029 -0.025 -1.5707 0 0</pose>
<inertial>
<mass>2</mass>
<inertia>
<ixx>0.145833</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.145833</iyy>
<iyz>0</iyz>
<izz>0.125</izz>
</inertia>
</inertial>
<visual name='visual'>
<geometry>
<sphere>
<radius>0.3</radius>
</sphere>
</geometry>
<material>
<ambient>0.2 0.2 0.2 1</ambient>
<diffuse>0.2 0.2 0.2 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
</material>
</visual>
<collision name='collision'>
<geometry>
<sphere>
<radius>0.3</radius>
</sphere>
</geometry>
</collision>
</link>
<link name='caster'>
<pose>-0.957138 -0 -0.125 0 -0 0</pose>
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.1</iyy>
<iyz>0</iyz>
<izz>0.1</izz>
</inertia>
</inertial>
<visual name='visual'>
<geometry>
<sphere>
<radius>0.2</radius>
</sphere>
</geometry>
<material>
<ambient>0.2 0.2 0.2 1</ambient>
<diffuse>0.2 0.2 0.2 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
</material>
</visual>
<collision name='collision'>
<geometry>
<sphere>
<radius>0.2</radius>
</sphere>
</geometry>
</collision>
</link>
<joint name='lidar_sensor_joint' type='fixed'>
<parent>chassis</parent>
<child>lidar_link</child>
</joint>
<joint name='left_wheel_joint' type='revolute'>
<parent>chassis</parent>
<child>left_wheel</child>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-1.79769e+308</lower>
<upper>1.79769e+308</upper>
</limit>
</axis>
</joint>
<joint name='right_wheel_joint' type='revolute'>
<parent>chassis</parent>
<child>right_wheel</child>
<axis>
<xyz>0 0 1</xyz>
<limit>
<lower>-1.79769e+308</lower>
<upper>1.79769e+308</upper>
</limit>
</axis>
</joint>
<joint name='caster_wheel' type='ball'>
<parent>chassis</parent>
<child>caster</child>
</joint>
<plugin
filename="gz-sim-diff-drive-system"
name="gz::sim::systems::DiffDrive">
<left_joint>left_wheel_joint</left_joint>
<right_joint>right_wheel_joint</right_joint>
<wheel_separation>1.25</wheel_separation>
<wheel_radius>0.3</wheel_radius>
<odom_publish_frequency>1</odom_publish_frequency>
<topic>cmd_vel</topic>
</plugin>
</model>
<!-- Moving Forward-->
<plugin filename="gz-sim-triggered-publisher-system"
name="gz::sim::systems::TriggeredPublisher">
<input type="gz.msgs.Int32" topic="/joy">
<match field="data">40</match>
</input>
<output type="gz.msgs.Twist" topic="/cmd_vel">
linear: {x: 0.5}, angular: {z: 0.0}
</output>
</plugin>
<!-- Moving Backward-->
<plugin filename="gz-sim-triggered-publisher-system"
name="gz::sim::systems::TriggeredPublisher">
<input type="gz.msgs.Int32" topic="/joy">
<match field="data">30</match>
</input>
<output type="gz.msgs.Twist" topic="/cmd_vel">
linear: {x: -0.5}, angular: {z: 0.0}
</output>
</plugin>
<!-- Moving leftward-->
<plugin filename="gz-sim-triggered-publisher-system"
name="gz::sim::systems::TriggeredPublisher">
<input type="gz.msgs.Int32" topic="/joy">
<match field="data">20</match>
</input>
<output type="gz.msgs.Twist" topic="/cmd_vel">
linear: {x: 0.0}, angular: {z: 0.5}
</output>
</plugin>
<!-- Moving rightwards-->
<plugin filename="gz-sim-triggered-publisher-system"
name="gz::sim::systems::TriggeredPublisher">
<input type="gz.msgs.Int32" topic="/joy">
<match field="data">10</match>
</input>
<output type="gz.msgs.Twist" topic="/cmd_vel">
linear: {x: 0.0}, angular: {z: -0.5}
</output>
</plugin>
<!-- stop moving-->
<plugin filename="gz-sim-triggered-publisher-system"
name="gz::sim::systems::TriggeredPublisher">
<input type="gz.msgs.Int32" topic="/joy">
<match field="data">50</match>
</input>
<output type="gz.msgs.Twist" topic="/cmd_vel">
linear: {x: 0.0}, angular: {z: 0.0}
</output>
</plugin>
</world>
</sdf>
#here is the cmake list
cmake_minimum_required(VERSION 3.8)
project(gz_train1)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(ament_cmake_python REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(diagnostic_msgs REQUIRED)
find_package(diagnostic_updater REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(rclpy REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
include_directories(include)
add_executable(gz_sub src/gz_sub.cpp)
add_executable(joy1 src/joytest.cpp)
add_executable(slpub src/gz_pub_sleep.cpp)
add_executable(joy2 src/joytest2.cpp)
add_executable(tf2_broad src/tf2_staticbroad_test1.cpp)
ament_python_install_package(scripts)
ament_python_install_package(gz_train1)
ament_target_dependencies(
slpub
geometry_msgs
std_msgs
rclcpp
)
ament_target_dependencies(
tf2_broad
geometry_msgs
rclcpp
tf2
tf2_ros
)
ament_target_dependencies(
gz_sub
std_msgs
rclcpp
)
ament_target_dependencies(
joy1
std_msgs
rclcpp
)
ament_target_dependencies(
joy2
std_msgs
rclcpp
)
install(TARGETS
slpub
joy1
joy2
gz_sub
tf2_broad
DESTINATION lib/${PROJECT_NAME})
install(DIRECTORY
launch urdf worlds rviz scripts gz_train1
DESTINATION share/${PROJECT_NAME})
# Install Python executables in the ros2_ws
install(PROGRAMS
scripts/test_pub.py
DESTINATION lib/${PROJECT_NAME}
)
ament_package()