There are multiple radar function packages under the lidar folder.
It also includes a udev rule conversion script. The radar uses cp2102usb to serial port chip.
Multiple devices are connected to the host through usb, so it is difficult to distinguish the devices corresponding to each port name.
Set rules for the chip of USB to serial port, and filter according to the Vender ID and Product ID of the chip. After reading the chip, make a symbolic link to the corresponding port. Named rplidar et al.
initenv.sh
#!/bin/bash
echo 'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0777", SYMLINK+="hldslidar"' >/etc/udev/rules.d/hldslidar.rules
echo 'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0777", GROUP:="dialout", SYMLINK+="ydlidar"' >/etc/udev/rules.d/ydlidar.rules
echo 'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0777", GROUP:="dialout", SYMLINK+="iiilidar"' >/etc/udev/rules.d/iiilidar.rules
echo 'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0777", GROUP:="dialout", SYMLINK+="rplidar"' >/etc/udev/rules.d/rplidar.rules
echo 'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0777", GROUP:="dialout", SYMLINK+="sclidar"' >/etc/udev/rules.d/sclidar.rules
systemctl daemon-reload
service udev reload
sleep 1
service udev restart
robot_ The navigation folder is very important.
robot: roslaunch robot_navigation lidar.launch Start the radar PC: roslaunch robot_navigation lidar_rviz.launch function rviz tool robot: roslaunch robot_navigation robot_slam_laser.launch Lidar slam Mapping pc: roslaunch robot_navigation slam_rviz.launch robot: Save map roscd robot_navigation/maps rosrun map_server map_saver -f map robot: roslaunch robot_navigation robot_navigation.launch Navigation and obstacle avoidance pc: roslaunch robot_navigation navigation_rviz.launch robot: roslaunch robot_navigation robot_slam_laser.launch slam_methods:=hector Graph building switching algorithm pc: roslaunch robot_navigation navigation_rviz.launch robot: roslaunch robot_navigation robot_navigation.launch planner:=dwa Path planning algorithm switching robot: roslaunch robot_navigation robot_slam_laser.launch planner:=teb Navigation mapping pc: roslaunch robot_navigation slam_rviz.launch
The document contains:
rviz folder: rviz Related configuration files maps Folder: files related to map files and map configuration information param folder: yaml file launch folder script folder:Script file
In the launch folder:
includes Folder: of mapping algorithm launch file lidar Folder: radar launch file rviz folder: rviz of launch file
robot_slam_laser.launch
robot_slam_laser.launch:
This is mainly the circular call of the launch file, for example, through the robot_ slam_ laser. The launch file called the robot_lidar.launch file.
The advantage of this is modularity. You can avoid repeatedly starting multiple nodes.
<launch>
<!-- Arguments -->
<arg name="slam_methods" default="gmapping" doc="slam type [gmapping, hector, karto, cartographer]"/><!-- Passable parameters slam_methods，choice SLAM Mapping algorithm -->
<arg name="open_rviz" default="false"/>
<arg name="simulation" default= "false"/> <!-- Determine whether to start the physical robot or stage simulator -->
<arg name="planner" default="" doc="opt: dwa, teb"/> <!-- Navigation while creating a map in the environment without map. The optional parameters are dwa,teb Local path planner -->
<param name="/use_sim_time" value="$(arg simulation)" />
<!-- simulation robot with lidar and map -->
<!-- simulation by true Start the simulation software and run the simulation robot-->
<group if="$(arg simulation)">
<include file="$(find robot_simulation)/launch/simulation_one_robot.launch"/>
</group>
<!-- robot with lidar -->
<!--simulation by false start-up launch Under folder robot_lidar.launch-->
<group unless="$(arg simulation)">
<include file="$(find robot_navigation)/launch/robot_lidar.launch"/>
</group>
<!-- SLAM: Gmapping, Cartographer, Hector, Karto -->
<!-- according to slam_methods Different mapping algorithms are selected for parameters -->
<include file="$(find robot_navigation)/launch/includes/$(arg slam_methods).launch">
<arg name="simulation" value="$(arg simulation)"/>
</include>
<!-- move_base No map environment navigation map building function, incoming planner Parameter is dwa or teb，The navigation stack is started-->
<group unless="$(eval planner == '')"><!--start-up move_base.launch-->
<include file="$(find robot_navigation)/launch/move_base.launch" unless="$(eval planner == '')">
<arg name="planner" value="$(arg planner)"/>
</include>
</group>
<!--because gmapping Will release map Topic: you can navigate normally without starting the map server to read the map locally-->
<!-- rviz open_rviz Parameter is true Open when rviz-->
<group if="$(arg open_rviz)">
<node pkg="rviz" type="rviz" name="rviz" required="true"
args="-d $(find robot_navigation)/rviz/slam.rviz"/><!-- use rviz Profile preset in folder slam.rviz -->
</group>
</launch>
robot_lidar.launch
robot_lidar.launch: start the chassis and radar
<launch>
<!-- config param -->
<arg name="pub_imu" default="False" />
<arg name="sub_ackermann" default="False" />
<arg name="lidar_frame" default="base_laser_link"/>
<!-- include The label contains two launch -->
<include file="$(find base_control)/launch/base_control.launch">
<arg name="pub_imu" value="$(arg pub_imu)"/>
<arg name="sub_ackermann" value="$(arg sub_ackermann)"/>
</include>
<include file="$(find robot_navigation)/launch/lidar.launch">
<arg name="lidar_frame" value="$(arg lidar_frame)"/>
</include>
</launch>
lidar.launch
lidar.launch: the launch file of the radar is started here
<launch>
<!--robot bast type use different tf value-->
<!-- Read robot chassis type -->
<arg name="base_type" default="$(env BASE_TYPE)" />
<!-- robot frame -->
<arg name="base_frame" default="/base_footprint" />
<!-- Read robot radar type -->
<arg name="lidar_type" default="$(env LIDAR_TYPE)" />
<arg name="lidar_frame" default="base_laser_link"/>
<!-- according to lidar_type Type startup node, if yes rplidar，Just start rplidar.launch -->
<include file="$(find robot_navigation)/launch/lidar/$(arg lidar_type).launch">
<arg name="lidar_frame" value="$(arg lidar_frame)"/>
</include>
<!-- TF coordinate transformation -->
<group if="$(eval base_type == 'NanoRobot')">
<node pkg="tf" type="static_transform_publisher" name="base_footprint_to_laser"
args="-0.01225 0.0 0.18 3.14159265 0.0 0.0 $(arg base_frame) $(arg lidar_frame) 20">
</node>
</group>
<group if="$(eval base_type == 'NanoRobot_Pro')">
<node pkg="tf" type="static_transform_publisher" name="base_footprint_to_laser"
args="-0.0515 0.0 0.18 -1.5708 0.0 0.0 $(arg base_frame) $(arg lidar_frame) 20">
</node>
</group>
<group if="$(eval base_type == '4WD')">
<node pkg="tf" type="static_transform_publisher" name="base_footprint_to_laser"
args="0.01 0.0 0.25 3.14159265 0.0 0.0 $(arg base_frame) $(arg lidar_frame) 20">
</node>
</group>
<group if="$(eval base_type == '4WD_OMNI')">
<node pkg="tf" type="static_transform_publisher" name="base_footprint_to_laser"
args="0.01 0.0 0.25 3.14159265 0.0 0.0 $(arg base_frame) $(arg lidar_frame) 20">
</node>
</group>
<group if="$(eval base_type == 'NanoCar')">
<node pkg="tf" type="static_transform_publisher" name="base_footprint_to_laser"
args="0.1037 0.0 0.115 3.14159265 0.0 0.0 $(arg base_frame) $(arg lidar_frame) 20">
</node>
</group>
<group if="$(eval base_type == 'NanoCar_Pro')">
<node pkg="tf" type="static_transform_publisher" name="base_footprint_to_laser"
args="0.1037 0.0 0.165 3.14159265 0.0 0.0 $(arg base_frame) $(arg lidar_frame) 20">
</node>
</group>
<group if="$(eval base_type == 'NanoCar_SE')">
<node pkg="tf" type="static_transform_publisher" name="base_footprint_to_laser"
args="0.0955 0.0 0.115 1.5708 0.0 0.0 $(arg base_frame) $(arg lidar_frame) 20">
</node>
</group>
</launch>
raplidar.launch
raplidar.launch radar data
The port name uses the name of the udev rule.
<param name="serial_port" type="string" value="/dev/rplidar"/>
<launch>
<arg name="lidar_frame" default="lidar"/>
<node name="rplidarNode" pkg="rplidar_ros" type="rplidarNode" output="screen">
<param name="serial_port" type="string" value="/dev/rplidar"/>
<param name="serial_baudrate" type="int" value="115200"/><!--A1/A2 -->
<!--param name="serial_baudrate" type="int" value="256000"--><!--A3 -->
<param name="frame_id" type="string" value="$(arg lidar_frame)"/>
<param name="inverted" type="bool" value="false"/>
<param name="angle_compensate" type="bool" value="true"/>
</node>
</launch>
karto.launch
karto.launch file. The configuration parameters are loaded through yaml file.
<launch>
<arg name="simulation" default= "false"/>
<param name="/use_sim_time" value="$(arg simulation)" />
<!-- slam_karto -->
<node pkg="slam_karto" type="slam_karto" name="slam_karto" output="screen">
<rosparam command="load" file="$(find robot_navigation)/config/karto_params.yaml" />
</node>
</launch>
karto_params.yaml
# General Parameters base_frame: base_footprint map_update_interval: 5 use_scan_matching: true use_scan_barycenter: true minimum_travel_distance: 0.12 minimum_travel_heading: 0.174 #in radians scan_buffer_size: 70 scan_buffer_maximum_scan_distance: 3.5 link_match_minimum_response_fine: 0.12 link_scan_maximum_distance: 3.5 loop_search_maximum_distance: 3.5 do_loop_closing: true loop_match_minimum_chain_size: 10 loop_match_maximum_variance_coarse: 0.4 # gets squared later loop_match_minimum_response_coarse: 0.8 loop_match_minimum_response_fine: 0.8 # Correlation Parameters - Correlation Parameters correlation_search_space_dimension: 0.3 correlation_search_space_resolution: 0.01 correlation_search_space_smear_deviation: 0.03 # Correlation Parameters - Loop Closure Parameters loop_search_space_dimension: 8.0 loop_search_space_resolution: 0.05 loop_search_space_smear_deviation: 0.03 # Scan Matcher Parameters distance_variance_penalty: 0.3 # gets squared later angle_variance_penalty: 0.349 # in degrees (gets converted to radians then squared) fine_search_angle_offset: 0.00349 # in degrees (gets converted to radians) coarse_search_angle_offset: 0.349 # in degrees (gets converted to radians) coarse_angle_resolution: 0.0349 # in degrees (gets converted to radians) minimum_angle_penalty: 0.9 minimum_distance_penalty: 0.5 use_response_expansion: false
In comparison with the above, parameters are directly transmitted through the launch file. The two of them have the same effect.
gmapping.launch
<launch>
<arg name="set_base_frame" default="base_footprint"/>
<arg name="set_odom_frame" default="odom"/>
<arg name="set_map_frame" default="map"/>
<arg name="scan_topic" default="/scan" />
<arg name="simulation" default= "false"/>
<param name="/use_sim_time" value="$(arg simulation)" />
<!-- Gmapping -->
<node pkg="gmapping" type="slam_gmapping" name="gmapping" output="screen">
<param name="base_frame" value="$(arg set_base_frame)"/>
<param name="odom_frame" value="$(arg set_odom_frame)"/>
<param name="map_frame" value="$(arg set_map_frame)"/>
<param name="map_update_interval" value="2.0"/>
<param name="maxUrange" value="5.0"/>
<param name="sigma" value="0.05"/>
<param name="kernelSize" value="1"/>
<param name="lstep" value="0.05"/>
<param name="astep" value="0.05"/>
<param name="iterations" value="5"/>
<param name="lsigma" value="0.075"/>
<param name="ogain" value="3.0"/>
<param name="lskip" value="0"/>
<param name="minimumScore" value="50"/>
<param name="srr" value="0.1"/>
<param name="srt" value="0.2"/>
<param name="str" value="0.1"/>
<param name="stt" value="0.2"/>
<param name="linearUpdate" value="1.0"/>
<param name="angularUpdate" value="0.2"/>
<param name="temporalUpdate" value="0.5"/>
<param name="resampleThreshold" value="0.5"/>
<param name="particles" value="100"/>
<param name="xmin" value="-10.0"/>
<param name="ymin" value="-10.0"/>
<param name="xmax" value="10.0"/>
<param name="ymax" value="10.0"/>
<param name="delta" value="0.05"/>
<param name="llsamplerange" value="0.01"/>
<param name="llsamplestep" value="0.01"/>
<param name="lasamplerange" value="0.005"/>
<param name="lasamplestep" value="0.005"/>
<remap from="scan" to="$(arg scan_topic)"/>
</node>
</launch>
cartographer.launch
It uses lua scripting language to load configuration files.
-configuration_basename revo_lds_$(arg version).lua"
revo_lds_melodic.lua
include "map_builder.lua"
include "trajectory_builder.lua"
options = {
map_builder = MAP_BUILDER,
trajectory_builder = TRAJECTORY_BUILDER,
map_frame = "map",
-- tracking_frame = "base_laser_link",
-- published_frame = "base_laser_link",
-- odom_frame = "odom",
-- provide_odom_frame = true,
-- use_odometry = false,
tracking_frame = "base_footprint",
published_frame = "odom",
odom_frame = "odom",
provide_odom_frame = false,
publish_frame_projected_to_2d = false,
use_odometry = true,
use_nav_sat = false,
use_landmarks = false,
num_laser_scans = 1,
num_multi_echo_laser_scans = 0,
num_subdivisions_per_laser_scan = 1,
num_point_clouds = 0,
lookup_transform_timeout_sec = 0.2,
submap_publish_period_sec = 0.3,
pose_publish_period_sec = 5e-3,
trajectory_publish_period_sec = 30e-3,
rangefinder_sampling_ratio = 1.,
odometry_sampling_ratio = 1.,
fixed_frame_pose_sampling_ratio = 1.,
imu_sampling_ratio = 1.,
landmarks_sampling_ratio = 1.,
}
MAP_BUILDER.use_trajectory_builder_2d = true
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 35
TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 8.
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 1.
TRAJECTORY_BUILDER_2D.use_imu_data = false
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.linear_search_window = 0.1
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.translation_delta_cost_weight = 10.
TRAJECTORY_BUILDER_2D.real_time_correlative_scan_matcher.rotation_delta_cost_weight = 1e-1
POSE_GRAPH.optimization_problem.huber_scale = 1e2
POSE_GRAPH.optimize_every_n_nodes = 35
POSE_GRAPH.constraint_builder.min_score = 0.65
return options
cartographer.launch
<launch>
<arg name="simulation" default= "true"/>
<arg name="version" default="$(env ROS_DISTRO)"/><!-- Read environment variables ROS_DISTRO Version of -->
<param name="/use_sim_time" value="$(arg simulation)" />
<!-- cartographer_node -->
<node name="cartographer_node" pkg="cartographer_ros"
type="cartographer_node" args="
-configuration_directory $(find robot_navigation)/config
-configuration_basename revo_lds_$(arg version).lua"
output="screen"> <!--according to version Parameter selection profile for-->
</node>
<group if="$(eval version == 'melodic')"><!--about melodic Version, manually start the following node-->
<node pkg="cartographer_ros" type="cartographer_occupancy_grid_node" name="cartographer_occupancy_grid_node"/>
</group>
</launch>
robot_navigation.launch
Two group s, each of which realizes the startup of robot, radar, map and amcl.
<launch>
<!-- Arguments -->
<!--The path where the incoming map file is located-->
<arg name="map_file" default="$(find robot_navigation)/maps/map.yaml"/><!--parameter map_file afferent maps Under file map.yaml file -->
<!--If the map name and save path are customized, you only need to modify the passed in map map_file Value of-->
<arg name="simulation" default= "false"/>
<arg name="planner" default="teb" doc="opt: dwa, teb"/>
<arg name="open_rviz" default="false"/>
<arg name="use_dijkstra" default= "true"/>
<group if="$(arg simulation)">
<!-- simulation robot with lidar and map-->
<!-- simulation Parameter is true，Over there launch Start map service in file -->
<include file="$(find robot_simulation)/launch/simulation_one_robot_with_map.launch"/>
</group>
<group unless="$(arg simulation)">
<!-- robot with lidar Start chassis and radar-->
<include file="$(find robot_navigation)/launch/robot_lidar.launch">
<!--<arg name="robot_name" value="$(arg robot_name)"/>-->
</include>
<!-- Map server Start map server read in map_file Parameter get map file-->
<node pkg="map_server" name="map_server" type="map_server" args="$(arg map_file)">
<param name="frame_id" value="map"/>
</node>
<!-- AMCL，Start a positioning node amcl -->
<node pkg="amcl" type="amcl" name="amcl" output="screen">
<!-- adopt yaml File read configuration file-->
<rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/amcl_params.yaml" command="load" />
<!-- Through the following settings, the robot is at the map zero point by default when starting -->
<!-- If the robot starts from a fixed position and is not the origin, the parameters can be modified -->
<param name="initial_pose_x" value="0.0"/>
<param name="initial_pose_y" value="0.0"/>
<param name="initial_pose_a" value="0.0"/><!--course-->
</node>
</group>
<!-- move_base Navigation stack-->
<include file="$(find robot_navigation)/launch/move_base.launch" >
<!--Parameters passed in-->
<arg name="planner" value="$(arg planner)"/>
<arg name="simulation" value="$(arg simulation)"/>
<arg name="use_dijkstra" value="$(arg use_dijkstra)"/>
</include>
<!-- rviz -->
<group if="$(arg open_rviz)">
<node pkg="rviz" type="rviz" name="rviz" required="true"
args="-d $(find robot_navigation)/rviz/navigation.rviz"/>
</group>
</launch>
map.yaml
It includes some basic configuration information of the map.
image: map.pgm resolution: 0.050000 origin: [-10.000000, -10.000000, 0.000000] negate: 0 occupied_thresh: 0.65 free_thresh: 0.196
Meaning of parameters: you can find the corresponding function package in Wiki, and introduce the parameters in English.
image: Map name. resolution: Map resolution, meters/pixel origin: The two-dimensional pose of the pixels in the lower left corner of the map is x，y，yaw. Yaw is counterclockwise rotation, yaw=0 Indicates no rotation occupied_thresh: Pixels larger than this threshold are fully occupied. free_thresh: Pixels less than this threshold are completely idle. negate: Pixel COLOR value x stay[0,255]Within the scope. First, set the integer x Convert to a floating point number p，The conversion formula depends on yaml in negate Definition of signs. If negate=0，be p =(255 - x)/ 255.0. This means that black (0) now has the highest value (1).0)White (255) has the lowest value (0.0). If negate=1，be p = x / 255.0.
map.pgm
pgm is an image format.
amcl_params.yaml
You can view the parameter information through the ros wiki introduction page of amcl.
use_map_topic: true odom_frame_id: "odom" base_frame_id: "base_footprint" global_frame_id: "map" ## Publish scans from best pose at a max of 10 Hz odom_model_type: "diff" odom_alpha5: 0.1 gui_publish_rate: 10.0 laser_max_beams: 60 laser_max_range: 12.0 min_particles: 500 max_particles: 2000 kld_err: 0.05 kld_z: 0.99 odom_alpha1: 0.2 odom_alpha2: 0.2 ## translation std dev, m odom_alpha3: 0.2 odom_alpha4: 0.2 laser_z_hit: 0.5 aser_z_short: 0.05 laser_z_max: 0.05 laser_z_rand: 0.5 laser_sigma_hit: 0.2 laser_lambda_short: 0.1 laser_model_type: "likelihood_field" # "likelihood_field" or "beam" laser_likelihood_max_dist: 2.0 update_min_d: 0.25 update_min_a: 0.2 resample_interval: 1 ## Increase tolerance because the computer can get quite busy transform_tolerance: 1.0 recovery_alpha_slow: 0.001 recovery_alpha_fast: 0.1
move_base.launch
move_base.launch: navigation stack under ros
The default passed in planner parameter is dwa, but robot_navigation.launch calls move_ base. When launching, the parameter passed in is teb. So here the planner parameter is set to teb.
<launch>
<!-- Arguments -->
<arg name="cmd_vel_topic" default="cmd_vel" />
<arg name="odom_topic" default="odom" />
<arg name="planner" default="dwa" doc="opt: dwa, teb"/> <!--Default incoming planner Parameter is dwa-->
<arg name="simulation" default= "false"/>
<arg name="use_dijkstra" default= "true"/>
<arg name="base_type" default= "$(env BASE_TYPE)"/> <!--Different models and configurations-->
<arg name="move_forward_only" default="false"/><!--Using depth camera for navigation and mapping, there is only forward view and no backward view-->
<!--The backward operation of the vehicle is prohibited to ensure safe operation-->
<!-- Two were used group distinguish DWA and TEB Two different local path planning algorithms -->
<!-- move_base use DWA planner-->
<group if="$(eval planner == 'dwa')">
<!-- start-up move_base node -->
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<!-- use global_planner replace default navfn as global planner ,global_planner support A* and dijkstra algorithm-->
<!--Set the parameters of the incoming node-->
<param name="base_global_planner" value="global_planner/GlobalPlanner"/>
<param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
<!-- <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/global_planner_params.yaml" command="load" /> -->
<rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/local_costmap_params.yaml" command="load" />
<rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/global_costmap_params.yaml" command="load" />
<rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/move_base_params.yaml" command="load" />
<rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/dwa_local_planner_params.yaml" command="load" />
<remap from="cmd_vel" to="$(arg cmd_vel_topic)"/>
<remap from="odom" to="$(arg odom_topic)"/>
<!-- move_forward_only No negative speed (backward movement) is allowed when -->
<param name="DWAPlannerROS/min_vel_x" value="0.0" if="$(arg move_forward_only)" />
<!--default is True,use dijkstra algorithm;set to False,usd A* algorithm-->
<param name="GlobalPlanner/use_dijkstra " value="$(arg use_dijkstra)" />
</node>
</group>
<!-- move_base use TEB planner-->
<group if="$(eval planner == 'teb')">
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<!-- use global_planner replace default navfn as global planner ,global_planner support A* and dijkstra algorithm-->
<param name="base_global_planner" value="global_planner/GlobalPlanner"/>
<param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
<!-- <rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/global_planner_params.yaml" command="load" /> -->
<rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/local_costmap_params.yaml" command="load" />
<rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/global_costmap_params.yaml" command="load" />
<rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/move_base_params.yaml" command="load" />
<rosparam file="$(find robot_navigation)/param/$(env BASE_TYPE)/teb_local_planner_params.yaml" command="load" />
<remap from="cmd_vel" to="$(arg cmd_vel_topic)"/>
<remap from="odom" to="$(arg odom_topic)"/>
<param name="TebLocalPlannerROS/max_vel_x_backwards" value="0.0" if="$(arg move_forward_only)" />
<!--default is True,use dijkstra algorithm;set to False,usd A* algorithm-->
<param name="GlobalPlanner/use_dijkstra " value="$(arg use_dijkstra)" />
<!--stage simulator takes the angle instead of the rotvel as input (twist message)-->
</node>
</group>
<!-- move_base -->
</launch>
costmap_common_params.yaml
Costmap of one of the models_ common_ params. Yaml file is as follows.
#---(in meters)---
#The unit is meter
footprint: [ [-0.035,-0.1], [0.18,-0.1], [0.18,0.1], [-0.035,0.1] ]
#Rectangular outline dimension of robot
#Taking the center of the robot as the origin of the coordinate axis, the four coordinate points (x,y) correspond to the four corners of the robot respectively
#The path planner obtains the outline size of the robot and plans the corresponding path
#The meaning of the parameter is through move_base's ros wiki view
transform_tolerance: 0.2
obstacle_layer:
enabled: true
obstacle_range: 2.5
raytrace_range: 3.0
inflation_radius: 0.05
track_unknown_space: false
combination_method: 1
observation_sources: laser_scan_sensor
laser_scan_sensor: {data_type: LaserScan, topic: scan, marking: true, clearing: true}
inflation_layer:
enabled: true
cost_scaling_factor: 10.0 # exponential rate at which the obstacle cost drops off (default: 10)
inflation_radius: 0.1 # max. distance from an obstacle at which costs are incurred for planning paths.
static_layer:
enabled: true
map_topic: "/map"
local_costmap_params.yaml
The obstacle avoidance function of robot is realized by local costmap. There are no obstacles in the global map. Find the obstacles through the local map and re plan the route.
#Specify the frequency of publication and update
local_costmap:
global_frame: map
robot_base_frame: base_footprint
update_frequency: 5.0
publish_frequency: 5.0
rolling_window: true#sliding window
#3 * 3 sliding window centered on the position of the robot
#The smaller the box is set, the worse the path planning effect is, and the greater the calculation burden of the robot is
width: 3
height: 3
resolution: 0.05
transform_tolerance: 0.5
plugins:#Set up a map layer
#Static layer
- {name: static_layer, type: "costmap_2d::StaticLayer"}
#Obstacle layer: the obstacles that do not exist in the map but are actually scanned will be recognized in the obstacle layer.
- {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
global_costmap_params.yaml
#Set some information such as publishing frequency
global_costmap:
global_frame: map
robot_base_frame: base_footprint
update_frequency: 1.0
publish_frequency: 0.5
# static_map: true
transform_tolerance: 0.5
plugins:
- {name: static_layer, type: "costmap_2d::StaticLayer"}
- {name: obstacle_layer, type: "costmap_2d::VoxelLayer"}
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
move_base_params.yaml:
#Make some frequency settings
#Control frequency of chassis
controller_frequency: 2.0
controller_patience: 15.0
#Path planning frequency
planner_frequency: 0.2
planner_patience: 5.0
conservative_reset_dist: 3.0
#Maximum allowable oscillation time when the robot vibrates continuously at one point
oscillation_timeout: 10.0
oscillation_distance: 0.2
clearing_rotation_allowed: false
#clearing_ rotation_ When allowed is True, the robot is trapped and will turn around 360 degrees to find a way out (lidar is not required for 360)
#Ackerman structure does not have the ability to turn in place and needs to be disabled
dwa_local_planner_params.yaml
DWAPlannerROS:
# Robot Configuration Parameters
#Set the maximum and minimum speed of robot navigation
#Absolute value of maximum speed in x direction, unit: m/s
max_vel_x: 0.25
#The absolute value of the minimum value in the x direction. If it is negative, it means you can go back
min_vel_x: -0.25
#y-axis speed, only for mcnamm wheel
max_vel_y: 0.0
min_vel_y: 0.0
# The velocity when robot is moving in a straight line
#Maximum angular velocity in rad/s
max_vel_trans: 0.26
min_vel_trans: 0.13
max_vel_theta: 1.0
min_vel_theta: 0.5
#Absolute value of acceleration in x direction, m/s2
acc_lim_x: 2.5
#Absolute value of acceleration in y direction, which can only be configured for omni-directional moving machines
acc_lim_y: 0.0
#Absolute value of rotational acceleration rad/s2
acc_lim_theta: 3.2
# Goal Tolerance Parametes
#It allows the deviation of the coordinates (in meters) of the target reached by the robot. Too small may cause the robot to constantly adjust to the accurate target position near the target position
xy_goal_tolerance: 0.1
#Tolerance of angle
yaw_goal_tolerance: 0.2
latch_xy_goal_tolerance: false
# Forward Simulation Parameters
sim_time: 2.0
vx_samples: 20
vy_samples: 0
vtheta_samples: 40
controller_frequency: 10.0
# Trajectory Scoring Parameters
path_distance_bias: 32.0
goal_distance_bias: 20.0
occdist_scale: 0.02
forward_point_distance: 0.325
stop_time_buffer: 0.2
scaling_speed: 0.25
max_scaling_factor: 0.2
# Oscillation Prevention Parameters
oscillation_reset_dist: 0.05
# Debugging
publish_traj_pc : true
publish_cost_grid_pc: true
prune_plan: false
use_dwa: true
restore_defaults: false
teb_local_planner_params.yaml
TebLocalPlannerROS:
odom_topic: odom
# Trajectory
teb_autosize: True
dt_ref: 0.3
dt_hysteresis: 0.1
max_samples: 500
global_plan_overwrite_orientation: True
allow_init_with_backwards_motion: True
max_global_plan_lookahead_dist: 3.0
global_plan_viapoint_sep: -1
global_plan_prune_distance: 1
exact_arc_length: False
feasibility_check_no_poses: 2
publish_feedback: False
# Robot
#Max x forward speed
max_vel_x: 0.2
#Maximum x reverse speed
max_vel_x_backwards: 0.2
max_vel_y: 0.0
#Maximum steering angular speed
max_vel_theta: 0.53 # the angular velocity is also bounded by min_turning_radius in case of a carlike robot (r = v / omega)
#Maximum x acceleration
acc_lim_x: 0.5
#Maximum angular velocity
acc_lim_theta: 0.5
# ********************** Carlike robot parameters ********************
#Set parameters for minimum turning radius
min_turning_radius: 0.375 # Min turning radius of the carlike robot (compute value using a model or adjust with rqt_reconfigure manually)
#Track parameters
wheelbase: 0.145 # Wheelbase of our robot
#Convert the received angle message into an operational angle change
cmd_angle_instead_rotvel: False # stage simulator takes the angle instead of the rotvel as input (twist message)
#Use the simulator to simulate the Ackerman model, and CMD_ angle_ instead_ Change rotvel to true,
#CMD accepted by "Car" model in stage simulator_ The z-axis angular velocity in the vel topic is not the given angular velocity of the robot, but the steering angle of the steering structure.
#cmd_angle_instead_rotvel: False, the angular velocity is the real angular velocity of the fixed robot
# ********************************************************************
footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
type: "line"
line_start: [0.05, 0.0] # for type "line"
line_end: [0.10, 0.0] # for type "line"
# GoalTolerance
xy_goal_tolerance: 0.1
yaw_goal_tolerance: 0.2
free_goal_vel: False
complete_global_plan: True
# Obstacles
min_obstacle_dist: 0.12 # This value must also include our robot's expansion, since footprint_model is set to "line".
inflation_dist: 0.6
include_costmap_obstacles: True
costmap_obstacles_behind_robot_dist: 1.5
obstacle_poses_affected: 15
dynamic_obstacle_inflation_dist: 0.6
include_dynamic_obstacles: True
costmap_converter_plugin: ""
costmap_converter_spin_thread: True
costmap_converter_rate: 5
# Optimization
no_inner_iterations: 5
no_outer_iterations: 4
optimization_activate: True
optimization_verbose: False
penalty_epsilon: 0.1
obstacle_cost_exponent: 4
weight_max_vel_x: 2
weight_max_vel_theta: 1
weight_acc_lim_x: 1
weight_acc_lim_theta: 1
weight_kinematics_nh: 1000
weight_kinematics_forward_drive: 1
weight_kinematics_turning_radius: 1
weight_optimaltime: 1 # must be > 0
weight_shortest_path: 0
weight_obstacle: 100
weight_inflation: 0.2
weight_dynamic_obstacle: 10 # not in use yet
weight_dynamic_obstacle_inflation: 0.2
weight_viapoint: 1
weight_adapt_factor: 2
# Homotopy Class Planner
enable_homotopy_class_planning: False
enable_multithreading: True
max_number_classes: 4
selection_cost_hysteresis: 1.0
selection_prefer_initial_plan: 0.95
selection_obst_cost_scale: 1.0
selection_alternative_time_cost: False
roadmap_graph_no_samples: 15
roadmap_graph_area_width: 5
roadmap_graph_area_length_scale: 1.0
h_signature_prescaler: 0.5
h_signature_threshold: 0.1
obstacle_heading_threshold: 0.45
switching_blocking_period: 0.0
viapoints_all_candidates: True
delete_detours_backwards: True
max_ratio_detours_duration_best_duration: 3.0
visualize_hc_graph: False
visualize_with_time_as_z_axis_scale: False
# Recovery
shrink_horizon_backup: True
shrink_horizon_min_duration: 10
oscillation_recovery: False
oscillation_v_eps: 0.1
oscillation_omega_eps: 0.1
oscillation_recovery_min_duration: 10
oscillation_filter_duration: 10
Multipoint navigation
Multipoint navigation: Ordinary navigation can only publish one target point. Before reaching the target point, if a new target point is released, the original target point will be abandoned and go directly to the new target point. Multi-target point navigation is to go to the target point in turn according to the release order of the target point. stage Simulation: roslaunch robot_navigation robot_navigation.launch simulation:=true Running on a solid robot does not require input simulation:=true To launch the multipoint navigation tool: roslaunch robot_navigation multi_points_navigation.launch start-up rviz: roslaunch robot_navigation multi_navigation.launch rviz There is an extra 2 D Nav Goal Button can continuously assign multiple target points to the robot, and the robot will go to each target point in turn
multi_points_navigation.launch
multi_points_navigation.launch, multi-point navigation
<launch>
<!--<node name="robot_pose_publisher" pkg="robot_pose_publisher" type="robot_pose_publisher" />-->
<!--Started a multi_goal_point.py Node of-->
<node name="multi_mark" pkg="robot_navigation" type="multi_goal_point.py" output="screen" />
</launch>
multi_navigation.launch
<launch>
<!-- rviz -->
<node pkg="rviz" type="rviz" name="rviz" required="true"
args="-d $(find robot_navigation)/rviz/multi_navigation.rviz"/>
</launch>
multi_goal_point.py:
Core idea:
Through the rviz button, publish the goal topic instead of / move_base_simple/goal topic, because / move_ base_ The simple / goal topic will be directly sent to movebase to directly generate a new navigation target.
Cache the goal through markerArray and move_base triggers status callback to execute the next target point according to the navigation result of each point.
move in circles.
#!/usr/bin/python
from visualization_msgs.msg import Marker
from visualization_msgs.msg import MarkerArray
import rospy
import math
from geometry_msgs.msg import PointStamped,PoseStamped
import actionlib
from move_base_msgs.msg import *
import tf
def status_callback(msg):#When receiving the move base navigation result, judge whether to read the next point and continue publishing; Or mark the last point at present
global goal_pub, index,markerArray
global add_more_point,try_again
if(msg.status.status == 3):#The status is 3 and the navigation is successful to the target point
try_again = 1
if add_more_point == 0:
print 'Goal reached'
if index < count:
pose = PoseStamped()
pose.header.frame_id = "map"
pose.header.stamp = rospy.Time.now()
pose.pose.position.x = markerArray.markers[index].pose.position.x#Give pose a new value
#Read a new target point from markerArray
pose.pose.position.y = markerArray.markers[index].pose.position.y
pose.pose.orientation.w = 1
goal_pub.publish(pose)#Publish to move base
index += 1
elif index == count:#Execute to the last point
add_more_point = 1
else:
# status value and its meaning
# uint8 PENDING = 0 # The goal has yet to be processed by the action server
# uint8 ACTIVE = 1 # The goal is currently being processed by the action server
# uint8 PREEMPTED = 2 # The goal received a cancel request after it started executing
# # and has since completed its execution (Terminal State)
# uint8 SUCCEEDED = 3 # The goal was achieved successfully by the action server (Terminal State)
# uint8 ABORTED = 4 # The goal was aborted during execution by the action server due
# # to some failure (Terminal State)
# uint8 REJECTED = 5 # The goal was rejected by the action server without being processed,
# # because the goal was unattainable or invalid (Terminal State)
# uint8 PREEMPTING = 6 # The goal received a cancel request after it started executing
# # and has not yet completed execution
# uint8 RECALLING = 7 # The goal received a cancel request before it started executing,
# # but the action server has not yet confirmed that the goal is canceled
# uint8 RECALLED = 8 # The goal received a cancel request before it started executing
# # and was successfully cancelled (Terminal State)
# uint8 LOST = 9 # An action client can determine that a goal is LOST. This should not be
# # sent over the wire by an action server
#If it is a failure
print 'Goal cannot reached has some error :',msg.status.status," try again!!!!"
if try_again == 1:#Send the target point to move base and ask it to try again
pose = PoseStamped()
pose.header.frame_id = "map"
pose.header.stamp = rospy.Time.now()
pose.pose.position.x = markerArray.markers[index-1].pose.position.x
pose.pose.position.y = markerArray.markers[index-1].pose.position.y
pose.pose.orientation.w = 1
goal_pub.publish(pose)
try_again = 0
else:#If you have tried again but still can't arrive, skip it and execute the next point
if index < len(markerArray.markers):
pose = PoseStamped()
pose.header.frame_id = "map"
pose.header.stamp = rospy.Time.now()
pose.pose.position.x = markerArray.markers[index].pose.position.x
pose.pose.position.y = markerArray.markers[index].pose.position.y
pose.pose.orientation.w = 1
goal_pub.publish(pose)
index += 1
def click_callback(msg):#rviz given a multi-target point, it will trigger
global markerArray,count
global goal_pub,index
global add_more_point
marker = Marker()
marker.header.frame_id = "map"
marker.header.stamp = rospy.Time.now()
# marker.type = marker.TEXT_VIEW_FACING
marker.type = marker.CYLINDER
marker.action = marker.ADD
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.5
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.pose.orientation.x = 0.0
marker.pose.orientation.y = 0.0
marker.pose.orientation.z = 0.0
marker.pose.orientation.w = 1.0
marker.pose.position.x = msg.pose.position.x#Read out the location information in msg and send it to the marker message
marker.pose.position.y = msg.pose.position.y
marker.pose.position.z = msg.pose.position.z
markerArray.markers.append(marker)
# Renumber the marker IDs
id = 0
for m in markerArray.markers:
m.id = id
id += 1
# Publish the MarkerArray
mark_pub.publish(markerArray)#release
#first goal: when publishing multi-target points for the first time, directly publish the topic
if count==0:#When there is no target point
pose = PoseStamped()#Read the topic to the pose message
pose.header.frame_id = "map"
pose.header.stamp = rospy.Time.now()
pose.pose.position.x = msg.pose.position.x
pose.pose.position.y = msg.pose.position.y
pose.pose.position.z = msg.pose.position.z
pose.pose.orientation.x = msg.pose.orientation.x
pose.pose.orientation.y = msg.pose.orientation.y
pose.pose.orientation.z = msg.pose.orientation.z
pose.pose.orientation.w = msg.pose.orientation.w
goal_pub.publish(pose)#Through goal_pub Publishing
#goal_pub publishes the goal topics required by move base. Contains the position and angle information of the target point
index += 1
#add_ more_ When point = 1, issue a status=3 in ros and trigger status_callback, so that the new target point can be released again
#To solve the problem of publishing new navigation points after all navigation points are completed
if add_more_point and count > 0:
add_more_point = 0
move =MoveBaseActionResult()
move.status.status = 3
move.header.stamp = rospy.Time.now()
goal_status_pub.publish(move)
quaternion = (msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w)
theta = tf.transformations.euler_from_quaternion( quaternion)[2]
count += 1#count plus 1 for each click
# print 'add a path goal point %f %f %f'%(msg.pose.position.x,msg.pose.position.y,theta*180.0/3.14)
markerArray = MarkerArray()#Define a variable
count = 0 #total goal num counts the number of published target points
index = 0 #current goal point index counts which target point is currently executed
add_more_point = 0 # after all goal arrive, if add some more goal flag bit
try_again = 1 # try the fail goal once again
rospy.init_node('multi_goal_point_demo') #Create node
mark_pub = rospy.Publisher('/path_point_array', MarkerArray,queue_size=100)#Publisher publishes MarkARRAY type data
click_goal_sub = rospy.Subscriber('/goal',PoseStamped,click_callback)#The subscriber subscribes to the goal topic, publishes the topic published by multi-target points in rviz, and enters the click every time it receives the goal_ callback
goal_pub = rospy.Publisher('/move_base_simple/goal',PoseStamped,queue_size=1)#Publisher publish / move_base_simple/goal topic.
#Trigger move_base navigation topic
goal_status_sub = rospy.Subscriber('/move_base/result',MoveBaseActionResult,status_callback)#Subscribe to the results of movebase navigation.
#after all goal arrive, if add some more goal
#we deleberate pub a topic to trig the goal sent
goal_status_pub = rospy.Publisher('/move_base/result',MoveBaseActionResult,queue_size=1)
rospy.spin()#Publish / move in node_ base/result
#
Multipoint automatic cruise
Simulation: roslaunch robot_navigation robot_navigation.launch simulation:=true Running on a solid robot does not require input simulation:=true parameter rviz: roslaunch robot_navigation navigation_rviz.launch pc Open terminal: rostopic echo /move_base_simple/goal (subscribe/move_base_simple/goal topic of conversation) By "2" D Nav Goal"Button to set the cruise point and record the output content. pose: position: x: y: z orizntation: x v z: open robot_navigation Under function pack way_point.launch Document modification goallistx，y，z Number of cruises: roslaunch robot_navigation way_point.launch loopTimes loopTimes:=2 loopTimes The parameter is the number of cruises. 2. The robot stops after cruising back and forth twice at the cruising point; 0, cruise all the time. Each time a cruise point is reached during cruise, the target of the next cruise point is output ID，When the number of cruises reaches, it will be output Loop: 2 Times Finshed Information.
way_point.launch
<launch>
<!-- For Simulation -->
<arg name="sim_mode" default="false" />
<param name="/use_sim_time" value="$(arg sim_mode)"/>
<arg name="loopTimes" default="0" />
<!-- move base -->
<!--launch File startup way_point.py node-->
<node pkg="robot_navigation" type="way_point.py" respawn="false" name="way_point" output="screen">
<!-- params for move_base Write a preset location-->
<param name="goalListX" value="[2.0, 4.0, 3.0]" />
<param name="goalListY" value="[3.0, 2.0, 2.0]" />
<param name="goalListZ" value="[0.0, 0.0, 0.0]" />
<param name="loopTimes" value="$(arg loopTimes)"/>
<param name="map_frame" value="map" />
</node>
</launch>
multi_goal_point.py
Core idea:
First preset a series of path points, and then publish them in turn. Only after the previous point arrives, can the next point be issued.
#!/usr/bin/python
import rospy
import string
import math
import time
import sys
from std_msgs.msg import String
from move_base_msgs.msg import MoveBaseActionResult
from actionlib_msgs.msg import GoalStatusArray
from geometry_msgs.msg import PoseStamped
import tf
class MultiGoals:
def __init__(self, goalListX, goalListY, goalListZ,loopTimes, map_frame):
#Subscriber, subscribe to move_base/result, move base, the navigation result topic is published. Each time the navigation result topic is received, it will enter the statusCB callback
self.sub = rospy.Subscriber('move_base/result', MoveBaseActionResult, self.statusCB, queue_size=10)
#Publisher, publish move_base_simple/goal，move_ The target topics that base really receives
self.pub = rospy.Publisher('move_base_simple/goal', PoseStamped, queue_size=10)
# params & variables
self.goalListX = goalListX
self.goalListY = goalListY
self.goalListZ = goalListZ
self.loopTimes = loopTimes
self.loop = 1
self.wayPointFinished = False
self.goalId = 0#The serial number of the current target point of execution
self.goalMsg = PoseStamped()
self.goalMsg.header.frame_id = map_frame
self.goalMsg.pose.orientation.z = 0.0
self.goalMsg.pose.orientation.w = 1.0
# Publish the first goal
time.sleep(1)
#Pay xyz in goalList to goalMsg message respectively
self.goalMsg.header.stamp = rospy.Time.now()
self.goalMsg.pose.position.x = self.goalListX[self.goalId]
self.goalMsg.pose.position.y = self.goalListY[self.goalId]
self.goalMsg.pose.orientation.x = 0.0
self.goalMsg.pose.orientation.y = 0.0
if abs(self.goalListZ[self.goalId]) > 1.0:
self.goalMsg.pose.orientation.z = 0.0
self.goalMsg.pose.orientation.w = 1.0
else:
w = math.sqrt(1 - (self.goalListZ[self.goalId]) ** 2)
self.goalMsg.pose.orientation.z = self.goalListZ[self.goalId]
self.goalMsg.pose.orientation.w = w
self.pub.publish(self.goalMsg) #After the assignment of goalMsg, it is published to move ase through the publisher
rospy.loginfo("Current Goal ID is: %d", self.goalId) #Output log
self.goalId = self.goalId + 1 #When the target point is sent out, it will wait for the statusCB callback
#When the robot reaches a target point, it enters the statusCB function
def statusCB(self, data):
if self.loopTimes and (self.loop > self.loopTimes):
rospy.loginfo("Loop: %d Times Finshed", self.loopTimes)
self.wayPointFinished = True
if data.status.status == 3 and (not self.wayPointFinished): # reached, and the path cruise is not over
self.goalMsg.header.stamp = rospy.Time.now()
self.goalMsg.pose.position.x = self.goalListX[self.goalId]
self.goalMsg.pose.position.y = self.goalListY[self.goalId]
if abs(self.goalListZ[self.goalId]) > 1.0:
self.goalMsg.pose.orientation.z = 0.0
self.goalMsg.pose.orientation.w = 1.0
else:
w = math.sqrt(1 - (self.goalListZ[self.goalId]) ** 2)
self.goalMsg.pose.orientation.z = self.goalListZ[self.goalId]
self.goalMsg.pose.orientation.w = w
self.pub.publish(self.goalMsg) #Read the next point and publish it
rospy.loginfo("Current Goal ID is: %d", self.goalId)
if self.goalId < (len(self.goalListX)-1):
self.goalId = self.goalId + 1
else:
self.goalId = 0
self.loop += 1
if __name__ == "__main__":
try:
# ROS Init
rospy.init_node('way_point', anonymous=True)
# Get params get the goalListXYZ passed in from the launch file
goalListX = rospy.get_param('~goalListX', '[2.0, 2.0]')
goalListY = rospy.get_param('~goalListY', '[2.0, 4.0]')
goalListZ = rospy.get_param('~goalListZ', '[0.0, 0.0]')
map_frame = rospy.get_param('~map_frame', 'map' )
loopTimes = int(rospy.get_param('~loopTimes', '0'))
goalListX = goalListX.replace("[","").replace("]","")
goalListY = goalListY.replace("[","").replace("]","")
goalListZ = goalListZ.replace("[","").replace("]","")
goalListX = [float(x) for x in goalListX.split(",")]
goalListY = [float(y) for y in goalListY.split(",")]
goalListZ = [float(z) for z in goalListZ.split(",")]#Processing into an array
if len(goalListX) == len(goalListY) == len(goalListZ) & len(goalListY) >=2: #Judge the length of three arrays, etc
# Constract MultiGoals Obj
rospy.loginfo("Multi Goals Executing...")
mg = MultiGoals(goalListX, goalListY, goalListZ, loopTimes, map_frame)#Cycle times map base coordinates
rospy.spin()
else:
rospy.errinfo("Lengths of goal lists are not the same")
except KeyboardInterrupt:
print("shutting down")