Lesson 11: Navigation-Location & planner

In the case of existing maps, the need to allow the robot to locate the location of the map, which requires the use of ROS acml package to achieve, while the release of the target location through the move_base to do the path planning, bypass obstacles to reach destination.

1.Location

1.1 Write acml launch script

<launch>
<master auto="start"/>

<include file="$(find flashgo)/launch/lidar.launch" />

<node name="arduino" pkg="ros_arduino_python" type="arduino_node.py" output="screen">
<rosparam file="$(find ros_arduino_python)/config/my_arduino_params.yaml" command="load" />
</node>

<node pkg="tf" type="static_transform_publisher" name="base_frame_2_laser_link" args="0.0 0.0 0.2 3.14 3.14 0 /base_link /laser 40"/>

<!-- Map server -->
<node name="map_server" pkg="map_server" type="map_server" args="$(find diego_nav)/maps/f4_gmapping.yaml" />

<!-- amcl node -->
<node pkg="amcl" type="amcl" name="amcl" output="screen">
<remap from="scan" to="scan"/>
<!-- Publish scans from best pose at a max of 10 Hz -->
<param name="initial_pose_x" value="0.0"/>
<param name="initial_pose_y" value="0.0"/>
<param name="initial_pose_a" value="0.0"/>
<param name="use_map_topic" value="true"/>
<param name="odom_model_type" value="diff"/>
<param name="odom_alpha5" value="0.1"/>
<param name="transform_tolerance" value="0.5" />
<param name="gui_publish_rate" value="10.0"/>
<param name="laser_max_beams" value="300"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.1"/>
<param name="kld_z" value="0.99"/>
<param name="odom_alpha1" value="0.1"/>
<param name="odom_alpha2" value="0.1"/>
<!-- translation std dev, m -->
<param name="odom_alpha3" value="0.1"/>
<param name="odom_alpha4" value="0.1"/>
<param name="laser_z_hit" value="0.9"/>
<param name="laser_z_short" value="0.05"/>
<param name="laser_z_max" value="0.05"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.2"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_model_type" value="likelihood_field"/>
<!-- <param name="laser_model_type" value="beam"/> -->
<param name="laser_min_range" value="1"/>
<param name="laser_max_range" value="8"/>
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.5"/>
<param name="resample_interval" value="1"/>
<param name="transform_tolerance" value="0.1"/>
<param name="recovery_alpha_slow" value="0.0"/>
<param name="recovery_alpha_fast" value="0.0"/>
</node>
</launch>

Here is the need to emphasize that the robot must be configured in fact the location, the robot casually put a position, amcl will not be positioned to the need to specify the initial location, the map is the origin of the map with gmapping / hector when the starting point, The file needs to set the initial position, we put the robot here in the starting position, the value is set to 0.0.

<param name="initial_pose_x" value="0.0"/>
<param name="initial_pose_y" value="0.0"/>

1.2 start acml node

roslaunch diego_nav diego_run_gmapping_amcl_flashgo.launch

Open a new terminal and start the keyboard control node

rosrun teleop_twist_keyboard teleop_twist_keyboard.py

At this time we can use the keyboard to control the robot to move up and open rviz to view the positioning

rosin rviz rviz

In this picture we can see a large number of red arrows and Monte Carlo positioning algorithm generated by the distribution of particles, in which the direction of the arrow is the direction of the robot movement

This picture can be seen in the chart more aggregates than the previous chart, from the actual observation of the case of high degree of polymerization more positioning effect.

2.route plan

Path planning and how to allow the robot to move from one location to another location, the way to avoid obstacles, is also the usual sense of the most intuitive navigation, in the ROS path planning is achieved through the move_base package.

2.1 Write the launch_base launch script

<launch>

<!-- move_base node -->
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<rosparam file="$(find diego_nav)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find diego_nav)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find diego_nav)/config/local_costmap_params.yaml" command="load" />
<rosparam file="$(find diego_nav)/config/global_costmap_params.yaml" command="load" />
<rosparam file="$(find diego_nav)/config/base_local_planner_params.yaml" command="load" />
</node>

</launch>
The contents of the launch file can also be combined with the acml startup script in a file.
The configuration parameters of the move_base node are distributed in four configuration files:
  • costmap_common_params.yaml
  • global_costmap_params.yaml
  • local_costmap_params.yaml
  • base_local_planner_params.yaml

2.2 costmap_common_params.yaml

Cost map general configuration file

obstacle_range: 2.5  #Maximum obstacle detection range
raytrace_range: 3.0. #Detects the maximum range of free space
footprint: [[0.14, 0.14], [0.14, -0.14], [-0.14, 0.14], [-0.14, -0.14]] #The robot is rectangular and sets the area occupied by the machine in the coordinate system
inflation_radius: 0.55 #And the safety factor of the obstacle

observation_sources: laser_scan_sensor #Only focus on the data of the lidar

laser_scan_sensor: {sensor_frame: laser, data_type: LaserScan, topic: scan, marking: true, clearing: true} #Set the relevant parameters of the lidar

 

2.3 global_costmap_params.yaml

Global cost map configuration file

global_costmap: 
 global_frame: /map #the global cost map reference is /map
 robot_base_frame: base_link #base_frame is base_link
 update_frequency: #Specify the map update frequency
 static_map: true 5.0 #Use static maps and initialize them
 transform_tolerance: 0.8 #Set tf update tolerance of 0.8, can be more hardware to adjust the actual situation of this parameter, in the raspberry sent less than 0.8 will continue to report tf release timeout warning message

 

2.4 local_costmap_params.yaml

Local cost map configuration file

local_costmap:
 global_frame: odom #the local cost map reference is odom
 robot_base_frame: base_link #base_frame is base_link
 update_frequency: 5.0 #the map update frequency
 publish_frequency: 2.0 #Cost Map The frequency at which visual information is published
 static_map: false #The local cost map will continue to update the map, so here is set to false
 rolling_window: true #设Set the scroll window so that the robot is always in the center of the form
 width: 4.0 #the width of cost map
 height: 6.0 #the length of cost map
 resolution: 0.05 #Cost map resolution

 

2.5 base_local_planner_params.yaml

Local Planner configuration file

TrajectoryPlannerROS:
 max_vel_x: 0.45 #Maximum speed in the x-axis direction
 min_vel_x: 0.1 #xMinimum speed in the direction of the shaft
 max_vel_theta: 1.0 #Maximum angular velocity
 min_in_place_vel_theta: 0.4

 acc_lim_theta: 3.2 #Maximum angular acceleration
 acc_lim_x: 2.5 #Maximum acceleration in the x-axis direction
 acc_lim_y: 2.5 #Maximum acceleration in the y-axis direction

2.6start move_base node

roslaunch diego_nav diego_run_gmapping_amcl_flashgo.launch
Diego 1 # already has a positioning, navigation function, then we conducted a navigation test

3.Navigation test

Navigation can be placed in the indoor fixed position obstacles, using gmapping or hector to draw a map of obstacles, and then use the above method to start acml and move_base for positioning and navigation。

3.1Navigate the test code

We can modify the navigation test code, change the location to the location on the actual map

#!/usr/bin/env python
import roslib;
import rospy
import actionlib
from actionlib_msgs.msg import *
from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from random import sample
from math import pow, sqrt

class NavTest():
def __init__(self):
rospy.init_node('nav_test', anonymous=True)

rospy.on_shutdown(self.shutdown)

# How long in seconds should the robot pause at each location?
self.rest_time = rospy.get_param("~rest_time", 10)

# Are we running in the fake simulator?
self.fake_test = rospy.get_param("~fake_test", False)

# Goal state return values
goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED',
'SUCCEEDED', 'ABORTED', 'REJECTED',
'PREEMPTING', 'RECALLING', 'RECALLED',
'LOST']

# Set up the goal locations. Poses are defined in the map frame.
# An easy way to find the pose coordinates is to point-and-click
# Nav Goals in RViz when running in the simulator.
# Pose coordinates are then displayed in the terminal
# that was used to launch RViz.
locations = dict()

locations['point_0'] = Pose(Point(0.0, 2.0, 0.000), Quaternion(0.000, 0.000, 0.223, 0.975))
locations['point_1'] = Pose(Point(0.5, 2.0, 0.000), Quaternion(0.000, 0.000, -0.670, 0.743))

# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)

# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)

rospy.loginfo("Waiting for move_base action server...")

# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))

rospy.loginfo("Connected to move base server")

# A variable to hold the initial pose of the robot to be set by
# the user in RViz
initial_pose = PoseWithCovarianceStamped()

# Variables to keep track of success rate, running time,
# and distance traveled
n_locations = len(locations)
n_goals = 0
n_successes = 0
i = n_locations
distance_traveled = 0
start_time = rospy.Time.now()
running_time = 0
location = ""
last_location = ""

# Get the initial pose from the user
#rospy.loginfo("*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose...")
#rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)
#self.last_location = Pose()
#rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose)

# Make sure we have the initial pose
#while initial_pose.header.stamp == "":
# rospy.sleep(1)

rospy.loginfo("Starting navigation test")

# Begin the main loop and run through a sequence of locations
while not rospy.is_shutdown():
# If we've gone through the current sequence,
# start with a new random sequence
if i == n_locations:
i = 0
sequence = sample(locations, n_locations)
# Skip over first location if it is the same as
# the last location
if sequence[0] == last_location:
i = 1

# Get the next location in the current sequence
location = sequence[i]

# Keep track of the distance traveled.
# Use updated initial pose if available.
if initial_pose.header.stamp == "":
distance = sqrt(pow(locations[location].position.x -
locations[last_location].position.x, 2) +
pow(locations[location].position.y -
locations[last_location].position.y, 2))
else:
rospy.loginfo("Updating current pose.")
distance = sqrt(pow(locations[location].position.x -
initial_pose.pose.pose.position.x, 2) +
pow(locations[location].position.y -
initial_pose.pose.pose.position.y, 2))
initial_pose.header.stamp = ""

# Store the last location for distance calculations
last_location = location

# Increment the counters
i += 1
n_goals += 1

# Set up the next goal location
self.goal = MoveBaseGoal()
self.goal.target_pose.pose = locations[location]
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time.now()

# Let the user know where the robot is going next
rospy.loginfo("Going to: " + str(location))

# Start the robot toward the next location
self.move_base.send_goal(self.goal)

# Allow 5 minutes to get there
finished_within_time = self.move_base.wait_for_result(rospy.Duration(300))

# Check for success or failure
if not finished_within_time:
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal")
else:
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal succeeded!")
n_successes += 1
distance_traveled += distance
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo("Goal failed with error code: " + str(goal_states[state]))

# How long have we been running?
running_time = rospy.Time.now() - start_time
running_time = running_time.secs / 60.0

# Print a summary success/failure, distance traveled and time elapsed
rospy.loginfo("Success so far: " + str(n_successes) + "/" +
str(n_goals) + " = " +
str(100 * n_successes/n_goals) + "%")
rospy.loginfo("Running time: " + str(trunc(running_time, 1)) +
" min Distance: " + str(trunc(distance_traveled, 1)) + " m")
rospy.sleep(self.rest_time)

def update_initial_pose(self, initial_pose):
self.initial_pose = initial_pose

def shutdown(self):
rospy.loginfo("Stopping the robot...")
self.move_base.cancel_goal()
rospy.sleep(2)
self.cmd_vel_pub.publish(Twist())
rospy.sleep(1)

def trunc(f, n):
# Truncates/pads a float f to n decimal places without rounding
slen = len('%.*f' % (n, f))
return float(str(f)[:slen])

if __name__ == '__main__':
try:
NavTest()
rospy.spin()
except rospy.ROSInterruptException:
rospy.loginfo("AMCL navigation test finished.")

3.2Start navigation

rosrun diego_nav nav_test.py

After the start, the robot will move in accordance with the location set in the navigation test file one by one, and can avoid obstacles in the map, but if it is in a small space, the machine will keep spinning, and no way out, this may Set the parameters related to the need to achieve good results also need to constantly debug parameters.

Lesson 10: Navigation-Create map based on the laser

Construction of the map is the basis of SLAM, positioning and path planning are based on a certain map to achieve, this section we will be based on the laser radar, using gmapping and hector slam two packages to build the map

1.Lidar

Lidar has a very high measurement accuracy characteristics, is a very good choice of distance measurement, but at the same time he also has high power consumption, high cost shortcomings, the current widespread use of 2D laser radar, intelligent measurement of a plane around the distance, and The cost of 3D laser radar is high and can not be widely promoted at present. 2D laser radar can only measure a plane problem, but also affect the use of the scene, if used as navigation, only suitable for some relatively simple rules in the environment.

In the Diego robot, we use the Flash go F4 radar from EAI, which features the following:

• 360 degree omni-directional scanning, 10 Hz adaptive scanning frequency, 4500 laser strokes per second
• Not less than 8 m range of ranging, measuring the resolution of 1% of the range
• Low noise and long life
Class 1 laser safety standard

F4 using USB interface and the host connection, at the same time with power and data transmission function, so the installation is still very convenient, only need to plug in the host’s USB interface

1.1. The installation of the laser radar ROS driver package

Execute the following command

cd ~/catkin_ws/src
git clone https://github.com/EAIBOT/flashgo.git
cd ..
catkin_make

After the completion of the implementation of the src directory to add a flashgo directory, is the F4 ROS package

 

 

 

 

 

 

 

 

 

1.2. Modify the serial number in the configuration file

F4 USB and motherboard connection, and raspberry connection, the system will appear in a serial port, because Arduino UNO is also connected through the USB serial communication, so there will be two serial devices in the system, the configuration must be clearly distinguish The serial number of the two devices


As shown above ttyACM0, and ttyACM1 is the corresponding serial port number, in the diego1 # ttyACM1 corresponds to arduino, ttyACM0 corresponding F4 laser radar, we need to modify the corresponding parameters, first modify the Ardunio configuration file, open the config directory my_arduino_params.yaml

 

 

 

 

 

Modify the arduino_bridge connection to the serial port for ttyACM1

 

 

 

 

 

 

Modify the F4 configuration file, open the flashar / launch directory under the lidar.launch file

 

 

 

 

 

Modify the F4 connection to the serial port ttyACM0

 

 

 

 

 

 

2.Use hector slam to build a map

2.1Install hector slam

sudo apt-get install ros-kinetic-hector-slam

2.2Write the hector slam to start the launch file

The following code is written hector start launch the file, here need to pay attention to the point of the EAI Flash lidar using the left hand coordinate system, and ROS use the right hand coordinate system, so the need to prepare a file in the static tf type base_frame_2_laser coordinate conversion.

<?xml version="1.0"?>

<launch>
  <arg name="tf_map_scanmatch_transform_frame_name" default="/scanmatcher_frame"/>
  <arg name="pub_map_odom_transform" value="true"/> 
  <arg name="map_frame" value="map"/> 
  <arg name="base_frame" value="base_link"/> 
  <arg name="odom_frame" value="base_link"/>
  <arg name="scan_subscriber_queue_size" default="5"/>
  <arg name="scan_topic" default="scan"/>
  <arg name="map_size" default="2048"/>

  <master auto="start"/>

  <include file="$(find flashgo)/launch/lidar.launch" />

  <node name="arduino" pkg="ros_arduino_python" type="arduino_node.py" output="screen">
      <rosparam file="$(find ros_arduino_python)/config/my_arduino_params.yaml" command="load" />
  </node> 

  <node pkg="tf" type="static_transform_publisher" name="base_frame_2_laser" args="0 0 0 0 0 0 /$(arg base_frame) /laser 100"/>  
  <node pkg="tf" type="static_transform_publisher" name="map_2_odom" args="0.0 0.0 0.0 0 0 0.0 /odom /$(arg base_frame) 10"/>


  <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">   
  <!-- Frame names -->
  <param name="map_frame" value="$(arg map_frame)" />
  <param name="base_frame" value="$(arg base_frame)" />
  <param name="odom_frame" value="$(arg base_frame)" />   
   <!-- Tf use -->
  <param name="use_tf_scan_transformation" value="true"/>
  <param name="use_tf_pose_start_estimate" value="false"/>
  <param name="pub_map_odom_transform" value="$(arg pub_map_odom_transform)"/> 
  <!-- Map size / start point -->
  <param name="map_resolution" value="0.050"/>
  <param name="map_size" value="$(arg map_size)"/>
  <param name="map_start_x" value="0.5"/>
  <param name="map_start_y" value="0.5" />
  <param name="map_multi_res_levels" value="2" />
  <!-- Map update parameters -->
  <param name="update_factor_free" value="0.4"/>
  <param name="update_factor_occupied" value="0.7" />   
  <param name="map_update_distance_thresh" value="0.2"/>
  <param name="map_update_angle_thresh" value="0.9" />
  <param name="laser_z_min_value" value = "-1.0" />
  <param name="laser_z_max_value" value = "1.0" />  
  <!-- Advertising config -->
  <param name="advertise_map_service" value="true"/>
  <param name="scan_subscriber_queue_size" value="$(arg scan_subscriber_queue_size)"/>
  <param name="scan_topic" value="$(arg scan_topic)"/>
  <param name="tf_map_scanmatch_transform_frame_name" value="$(arg tf_map_scanmatch_transform_frame_name)" />
  </node>

  <include file="$(find hector_geotiff)/launch/geotiff_mapper.launch"/> 

</launch>

2.3 start hector slam

Now you can execute the following code to start the lidar

roslaunch diego_nav diego_run_hector_flashgo.launch

At this time we can control through the keyboard diego # in the room movement, start rviz can draw the corresponding map, in different terminals in the implementation of the following order
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
rosrun rviz rviz

3.Use gmapping to map

3.1Install the ros navigation package

sudo apt-get install ros-kinetic-navigation

gmapping,acml,move_base will be installed after the implementation

3.2Write the gmapping package to launch the launch file

<launch>
<master auto="start"/>

<include file="$(find flashgo)/launch/lidar.launch" />

<node name="arduino" pkg="ros_arduino_python" type="arduino_node.py" output="screen">
<rosparam file="$(find ros_arduino_python)/config/my_arduino_params.yaml" command="load" />
</node>

<node pkg="tf" type="static_transform_publisher" name="base_frame_2_laser_link" args="0.0 0.0 0.2 3.14 3.14 0 /base_link /laser 40"/>

<!-- gmapping node -->
<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping">
<param name="base_frame" value="base_link"/>
<param name="odom_frame" value="odom"/>
<param name="maxUrange" value="4.0"/>
<param name="maxRange" value="5.0"/>
<param name="sigma" value="0.05"/>
<param name="kernelSize" value="3"/>
<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="30"/>
<param name="srr" value="0.01"/>
<param name="srt" value="0.02"/>
<param name="str" value="0.01"/>
<param name="stt" value="0.02"/>
<param name="linearUpdate" value="0.05"/>
<param name="angularUpdate" value="0.0436"/>
<param name="temporalUpdate" value="-1.0"/>
<param name="resampleThreshold" value="0.5"/>
<param name="particles" value="8"/>
<!--
<param name="xmin" value="-50.0"/>
<param name="ymin" value="-50.0"/>
<param name="xmax" value="50.0"/>
<param name="ymax" value="50.0"/>
make the starting size small for the benefit of the Android client's memory...
-->
<param name="xmin" value="-1.0"/>
<param name="ymin" value="-1.0"/>
<param name="xmax" value="1.0"/>
<param name="ymax" value="1.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"/>
</node>
</launch>

3.3 start gmapping

Now you can execute the following code to start the lidar

roslaunch diego_nav diego_run_gmapping_flashgo.launch

At this time we can control through the keyboard diego # in the room movement, start rviz can draw the corresponding map, in different terminals in the implementation of the following order
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
rosrun rviz rviz

4.Generate map files

Whether you use hector or gmapping to create a map dynamically, we can generate a map file by command

4.1.Create the maps folder

The map file needs to give it permission to 777, allowing map_server to generate a map file in this folder

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

4.2.Generate map files

Open the terminal cd into the maps directory and execute the map generation command

cd ~/catkin_ws/src/diego_nav/maps/

rosrun map_server map_saver -f f4_gmapping

 

Scroll to top