In my previous post on the ROS Navigation Stack, when we wanted to give our robot a goal location, we used the RViz graphical user interface. However, if you want to send goals to the ROS Navigation Stack using code, you can do that too. We’ll use C++.
The official tutorial is on this page, but I will walk you through all the steps below.
Real-World Applications
This project has a number of real-world applications:
Indoor Delivery Robots
Room Service Robots
Mapping of Underground Mines, Caves, and Hard-to-Reach Environments
Robot Vacuums
Order Fulfillment
Factories
Prerequisites
You have a robot that is running the ROS Navigation Stack. I will be continuing from this tutorial.
Determine the Coordinates of the Goal Locations
Open a new terminal window, and launch the launch file.
roslaunch navstack_pub jetson_nano_bot.launch
Make a note of the X and Y coordinates of each desired goal location. I use RViz Point Publish button to accomplish this. When you click that button, you can see the coordinate values by typing the following command in a terminal:
ros topic echo /clicked_point
I want to have an X, Y coordinate for the following six goal locations in my apartment.
1 = Bathroom
2 = Bedroom
3 = Front Door
4 = Living Room
5 = Home Office
6 = Kitchen
You notice how I numbered the goal locations above. That was intentional. I want to be able to type a number into a terminal window and have the robot navigate to that location.
For example, if I type 6, the robot will move to the kitchen. If I type 2, the robot will go to my bedroom.
Write the Code
Now let’s write some C++.
Open a terminal window.
roscd navstack_pub
cd src
gedit send_goals.cpp
/*
* Author: Automatic Addison
* Date: May 30, 2021
* ROS Version: ROS 1 - Melodic
* Website: https://automaticaddison.com
* This ROS node sends the robot goals to move to a particular location on
* a map. I have configured this program to the map of my own apartment.
*
* 1 = Bathroom
* 2 = Bedroom
* 3 = Front Door
* 4 = Living Room
* 5 = Home Office
* 6 = Kitchen (Default)
*/
#include <ros/ros.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <iostream>
using namespace std;
// Action specification for move_base
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
int main(int argc, char** argv){
// Connect to ROS
ros::init(argc, argv, "simple_navigation_goals");
//tell the action client that we want to spin a thread by default
MoveBaseClient ac("move_base", true);
// Wait for the action server to come up so that we can begin processing goals.
while(!ac.waitForServer(ros::Duration(5.0))){
ROS_INFO("Waiting for the move_base action server to come up");
}
int user_choice = 6;
char choice_to_continue = 'Y';
bool run = true;
while(run) {
// Ask the user where he wants the robot to go?
cout << "\nWhere do you want the robot to go?" << endl;
cout << "\n1 = Bathroom" << endl;
cout << "2 = Bedroom" << endl;
cout << "3 = Front Door" << endl;
cout << "4 = Living Room" << endl;
cout << "5 = Home Office" << endl;
cout << "6 = Kitchen" << endl;
cout << "\nEnter a number: ";
cin >> user_choice;
// Create a new goal to send to move_base
move_base_msgs::MoveBaseGoal goal;
// Send a goal to the robot
goal.target_pose.header.frame_id = "map";
goal.target_pose.header.stamp = ros::Time::now();
bool valid_selection = true;
// Use map_server to load the map of the environment on the /map topic.
// Launch RViz and click the Publish Point button in RViz to
// display the coordinates to the /clicked_point topic.
switch (user_choice) {
case 1:
cout << "\nGoal Location: Bathroom\n" << endl;
goal.target_pose.pose.position.x = 10.0;
goal.target_pose.pose.position.y = 3.7;
goal.target_pose.pose.orientation.w = 1.0;
break;
case 2:
cout << "\nGoal Location: Bedroom\n" << endl;
goal.target_pose.pose.position.x = 8.1;
goal.target_pose.pose.position.y = 4.3;
goal.target_pose.pose.orientation.w = 1.0;
break;
case 3:
cout << "\nGoal Location: Front Door\n" << endl;
goal.target_pose.pose.position.x = 10.5;
goal.target_pose.pose.position.y = 2.0;
goal.target_pose.pose.orientation.w = 1.0;
break;
case 4:
cout << "\nGoal Location: Living Room\n" << endl;
goal.target_pose.pose.position.x = 5.3;
goal.target_pose.pose.position.y = 2.7;
goal.target_pose.pose.orientation.w = 1.0;
break;
case 5:
cout << "\nGoal Location: Home Office\n" << endl;
goal.target_pose.pose.position.x = 2.5;
goal.target_pose.pose.position.y = 2.0;
goal.target_pose.pose.orientation.w = 1.0;
break;
case 6:
cout << "\nGoal Location: Kitchen\n" << endl;
goal.target_pose.pose.position.x = 3.0;
goal.target_pose.pose.position.y = 6.0;
goal.target_pose.pose.orientation.w = 1.0;
break;
default:
cout << "\nInvalid selection. Please try again.\n" << endl;
valid_selection = false;
}
// Go back to beginning if the selection is invalid.
if(!valid_selection) {
continue;
}
ROS_INFO("Sending goal");
ac.sendGoal(goal);
// Wait until the robot reaches the goal
ac.waitForResult();
if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
ROS_INFO("The robot has arrived at the goal location");
else
ROS_INFO("The robot failed to reach the goal location for some reason");
// Ask the user if he wants to continue giving goals
do {
cout << "\nWould you like to go to another destination? (Y/N)" << endl;
cin >> choice_to_continue;
choice_to_continue = tolower(choice_to_continue); // Put your letter to its lower case
} while (choice_to_continue != 'n' && choice_to_continue != 'y');
if(choice_to_continue =='n') {
run = false;
}
}
return 0;
}
Save and close the file.
Now open a new terminal window, and type the following command:
In this tutorial, we will learn how to set up and configure the ROS Navigation Stack for a mobile robot. The video below shows the final output you will be able to achieve once you complete this tutorial.
What is the ROS Navigation Stack?
The ROS Navigation Stack is a collection of software packages that you can use to help your robot move from a starting location to a goal location safely.
The official steps for setup and configuration are at this link on the ROS website, but we will walk through everything together, step-by-step, because those instructions leave out a lot of detail. Where possible, I will link to other tutorials that I’ve written that have detailed instructions on how to implement specific pieces of the Navigation Stack.
IMPORTANT: For your reference, all our code will be locatedin this folder, which I named jetson_nano_bot.
Real-World Applications
The underside of a robot vacuum
This project has a number of real-world applications:
Indoor Delivery Robots
Room Service Robots
Mapping of Underground Mines, Caves, and Hard-to-Reach Environments
Remove the hashtag on line 5 to make sure that C++11 support is enabled.
cmake_minimum_required(VERSION 3.0.2)
project(navstack_pub)
## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
move_base
nav_msgs
roscpp
rospy
sensor_msgs
std_msgs
tf
tf2_ros
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# geometry_msgs# nav_msgs# sensor_msgs# std_msgs
# )
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES navstack_pub
# CATKIN_DEPENDS geometry_msgs move_base nav_msgs roscpp rospy sensor_msgs std_msgs tf tf2_ros
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
${catkin_INCLUDE_DIRS}
)
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/navstack_pub.cpp
# )
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/navstack_pub_node.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
# ${catkin_LIBRARIES}
# )
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# catkin_install_python(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
# install(TARGETS ${PROJECT_NAME}
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_navstack_pub.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
INCLUDE_DIRECTORIES(/usr/local/lib)
LINK_DIRECTORIES(/usr/local/lib)
add_executable(send_goals src/send_goals.cpp)
target_link_libraries(send_goals ${catkin_LIBRARIES})
Save and close the file.
Now let’s compile the package.
cd ~/catkin_ws/
catkin_make --only-pkg-with-deps navstack_pub
Now we’re going to put together our launch file.
Open a new terminal window, and move to your navstack_pub package.
roscd navstack_pub
mkdir launch
cd launch
Create your launch file.
gedit jetson_nano_bot.launch
Add the following code:
<launch>
</launch>
Save the file, and close it.
Transform Configuration
The ROS Navigation Stack requires that we publish information about the relationships between coordinate frames of the robot using the tf ROS package.
Open a terminal window and type:
roscd navstack_pub
cd launch
Open your launch file.
gedit jetson_nano_bot.launch
Add the “Transformation Configuration…” block of this code (you will need to download the launch file in order to copy the code) to your launch file. You will need to modify it for your own robot. A full explanation of how to do that can be found on this post.
Sensor Information
The ROS Navigation Stack uses sensor information to help the robot avoid obstacles in the environment. It assumes that the sensor publishes either sensor_msgs/LaserScan or sensor_msgs/PointCloud messages over ROS.
LIDAR Information
Open a terminal window and type:
roscd navstack_pub
cd launch
Open your launch file.
gedit jetson_nano_bot.launch
Add the “Lidar Data Publisher Using RPLIDAR…” block of this code to your launch file. This code requires no modifications. A full explanation of how to set up your LIDAR so the launch file can read it properly can be found on this post. The full tutorial for setting up your LIDAR from scratch can be found on this post.
Odometry Information
The navigation stack requires that odometry information be published using tf and the nav_msgs/Odometry message. To set this up, you will need to have completed the following three tutorials:
The robot_pose_ekf package, which uses an Extended Kalman Filter to fuse the data provided by the wheel encoders and the BNO055 IMU sensor.
Once you have completed the three tutorials above, you can move to the next step to add the appropriate code to your main launch file.
Open a terminal window and type:
roscd navstack_pub
cd launch
Open your launch file.
gedit jetson_nano_bot.launch
Add the “Wheel Odometry Publisher” block of this code to your launch file.
Add the “IMU Data Publisher Using the BNO055 IMU Sensor” block of this code to your launch file.
Add the “Extended Kalman Filter from robot_pose_ekf Node” block of this code to your launch file.
Base Controller
The ROS Navigation Stack requires a node that subscribes to the “cmd_vel” (i.e. velocity command) topic that takes velocities and converts them into motor commands. To set this up, you will need to have completed the following tutorial:
You will also need a way to convert the initial pose of the robot and your desired goal destination into a usable format. I show you how to do that in the tutorial below:
Once you have the two map files and your initial pose and goal publisher, you can add the relevant code to your launch file.
Open a terminal window and type:
roscd navstack_pub
cd launch
Open your launch file.
gedit jetson_nano_bot.launch
Add the “Map File” block of this code to your launch file. This code loads the saved map files.
Add the “Map Server” block of this code to your launch file. This code calls on ROS to serve the map.
Add the “Initial Pose and Goal Publisher” block of this code to your launch file. This code enables you to launch RViz. You will need to delete the following argument:
Add the “Subscribe: /initialpose, /move_base_simple/goal” block of this code to your launch file. This code makes sure that, when you click the buttons in RViz to set the initial pose and the goal destination, the pose and goal get converted into a usable format.
Costmap Configuration (Global and Local Costmaps)
The ROS Navigation Stack uses two costmaps to store information about obstacles in the world.
Global costmap: This costmap is used to generate long term plans over the entire environment….for example, to calculate the shortest path from point A to point B on a map.
Local costmap: This costmap is used to generate short term plans over the environment….for example, to avoid obstacles.
We have to configure these costmaps for our project. We set the configurations in .yaml files.
Common Configuration (Global and Local Costmap)
Let’s create a configuration file that will house parameters that are common to both the global and the local costmap. The name of this file will be costmap_common_params.yaml.
In addition to the costmap configurations we did in the previous section, we need to configure ROS Navigation Stack’s base local planner. The base_local_planner computes velocity commands that are sent to the robot base controller. The values that you use for your base_local_planner will depend on your robot.
Now that we have created our configuration files, we need to add them to the launch file. The configuration files will be used by ROS Navigation Stack’s move_base node. The move_base node is the work horse behind the scenes that is responsible for planning a collision-free path from a starting location to a goal location for a mobile robot.
The move-base node subscribes to the following topics:
/move_base_simple/goal : Goal position and orientation (geometry_msgs::PoseStamped). The messages on this topic are generated using the goal button on RViz.
The publisher will publish to the following topics:
Add the “Move Base Node” block of this code to your launch file. This code loads all the configuration files we have created so far. It also loads the move_base node.
AMCL Configuration
The ROS Navigation Stack requires the use of AMCL (Adaptive Monte Carlo Localization), a probabilistic localization system for a robot. AMCL is used to track the pose of a robot against a known map. It takes as input a map, LIDAR scans, and transform messages, and outputs an estimated pose. You can learn more about this package here on the ROS website.
/initialpose : The initial position and orientation of the robot using quaternions. (geometry_msgs/PoseWithCovarianceStamped) — RViz initial pose button publishes to this topic.
/map : The occupancy grid map created using gmapping, Hector SLAM, or manually using an image (nav_msgs/OccupancyGrid).
The amcl node will publish to the following topics:
/particlecloud: The set of pose estimates being maintained by the filter (geometry_msgs/PoseArray).
/tf (tf/tfMessage): Publishes the transform from odom (which can be remapped via the ~odom_frame_id parameter) to map.
Let’s add the AMCL node to the launch file.
Open a terminal window, and type:
cd ~/catkin_ws
roscd navstack_pub
cd launch
gedit jetson_nano_bot.launch
Add the “Add AMCL example for differential drive robots” block of this code to your launch file. This code loads the ACML code for a differential drive robot.
Launch the Autonomous Mobile Robot!
We’re almost at the finished line!
Let’s compile the package. Open a new terminal, and type:
cd ~/catkin_ws/
catkin_make --only-pkg-with-deps navstack_pub
Turn the motors of your robot on.
Open a new terminal window and launch the launch file.
roslaunch navstack_pub jetson_nano_bot.launch
If necessary, set the topics for each of the RViz plugins so that you can see the axis of your robot on the screen along with the map and costmaps.
Set the initial pose of the robot by clicking the 2D Pose Estimate button at the top of RViz and then clicking on the map.
Give the robot a goal by clicking on the 2D Nav Goal button at the top of RViz and then clicking on the map.
You should see the planned path automatically drawn on the map. Your robot should then begin to follow this path.
Open a new terminal, and see the tf tree.
rosrun tf view_frames
evince frames.pdf
The ROS node graph for my project
The standard coordinate frames for a mobile robot that is using the ROS Navigation Stack. Image Source: ROS Website
In this tutorial, I will show you how to create a map for RViz (3D visualizer program for ROS) from a floor plan or blueprint. Creating a map for RViz is an important step for enabling a robot to navigate autonomously in an environment.
We often use a robot’s LIDAR to build a map. That is fine and good, but you can often build a better, more accurate map if you create one from a floor plan or blueprint you already have on hand.
Real-World Applications
This project has a number of real-world applications:
The first thing you need to do is to grab a floor plan or blueprint. Make sure it is in .png format.
Save the image to the directory where you want to eventually load your map.
I will create a new folder inside the catkin_ws called maps.
cd ~/catkin_ws
mkdir maps
cd maps
Install the map server.
sudo apt-get install ros-melodic-map-server
Edit the image file as you wish using a program like Paint.net.
Convert the image to binary format using OpenCV. Here is the code for that. I named the program convert_to_binary.py.
import cv2 # Import OpenCV
# read the image file
img = cv2.imread('mii_floor_plan_4.png')
ret, bw_img = cv2.threshold(img, 220, 255, cv2.THRESH_BINARY)
# converting to its binary form
bw = cv2.threshold(img, 240, 255, cv2.THRESH_BINARY)
# Display and save image
cv2.imshow("Binary", bw_img)
cv2.imwrite("mii_floor_plan_4_binary.png", bw_img)
cv2.waitKey(0)
cv2.destroyAllWindows()
The name of my image is mii_floor_plan_4_binary.png. Here is the image.
Now, let’s create our .pgm and .yaml map files. ROS needs both of these file formats to display the map on RViz. Write the code below inside the same directory as your .png image file.
gedit MakeROSMap.py
import numpy as np
import cv2
import math
import os.path
#
# This is a start for the map program
#
prompt = '> '
print("What is the name of your floor plan you want to convert to a ROS map:")
file_name = input(prompt)
print("You will need to choose the x coordinates horizontal with respect to each other")
print("Double Click the first x point to scale")
#
# Read in the image
#
image = cv2.imread(file_name)
#
# Some variables
#
ix,iy = -1,-1
x1 = [0,0,0,0]
y1 = [0,0,0,0]
font = cv2.FONT_HERSHEY_SIMPLEX
#
# mouse callback function
# This allows me to point and
# it prompts me from the command line
#
def draw_point(event,x,y,flags,param):
global ix,iy,x1,y1n,sx,sy
if event == cv2.EVENT_LBUTTONDBLCLK:
ix,iy = x,y
print(ix,iy)
#
# Draws the point with lines around it so you can see it
#
image[iy,ix]=(0,0,255)
cv2.line(image,(ix+2,iy),(ix+10,iy),(0,0,255),1)
cv2.line(image,(ix-2,iy),(ix-10,iy),(0,0,255),1)
cv2.line(image,(ix,iy+2),(ix,iy+10),(0,0,255),1)
cv2.line(image,(ix,iy-2),(ix,iy-10),(0,0,255),1)
#
# This is for the 4 mouse clicks and the x and y lengths
#
if x1[0] == 0:
x1[0]=ix
y1[0]=iy
print('Double click a second x point')
elif (x1[0] != 0 and x1[1] == 0):
x1[1]=ix
y1[1]=iy
prompt = '> '
print("What is the x distance in meters between the 2 points?")
deltax = float(input(prompt))
dx = math.sqrt((x1[1]-x1[0])**2 + (y1[1]-y1[0])**2)*.05
sx = deltax / dx
print("You will need to choose the y coordinates vertical with respect to each other")
print('Double Click a y point')
elif (x1[1] != 0 and x1[2] == 0):
x1[2]=ix
y1[2]=iy
print('Double click a second y point')
else:
prompt = '> '
print("What is the y distance in meters between the 2 points?")
deltay = float(input(prompt))
x1[3]=ix
y1[3]=iy
dy = math.sqrt((x1[3]-x1[2])**2 + (y1[3]-y1[2])**2)*.05
sy = deltay/dy
print(sx, sy)
res = cv2.resize(image, None, fx=sx, fy=sy, interpolation = cv2.INTER_CUBIC)
# Convert to grey
res = cv2.cvtColor(res, cv2.COLOR_BGR2GRAY)
cv2.imwrite("KEC_BuildingCorrected.pgm", res );
cv2.imshow("Image2", res)
#for i in range(0,res.shape[1],20):
#for j in range(0,res.shape[0],20):
#res[j][i][0] = 0
#res[j][i][1] = 0
#res[j][i][2] = 0
#cv2.imwrite("KEC_BuildingCorrectedDots.pgm",res)
# Show the image in a new window
# Open a file
prompt = '> '
print("What is the name of the new map?")
mapName = input(prompt)
prompt = '> '
print("Where is the desired location of the map and yaml file?")
print("NOTE: if this program is not run on the TurtleBot, Please input the file location of where the map should be saved on TurtleBot. The file will be saved at that location on this computer. Please then tranfer the files to TurtleBot.")
mapLocation = input(prompt)
completeFileNameMap = os.path.join(mapLocation, mapName +".pgm")
completeFileNameYaml = os.path.join(mapLocation, mapName +".yaml")
yaml = open(completeFileNameYaml, "w")
cv2.imwrite(completeFileNameMap, res );
#
# Write some information into the file
#
yaml.write("image: " + mapLocation + "/" + mapName + ".pgm\n")
yaml.write("resolution: 0.050000\n")
yaml.write("origin: [" + str(-1) + "," + str(-1) + ", 0.000000]\n")
yaml.write("negate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.196")
yaml.close()
exit()
cv2.namedWindow('image', cv2.WINDOW_NORMAL)
cv2.setMouseCallback('image',draw_point)
#
# Waiting for a Esc hit to quit and close everything
#
while(1):
cv2.imshow('image',image)
k = cv2.waitKey(20) & 0xFF
if k == 27:
break
elif k == ord('a'):
print('Done')
cv2.destroyAllWindows()
To run the code, type:
python MakeROSMap.py
If that doesn’t work on your system, you might need to type:
python3 MakeROSMap.py
When the program asks you for the desired location, press Enter.
My two output files are floorplan4.pgm and floorplan4.yaml.
Open the yaml file and put the full path for the pgm image in the image tag.
