Note: This tutorial assumes that you have completed the previous tutorials: How to set up a Pilz PSENscan laser scanner with Ethernet, How to set up ROS to communicate with the scanner. |
Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Launching multiple scanners
Description: This tutorial explains how to use multiple scanners.Keywords: PSENscan, psen_scan, psen_scan_v2
Tutorial Level: BEGINNER
Contents
Introduction
This part will teach you how to start multiple scanners in master mode with a single launchfile, by showing an example of how to launch two scanners simultaneously.
Prerequisites
Make sure to complete the preceding tutorials successfully!
Setup an architecture similar to that one shown in the picture above.
Creating the config files
Navigate to your config folder:
roscd psen_scan_v2_tutorials/config
We will reuse the first config-file from the previous tutorial and create a second config-file:
gedit scanner2_config.yaml
Fill the yaml-file with key: value pairs that match the parameters you would like to store. Here is an example:
sensor_ip: "192.168.0.20" # IP address of the laser scanner tf_prefix: laser_2 # This name is prefixed to the TF, where the scanner is mounted host_udp_port_control: 50505 # Make sure to use different ports than in the first scanner config. host_udp_port_data: 50506 ## The following parameters are optional angle_start: deg(-137.4) # The angle of the first measurement angle_end: deg(137.4) # The angle of the last measurement intensities: false # Receive intensity values resolution: deg(0.1) # Scan resolution host_ip: "auto" # Change the IP to the IP of your PC. fragmented_scans: false # Receive fragmented scans a.s.a.p. instead of full scan
Note: You cannot assign adjacent IP addresses to different laser scanners, e.g. 192.168.0.10 and 192.168.0.11, because a single scanner takes up two IP-Adresses.
Note: If all your scanners send data to the same host, they have to send to different host_udp_ports to avoid conflicts.
Note: You need to choose unique names for each scanner to be able to position them independently from one another inside your ROS environment.
Creating the launch file
Create a new launch file inside your launch directory
roscd psen_scan_v2_tutorials/launch gedit two_scanners.launch
Here is an example launch file:
<launch> <include file="$(find psen_scan_v2)/launch/bringup.launch"> <arg name="tf_prefix" value="laser_1" /> </include> <rosparam ns="laser_1" command="load" file="$(find psen_scan_v2_tutorials)/config/scanner_config.yaml" /> <include file="$(find psen_scan_v2)/launch/bringup.launch"> <arg name="tf_prefix" value="laser_2" /> </include> <rosparam ns="laser_2" command="load" file="$(find psen_scan_v2_tutorials)/config/scanner2_config.yaml" /> <param name="robot_description" command="$(find xacro)/xacro '$(find psen_scan_v2_tutorials)/urdf/two_scanners.urdf.xacro'" /> <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" /> <node name="$(anon rviz)" type="rviz" pkg="rviz" args="-d $(find psen_scan_v2)/config/config.rviz" /> </launch>
Note: The configuration has to be loaded into the node's namespaces. The node names are given by the name argument.
In this launch file we start two laser scanner nodes. Each node publishes the data of the respective scanner on a separate topic.
Creating the urdf.xacro file
Create a xacro file
roscd ../urdf gedit two_scanners.urdf.xacro
Here is an example launch file:
<robot xmlns:xacro="http://ros.org/wiki/xacro" name="psen_scan"> <xacro:include filename="$(find psen_scan_v2)/urdf/psen_scan.urdf.xacro" /> <link name="base_link"/> <joint name="laser_1_base_link_joint" type="fixed"> <parent link="base_link" /> <child link="laser_1_mount_link" /> <origin rpy="0 0 ${-pi/2}" xyz="0 -1 0"/> </joint> <xacro:psen_scan prefix="laser_1" /> <joint name="laser_2_base_link_joint" type="fixed"> <parent link="base_link" /> <child link="laser_2_mount_link" /> <origin rpy="0 0 ${pi/2}" xyz="0 1 0"/> </joint> <xacro:psen_scan prefix="laser_2" /> </robot>
This is the file, that will in future describe the full robot. Now it contains the following aspects:
<xacro:include filename="$(find psen_scan_v2)/urdf/psen_scan.urdf.xacro" /> <link name="base_link"/>
Imports our psen_scan macro and introduces a base_link. This will later be the main coordinate frame of your robot.
<joint name="laser_1_base_link_joint" type="fixed"> <parent link="base_link" /> <child link="laser_1_mount_link" /> <origin rpy="0 0 ${-pi/2}" xyz="0 -1 0"/> </joint> <xacro:psen_scan prefix="laser_1" />
Uses the previously imported macro to create a scanner instance and a joint to define its relation to the base_link coordinate frame.
<joint name="laser_2_base_link_joint" type="fixed"> <parent link="base_link" /> <child link="laser_2_mount_link" /> <origin rpy="0 0 ${pi/2}" xyz="0 1 0"/> </joint> <xacro:psen_scan prefix="laser_2" />
Same as above but for the second scanner.
Start the application
Enter the following to start Rviz together with the ROS node for the PSENscan application.
roslaunch psen_scan_v2_tutorials two_scanners.launch
Rviz will only show a single scanner. To add a second scanner to the visualization you need to add a LaserScan Object to Rviz and then choose "laser_2/scan" as it's topic.
In the section Global Options in Rviz set Fixed Frame to base_link
Conclusion
You are now able to launch multiple scanners in master mode at the same time.