initial commit

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()

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}

launch/map.launch

@@ -0,0 +1,15 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+  <arg name="map"        default=""/>
+  <arg name="symlink"    default=""/>
+  <arg name="octomap"    default="false"/>
+
+  <!-- Setup map -->
+  <node name="map_setup" pkg="pal_navigation_sm" type="map_setup.py">
+    <param name="map" value="$(env HOME)/.pal/tiago_maps/config"/>
+  </node>
+
+  <!-- Run the map server -->
+  <node name="map_server" pkg="map_server" type="map_server" args="$(env HOME)/.pal/tiago_maps/config/map.yaml"/>
+
+</launch>

launch/pal_navigation_sm.launch

@@ -0,0 +1,18 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+
+  <arg name="state"  default="localization"/> <!-- localization, mapping -->
+  <arg name="config" default=""/>
+
+  <arg name="frequency" default="1.0"/>
+
+  <!-- Pal Navigation SM SMACH -->
+  <node pkg ="pal_navigation_sm"
+        type="pal_navigation_main_sm.py"
+        name="pal_navigation_sm"
+        output="screen">
+    <rosparam file="$(arg config)" command="load"/>
+    <param name="state" value="$(arg state)"/>
+  </node>
+
+</launch>

package.xml

@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<package>
+  <name>pal_navigation_sm</name>
+  <version>0.1.7</version>
+  <description>
+    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.
+  </description>
+
+  <maintainer email="[email protected]">TIAGo support team</maintainer>
+  <author email="[email protected]">Jeremie Deray</author>
+  <author email="[email protected]">Enrique Fernandez</author>
+  <author email="[email protected]">Siegfried-A. Gevatter Pujals</author>
+  <author email="[email protected]">Ricardo Tellez</author>
+
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <run_depend>rospy</run_depend>
+  <run_depend>rosnode</run_depend>
+  <run_depend>rosparam</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>map_server</run_depend>
+  <run_depend>pal_navigation_msgs</run_depend>
+  <run_depend>pal_python</run_depend>
+</package>

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()

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 <robot> [<state>] [<localization method>] [<mapping method>] [<map>]
+
+# Check parameters:
+if [ $# -lt 1 ]; then
+  echo "Usage: $0 <robot> [<state>] [<localization method>] [<mapping method>] [<map>] [<octomap>]"
+  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
+
+
+

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 = """<MapTransformationConfig scale="1.0">
+  <location3D>
+    <translation x="0"  y="0"  z="0" ></translation>
+    <matrixRot rodrigues_x="0"  rodrigues_y="0"  rodrigues_z="0" ></matrixRot>
+    <rotationRollPitchYaw roll="0"  pitch="0"  yaw="0" ></rotationRollPitchYaw>
+  </location3D>
+</MapTransformationConfig>
+"""
+
+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()

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)