diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..145d686 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,20 @@ +cmake_minimum_required(VERSION 2.8.3) +project(pal_navigation_sm) + +find_package(catkin REQUIRED) + +catkin_python_setup() + +catkin_package() + +install( + PROGRAMS + scripts/map_setup.py + scripts/pal_navigation_main_sm.py + scripts/navigation.sh + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + +foreach(dir config launch) + install(DIRECTORY ${dir}/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) +endforeach() diff --git a/config/pose.yaml b/config/pose.yaml new file mode 100644 index 0000000..90387ae --- /dev/null +++ b/config/pose.yaml @@ -0,0 +1,2 @@ +{initial_cov_aa: 0.02, initial_cov_xx: 0.01, initial_cov_yy: 0.01, initial_pose_a: 0.0, + initial_pose_x: 0.0, initial_pose_y: 0.0} diff --git a/launch/map.launch b/launch/map.launch new file mode 100644 index 0000000..c0a07e8 --- /dev/null +++ b/launch/map.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/launch/pal_navigation_sm.launch b/launch/pal_navigation_sm.launch new file mode 100644 index 0000000..8aba77b --- /dev/null +++ b/launch/pal_navigation_sm.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..82ae5af --- /dev/null +++ b/package.xml @@ -0,0 +1,29 @@ + + + pal_navigation_sm + 0.1.7 + + Dummy public version of PAL Navigation State Machine. It allows saving a map and loading that map for navigation purposes. + The complete version allows changing form localization to + mapping (and viceversa), provides the pose saver and the map manager + service to save and change the current map. + + + TIAGo support team + Jeremie Deray + Enrique Fernandez + Siegfried-A. Gevatter Pujals + Ricardo Tellez + + BSD + + catkin + + rospy + rosnode + rosparam + std_msgs + map_server + pal_navigation_msgs + pal_python + diff --git a/scripts/map_setup.py b/scripts/map_setup.py new file mode 100755 index 0000000..cbf7adb --- /dev/null +++ b/scripts/map_setup.py @@ -0,0 +1,62 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +# +# map_setup.py +# +# Copyright (c) 2012-2013 PAL Robotics SL. All Rights Reserved +# +# Authors: +# * Enrique Fernández + +import rospy +import rosparam + +from std_msgs.msg import String + +import os + +class MapSetupException(Exception): + pass + +class MapSetup: + + def __init__(self): + try: + self._map = rospy.get_param("~map") + except KeyError as e: + rospy.logfatal("map param not set!: %s " % e) + raise MapSetupException("map param not set") + + # Clean/delete params: + try: + rospy.delete_param("mmap") + except: + pass + + # Publish map_in_use (latched): + self._map_in_use_pub = rospy.Publisher("map_in_use", String, queue_size=1, latch=True) + self._map_in_use_pub.publish("submap_0") + + # Set params: + rospy.set_param("map_package_in_use" , "deprecated") + rospy.set_param("nice_map_in_use" , os.path.join(self._map, "map.bmp")) + rospy.set_param("map_transformation_in_use", os.path.join(self._map, "transformation.xml")) + + # Load + paramlist = rosparam.load_file(os.path.join(self._map, "mmap.yaml"), "mmap") + for param, ns in paramlist: + try: + rosparam.upload_params(ns, param) + except: + pass # ignore empty params + +def main(): + rospy.init_node("map_setup", log_level=rospy.ERROR) + + MapSetup() + + while not rospy.is_shutdown(): + rospy.spin() + +if __name__ == "__main__": + main() diff --git a/scripts/navigation.sh b/scripts/navigation.sh new file mode 100755 index 0000000..75f8268 --- /dev/null +++ b/scripts/navigation.sh @@ -0,0 +1,58 @@ +#! /bin/sh +# +# Check if the $HOME/.pal folder is available and creates it if needed before +# launching navigation. +# +# Usage: $0 [] [] [] [] + +# Check parameters: +if [ $# -lt 1 ]; then + echo "Usage: $0 [] [] [] [] []" + echo "Check if the $HOME/.pal folder is availabe and creates it if needed" + echo "before launching navigation." + exit 1 +else + ROBOT=$1 +fi + +if [ $# -lt 2 ]; then + STATE=localization +else + STATE=$2 +fi + +if [ $# -lt 3 ]; then + LOCALIZATION=amcl +else + LOCALIZATION=$3 +fi + +if [ $# -lt 4 ]; then + MAPPING=gmapping +else + MAPPING=$4 +fi + +if [ $# -lt 5 ]; then + MAP=$HOME/.pal/${ROBOT}_maps/config +else + MAP=$5 +fi + +#if [ -d "$MAP" -a "$STATE" = "mapping" ]; then +# rm -rf $MAP +#fi + +if [ ! -d "$MAP" ]; then + mkdir -p $MAP + if [ $? -ne 0 ]; then + echo "Error: Failed to create path $MAP" + exit 3 + fi +fi + +# Run localization/mapping +roslaunch ${ROBOT}_2dnav $STATE.launch localization:=$LOCALIZATION mapping:=$MAPPING map:=$MAP + + + diff --git a/scripts/pal_navigation_main_sm.py b/scripts/pal_navigation_main_sm.py new file mode 100755 index 0000000..6fdf33f --- /dev/null +++ b/scripts/pal_navigation_main_sm.py @@ -0,0 +1,113 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +# +# pal_navigation_main_sm.py +# +# Copyright (c) 2016 PAL Robotics SL. All Rights Reserved +# +# This is a fake version of pal_navigation_sm. The complete version implements a real SMACH able to manage multiple maps, +# change dynamically between mapping and localization and many other features. +# +# Authors: +# * Ricardo Tellez +# * Siegfried-A. Gevatter +# * Jordi Pages +# + +import roslib; roslib.load_manifest('pal_navigation_sm') +import rospy +import rosparam +import os +import shutil + +import rospkg +from pal_navigation_msgs.srv import SaveMap, SaveMapResponse +from pal_python import pal_launch + +DEFAULT_TRANSFORMATION = """ + + + + + + +""" + +class MapsManagerService(): + + def __init__(self): + + self._save_map_srv = rospy.Service('pal_map_manager/save_map', SaveMap, self._save_map_request) + self._robot = rospy.get_param("~robot", "tiago") + + def _save_map_request(self, req): + + map_name = 'config' + map_path = os.path.expanduser('~') + '/.pal/' + self._robot + '_maps/' + + shutil.rmtree(map_path+map_name) + os.makedirs(map_path+map_name) + + rospy.logdebug("Saving map with map_name '" + map_name + "' and map_path '" + map_path + "'") + + # Save map (image and meta-data). + filename = 'submap_0' + pal_launch.execute_command('rosrun', 'map_server', 'map_saver', '-f', filename) + + # Update /mmap/numberOfSubMaps parameter + # [from saverFunctions::configSaver] + rospy.set_param('/mmap/numberOfSubMaps', 1) + map_data_file = os.path.join(map_path+map_name+'/', 'mmap.yaml') + if rospy.has_param('/mmap'): + rosparam.dump_params(map_data_file, '/mmap') + else: + try: + os.remove(map_data_file) + except OSError: + pass + map_data_file = None + + # Move map to maps folder + # (this is required to have the image relative path in map.yaml): + shutil.move(filename + '.pgm', map_path+map_name) + shutil.move(filename + '.yaml',map_path+map_name+'/map.yaml') + + # Create nice map + shutil.copyfile(os.path.join(map_path+map_name, 'submap_0.pgm'), os.path.join(map_path+map_name, 'map.pgm')) + with open(os.path.join(map_path+map_name, 'transformation.xml'), 'w') as transf: + transf.write(DEFAULT_TRANSFORMATION) + + # Set permisions + self._chmod(map_path, 0777) + + # Save initial pose of the robot in the map + rospack = rospkg.RosPack() + shutil.copyfile(rospack.get_path('pal_navigation_sm') + '/config/pose.yaml', map_path+'../pose.yaml') + + return SaveMapResponse(True, map_name, map_path, 'Map saved: %s' % map_name) + + def _chmod(self, path, mode): + """ + Change the permissions of `path' recursively, including + `path' itself. + """ + os.chmod(path, mode) + for root, dirs, files in os.walk(path): + for d in dirs: + os.chmod(os.path.join(root, d), mode) + for f in files: + os.chmod(os.path.join(root, f), mode) + + + + +def main(): + rospy.init_node('navigation_sm', log_level=rospy.ERROR) + + MapsManagerService() + + rospy.spin() + + +if __name__ == "__main__": + main() diff --git a/setup.py b/setup.py new file mode 100644 index 0000000..3d8a1f1 --- /dev/null +++ b/setup.py @@ -0,0 +1,11 @@ +#!/usr/bin/env python + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['pal_navigation_sm'], + package_dir={'': 'src'} +) + +setup(**d) diff --git a/src/pal_navigation_sm/__init__.py b/src/pal_navigation_sm/__init__.py new file mode 100644 index 0000000..e69de29