diff --git a/homer_map_manager/include/homer_map_manager/Managers/MapManager.h b/homer_map_manager/include/homer_map_manager/Managers/MapManager.h
index d5a74ce4be03bccc050bb037035a18b37e859814..f3103ab11609dee3a10534b0dce2cfe3ffbc4120 100644
--- a/homer_map_manager/include/homer_map_manager/Managers/MapManager.h
+++ b/homer_map_manager/include/homer_map_manager/Managers/MapManager.h
@@ -1,99 +1,102 @@
#ifndef MAPMANAGER_H
#define MAPMANAGER_H
-#include <string>
-#include <map>
-#include <list>
#include <stdio.h>
#include <stdlib.h>
+#include <list>
+#include <map>
#include <sstream>
#include <string>
+#include <string>
#include <vector>
-#include <nav_msgs/OccupancyGrid.h>
#include <geometry_msgs/Pose.h>
-#include <nav_msgs/OccupancyGrid.h>
-#include <homer_mapnav_msgs/ModifyMap.h>
#include <homer_mapnav_msgs/MapLayers.h>
-#include <sensor_msgs/LaserScan.h>
+#include <homer_mapnav_msgs/ModifyMap.h>
#include <homer_nav_libs/tools.h>
+#include <nav_msgs/OccupancyGrid.h>
+#include <nav_msgs/OccupancyGrid.h>
+#include <sensor_msgs/LaserScan.h>
#include <tf/tf.h>
#include "ros/ros.h"
/** @class MapManager
* @author Malte Knauf, David Gossow (RX), Susanne Maur
- * @brief This class holds all current map layers, updates them and publishes them in one merged map.
+ * @brief This class holds all current map layers, updates them and publishes
+ * them in one merged map.
*/
class MapManager
{
- public:
-
- /**
- * @brief Constructor
- * @param nh node handle
- */
- MapManager(ros::NodeHandle* nh);
-
- /**
- * @brief getMapLayer search for map layer of given type and return it
- * @param type type of map layer to search for
- * @return map layer
- */
- nav_msgs::OccupancyGrid::ConstPtr getMapLayer(int type);
-
- /**
- * @brief updateMapLayer replaces map layer of given type
- * @param type type of map layer
- * @param layer new map layer
- */
- void updateMapLayer(int type, nav_msgs::OccupancyGrid::ConstPtr layer);
-
- /**
- * @brief toggleMapVisibility toggles visibility of each map layer
- * @param type type of map layer to toggle
- * @param state visible or not
- */
- void toggleMapVisibility(int type, bool state );
-
- /**
- * @brief clearMapLayers Clear all map layers
- */
- void clearMapLayers();
-
- void updateLaser(int layer, const sensor_msgs::LaserScan::ConstPtr& msg );
-
- /** merges all map layers and publishes the merged map */
- void sendMergedMap();
-
- void updatePose(const geometry_msgs::PoseStamped::ConstPtr& msg){m_pose = msg;}
-
- /** destructor */
- virtual ~MapManager();
-
- private:
- /**
- * The map data of each available map layer ist stored in this map
- */
- std::map< int, nav_msgs::OccupancyGrid::ConstPtr > m_MapLayers;
- std::map< int, sensor_msgs::LaserScan::ConstPtr > m_laserLayers;
- std::vector< int > m_map_layers;
- std::vector< int > m_laser_layers;
-
- tf::TransformListener m_TransformListener;
-
- /**
- * This map stores which map layers are enabled and which are disabled
- */
- std::map<int, bool> m_MapVisibility;
-
- //sizes of the last slam map
- bool m_got_transform;
- geometry_msgs::PoseStamped::ConstPtr m_pose;
- tf::StampedTransform m_sick_transform;
- tf::StampedTransform m_hokuyo_transform;
-
- /** map publisher */
- ros::Publisher m_MapPublisher;
+public:
+ /**
+ * @brief Constructor
+ * @param nh node handle
+ */
+ MapManager(ros::NodeHandle* nh);
+
+ /**
+ * @brief getMapLayer search for map layer of given type and return it
+ * @param type type of map layer to search for
+ * @return map layer
+ */
+ nav_msgs::OccupancyGrid::ConstPtr getMapLayer(int type);
+
+ /**
+ * @brief updateMapLayer replaces map layer of given type
+ * @param type type of map layer
+ * @param layer new map layer
+ */
+ void updateMapLayer(int type, nav_msgs::OccupancyGrid::ConstPtr layer);
+
+ /**
+ * @brief toggleMapVisibility toggles visibility of each map layer
+ * @param type type of map layer to toggle
+ * @param state visible or not
+ */
+ void toggleMapVisibility(int type, bool state);
+
+ /**
+ * @brief clearMapLayers Clear all map layers
+ */
+ void clearMapLayers();
+
+ void updateLaser(int layer, const sensor_msgs::LaserScan::ConstPtr& msg);
+
+ /** merges all map layers and publishes the merged map */
+ void sendMergedMap();
+
+ void updatePose(const geometry_msgs::PoseStamped::ConstPtr& msg)
+ {
+ m_pose = msg;
+ }
+
+ /** destructor */
+ virtual ~MapManager();
+
+private:
+ /**
+ * The map data of each available map layer ist stored in this map
+ */
+ std::map<int, nav_msgs::OccupancyGrid::ConstPtr> m_MapLayers;
+ std::map<int, sensor_msgs::LaserScan::ConstPtr> m_laserLayers;
+ std::vector<int> m_map_layers;
+ std::vector<int> m_laser_layers;
+
+ tf::TransformListener m_TransformListener;
+
+ /**
+ * This map stores which map layers are enabled and which are disabled
+ */
+ std::map<int, bool> m_MapVisibility;
+
+ // sizes of the last slam map
+ bool m_got_transform;
+ geometry_msgs::PoseStamped::ConstPtr m_pose;
+ tf::StampedTransform m_sick_transform;
+ tf::StampedTransform m_hokuyo_transform;
+
+ /** map publisher */
+ ros::Publisher m_MapPublisher;
};
#endif
diff --git a/homer_map_manager/include/homer_map_manager/Managers/MaskingManager.h b/homer_map_manager/include/homer_map_manager/Managers/MaskingManager.h
index a7b3c7c98afa23f4b9271d81e5e55277d14d9991..b003761ea547594ea5306bc67de1f63240340a62 100644
--- a/homer_map_manager/include/homer_map_manager/Managers/MaskingManager.h
+++ b/homer_map_manager/include/homer_map_manager/Managers/MaskingManager.h
@@ -15,39 +15,41 @@
* in a separate layer
* @author Malte Knauf, David Gossow
*/
-class MaskingManager {
- public:
- /** @brief The constructor. */
- MaskingManager(nav_msgs::MapMetaData mapInfo);
-
- /** @brief The destructor. */
- virtual ~MaskingManager();
-
- nav_msgs::OccupancyGrid::ConstPtr updateMapInfo(const nav_msgs::MapMetaData &mapInfo);
-
- /** modifies either the masking layer or the slam layer (accordingly to the
- * given map layer in the msg */
- nav_msgs::OccupancyGrid::ConstPtr modifyMap(
- homer_mapnav_msgs::ModifyMap::ConstPtr msg);
-
- /** resets the masking map layer */
- nav_msgs::OccupancyGrid::ConstPtr resetMap();
-
- /** replaces the masking map layer */
- void replaceMap(nav_msgs::OccupancyGrid map);
-
- private:
- /** stores the masking values in the dedicated masking map */
- nav_msgs::OccupancyGrid m_MaskingMap;
- /** stores the masking values that are afterwards sent to the slam map */
- nav_msgs::OccupancyGrid m_SlamMap;
-
- /** tools to draw masking polygons */
- void drawPolygon(std::vector<geometry_msgs::Point> vertices, int value,
- int mapLayer);
- void drawLine(std::vector<int> &data, int startX, int startY, int endX,
- int endY, int value);
- void fillPolygon(std::vector<int> &data, int x, int y, int value);
+class MaskingManager
+{
+public:
+ /** @brief The constructor. */
+ MaskingManager(nav_msgs::MapMetaData mapInfo);
+
+ /** @brief The destructor. */
+ virtual ~MaskingManager();
+
+ nav_msgs::OccupancyGrid::ConstPtr
+ updateMapInfo(const nav_msgs::MapMetaData &mapInfo);
+
+ /** modifies either the masking layer or the slam layer (accordingly to the
+ * given map layer in the msg */
+ nav_msgs::OccupancyGrid::ConstPtr
+ modifyMap(homer_mapnav_msgs::ModifyMap::ConstPtr msg);
+
+ /** resets the masking map layer */
+ nav_msgs::OccupancyGrid::ConstPtr resetMap();
+
+ /** replaces the masking map layer */
+ void replaceMap(nav_msgs::OccupancyGrid map);
+
+private:
+ /** stores the masking values in the dedicated masking map */
+ nav_msgs::OccupancyGrid m_MaskingMap;
+ /** stores the masking values that are afterwards sent to the slam map */
+ nav_msgs::OccupancyGrid m_SlamMap;
+
+ /** tools to draw masking polygons */
+ void drawPolygon(std::vector<geometry_msgs::Point> vertices, int value,
+ int mapLayer);
+ void drawLine(std::vector<int> &data, int startX, int startY, int endX,
+ int endY, int value);
+ void fillPolygon(std::vector<int> &data, int x, int y, int value);
};
#endif
diff --git a/homer_map_manager/include/homer_map_manager/Managers/PoiManager.h b/homer_map_manager/include/homer_map_manager/Managers/PoiManager.h
index f0562e9e8fb98f955572c02d684c0bf0a07a329b..7885479d896cab821958d23b6a0dab50b8460134 100644
--- a/homer_map_manager/include/homer_map_manager/Managers/PoiManager.h
+++ b/homer_map_manager/include/homer_map_manager/Managers/PoiManager.h
@@ -3,8 +3,8 @@
#include <list>
-#include <homer_mapnav_msgs/PointOfInterest.h>
#include <homer_mapnav_msgs/ModifyPOI.h>
+#include <homer_mapnav_msgs/PointOfInterest.h>
#include <ros/ros.h>
@@ -12,78 +12,78 @@
* @author Malte Knauf, David Gossow
* @brief This class manages the List of points of interest (POIs)
*
- * This class keeps a list of all POIs within the current map. It provides the usual functions
+ * This class keeps a list of all POIs within the current map. It provides the
+ * usual functions
* to edit the list.
*/
-class PoiManager {
-
- public:
-
- /** The constructor of the class. */
- PoiManager(ros::NodeHandle* nh);
-
- /** constructor initializing the poi list */
- PoiManager( std::vector< homer_mapnav_msgs::PointOfInterest > pois );
-
- /** Does nothing. */
- ~PoiManager() {}
-
- /** Adds a new POI to the list if no POI with the same name exists
- * @param poi pointer to the PointOfInterest message with the POI to be added
- * @return true if successful, false otherwise
- */
- bool addPointOfInterest( const homer_mapnav_msgs::PointOfInterest::ConstPtr& poi );
-
- /** Replaces a POI with a new one
- * @param poi pointer with the PointOfInterest to be inserted
- * the POI with the same ID as the new one is first deleted
- * @return true if the old POI was found and could be deleted
- * false otherwise
- */
- bool modifyPointOfInterest( const homer_mapnav_msgs::ModifyPOI::ConstPtr& poi );
-
- /** Deletes a POI with a certain name from the list
- * @param name Name of the POI to be deleted
- * @return true if the POI was found and could be deleted
- * false otherwise
- */
- bool deletePointOfInterest( std::string name );
-
- /**
- * @brief place the current poi list
- * @param poilist new poi list
- */
- void replacePOIList(std::vector<homer_mapnav_msgs::PointOfInterest> poilist);
-
- /**
- * @brief Publishes a PointsOfInterest Message with current POIs
-
- */
- void broadcastPoiList();
-
- /** Returns current POI list
- * @return the POI list
- */
- std::vector< homer_mapnav_msgs::PointOfInterest > getList();
-
- private:
-
- /** Looks for POI with name in the list
- * @param name Name of the POI
- */
- bool poiExists( std::string name );
-
-
- /** The copy constructor of the class.
- * It's kept private, because it will never be used.
- */
- PoiManager( const PoiManager& instance );
-
- /** Holds the POI vector */
- std::vector< homer_mapnav_msgs::PointOfInterest > m_Pois;
-
- /** publisher that publishes the current poi list */
- ros::Publisher m_POIsPublisher;
-
+class PoiManager
+{
+public:
+ /** The constructor of the class. */
+ PoiManager(ros::NodeHandle* nh);
+
+ /** constructor initializing the poi list */
+ PoiManager(std::vector<homer_mapnav_msgs::PointOfInterest> pois);
+
+ /** Does nothing. */
+ ~PoiManager()
+ {
+ }
+
+ /** Adds a new POI to the list if no POI with the same name exists
+ * @param poi pointer to the PointOfInterest message with the POI to be added
+ * @return true if successful, false otherwise
+ */
+ bool
+ addPointOfInterest(const homer_mapnav_msgs::PointOfInterest::ConstPtr& poi);
+
+ /** Replaces a POI with a new one
+ * @param poi pointer with the PointOfInterest to be inserted
+ * the POI with the same ID as the new one is first deleted
+ * @return true if the old POI was found and could be deleted
+ * false otherwise
+ */
+ bool modifyPointOfInterest(const homer_mapnav_msgs::ModifyPOI::ConstPtr& poi);
+
+ /** Deletes a POI with a certain name from the list
+ * @param name Name of the POI to be deleted
+ * @return true if the POI was found and could be deleted
+ * false otherwise
+ */
+ bool deletePointOfInterest(std::string name);
+
+ /**
+ * @brief place the current poi list
+ * @param poilist new poi list
+ */
+ void replacePOIList(std::vector<homer_mapnav_msgs::PointOfInterest> poilist);
+
+ /**
+ * @brief Publishes a PointsOfInterest Message with current POIs
+
+ */
+ void broadcastPoiList();
+
+ /** Returns current POI list
+ * @return the POI list
+ */
+ std::vector<homer_mapnav_msgs::PointOfInterest> getList();
+
+private:
+ /** Looks for POI with name in the list
+ * @param name Name of the POI
+ */
+ bool poiExists(std::string name);
+
+ /** The copy constructor of the class.
+ * It's kept private, because it will never be used.
+ */
+ PoiManager(const PoiManager& instance);
+
+ /** Holds the POI vector */
+ std::vector<homer_mapnav_msgs::PointOfInterest> m_Pois;
+
+ /** publisher that publishes the current poi list */
+ ros::Publisher m_POIsPublisher;
};
#endif
diff --git a/homer_map_manager/include/homer_map_manager/Managers/RoiManager.h b/homer_map_manager/include/homer_map_manager/Managers/RoiManager.h
index ca0d893f0b6948556b6edf02ed605a46966f3537..93a962d72701f326f5f7c3a306f99b440a15d59b 100644
--- a/homer_map_manager/include/homer_map_manager/Managers/RoiManager.h
+++ b/homer_map_manager/include/homer_map_manager/Managers/RoiManager.h
@@ -1,127 +1,128 @@
#ifndef ROI_MANAGER_H
#define ROI_MANAGER_H
-#include <ros/ros.h>
+#include <geometry_msgs/PointStamped.h>
#include <homer_mapnav_msgs/RegionOfInterest.h>
#include <homer_mapnav_msgs/RegionsOfInterest.h>
-#include <geometry_msgs/PointStamped.h>
-#include <sstream>
-#include <Eigen/Geometry>
+#include <ros/ros.h>
#include <tf/transform_listener.h>
-
+#include <Eigen/Geometry>
+#include <sstream>
/**
* @class RoiManager (Roi = Region of interest)
* @author Viktor Seib (R23)
* @brief This class manages the List of regions of interest (ROIs)
*
- * This class keeps a list of all ROIs within the current map. It provides the usual functions
+ * This class keeps a list of all ROIs within the current map. It provides the
+ * usual functions
* to edit the list.
*/
class RoiManager
{
-
- public:
-
- /** The constructor of the class. */
- RoiManager(ros::NodeHandle* nh);
-
- /** constructor initializing the roi list */
- RoiManager( std::vector< homer_mapnav_msgs::RegionOfInterest > rois );
-
- /** Does nothing. */
- ~RoiManager() {}
-
- /** Adds a new ROI to the list, in contrast to POIs, several ROIs with the same name are allowed
- * @param roi RegionOfInterest message with the ROI to be added
- * @return true if successful, false otherwise
- */
- bool addRegionOfInterest( const homer_mapnav_msgs::RegionOfInterest::ConstPtr& roi );
-
- /** Tests all Rois and returns a vector of ids in which the pose is
- * @param pose Pose which is tested to be inside the Rois
- * @return vector of ids in which the pose is
- */
- std::vector<homer_mapnav_msgs::RegionOfInterest> pointInsideRegionOfInterest( const geometry_msgs::PointStamped point );
-
- /** Replaces a ROI with a new one
- * @param roi RegionOfInterest to be inserted
- * the ROI with the same ID as the new one is first deleted
- * @return true if the old ROI was found and could be deleted
- * false otherwise
- */
- bool modifyRegionOfInterest( const homer_mapnav_msgs::RegionOfInterest::ConstPtr& roi );
-
- /** Deletes all ROIs with a certain name from the list
- * @param name Name of the ROIs to be deleted
- * @return true if the ROI was found and could be deleted
- * false otherwise
- */
- bool deleteRegionOfInterest( std::string name );
-
- /** Deltes ROI with the given id
- * @param id ID of ROI to be deleted
- * @return true if the ROI was found and could be deleted
- * false otherwise
- */
- bool deleteRegionOfInterest( int id );
-
- /**
- * @brief place the current roi list
- * @param roilist new roi list
- */
- void replaceROIList(std::vector<homer_mapnav_msgs::RegionOfInterest> roilist);
-
-
- /** Returns current ROI list
- * @return the ROI list
- */
- std::vector< homer_mapnav_msgs::RegionOfInterest > getList();
-
- /**
- * @brief Publishes a RegionsOfInterest Message with current ROIs
-
- */
- void broadcastRoiList();
-
- /** Returns name of ROI
- * @param id of ROI
- * @return ROI name
- */
- std::string getROIName(int id);
-
- private:
-
- /** Looks for ROI with name in the list
- * @param id ID of the ROI
- */
- bool roiExists(int id );
-
- /** Looks for ROI with name in the list
- * @param name Name of the ROI
- */
- bool roiExists( std::string name );
-
-
- /**
- * @brief gets the highest id of all ROIs and save it into m_highest_id
- */
- void setHighestId();
-
- /** The copy constructor of the class.
- * It's kept private, because it will never be used.
- */
- RoiManager( const RoiManager& instance );
-
- /** Holds the ROI vector */
- std::vector< homer_mapnav_msgs::RegionOfInterest > m_Rois;
-
- /** publisher that publishes the current roi list */
- ros::Publisher m_ROIsPublisher;
-
- /** to set a new id, the current highest id is needed */
- int m_highest_id;
-
- tf::TransformListener tf_listener;
+public:
+ /** The constructor of the class. */
+ RoiManager(ros::NodeHandle* nh);
+
+ /** constructor initializing the roi list */
+ RoiManager(std::vector<homer_mapnav_msgs::RegionOfInterest> rois);
+
+ /** Does nothing. */
+ ~RoiManager()
+ {
+ }
+
+ /** Adds a new ROI to the list, in contrast to POIs, several ROIs with the
+ * same name are allowed
+ * @param roi RegionOfInterest message with the ROI to be added
+ * @return true if successful, false otherwise
+ */
+ bool
+ addRegionOfInterest(const homer_mapnav_msgs::RegionOfInterest::ConstPtr& roi);
+
+ /** Tests all Rois and returns a vector of ids in which the pose is
+ * @param pose Pose which is tested to be inside the Rois
+ * @return vector of ids in which the pose is
+ */
+ std::vector<homer_mapnav_msgs::RegionOfInterest>
+ pointInsideRegionOfInterest(const geometry_msgs::PointStamped point);
+
+ /** Replaces a ROI with a new one
+ * @param roi RegionOfInterest to be inserted
+ * the ROI with the same ID as the new one is first deleted
+ * @return true if the old ROI was found and could be deleted
+ * false otherwise
+ */
+ bool modifyRegionOfInterest(
+ const homer_mapnav_msgs::RegionOfInterest::ConstPtr& roi);
+
+ /** Deletes all ROIs with a certain name from the list
+ * @param name Name of the ROIs to be deleted
+ * @return true if the ROI was found and could be deleted
+ * false otherwise
+ */
+ bool deleteRegionOfInterest(std::string name);
+
+ /** Deltes ROI with the given id
+ * @param id ID of ROI to be deleted
+ * @return true if the ROI was found and could be deleted
+ * false otherwise
+ */
+ bool deleteRegionOfInterest(int id);
+
+ /**
+ * @brief place the current roi list
+ * @param roilist new roi list
+ */
+ void replaceROIList(std::vector<homer_mapnav_msgs::RegionOfInterest> roilist);
+
+ /** Returns current ROI list
+ * @return the ROI list
+ */
+ std::vector<homer_mapnav_msgs::RegionOfInterest> getList();
+
+ /**
+ * @brief Publishes a RegionsOfInterest Message with current ROIs
+
+ */
+ void broadcastRoiList();
+
+ /** Returns name of ROI
+ * @param id of ROI
+ * @return ROI name
+ */
+ std::string getROIName(int id);
+
+private:
+ /** Looks for ROI with name in the list
+ * @param id ID of the ROI
+ */
+ bool roiExists(int id);
+
+ /** Looks for ROI with name in the list
+ * @param name Name of the ROI
+ */
+ bool roiExists(std::string name);
+
+ /**
+ * @brief gets the highest id of all ROIs and save it into m_highest_id
+ */
+ void setHighestId();
+
+ /** The copy constructor of the class.
+ * It's kept private, because it will never be used.
+ */
+ RoiManager(const RoiManager& instance);
+
+ /** Holds the ROI vector */
+ std::vector<homer_mapnav_msgs::RegionOfInterest> m_Rois;
+
+ /** publisher that publishes the current roi list */
+ ros::Publisher m_ROIsPublisher;
+
+ /** to set a new id, the current highest id is needed */
+ int m_highest_id;
+
+ tf::TransformListener tf_listener;
};
#endif
diff --git a/homer_map_manager/include/homer_map_manager/MapIO/image_loader.h b/homer_map_manager/include/homer_map_manager/MapIO/image_loader.h
index 930bef390094bb07069cd3896ff6e51bcf8165b2..3ae64c44ad82d6909edb1179e54fa8a9dfd35faf 100644
--- a/homer_map_manager/include/homer_map_manager/MapIO/image_loader.h
+++ b/homer_map_manager/include/homer_map_manager/MapIO/image_loader.h
@@ -1,10 +1,10 @@
/*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
- *
+ *
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
- *
+ *
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
@@ -13,7 +13,7 @@
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
- *
+ *
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
@@ -37,9 +37,8 @@
namespace map_server
{
-
-/** Read the image from file and fill out the resp object, for later
- * use when our services are requested.
+/** Read the image from file and fill out the resp object, for later
+ * use when our services are requested.
*
* @param resp The map wil be written into here
* @param fname The image file to read from
@@ -52,9 +51,9 @@ namespace map_server
*
* @throws std::runtime_error If the image file can't be loaded
* */
-void loadMapFromFile(nav_msgs::OccupancyGrid* map,
- const char* fname, double res, bool negate,
- double occ_th, double free_th, double* origin);
+void loadMapFromFile(nav_msgs::OccupancyGrid* map, const char* fname,
+ double res, bool negate, double occ_th, double free_th,
+ double* origin);
}
#endif
diff --git a/homer_map_manager/include/homer_map_manager/MapIO/map_loader.h b/homer_map_manager/include/homer_map_manager/MapIO/map_loader.h
index ead7a619e411c171910a095e06a743be8693431f..3e2bf2f456f09233cf1ccf27305f1ff16f109b8c 100644
--- a/homer_map_manager/include/homer_map_manager/MapIO/map_loader.h
+++ b/homer_map_manager/include/homer_map_manager/MapIO/map_loader.h
@@ -29,21 +29,22 @@
/* Author: Brian Gerkey */
-#define USAGE "\nUSAGE: map_server <map.yaml>\n" \
- " map.yaml: map description file\n" \
- "DEPRECATED USAGE: map_server <map> <resolution>\n" \
- " map: image file to load\n"\
- " resolution: map resolution [meters/pixel]"
+#define USAGE \
+ "\nUSAGE: map_server <map.yaml>\n" \
+ " map.yaml: map description file\n" \
+ "DEPRECATED USAGE: map_server <map> <resolution>\n" \
+ " map: image file to load\n" \
+ " resolution: map resolution [meters/pixel]"
+#include <libgen.h>
#include <stdio.h>
#include <stdlib.h>
-#include <libgen.h>
#include <fstream>
-#include "ros/ros.h"
-#include "ros/console.h"
#include <homer_map_manager/MapIO/image_loader.h>
#include <nav_msgs/OccupancyGrid.h>
+#include "ros/console.h"
+#include "ros/ros.h"
#include "yaml-cpp/yaml.h"
#include <homer_mapnav_msgs/PointOfInterest.h>
@@ -51,29 +52,25 @@
class MapServer
{
- public:
- /** Trivial constructor */
- MapServer(const std::string fname, bool &success);
-
- nav_msgs::OccupancyGrid getSLAMMap();
-
- nav_msgs::OccupancyGrid getMaskingMap();
-
- std::vector<homer_mapnav_msgs::PointOfInterest> getPois();
+public:
+ /** Trivial constructor */
+ MapServer(const std::string fname, bool &success);
- std::vector<homer_mapnav_msgs::RegionOfInterest> getRois();
+ nav_msgs::OccupancyGrid getSLAMMap();
-
- private:
+ nav_msgs::OccupancyGrid getMaskingMap();
- /** The map data is cached here
- */
- nav_msgs::GetMap::Response map_resp_;
- nav_msgs::OccupancyGrid m_SLAMMap;
- nav_msgs::OccupancyGrid m_MaskingMap;
+ std::vector<homer_mapnav_msgs::PointOfInterest> getPois();
+ std::vector<homer_mapnav_msgs::RegionOfInterest> getRois();
- std::vector<homer_mapnav_msgs::PointOfInterest> poiList;
- std::vector<homer_mapnav_msgs::RegionOfInterest> roiList;
+private:
+ /** The map data is cached here
+ */
+ nav_msgs::GetMap::Response map_resp_;
+ nav_msgs::OccupancyGrid m_SLAMMap;
+ nav_msgs::OccupancyGrid m_MaskingMap;
+ std::vector<homer_mapnav_msgs::PointOfInterest> poiList;
+ std::vector<homer_mapnav_msgs::RegionOfInterest> roiList;
};
diff --git a/homer_map_manager/include/homer_map_manager/MapIO/map_saver.h b/homer_map_manager/include/homer_map_manager/MapIO/map_saver.h
index 3ae7e702c3b8351ecae7e271faeafec27bcbaf52..ab3da11f60a9cc29b0ad93eb9bb9d7f45e9f1e77 100644
--- a/homer_map_manager/include/homer_map_manager/MapIO/map_saver.h
+++ b/homer_map_manager/include/homer_map_manager/MapIO/map_saver.h
@@ -2,10 +2,10 @@
* map_saver
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
- *
+ *
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
- *
+ *
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
@@ -14,7 +14,7 @@
* * Neither the name of the <ORGANIZATION> nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
- *
+ *
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
@@ -29,9 +29,9 @@
*/
#include <cstdio>
-#include "ros/ros.h"
-#include "ros/console.h"
#include "nav_msgs/GetMap.h"
+#include "ros/console.h"
+#include "ros/ros.h"
//#include "LinearMath/btMatrix3x3.h"
#include "geometry_msgs/Quaternion.h"
@@ -39,27 +39,23 @@
#include <homer_mapnav_msgs/RegionOfInterest.h>
using namespace std;
-
+
/**
* @brief Map generation node.
*/
-class MapGenerator
+class MapGenerator
{
+public:
+ MapGenerator(const std::string mapname);
- public:
- MapGenerator(const std::string mapname);
+ void save(const nav_msgs::OccupancyGridConstPtr& SLAMMap,
+ const nav_msgs::OccupancyGridConstPtr& maskingMap,
+ std::vector<homer_mapnav_msgs::PointOfInterest> poiList,
+ std::vector<homer_mapnav_msgs::RegionOfInterest> roiList);
- void save(const nav_msgs::OccupancyGridConstPtr& SLAMMap,
- const nav_msgs::OccupancyGridConstPtr& maskingMap,
- std::vector<homer_mapnav_msgs::PointOfInterest> poiList,
- std::vector<homer_mapnav_msgs::RegionOfInterest> roiList );
-
- private:
- std::string m_Mapname;
-
- void saveMapLayer(const nav_msgs::OccupancyGridConstPtr& map, std::string fileName);
+private:
+ std::string m_Mapname;
+ void saveMapLayer(const nav_msgs::OccupancyGridConstPtr& map,
+ std::string fileName);
};
-
-
-
diff --git a/homer_map_manager/include/homer_map_manager/Workers/PointOfInterest/PointOfInterest.h b/homer_map_manager/include/homer_map_manager/Workers/PointOfInterest/PointOfInterest.h
index fcc9912eec7023670ad5c51a5c6c21f4f4f823a0..39f2adf483adc80d6ce761453eb593bc22d22883 100644
--- a/homer_map_manager/include/homer_map_manager/Workers/PointOfInterest/PointOfInterest.h
+++ b/homer_map_manager/include/homer_map_manager/Workers/PointOfInterest/PointOfInterest.h
@@ -1,16 +1,16 @@
#ifndef POINTOFINTEREST_H
#define POINTOFINTEREST_H
-#include <string>
#include <map>
+#include <string>
#include <homer_nav_libs/Math/Pose.h>
using namespace std;
-typedef std::map< std::string, std::string > StringMapT;
-typedef std::map< std::string, float > FloatMapT;
-typedef std::map< std::string, int > IntMapT;
+typedef std::map<std::string, std::string> StringMapT;
+typedef std::map<std::string, float> FloatMapT;
+typedef std::map<std::string, int> IntMapT;
/**
* @class PointOfInterest
@@ -25,160 +25,213 @@ typedef std::map< std::string, int > IntMapT;
* the world position and orientation.
*/
-class PointOfInterest: public Pose
+class PointOfInterest : public Pose
{
-
- public:
-
- enum PoiType { DEFAULT =100,
- VICTIM =200,
- OBJECT =300,
- GRIPPABLE_OBJECT =400,
- PERSON =600,
- ROOMBA =700,
- HAZARD_MATERIAL =800,
- START_POSITION =900,
- START_ORIENTATION =1000
- };
-
- PointOfInterest( );
-
- /**
- * @brief The constructor
- *
- * Creates a point of interest with orientation 0.0.
- *
- * @param id unique identification number
- * @param name name of the poi
- * @param type type of the poi (victim, etc.)
- * @param posX,posY position within the map
- * @param remarks additional information associated with the poi
- */
- PointOfInterest ( int id, string name, PoiType type, float posX, float posY, string remarks,
- StringMapT stringMap=StringMapT(), FloatMapT floatMap=FloatMapT() ,
- IntMapT intMap=IntMapT() )
- : Pose ( posX, posY, 0.0f )
- {
- init ( id, name, type , remarks, stringMap, floatMap, intMap );
- }
-
- /** @brief The constructor
- * @param id unique identification number
- * @param name name of the poi
- * @param type type of the poi (victim, etc.)
- * @param posX,posY position within the map
- * @param theta orientation of the poi
- * @param remarks additional information associated with the poi
- */
- PointOfInterest ( int id, string name, PoiType type, float posX, float posY, float theta, string remarks,
- StringMapT stringMap=StringMapT(), FloatMapT floatMap=FloatMapT(), IntMapT intMap=IntMapT() )
- : Pose ( posX, posY, theta )
- {
- init ( id, name, type , remarks, stringMap, floatMap, intMap );
- }
-
- /** @brief Alternative constructor omitting the id parameter */
- PointOfInterest ( string name, PoiType type, float posX, float posY , string remarks )
- : Pose ( posX, posY, 0.0f )
- {
- init ( -1, name, type , remarks, StringMapT(), FloatMapT(), IntMapT() );
- }
-
- /** @brief Alternative constructor omitting the id parameter */
- PointOfInterest ( string name, PoiType type, float posX, float posY , float theta, string remarks )
- : Pose ( posX, posY, theta )
- {
- init ( -1, name, type , remarks, StringMapT(), FloatMapT(), IntMapT() );
- }
-
- /** @brief Copy constructor, assigns a new id */
- PointOfInterest ( int id, const PointOfInterest* poi );
-
- /** @brief The standard destructor */
- virtual ~PointOfInterest() {}
-
- /** @brief Tests whether this PointOfInterest has the specified name or not, ignoring case.
- * @param name The name to test as a string.
- * @return true if the given name matches the PointOfInterest's name (not case-sensitive), false if not.
- */
- bool hasName ( string name ) const;
-
- /** @brief Tests whether this PointOfInterest has the specified string in its name or not, ignoring case.
- * @param part The string to test.
- * @return true if the given string is contained in the PointOfInterest's name (not case-sensitive), false if not.
- */
- bool hasInName ( string part ) const;
-
- // SETTER FUNCTIONS /////////////////////////////
-
- /** @param remarks Changes the contents of remarks attribute. */
- void setRemarks ( string remarks ) { m_Remarks = remarks; }
-
- void setName ( string name ) { m_Name = name; }
-
- /** @brief Add a new String to the StringMap */
- void addString ( string key, string data ) { m_StringMap.insert ( pair< string, string > ( key, data ) ); }
-
- /** @brief Add a new Float to the FloatMap */
- void addFloat ( string key, float data ) { m_FloatMap.insert ( pair< string, float > ( key, data ) ); }
-
- /** @brief Add a new Int to the IntMap */
- void addInt ( string key, int data ) { m_IntMap.insert ( pair< string, int > ( key, data ) ); }
-
- // GETTER FUNCTIONS //////////////////////
-
- /** @return id attribute */
- int getId() const { return m_Id; }
-
- /** @return name attribute */
- string getName() const { return m_Name; }
-
- /** @return type attribute */
- PoiType getType() const { return m_Type; }
-
- /** @return remarks attribute */
- string getRemarks() const { return m_Remarks; }
-
- /** Get the string to the matching string */
- string getString ( string key ) const { return m_StringMap.find ( key )->second; }
-
- /** Get the float to the matching string */
- float getFloat ( string key ) const { return m_FloatMap.find ( key )->second; }
-
- /** Get the Int to the matching string */
- int getInt ( string key ) const { return m_IntMap.find ( key )->second; }
-
- /** Get the string map */
- map<string, string> getStringMap() const { return m_StringMap; }
-
- /** Get the float map */
- map<string, float> getFloatMap() const { return m_FloatMap; }
-
- /** Get the int map */
- map<string, int> getIntMap() const { return m_IntMap; }
-
-
- // (DE)SERIALIZATION ///////////////////////////////////
- //TODO
- // void storer ( ExtendedOutStream& ) const ;
-// PointOfInterest ( ExtendedInStream& );
-
- /** @brief print description */
- void printOn ( ostream& strm ) const;
-
- private:
-
- void init ( int id, string name, PoiType type, string remarks, StringMapT stringMap, FloatMapT floatMap, IntMapT intMap );
-
- int m_Id;
- string m_Name;
- PoiType m_Type;
- string m_Remarks;
-
- StringMapT m_StringMap;
- IntMapT m_IntMap;
- FloatMapT m_FloatMap;
-
+public:
+ enum PoiType
+ {
+ DEFAULT = 100,
+ VICTIM = 200,
+ OBJECT = 300,
+ GRIPPABLE_OBJECT = 400,
+ PERSON = 600,
+ ROOMBA = 700,
+ HAZARD_MATERIAL = 800,
+ START_POSITION = 900,
+ START_ORIENTATION = 1000
+ };
+
+ PointOfInterest();
+
+ /**
+ * @brief The constructor
+ *
+ * Creates a point of interest with orientation 0.0.
+ *
+ * @param id unique identification number
+ * @param name name of the poi
+ * @param type type of the poi (victim, etc.)
+ * @param posX,posY position within the map
+ * @param remarks additional information associated with the poi
+ */
+ PointOfInterest(int id, string name, PoiType type, float posX, float posY,
+ string remarks, StringMapT stringMap = StringMapT(),
+ FloatMapT floatMap = FloatMapT(), IntMapT intMap = IntMapT())
+ : Pose(posX, posY, 0.0f)
+ {
+ init(id, name, type, remarks, stringMap, floatMap, intMap);
+ }
+
+ /** @brief The constructor
+ * @param id unique identification number
+ * @param name name of the poi
+ * @param type type of the poi (victim, etc.)
+ * @param posX,posY position within the map
+ * @param theta orientation of the poi
+ * @param remarks additional information associated with the poi
+ */
+ PointOfInterest(int id, string name, PoiType type, float posX, float posY,
+ float theta, string remarks,
+ StringMapT stringMap = StringMapT(),
+ FloatMapT floatMap = FloatMapT(), IntMapT intMap = IntMapT())
+ : Pose(posX, posY, theta)
+ {
+ init(id, name, type, remarks, stringMap, floatMap, intMap);
+ }
+
+ /** @brief Alternative constructor omitting the id parameter */
+ PointOfInterest(string name, PoiType type, float posX, float posY,
+ string remarks)
+ : Pose(posX, posY, 0.0f)
+ {
+ init(-1, name, type, remarks, StringMapT(), FloatMapT(), IntMapT());
+ }
+
+ /** @brief Alternative constructor omitting the id parameter */
+ PointOfInterest(string name, PoiType type, float posX, float posY,
+ float theta, string remarks)
+ : Pose(posX, posY, theta)
+ {
+ init(-1, name, type, remarks, StringMapT(), FloatMapT(), IntMapT());
+ }
+
+ /** @brief Copy constructor, assigns a new id */
+ PointOfInterest(int id, const PointOfInterest* poi);
+
+ /** @brief The standard destructor */
+ virtual ~PointOfInterest()
+ {
+ }
+
+ /** @brief Tests whether this PointOfInterest has the specified name or not,
+ * ignoring case.
+ * @param name The name to test as a string.
+ * @return true if the given name matches the PointOfInterest's name (not
+ * case-sensitive), false if not.
+ */
+ bool hasName(string name) const;
+
+ /** @brief Tests whether this PointOfInterest has the specified string in its
+ * name or not, ignoring case.
+ * @param part The string to test.
+ * @return true if the given string is contained in the PointOfInterest's name
+ * (not case-sensitive), false if not.
+ */
+ bool hasInName(string part) const;
+
+ // SETTER FUNCTIONS /////////////////////////////
+
+ /** @param remarks Changes the contents of remarks attribute. */
+ void setRemarks(string remarks)
+ {
+ m_Remarks = remarks;
+ }
+
+ void setName(string name)
+ {
+ m_Name = name;
+ }
+
+ /** @brief Add a new String to the StringMap */
+ void addString(string key, string data)
+ {
+ m_StringMap.insert(pair<string, string>(key, data));
+ }
+
+ /** @brief Add a new Float to the FloatMap */
+ void addFloat(string key, float data)
+ {
+ m_FloatMap.insert(pair<string, float>(key, data));
+ }
+
+ /** @brief Add a new Int to the IntMap */
+ void addInt(string key, int data)
+ {
+ m_IntMap.insert(pair<string, int>(key, data));
+ }
+
+ // GETTER FUNCTIONS //////////////////////
+
+ /** @return id attribute */
+ int getId() const
+ {
+ return m_Id;
+ }
+
+ /** @return name attribute */
+ string getName() const
+ {
+ return m_Name;
+ }
+
+ /** @return type attribute */
+ PoiType getType() const
+ {
+ return m_Type;
+ }
+
+ /** @return remarks attribute */
+ string getRemarks() const
+ {
+ return m_Remarks;
+ }
+
+ /** Get the string to the matching string */
+ string getString(string key) const
+ {
+ return m_StringMap.find(key)->second;
+ }
+
+ /** Get the float to the matching string */
+ float getFloat(string key) const
+ {
+ return m_FloatMap.find(key)->second;
+ }
+
+ /** Get the Int to the matching string */
+ int getInt(string key) const
+ {
+ return m_IntMap.find(key)->second;
+ }
+
+ /** Get the string map */
+ map<string, string> getStringMap() const
+ {
+ return m_StringMap;
+ }
+
+ /** Get the float map */
+ map<string, float> getFloatMap() const
+ {
+ return m_FloatMap;
+ }
+
+ /** Get the int map */
+ map<string, int> getIntMap() const
+ {
+ return m_IntMap;
+ }
+
+ // (DE)SERIALIZATION ///////////////////////////////////
+ // TODO
+ // void storer ( ExtendedOutStream& ) const ;
+ // PointOfInterest ( ExtendedInStream& );
+
+ /** @brief print description */
+ void printOn(ostream& strm) const;
+
+private:
+ void init(int id, string name, PoiType type, string remarks,
+ StringMapT stringMap, FloatMapT floatMap, IntMapT intMap);
+
+ int m_Id;
+ string m_Name;
+ PoiType m_Type;
+ string m_Remarks;
+
+ StringMapT m_StringMap;
+ IntMapT m_IntMap;
+ FloatMapT m_FloatMap;
};
#endif
diff --git a/homer_map_manager/include/homer_map_manager/map_manager_node.h b/homer_map_manager/include/homer_map_manager/map_manager_node.h
index 1346403661d77d4c5dec2746745ec6366eae1c3f..97e6e9680d1fc2e724e8262f7f3f8c2cbec6ce8d 100644
--- a/homer_map_manager/include/homer_map_manager/map_manager_node.h
+++ b/homer_map_manager/include/homer_map_manager/map_manager_node.h
@@ -5,171 +5,174 @@
#include <tf/transform_listener.h>
#include <homer_map_manager/Managers/MapManager.h>
+#include <homer_map_manager/Managers/MaskingManager.h>
#include <homer_map_manager/Managers/PoiManager.h>
#include <homer_map_manager/Managers/RoiManager.h>
-#include <homer_map_manager/Managers/MaskingManager.h>
-#include "std_msgs/String.h"
-#include "std_msgs/Int32.h"
-#include "std_msgs/Empty.h"
-#include "std_srvs/Empty.h"
-#include "nav_msgs/OccupancyGrid.h"
#include <homer_mapnav_msgs/GetPointsOfInterest.h>
-#include <homer_mapnav_msgs/GetRegionsOfInterest.h>
#include <homer_mapnav_msgs/GetRegionOfInterestName.h>
-#include <homer_mapnav_msgs/PointInsideRois.h>
-#include <homer_mapnav_msgs/ModifyPOI.h>
-#include <homer_mapnav_msgs/ModifyMap.h>
-#include <homer_mapnav_msgs/MapLayers.h>
-#include <homer_mapnav_msgs/SaveMap.h>
+#include <homer_mapnav_msgs/GetRegionsOfInterest.h>
#include <homer_mapnav_msgs/LoadMap.h>
+#include <homer_mapnav_msgs/MapLayers.h>
+#include <homer_mapnav_msgs/ModifyMap.h>
+#include <homer_mapnav_msgs/ModifyPOI.h>
+#include <homer_mapnav_msgs/PointInsideRois.h>
#include <homer_mapnav_msgs/RoiChange.h>
+#include <homer_mapnav_msgs/SaveMap.h>
#include <sensor_msgs/LaserScan.h>
+#include "nav_msgs/OccupancyGrid.h"
+#include "std_msgs/Empty.h"
+#include "std_msgs/Int32.h"
+#include "std_msgs/String.h"
+#include "std_srvs/Empty.h"
-#include <homer_map_manager/MapIO/map_saver.h>
#include <homer_map_manager/MapIO/map_loader.h>
+#include <homer_map_manager/MapIO/map_saver.h>
#include <homer_nav_libs/tools.h>
/** @class MapManagerNode
* @author Malte Knauf, Viktor Seib
- * @brief This class handles incoming and outgoing messages and redirects them to the according modules
+ * @brief This class handles incoming and outgoing messages and redirects them
+ * to the according modules
*/
class MapManagerNode
{
public:
- MapManagerNode(ros::NodeHandle* nh);
- ~MapManagerNode();
+ MapManagerNode(ros::NodeHandle* nh);
+ ~MapManagerNode();
private:
-
- /** callback functions */
-
- /** callbacks of MapManagerModule */
- void callbackSLAMMap( const nav_msgs::OccupancyGrid::ConstPtr& msg );
- void callbackSaveMap( const std_msgs::String::ConstPtr& msg );
- void callbackLoadMap( const std_msgs::String::ConstPtr& msg );
- void callbackMapVisibility( const homer_mapnav_msgs::MapLayers::ConstPtr& msg );
- void callbackResetMaps( const std_msgs::Empty::ConstPtr& msg );
-
- /** laser scan callback */
- void callbackLaserScan( const sensor_msgs::LaserScan::ConstPtr& msg );
- void callbackBackLaser( const sensor_msgs::LaserScan::ConstPtr& msg );
- void callbackFrontLaser(const sensor_msgs::LaserScan::ConstPtr& msg );
-
- /** callbacks of PointOfInterestManagerModule */
- void callbackAddPOI( const homer_mapnav_msgs::PointOfInterest::ConstPtr& msg );
- void callbackModifyPOI( const homer_mapnav_msgs::ModifyPOI::ConstPtr& msg );
- void callbackDeletePOI( const std_msgs::String::ConstPtr& msg );
-
- /** callbacks of RegionOfInterestManagerModule */
- void callbackAddROI( const homer_mapnav_msgs::RegionOfInterest::ConstPtr& msg );
- void callbackModifyROI( const homer_mapnav_msgs::RegionOfInterest::ConstPtr& msg );
- void callbackDeleteROIbyName( const std_msgs::String::ConstPtr& msg );
- void callbackDeleteROIbyID( const std_msgs::Int32::ConstPtr& msg );
-
-
- void callbackOctomapMap( const nav_msgs::OccupancyGrid::ConstPtr& msg);
- void callbackRapidMap( const nav_msgs::OccupancyGrid::ConstPtr& msg);
-
- /** callback of MaskingMapModule */
- void callbackModifyMap( const homer_mapnav_msgs::ModifyMap::ConstPtr& msg );
-
- /** service of PointOfInterestModule to get a list with all Points Of Interest */
- bool getPOIsService(homer_mapnav_msgs::GetPointsOfInterest::Request& req,
- homer_mapnav_msgs::GetPointsOfInterest::Response& res);
-
- /** service of RegionOfInterestModule to get a list with all Regions Of Interest */
- bool getROIsService(homer_mapnav_msgs::GetRegionsOfInterest::Request& req,
- homer_mapnav_msgs::GetRegionsOfInterest::Response& res);
-
- bool pointInsideRoisService(homer_mapnav_msgs::PointInsideRois::Request& req,
- homer_mapnav_msgs::PointInsideRois::Response& res);
-
- bool getROINameService(homer_mapnav_msgs::GetRegionOfInterestName::Request& req,
- homer_mapnav_msgs::GetRegionOfInterestName::Response& res);
-
- void poseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg);
-
- /** Service for saving a map**/
- bool saveMapService(homer_mapnav_msgs::SaveMap::Request& req,
- homer_mapnav_msgs::SaveMap::Response& res);
-
- /** Service for loading a map**/
- bool loadMapService(homer_mapnav_msgs::LoadMap::Request& req,
- homer_mapnav_msgs::LoadMap::Response& res);
-
- /** Service resetting the current map**/
- bool resetMapService(std_srvs::Empty::Request& req,
- std_srvs::Empty::Response& res);
- /** modules that are included in this node */
- MapManager* m_MapManager;
- PoiManager* m_POIManager;
- RoiManager* m_ROIManager;
- MaskingManager* m_MaskingManager;
-
- //subscriptions of MapManagerModule here
- ros::Subscriber m_SLAMMapSubscriber;
- ros::Subscriber m_SaveMapSubscriber;
- ros::Subscriber m_LoadMapSubscriber;
- ros::Subscriber m_MapVisibilitySubscriber;
- ros::Subscriber m_LaserScanSubscriber;
- ros::Subscriber m_BackLaserScanSubscriber;
- ros::Subscriber m_FrontLaserScanSubscriber;
-
- //subscriptions of PointOfInterestManagerModule
- ros::Subscriber m_AddPOISubscriber;
- ros::Subscriber m_ModifyPOISubscriber;
- ros::Subscriber m_DeletePOISubscriber;
-
- //subscriptions of RegionOfInterestManagerModule
- ros::Subscriber m_AddROISubscriber;
- ros::Subscriber m_ModifyROISubscriber;
- ros::Subscriber m_DeleteROIByIDSubscriber;
- ros::Subscriber m_DeleteROIByNameSubscriber;
- ros::Subscriber m_PoiInsideROISubscriber;
- ros::Subscriber m_PoseSubscriber;
- ros::Publisher m_RoiPollPublisher;
-
- //subscriptions of MaskingMapModule
- ros::Subscriber m_ModifyMapSubscriber;
- ros::Subscriber m_ResetMapsSubscriber;
-
- /** publisher for loaded maps */
- ros::Publisher m_LoadedMapPublisher;
-
- /** publisher to mask slam map */
- ros::Publisher m_MaskSlamMapPublisher;
-
- /** service of PointOfInterstModule */
- ros::ServiceServer m_GetPOIsService;
-
- /** service of RegionOfInterstModule */
- ros::ServiceServer m_GetROIsService;
- ros::ServiceServer m_GetROINameService;
- ros::ServiceServer m_PoiInsideROISService;
-
- /** Service for saving the current Map to a file **/
- ros::ServiceServer m_SaveMapService;
- /** Resetting the current map**/
- ros::ServiceServer m_ResetMapService;
- /** Service for loading a previously saved map **/
- ros::ServiceServer m_LoadMapService;
-
-
- tf::TransformListener m_TransformListener;
-
- /** timestamp of last incoming laser scan message */
- ros::Time m_LastLaserTime;
-
- ros::Subscriber m_OctomapSubscriber;
- ros::Subscriber m_RapidMapSubscriber;
-
- nav_msgs::OccupancyGrid::ConstPtr m_highSensitiveMap;
-
- bool m_roi_polling;
- std::vector<int> m_ids;
- ros::Time m_lastROIPoll;
- float m_roi_polling_time;
+ /** callback functions */
+
+ /** callbacks of MapManagerModule */
+ void callbackSLAMMap(const nav_msgs::OccupancyGrid::ConstPtr& msg);
+ void callbackSaveMap(const std_msgs::String::ConstPtr& msg);
+ void callbackLoadMap(const std_msgs::String::ConstPtr& msg);
+ void callbackMapVisibility(const homer_mapnav_msgs::MapLayers::ConstPtr& msg);
+ void callbackResetMaps(const std_msgs::Empty::ConstPtr& msg);
+
+ /** laser scan callback */
+ void callbackLaserScan(const sensor_msgs::LaserScan::ConstPtr& msg);
+ void callbackBackLaser(const sensor_msgs::LaserScan::ConstPtr& msg);
+ void callbackFrontLaser(const sensor_msgs::LaserScan::ConstPtr& msg);
+
+ /** callbacks of PointOfInterestManagerModule */
+ void callbackAddPOI(const homer_mapnav_msgs::PointOfInterest::ConstPtr& msg);
+ void callbackModifyPOI(const homer_mapnav_msgs::ModifyPOI::ConstPtr& msg);
+ void callbackDeletePOI(const std_msgs::String::ConstPtr& msg);
+
+ /** callbacks of RegionOfInterestManagerModule */
+ void callbackAddROI(const homer_mapnav_msgs::RegionOfInterest::ConstPtr& msg);
+ void
+ callbackModifyROI(const homer_mapnav_msgs::RegionOfInterest::ConstPtr& msg);
+ void callbackDeleteROIbyName(const std_msgs::String::ConstPtr& msg);
+ void callbackDeleteROIbyID(const std_msgs::Int32::ConstPtr& msg);
+
+ void callbackOctomapMap(const nav_msgs::OccupancyGrid::ConstPtr& msg);
+ void callbackRapidMap(const nav_msgs::OccupancyGrid::ConstPtr& msg);
+
+ /** callback of MaskingMapModule */
+ void callbackModifyMap(const homer_mapnav_msgs::ModifyMap::ConstPtr& msg);
+
+ /** service of PointOfInterestModule to get a list with all Points Of Interest
+ */
+ bool getPOIsService(homer_mapnav_msgs::GetPointsOfInterest::Request& req,
+ homer_mapnav_msgs::GetPointsOfInterest::Response& res);
+
+ /** service of RegionOfInterestModule to get a list with all Regions Of
+ * Interest */
+ bool getROIsService(homer_mapnav_msgs::GetRegionsOfInterest::Request& req,
+ homer_mapnav_msgs::GetRegionsOfInterest::Response& res);
+
+ bool
+ pointInsideRoisService(homer_mapnav_msgs::PointInsideRois::Request& req,
+ homer_mapnav_msgs::PointInsideRois::Response& res);
+
+ bool
+ getROINameService(homer_mapnav_msgs::GetRegionOfInterestName::Request& req,
+ homer_mapnav_msgs::GetRegionOfInterestName::Response& res);
+
+ void poseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg);
+
+ /** Service for saving a map**/
+ bool saveMapService(homer_mapnav_msgs::SaveMap::Request& req,
+ homer_mapnav_msgs::SaveMap::Response& res);
+
+ /** Service for loading a map**/
+ bool loadMapService(homer_mapnav_msgs::LoadMap::Request& req,
+ homer_mapnav_msgs::LoadMap::Response& res);
+
+ /** Service resetting the current map**/
+ bool resetMapService(std_srvs::Empty::Request& req,
+ std_srvs::Empty::Response& res);
+ /** modules that are included in this node */
+ MapManager* m_MapManager;
+ PoiManager* m_POIManager;
+ RoiManager* m_ROIManager;
+ MaskingManager* m_MaskingManager;
+
+ // subscriptions of MapManagerModule here
+ ros::Subscriber m_SLAMMapSubscriber;
+ ros::Subscriber m_SaveMapSubscriber;
+ ros::Subscriber m_LoadMapSubscriber;
+ ros::Subscriber m_MapVisibilitySubscriber;
+ ros::Subscriber m_LaserScanSubscriber;
+ ros::Subscriber m_BackLaserScanSubscriber;
+ ros::Subscriber m_FrontLaserScanSubscriber;
+
+ // subscriptions of PointOfInterestManagerModule
+ ros::Subscriber m_AddPOISubscriber;
+ ros::Subscriber m_ModifyPOISubscriber;
+ ros::Subscriber m_DeletePOISubscriber;
+
+ // subscriptions of RegionOfInterestManagerModule
+ ros::Subscriber m_AddROISubscriber;
+ ros::Subscriber m_ModifyROISubscriber;
+ ros::Subscriber m_DeleteROIByIDSubscriber;
+ ros::Subscriber m_DeleteROIByNameSubscriber;
+ ros::Subscriber m_PoiInsideROISubscriber;
+ ros::Subscriber m_PoseSubscriber;
+ ros::Publisher m_RoiPollPublisher;
+
+ // subscriptions of MaskingMapModule
+ ros::Subscriber m_ModifyMapSubscriber;
+ ros::Subscriber m_ResetMapsSubscriber;
+
+ /** publisher for loaded maps */
+ ros::Publisher m_LoadedMapPublisher;
+
+ /** publisher to mask slam map */
+ ros::Publisher m_MaskSlamMapPublisher;
+
+ /** service of PointOfInterstModule */
+ ros::ServiceServer m_GetPOIsService;
+
+ /** service of RegionOfInterstModule */
+ ros::ServiceServer m_GetROIsService;
+ ros::ServiceServer m_GetROINameService;
+ ros::ServiceServer m_PoiInsideROISService;
+
+ /** Service for saving the current Map to a file **/
+ ros::ServiceServer m_SaveMapService;
+ /** Resetting the current map**/
+ ros::ServiceServer m_ResetMapService;
+ /** Service for loading a previously saved map **/
+ ros::ServiceServer m_LoadMapService;
+
+ tf::TransformListener m_TransformListener;
+
+ /** timestamp of last incoming laser scan message */
+ ros::Time m_LastLaserTime;
+
+ ros::Subscriber m_OctomapSubscriber;
+ ros::Subscriber m_RapidMapSubscriber;
+
+ nav_msgs::OccupancyGrid::ConstPtr m_highSensitiveMap;
+
+ bool m_roi_polling;
+ std::vector<int> m_ids;
+ ros::Time m_lastROIPoll;
+ float m_roi_polling_time;
};
-#endif // MAP_MANAGER_NODE_H
+#endif // MAP_MANAGER_NODE_H
diff --git a/homer_map_manager/src/Managers/MapManager.cpp b/homer_map_manager/src/Managers/MapManager.cpp
index 40e1cc4575dfac9cdee571f14c79b160274ab664..5a23b3c6a4591e043e53d6067dd29fd3d3332e5f 100644
--- a/homer_map_manager/src/Managers/MapManager.cpp
+++ b/homer_map_manager/src/Managers/MapManager.cpp
@@ -1,73 +1,86 @@
#include <homer_map_manager/Managers/MapManager.h>
#include <omp.h>
-MapManager::MapManager(ros::NodeHandle *nh) {
- m_MapPublisher = nh->advertise<nav_msgs::OccupancyGrid>("/map", 1, true);
-
- m_map_layers.push_back(homer_mapnav_msgs::MapLayers::SLAM_LAYER);
- m_map_layers.push_back(homer_mapnav_msgs::MapLayers::KINECT_LAYER);
- m_map_layers.push_back(homer_mapnav_msgs::MapLayers::RAPID_MAP);
- m_map_layers.push_back(homer_mapnav_msgs::MapLayers::MASKING_LAYER);
- m_laser_layers.push_back(homer_mapnav_msgs::MapLayers::HOKUYO_FRONT);
- m_laser_layers.push_back(homer_mapnav_msgs::MapLayers::SICK_LAYER);
-
- // enable all map layers
-
- for (int i = 0; i < m_map_layers.size(); i++) {
- m_MapVisibility[m_map_layers[i]] = true;
- }
-
- for (int i = 0; i < m_laser_layers.size(); i++) {
- m_MapVisibility[m_laser_layers[i]] = true;
- }
-
- //try {
- //m_TransformListener.waitForTransform("/base_link", "/laser",
- //ros::Time(0), ros::Duration(3));
- //m_TransformListener.lookupTransform("/base_link", "/laser",
- //ros::Time(0), m_sick_transform);
- //m_TransformListener.lookupTransform("/base_link", "/hokuyo_front",
- //ros::Time(0), m_hokuyo_transform);
- //m_got_transform = true;
- //} catch (tf::TransformException ex) {
- //ROS_ERROR("%s", ex.what());
- //m_got_transform = false;
- //}
- geometry_msgs::PoseStamped pose;
- pose.pose.position.x = 0;
- pose.pose.position.y = 0;
- pose.pose.position.z = 0;
- pose.pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
-
- m_pose = boost::make_shared<geometry_msgs::PoseStamped>(pose);
+MapManager::MapManager(ros::NodeHandle *nh)
+{
+ m_MapPublisher = nh->advertise<nav_msgs::OccupancyGrid>("/map", 1, true);
+
+ m_map_layers.push_back(homer_mapnav_msgs::MapLayers::SLAM_LAYER);
+ m_map_layers.push_back(homer_mapnav_msgs::MapLayers::KINECT_LAYER);
+ m_map_layers.push_back(homer_mapnav_msgs::MapLayers::RAPID_MAP);
+ m_map_layers.push_back(homer_mapnav_msgs::MapLayers::MASKING_LAYER);
+ m_laser_layers.push_back(homer_mapnav_msgs::MapLayers::HOKUYO_FRONT);
+ m_laser_layers.push_back(homer_mapnav_msgs::MapLayers::SICK_LAYER);
+
+ // enable all map layers
+
+ for (int i = 0; i < m_map_layers.size(); i++)
+ {
+ m_MapVisibility[m_map_layers[i]] = true;
+ }
+
+ for (int i = 0; i < m_laser_layers.size(); i++)
+ {
+ m_MapVisibility[m_laser_layers[i]] = true;
+ }
+
+ // try {
+ // m_TransformListener.waitForTransform("/base_link", "/laser",
+ // ros::Time(0), ros::Duration(3));
+ // m_TransformListener.lookupTransform("/base_link", "/laser",
+ // ros::Time(0), m_sick_transform);
+ // m_TransformListener.lookupTransform("/base_link", "/hokuyo_front",
+ // ros::Time(0), m_hokuyo_transform);
+ // m_got_transform = true;
+ //} catch (tf::TransformException ex) {
+ // ROS_ERROR("%s", ex.what());
+ // m_got_transform = false;
+ //}
+ geometry_msgs::PoseStamped pose;
+ pose.pose.position.x = 0;
+ pose.pose.position.y = 0;
+ pose.pose.position.z = 0;
+ pose.pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
+
+ m_pose = boost::make_shared<geometry_msgs::PoseStamped>(pose);
}
-MapManager::~MapManager() {}
+MapManager::~MapManager()
+{
+}
void MapManager::updateMapLayer(int type,
- nav_msgs::OccupancyGrid::ConstPtr layer) {
- m_MapLayers[type] = layer;
- if (type == homer_mapnav_msgs::MapLayers::SLAM_LAYER) {
- sendMergedMap();
- }
+ nav_msgs::OccupancyGrid::ConstPtr layer)
+{
+ m_MapLayers[type] = layer;
+ if (type == homer_mapnav_msgs::MapLayers::SLAM_LAYER)
+ {
+ sendMergedMap();
+ }
}
-void MapManager::clearMapLayers() { m_MapLayers.clear(); }
+void MapManager::clearMapLayers()
+{
+ m_MapLayers.clear();
+}
-void MapManager::toggleMapVisibility(int type, bool state) {
- ROS_INFO_STREAM("MapManager: " << type << ": " << state);
- m_MapVisibility[type] = state;
+void MapManager::toggleMapVisibility(int type, bool state)
+{
+ ROS_INFO_STREAM("MapManager: " << type << ": " << state);
+ m_MapVisibility[type] = state;
}
void MapManager::updateLaser(int layer,
- const sensor_msgs::LaserScan::ConstPtr &msg) {
- m_laserLayers[layer] = msg;
+ const sensor_msgs::LaserScan::ConstPtr &msg)
+{
+ m_laserLayers[layer] = msg;
}
-nav_msgs::OccupancyGrid::ConstPtr MapManager::getMapLayer(int type) {
- if (m_MapLayers.find(type) == m_MapLayers.end())
- return nav_msgs::OccupancyGrid::ConstPtr();
- return m_MapLayers[type];
+nav_msgs::OccupancyGrid::ConstPtr MapManager::getMapLayer(int type)
+{
+ if (m_MapLayers.find(type) == m_MapLayers.end())
+ return nav_msgs::OccupancyGrid::ConstPtr();
+ return m_MapLayers[type];
}
/**
@@ -75,152 +88,158 @@ nav_msgs::OccupancyGrid::ConstPtr MapManager::getMapLayer(int type) {
* merged map layers to the gui node
*
*/
-void MapManager::sendMergedMap() {
- if (m_MapLayers.find(homer_mapnav_msgs::MapLayers::SLAM_LAYER) ==
- m_MapLayers.end()) {
- ROS_DEBUG_STREAM("SLAM map is missing!");
- return;
- }
- int k;
- nav_msgs::OccupancyGrid mergedMap(
- *(m_MapLayers[homer_mapnav_msgs::MapLayers::SLAM_LAYER]));
- // bake maps
- for (int j = 1; j < m_map_layers.size(); j++) {
- if (m_MapLayers.find(m_map_layers[j]) != m_MapLayers.end() &&
- m_MapVisibility[m_map_layers[j]]) {
- // calculating parallel on 8 threads
- omp_set_num_threads(8);
- size_t mapsize = m_MapLayers[m_map_layers[j]]->info.height *
- m_MapLayers[m_map_layers[j]]->info.width;
- const std::vector<signed char> *tempdata =
- &m_MapLayers[m_map_layers[j]]->data;
- const int frei = homer_mapnav_msgs::ModifyMap::FREE;
- signed char currentvalue = 0;
-#pragma omp parallel for private(currentvalue) shared(mapsize, tempdata, \
+void MapManager::sendMergedMap()
+{
+ if (m_MapLayers.find(homer_mapnav_msgs::MapLayers::SLAM_LAYER) ==
+ m_MapLayers.end())
+ {
+ ROS_DEBUG_STREAM("SLAM map is missing!");
+ return;
+ }
+ int k;
+ nav_msgs::OccupancyGrid mergedMap(
+ *(m_MapLayers[homer_mapnav_msgs::MapLayers::SLAM_LAYER]));
+ // bake maps
+ for (int j = 1; j < m_map_layers.size(); j++)
+ {
+ if (m_MapLayers.find(m_map_layers[j]) != m_MapLayers.end() &&
+ m_MapVisibility[m_map_layers[j]])
+ {
+ // calculating parallel on 8 threads
+ omp_set_num_threads(8);
+ size_t mapsize = m_MapLayers[m_map_layers[j]]->info.height *
+ m_MapLayers[m_map_layers[j]]->info.width;
+ const std::vector<signed char> *tempdata =
+ &m_MapLayers[m_map_layers[j]]->data;
+ const int frei = homer_mapnav_msgs::ModifyMap::FREE;
+ signed char currentvalue = 0;
+#pragma omp parallel for private(currentvalue) shared(mapsize, tempdata, \
mergedMap)
- for (int i = 0; i < mapsize; i++) {
- currentvalue = tempdata->at(i);
- if (currentvalue > 50 || currentvalue == frei) {
- mergedMap.data[i] = currentvalue;
- }
- }
+ for (int i = 0; i < mapsize; i++)
+ {
+ currentvalue = tempdata->at(i);
+ if (currentvalue > 50 || currentvalue == frei)
+ {
+ mergedMap.data[i] = currentvalue;
}
+ }
}
- // bake Lasers
-
- //try {
- //int data;
- //if (m_got_transform) {
- //for (int j = 0; j < m_laser_layers.size(); j++) {
- //if (m_laserLayers.find(m_laser_layers[j]) !=
- //m_laserLayers.end() &&
- //m_MapVisibility[m_laser_layers[j]]) {
- //if (m_laser_layers[j] ==
- //homer_mapnav_msgs::MapLayers::SICK_LAYER) {
- //data = homer_mapnav_msgs::ModifyMap::BLOCKED;
- //} else if (m_laser_layers[j] ==
- //homer_mapnav_msgs::MapLayers::HOKUYO_BACK) {
- //data = homer_mapnav_msgs::ModifyMap::HOKUYO;
- //} else if (m_laser_layers[j] ==
- //homer_mapnav_msgs::MapLayers::HOKUYO_FRONT) {
- //data = homer_mapnav_msgs::ModifyMap::HOKUYO;
- //}
- //tf::StampedTransform pose_transform;
- //m_TransformListener.waitForTransform(
- //"/map", "/base_link",
- //m_laserLayers[m_laser_layers[j]]->header.stamp,
- //ros::Duration(0.09));
- //m_TransformListener.lookupTransform(
- //"/map", "/base_link",
- //m_laserLayers[m_laser_layers[j]]->header.stamp,
- //pose_transform);
-
- //// pose_transform.setTransform(Transform);
-
- //for (int i = 0;
- //i < m_laserLayers[m_laser_layers[j]]->ranges.size();
- //i++) {
- //if (m_laserLayers[m_laser_layers[j]]->ranges[i] <
- //m_laserLayers[m_laser_layers[j]]->range_max &&
- //m_laserLayers[m_laser_layers[j]]->ranges[i] >
- //m_laserLayers[m_laser_layers[j]]->range_min) {
- //float alpha =
- //m_laserLayers[m_laser_layers[j]]->angle_min +
- //i *
- //m_laserLayers[m_laser_layers[j]]
- //->angle_increment;
- //geometry_msgs::Point point;
- //tf::Vector3 pin;
- //tf::Vector3 pout;
- //pin.setX(
- //cos(alpha) *
- //m_laserLayers[m_laser_layers[j]]->ranges[i]);
- //pin.setY(
- //sin(alpha) *
- //m_laserLayers[m_laser_layers[j]]->ranges[i]);
- //pin.setZ(0);
-
- //if (m_laser_layers[j] ==
- //homer_mapnav_msgs::MapLayers::SICK_LAYER) {
- //pout = pose_transform * m_sick_transform * pin;
- //} else if (m_laser_layers[j] ==
- //homer_mapnav_msgs::MapLayers::
- //HOKUYO_FRONT) {
- //pout =
- //pose_transform * m_hokuyo_transform * pin;
- //}
- //point.x = pout.x();
- //point.y = pout.y();
- //point.z = 0;
-
- //Eigen::Vector2i map_point = map_tools::toMapCoords(
- //point,
- //m_MapLayers
- //[homer_mapnav_msgs::MapLayers::SLAM_LAYER]
- //->info.origin,
- //m_MapLayers
- //[homer_mapnav_msgs::MapLayers::SLAM_LAYER]
- //->info.resolution);
- //k = map_point.y() *
- //m_MapLayers[homer_mapnav_msgs::MapLayers::
- //SLAM_LAYER]
- //->info.width +
- //map_point.x();
- //if (k < 0 ||
- //k > m_MapLayers[homer_mapnav_msgs::MapLayers::
- //SLAM_LAYER]
- //->data.size()) {
- //continue;
- //} else {
- //mergedMap.data[k] = data;
- //}
- //}
- //}
- //}
- //}
- //} else {
- //try {
- //if (m_TransformListener.waitForTransform("/base_link", "/laser",
- //ros::Time(0),
- //ros::Duration(0.1))) {
- //m_TransformListener.lookupTransform(
- //"/base_link", "/laser", ros::Time(0), m_sick_transform);
- //m_TransformListener.lookupTransform(
- //"/base_link", "/hokuyo_front", ros::Time(0),
- //m_hokuyo_transform);
- //m_got_transform = true;
- //}
- //} catch (tf::TransformException ex) {
- //ROS_ERROR("%s", ex.what());
- //m_got_transform = false;
- //}
- //}
- //} catch (tf::TransformException ex) {
- //ROS_ERROR_STREAM(ex.what());
- //}
- mergedMap.header.stamp = ros::Time::now();
- mergedMap.header.frame_id = "map";
-
- m_MapPublisher.publish(mergedMap);
- ROS_DEBUG_STREAM("Publishing map");
+ }
+ // bake Lasers
+
+ // try {
+ // int data;
+ // if (m_got_transform) {
+ // for (int j = 0; j < m_laser_layers.size(); j++) {
+ // if (m_laserLayers.find(m_laser_layers[j]) !=
+ // m_laserLayers.end() &&
+ // m_MapVisibility[m_laser_layers[j]]) {
+ // if (m_laser_layers[j] ==
+ // homer_mapnav_msgs::MapLayers::SICK_LAYER) {
+ // data = homer_mapnav_msgs::ModifyMap::BLOCKED;
+ //} else if (m_laser_layers[j] ==
+ // homer_mapnav_msgs::MapLayers::HOKUYO_BACK) {
+ // data = homer_mapnav_msgs::ModifyMap::HOKUYO;
+ //} else if (m_laser_layers[j] ==
+ // homer_mapnav_msgs::MapLayers::HOKUYO_FRONT) {
+ // data = homer_mapnav_msgs::ModifyMap::HOKUYO;
+ //}
+ // tf::StampedTransform pose_transform;
+ // m_TransformListener.waitForTransform(
+ //"/map", "/base_link",
+ // m_laserLayers[m_laser_layers[j]]->header.stamp,
+ // ros::Duration(0.09));
+ // m_TransformListener.lookupTransform(
+ //"/map", "/base_link",
+ // m_laserLayers[m_laser_layers[j]]->header.stamp,
+ // pose_transform);
+
+ //// pose_transform.setTransform(Transform);
+
+ // for (int i = 0;
+ // i < m_laserLayers[m_laser_layers[j]]->ranges.size();
+ // i++) {
+ // if (m_laserLayers[m_laser_layers[j]]->ranges[i] <
+ // m_laserLayers[m_laser_layers[j]]->range_max &&
+ // m_laserLayers[m_laser_layers[j]]->ranges[i] >
+ // m_laserLayers[m_laser_layers[j]]->range_min) {
+ // float alpha =
+ // m_laserLayers[m_laser_layers[j]]->angle_min +
+ // i *
+ // m_laserLayers[m_laser_layers[j]]
+ //->angle_increment;
+ // geometry_msgs::Point point;
+ // tf::Vector3 pin;
+ // tf::Vector3 pout;
+ // pin.setX(
+ // cos(alpha) *
+ // m_laserLayers[m_laser_layers[j]]->ranges[i]);
+ // pin.setY(
+ // sin(alpha) *
+ // m_laserLayers[m_laser_layers[j]]->ranges[i]);
+ // pin.setZ(0);
+
+ // if (m_laser_layers[j] ==
+ // homer_mapnav_msgs::MapLayers::SICK_LAYER) {
+ // pout = pose_transform * m_sick_transform * pin;
+ //} else if (m_laser_layers[j] ==
+ // homer_mapnav_msgs::MapLayers::
+ // HOKUYO_FRONT) {
+ // pout =
+ // pose_transform * m_hokuyo_transform * pin;
+ //}
+ // point.x = pout.x();
+ // point.y = pout.y();
+ // point.z = 0;
+
+ // Eigen::Vector2i map_point = map_tools::toMapCoords(
+ // point,
+ // m_MapLayers
+ //[homer_mapnav_msgs::MapLayers::SLAM_LAYER]
+ //->info.origin,
+ // m_MapLayers
+ //[homer_mapnav_msgs::MapLayers::SLAM_LAYER]
+ //->info.resolution);
+ // k = map_point.y() *
+ // m_MapLayers[homer_mapnav_msgs::MapLayers::
+ // SLAM_LAYER]
+ //->info.width +
+ // map_point.x();
+ // if (k < 0 ||
+ // k > m_MapLayers[homer_mapnav_msgs::MapLayers::
+ // SLAM_LAYER]
+ //->data.size()) {
+ // continue;
+ //} else {
+ // mergedMap.data[k] = data;
+ //}
+ //}
+ //}
+ //}
+ //}
+ //} else {
+ // try {
+ // if (m_TransformListener.waitForTransform("/base_link", "/laser",
+ // ros::Time(0),
+ // ros::Duration(0.1))) {
+ // m_TransformListener.lookupTransform(
+ //"/base_link", "/laser", ros::Time(0), m_sick_transform);
+ // m_TransformListener.lookupTransform(
+ //"/base_link", "/hokuyo_front", ros::Time(0),
+ // m_hokuyo_transform);
+ // m_got_transform = true;
+ //}
+ //} catch (tf::TransformException ex) {
+ // ROS_ERROR("%s", ex.what());
+ // m_got_transform = false;
+ //}
+ //}
+ //} catch (tf::TransformException ex) {
+ // ROS_ERROR_STREAM(ex.what());
+ //}
+ mergedMap.header.stamp = ros::Time::now();
+ mergedMap.header.frame_id = "map";
+
+ m_MapPublisher.publish(mergedMap);
+ ROS_DEBUG_STREAM("Publishing map");
}
diff --git a/homer_map_manager/src/Managers/MaskingManager.cpp b/homer_map_manager/src/Managers/MaskingManager.cpp
index e820dc900b317d7b93bd40f349878d2f3386ce15..460e3ae2b21e8cac36ec828edb4a4b26268d6d57 100644
--- a/homer_map_manager/src/Managers/MaskingManager.cpp
+++ b/homer_map_manager/src/Managers/MaskingManager.cpp
@@ -2,199 +2,229 @@
using namespace std;
-MaskingManager::MaskingManager(nav_msgs::MapMetaData mapInfo) {
- m_MaskingMap.info = mapInfo;
- m_MaskingMap.data.resize(m_MaskingMap.info.width *
- m_MaskingMap.info.height);
- std::fill(m_MaskingMap.data.begin(), m_MaskingMap.data.end(),
- homer_mapnav_msgs::ModifyMap::NOT_MASKED);
-
- m_SlamMap.info = mapInfo;
- m_SlamMap.data.resize(m_SlamMap.info.width * m_SlamMap.info.height);
- std::fill(m_SlamMap.data.begin(), m_SlamMap.data.end(),
- homer_mapnav_msgs::ModifyMap::NOT_MASKED);
+MaskingManager::MaskingManager(nav_msgs::MapMetaData mapInfo)
+{
+ m_MaskingMap.info = mapInfo;
+ m_MaskingMap.data.resize(m_MaskingMap.info.width * m_MaskingMap.info.height);
+ std::fill(m_MaskingMap.data.begin(), m_MaskingMap.data.end(),
+ homer_mapnav_msgs::ModifyMap::NOT_MASKED);
+
+ m_SlamMap.info = mapInfo;
+ m_SlamMap.data.resize(m_SlamMap.info.width * m_SlamMap.info.height);
+ std::fill(m_SlamMap.data.begin(), m_SlamMap.data.end(),
+ homer_mapnav_msgs::ModifyMap::NOT_MASKED);
}
-MaskingManager::~MaskingManager() {}
-
-nav_msgs::OccupancyGrid::ConstPtr MaskingManager::updateMapInfo(const nav_msgs::MapMetaData &mapInfo) {
- if (m_SlamMap.info.width < mapInfo.width || m_SlamMap.info.height < mapInfo.height) {
- m_SlamMap.info = mapInfo;
-
- int x_add_left =
- (m_MaskingMap.info.origin.position.x - mapInfo.origin.position.x + 0.025) /
- mapInfo.resolution;
- int y_add_up =
- (m_MaskingMap.info.origin.position.y - mapInfo.origin.position.y + 0.025) /
- mapInfo.resolution;
- ROS_INFO_STREAM("x add "<<x_add_left<<" y add "<<y_add_up);
-
- std::vector<signed char> tmpData = m_MaskingMap.data;
-
- m_MaskingMap.data.resize(mapInfo.width * mapInfo.height);
- std::fill(m_MaskingMap.data.begin(), m_MaskingMap.data.end(),
- homer_mapnav_msgs::ModifyMap::NOT_MASKED);
-
- for (int x = 0; x < m_MaskingMap.info.width; x++) {
- for (int y = 0; y < m_MaskingMap.info.height; y++) {
- int i = y * m_MaskingMap.info.width + x;
- int j = (y + y_add_up) * mapInfo.width + x + x_add_left;
- m_MaskingMap.data[j] = tmpData[i];
- }
- }
- m_MaskingMap.info = mapInfo;
- }
- return boost::make_shared<const ::nav_msgs::OccupancyGrid>(m_MaskingMap);
+MaskingManager::~MaskingManager()
+{
}
-nav_msgs::OccupancyGrid::ConstPtr MaskingManager::modifyMap(
- homer_mapnav_msgs::ModifyMap::ConstPtr msg) {
- // reset SLAM mask map before each masking
- m_SlamMap.data.resize(m_SlamMap.info.height * m_SlamMap.info.width);
- std::fill(m_SlamMap.data.begin(), m_SlamMap.data.end(),
- homer_mapnav_msgs::ModifyMap::NOT_MASKED);
- drawPolygon(msg->region, msg->maskAction, msg->mapLayer);
-
- nav_msgs::OccupancyGrid::ConstPtr ret;
- if (msg->mapLayer == 0) {
- ret = boost::make_shared<const ::nav_msgs::OccupancyGrid>(m_SlamMap);
- } else {
- ret = boost::make_shared<const ::nav_msgs::OccupancyGrid>(m_MaskingMap);
- }
- return ret;
-}
+nav_msgs::OccupancyGrid::ConstPtr
+MaskingManager::updateMapInfo(const nav_msgs::MapMetaData &mapInfo)
+{
+ if (m_SlamMap.info.width < mapInfo.width ||
+ m_SlamMap.info.height < mapInfo.height)
+ {
+ m_SlamMap.info = mapInfo;
-nav_msgs::OccupancyGrid::ConstPtr MaskingManager::resetMap() {
+ int x_add_left = (m_MaskingMap.info.origin.position.x -
+ mapInfo.origin.position.x + 0.025) /
+ mapInfo.resolution;
+ int y_add_up = (m_MaskingMap.info.origin.position.y -
+ mapInfo.origin.position.y + 0.025) /
+ mapInfo.resolution;
+ ROS_INFO_STREAM("x add " << x_add_left << " y add " << y_add_up);
+ std::vector<signed char> tmpData = m_MaskingMap.data;
- m_MaskingMap.info.width = 1;
- m_MaskingMap.info.height = 1;
- m_MaskingMap.info.resolution = 1;
- m_MaskingMap.info.origin.position.x = 0;
- m_MaskingMap.info.origin.position.y = 0;
- m_MaskingMap.data.resize(m_MaskingMap.info.width *
- m_MaskingMap.info.height);
+ m_MaskingMap.data.resize(mapInfo.width * mapInfo.height);
std::fill(m_MaskingMap.data.begin(), m_MaskingMap.data.end(),
homer_mapnav_msgs::ModifyMap::NOT_MASKED);
- m_SlamMap.info = m_MaskingMap.info;
- m_SlamMap.data.resize(m_SlamMap.info.width * m_SlamMap.info.height);
- std::fill(m_SlamMap.data.begin(), m_SlamMap.data.end(),
- homer_mapnav_msgs::ModifyMap::NOT_MASKED);
+ for (int x = 0; x < m_MaskingMap.info.width; x++)
+ {
+ for (int y = 0; y < m_MaskingMap.info.height; y++)
+ {
+ int i = y * m_MaskingMap.info.width + x;
+ int j = (y + y_add_up) * mapInfo.width + x + x_add_left;
+ m_MaskingMap.data[j] = tmpData[i];
+ }
+ }
+ m_MaskingMap.info = mapInfo;
+ }
+ return boost::make_shared<const ::nav_msgs::OccupancyGrid>(m_MaskingMap);
+}
- nav_msgs::OccupancyGrid::ConstPtr ret =
- boost::make_shared<const ::nav_msgs::OccupancyGrid>(m_MaskingMap);
- return ret;
+nav_msgs::OccupancyGrid::ConstPtr
+MaskingManager::modifyMap(homer_mapnav_msgs::ModifyMap::ConstPtr msg)
+{
+ // reset SLAM mask map before each masking
+ m_SlamMap.data.resize(m_SlamMap.info.height * m_SlamMap.info.width);
+ std::fill(m_SlamMap.data.begin(), m_SlamMap.data.end(),
+ homer_mapnav_msgs::ModifyMap::NOT_MASKED);
+ drawPolygon(msg->region, msg->maskAction, msg->mapLayer);
+
+ nav_msgs::OccupancyGrid::ConstPtr ret;
+ if (msg->mapLayer == 0)
+ {
+ ret = boost::make_shared<const ::nav_msgs::OccupancyGrid>(m_SlamMap);
+ }
+ else
+ {
+ ret = boost::make_shared<const ::nav_msgs::OccupancyGrid>(m_MaskingMap);
+ }
+ return ret;
}
-void MaskingManager::replaceMap(nav_msgs::OccupancyGrid map) {
- if (map.data.size() != 0)
- m_MaskingMap = map;
- else
- std::fill(m_MaskingMap.data.begin(), m_MaskingMap.data.end(),
- homer_mapnav_msgs::ModifyMap::NOT_MASKED);
+nav_msgs::OccupancyGrid::ConstPtr MaskingManager::resetMap()
+{
+ m_MaskingMap.info.width = 1;
+ m_MaskingMap.info.height = 1;
+ m_MaskingMap.info.resolution = 1;
+ m_MaskingMap.info.origin.position.x = 0;
+ m_MaskingMap.info.origin.position.y = 0;
+ m_MaskingMap.data.resize(m_MaskingMap.info.width * m_MaskingMap.info.height);
+ std::fill(m_MaskingMap.data.begin(), m_MaskingMap.data.end(),
+ homer_mapnav_msgs::ModifyMap::NOT_MASKED);
+
+ m_SlamMap.info = m_MaskingMap.info;
+ m_SlamMap.data.resize(m_SlamMap.info.width * m_SlamMap.info.height);
+ std::fill(m_SlamMap.data.begin(), m_SlamMap.data.end(),
+ homer_mapnav_msgs::ModifyMap::NOT_MASKED);
+
+ nav_msgs::OccupancyGrid::ConstPtr ret =
+ boost::make_shared<const ::nav_msgs::OccupancyGrid>(m_MaskingMap);
+ return ret;
+}
+
+void MaskingManager::replaceMap(nav_msgs::OccupancyGrid map)
+{
+ if (map.data.size() != 0)
+ m_MaskingMap = map;
+ else
+ std::fill(m_MaskingMap.data.begin(), m_MaskingMap.data.end(),
+ homer_mapnav_msgs::ModifyMap::NOT_MASKED);
}
void MaskingManager::drawPolygon(vector<geometry_msgs::Point> vertices,
- int value, int mapLayer) {
- if (vertices.size() == 0) {
- ROS_INFO_STREAM("No vertices given!");
- return;
- }
- // make temp. map
- std::vector<int> data(m_MaskingMap.info.width * m_MaskingMap.info.height);
- std::fill(data.begin(), data.end(), 0);
-
- // draw the lines surrounding the polygon
- for (unsigned int i = 0; i < vertices.size(); i++) {
- int i2 = (i + 1) % vertices.size();
- drawLine(data, vertices[i].x, vertices[i].y, vertices[i2].x,
- vertices[i2].y, 1);
- }
- // calculate a point in the middle of the polygon
- float midX = 0;
- float midY = 0;
- for (unsigned int i = 0; i < vertices.size(); i++) {
- midX += vertices[i].x;
- midY += vertices[i].y;
- }
- midX /= vertices.size();
- midY /= vertices.size();
- // fill polygon
- fillPolygon(data, (int)midX, (int)midY, 1);
-
- // copy polygon to masking map or slam map (according to parameter mapLayer)
- for (int i = 0; i < data.size(); i++) {
- if (data[i] != 0) {
- switch (mapLayer) {
- case 0: // SLAM map
- m_SlamMap.data[i] = value;
- break;
- case 1: // Kinect Map. apply masking to masking map
- case 2: // masking map
- m_MaskingMap.data[i] = value;
- break;
- }
- }
+ int value, int mapLayer)
+{
+ if (vertices.size() == 0)
+ {
+ ROS_INFO_STREAM("No vertices given!");
+ return;
+ }
+ // make temp. map
+ std::vector<int> data(m_MaskingMap.info.width * m_MaskingMap.info.height);
+ std::fill(data.begin(), data.end(), 0);
+
+ // draw the lines surrounding the polygon
+ for (unsigned int i = 0; i < vertices.size(); i++)
+ {
+ int i2 = (i + 1) % vertices.size();
+ drawLine(data, vertices[i].x, vertices[i].y, vertices[i2].x, vertices[i2].y,
+ 1);
+ }
+ // calculate a point in the middle of the polygon
+ float midX = 0;
+ float midY = 0;
+ for (unsigned int i = 0; i < vertices.size(); i++)
+ {
+ midX += vertices[i].x;
+ midY += vertices[i].y;
+ }
+ midX /= vertices.size();
+ midY /= vertices.size();
+ // fill polygon
+ fillPolygon(data, (int)midX, (int)midY, 1);
+
+ // copy polygon to masking map or slam map (according to parameter mapLayer)
+ for (int i = 0; i < data.size(); i++)
+ {
+ if (data[i] != 0)
+ {
+ switch (mapLayer)
+ {
+ case 0: // SLAM map
+ m_SlamMap.data[i] = value;
+ break;
+ case 1: // Kinect Map. apply masking to masking map
+ case 2: // masking map
+ m_MaskingMap.data[i] = value;
+ break;
+ }
}
+ }
}
void MaskingManager::drawLine(std::vector<int> &data, int startX, int startY,
- int endX, int endY, int value) {
- // bresenham algorithm
- int x, y, t, dist, xerr, yerr, dx, dy, incx, incy;
- // compute distances
- dx = endX - startX;
- dy = endY - startY;
-
- // compute increment
- if (dx < 0) {
- incx = -1;
- dx = -dx;
- } else {
- incx = dx ? 1 : 0;
- }
-
- if (dy < 0) {
- incy = -1;
- dy = -dy;
- } else {
- incy = dy ? 1 : 0;
+ int endX, int endY, int value)
+{
+ // bresenham algorithm
+ int x, y, t, dist, xerr, yerr, dx, dy, incx, incy;
+ // compute distances
+ dx = endX - startX;
+ dy = endY - startY;
+
+ // compute increment
+ if (dx < 0)
+ {
+ incx = -1;
+ dx = -dx;
+ }
+ else
+ {
+ incx = dx ? 1 : 0;
+ }
+
+ if (dy < 0)
+ {
+ incy = -1;
+ dy = -dy;
+ }
+ else
+ {
+ incy = dy ? 1 : 0;
+ }
+
+ // which distance is greater?
+ dist = (dx > dy) ? dx : dy;
+ // initializing
+ x = startX;
+ y = startY;
+ xerr = dx;
+ yerr = dy;
+
+ // compute cells
+ for (t = 0; t < dist; t++)
+ {
+ data[x + m_MaskingMap.info.width * y] = value;
+
+ xerr += dx;
+ yerr += dy;
+ if (xerr > dist)
+ {
+ xerr -= dist;
+ x += incx;
}
-
- // which distance is greater?
- dist = (dx > dy) ? dx : dy;
- // initializing
- x = startX;
- y = startY;
- xerr = dx;
- yerr = dy;
-
- // compute cells
- for (t = 0; t < dist; t++) {
- data[x + m_MaskingMap.info.width * y] = value;
-
- xerr += dx;
- yerr += dy;
- if (xerr > dist) {
- xerr -= dist;
- x += incx;
- }
- if (yerr > dist) {
- yerr -= dist;
- y += incy;
- }
+ if (yerr > dist)
+ {
+ yerr -= dist;
+ y += incy;
}
+ }
}
void MaskingManager::fillPolygon(std::vector<int> &data, int x, int y,
- int value) {
- int index = x + m_MaskingMap.info.width * y;
- if (value != data[index]) {
- data[index] = value;
- fillPolygon(data, x + 1, y, value);
- fillPolygon(data, x - 1, y, value);
- fillPolygon(data, x, y + 1, value);
- fillPolygon(data, x, y - 1, value);
- }
+ int value)
+{
+ int index = x + m_MaskingMap.info.width * y;
+ if (value != data[index])
+ {
+ data[index] = value;
+ fillPolygon(data, x + 1, y, value);
+ fillPolygon(data, x - 1, y, value);
+ fillPolygon(data, x, y + 1, value);
+ fillPolygon(data, x, y - 1, value);
+ }
}
diff --git a/homer_map_manager/src/Managers/PoiManager.cpp b/homer_map_manager/src/Managers/PoiManager.cpp
index c667432255963af8bbee06daec29459aff456e31..8aa6f611b5325b10c9fbe0673e2d1fff96c2e6fa 100644
--- a/homer_map_manager/src/Managers/PoiManager.cpp
+++ b/homer_map_manager/src/Managers/PoiManager.cpp
@@ -8,123 +8,141 @@
using namespace std;
-PoiManager::PoiManager(ros::NodeHandle *nh) {
- m_POIsPublisher = nh->advertise<homer_mapnav_msgs::PointsOfInterest>(
- "/map_manager/poi_list", 1, true);
- m_Pois.clear();
+PoiManager::PoiManager(ros::NodeHandle *nh)
+{
+ m_POIsPublisher = nh->advertise<homer_mapnav_msgs::PointsOfInterest>(
+ "/map_manager/poi_list", 1, true);
+ m_Pois.clear();
}
-PoiManager::PoiManager(std::vector<homer_mapnav_msgs::PointOfInterest> pois) {
- // copy POIs
- m_Pois = pois;
+PoiManager::PoiManager(std::vector<homer_mapnav_msgs::PointOfInterest> pois)
+{
+ // copy POIs
+ m_Pois = pois;
}
-std::vector<homer_mapnav_msgs::PointOfInterest> PoiManager::getList() {
- return m_Pois;
+std::vector<homer_mapnav_msgs::PointOfInterest> PoiManager::getList()
+{
+ return m_Pois;
}
bool PoiManager::addPointOfInterest(
- const homer_mapnav_msgs::PointOfInterest::ConstPtr &poi) {
- // make sure there's no POI with the same name
-
- if (poiExists(poi->name)) {
- ostringstream stream;
- stream << "Poi with name " << poi->name
- << " already exists! Doing nothing.";
- ROS_WARN_STREAM(stream.str());
- return false;
- }
+ const homer_mapnav_msgs::PointOfInterest::ConstPtr &poi)
+{
+ // make sure there's no POI with the same name
+
+ if (poiExists(poi->name))
+ {
+ ostringstream stream;
+ stream << "Poi with name " << poi->name << " already exists! Doing "
+ "nothing.";
+ ROS_WARN_STREAM(stream.str());
+ return false;
+ }
- // copy poi & assigning new id
- homer_mapnav_msgs::PointOfInterest new_poi = *poi;
+ // copy poi & assigning new id
+ homer_mapnav_msgs::PointOfInterest new_poi = *poi;
- ROS_INFO_STREAM("Adding POI '" << new_poi.name << "'.");
+ ROS_INFO_STREAM("Adding POI '" << new_poi.name << "'.");
- // insert into list
- m_Pois.push_back(new_poi);
+ // insert into list
+ m_Pois.push_back(new_poi);
- broadcastPoiList();
- return true;
+ broadcastPoiList();
+ return true;
}
bool PoiManager::modifyPointOfInterest(
- const homer_mapnav_msgs::ModifyPOI::ConstPtr &poi) {
- std::string name = poi->old_name;
+ const homer_mapnav_msgs::ModifyPOI::ConstPtr &poi)
+{
+ std::string name = poi->old_name;
- std::vector<homer_mapnav_msgs::PointOfInterest>::iterator it;
+ std::vector<homer_mapnav_msgs::PointOfInterest>::iterator it;
- for (it = m_Pois.begin(); it != m_Pois.end(); it++) {
- if (it->name == name) {
- *it = poi->poi;
- broadcastPoiList();
- return true;
- }
+ for (it = m_Pois.begin(); it != m_Pois.end(); it++)
+ {
+ if (it->name == name)
+ {
+ *it = poi->poi;
+ broadcastPoiList();
+ return true;
}
+ }
- ROS_ERROR_STREAM("Cannot modify: POI does not exist!");
+ ROS_ERROR_STREAM("Cannot modify: POI does not exist!");
- return false;
+ return false;
}
void PoiManager::replacePOIList(
- std::vector<homer_mapnav_msgs::PointOfInterest> poilist) {
- m_Pois = poilist;
- broadcastPoiList();
+ std::vector<homer_mapnav_msgs::PointOfInterest> poilist)
+{
+ m_Pois = poilist;
+ broadcastPoiList();
}
-bool PoiManager::poiExists(std::string name) {
- std::vector<homer_mapnav_msgs::PointOfInterest>::iterator it;
+bool PoiManager::poiExists(std::string name)
+{
+ std::vector<homer_mapnav_msgs::PointOfInterest>::iterator it;
- for (it = m_Pois.begin(); it != m_Pois.end(); it++) {
- if (it->name == name) {
- return true;
- }
+ for (it = m_Pois.begin(); it != m_Pois.end(); it++)
+ {
+ if (it->name == name)
+ {
+ return true;
}
+ }
- return false;
+ return false;
}
-bool PoiManager::deletePointOfInterest(std::string name) {
- std::vector<homer_mapnav_msgs::PointOfInterest>::iterator it;
+bool PoiManager::deletePointOfInterest(std::string name)
+{
+ std::vector<homer_mapnav_msgs::PointOfInterest>::iterator it;
- for (it = m_Pois.begin(); it != m_Pois.end();) // it++ ) Iterator darf nur
- // dann erhöht werden, wenn
- // kein POI gelöscht wird.
+ for (it = m_Pois.begin(); it != m_Pois.end();) // it++ ) Iterator darf nur
+ // dann erhöht werden, wenn
+ // kein POI gelöscht wird.
+ {
+ if (it->name == name)
{
- if (it->name == name) {
- ROS_INFO_STREAM("Erasing POI " << name << ".");
-
- // ! Achtung !
- // Wenn letztes Element gelöscht wird, wird it auf .end() gesetzt
- // der nachfolgende Aufruf von it++ führt zu einem Fehler.
- // Daher Iterator nur erhöhen, wenn kein erase durchgeführt wurde.
-
- it = m_Pois.erase(it);
- broadcastPoiList();
- return true;
- } else {
- it++;
- }
+ ROS_INFO_STREAM("Erasing POI " << name << ".");
+
+ // ! Achtung !
+ // Wenn letztes Element gelöscht wird, wird it auf .end() gesetzt
+ // der nachfolgende Aufruf von it++ führt zu einem Fehler.
+ // Daher Iterator nur erhöhen, wenn kein erase durchgeführt wurde.
+
+ it = m_Pois.erase(it);
+ broadcastPoiList();
+ return true;
+ }
+ else
+ {
+ it++;
}
+ }
- ROS_ERROR_STREAM("POI " << name << " does not exist.");
+ ROS_ERROR_STREAM("POI " << name << " does not exist.");
- return false;
+ return false;
}
-void PoiManager::broadcastPoiList() {
- ostringstream stream;
- // print the current list
- std::vector<homer_mapnav_msgs::PointOfInterest>::iterator it;
- stream << "Contents of POI list:\n";
- homer_mapnav_msgs::PointsOfInterest poiMsg;
- for (it = m_Pois.begin(); it != m_Pois.end(); it++) {
- stream << " POI " << it->name << "', " << it->type << ", ("
- << it->pose.position.x << "," << it->pose.position.y << "), '"
- << it->remarks << "'\n";
- }
- poiMsg.pois = m_Pois;
- ros::Rate poll_rate(10);
- m_POIsPublisher.publish(poiMsg);
- ROS_DEBUG_STREAM(stream.str());
+void PoiManager::broadcastPoiList()
+{
+ ostringstream stream;
+ // print the current list
+ std::vector<homer_mapnav_msgs::PointOfInterest>::iterator it;
+ stream << "Contents of POI list:\n";
+ homer_mapnav_msgs::PointsOfInterest poiMsg;
+ for (it = m_Pois.begin(); it != m_Pois.end(); it++)
+ {
+ stream << " POI " << it->name << "', " << it->type << ", ("
+ << it->pose.position.x << "," << it->pose.position.y << "), '"
+ << it->remarks << "'\n";
+ }
+ poiMsg.pois = m_Pois;
+ ros::Rate poll_rate(10);
+ m_POIsPublisher.publish(poiMsg);
+ ROS_DEBUG_STREAM(stream.str());
}
diff --git a/homer_map_manager/src/Managers/RoiManager.cpp b/homer_map_manager/src/Managers/RoiManager.cpp
index f30810c51c20136c980bb38adc69247ded96dcbf..f88432241041055e26dac274bfdf9c0d72965455 100644
--- a/homer_map_manager/src/Managers/RoiManager.cpp
+++ b/homer_map_manager/src/Managers/RoiManager.cpp
@@ -1,233 +1,275 @@
#include <homer_map_manager/Managers/RoiManager.h>
-RoiManager::RoiManager(ros::NodeHandle *nh) {
- m_ROIsPublisher = nh->advertise<homer_mapnav_msgs::RegionsOfInterest>(
- "/map_manager/roi_list", 1, true);
- m_highest_id = 0; // start with 0 so first ROI gets the 1
- m_Rois.clear();
+RoiManager::RoiManager(ros::NodeHandle *nh)
+{
+ m_ROIsPublisher = nh->advertise<homer_mapnav_msgs::RegionsOfInterest>(
+ "/map_manager/roi_list", 1, true);
+ m_highest_id = 0; // start with 0 so first ROI gets the 1
+ m_Rois.clear();
}
-RoiManager::RoiManager(std::vector<homer_mapnav_msgs::RegionOfInterest> rois) {
- m_Rois = rois;
+RoiManager::RoiManager(std::vector<homer_mapnav_msgs::RegionOfInterest> rois)
+{
+ m_Rois = rois;
}
-std::vector<homer_mapnav_msgs::RegionOfInterest> RoiManager::getList() {
- return m_Rois;
+std::vector<homer_mapnav_msgs::RegionOfInterest> RoiManager::getList()
+{
+ return m_Rois;
}
-std::string RoiManager::getROIName(int id) {
- if (roiExists(id)) {
- std::vector<homer_mapnav_msgs::RegionOfInterest>::iterator it;
- for (it = m_Rois.begin(); it != m_Rois.end(); it++) {
- if (it->id == id) {
- return it->name;
- }
- }
- return "";
- } else {
- return "";
+std::string RoiManager::getROIName(int id)
+{
+ if (roiExists(id))
+ {
+ std::vector<homer_mapnav_msgs::RegionOfInterest>::iterator it;
+ for (it = m_Rois.begin(); it != m_Rois.end(); it++)
+ {
+ if (it->id == id)
+ {
+ return it->name;
+ }
}
+ return "";
+ }
+ else
+ {
+ return "";
+ }
}
std::vector<homer_mapnav_msgs::RegionOfInterest>
-RoiManager::pointInsideRegionOfInterest(
- const geometry_msgs::PointStamped point) {
- std::vector<homer_mapnav_msgs::RegionOfInterest> rois;
- geometry_msgs::PointStamped mpoint;
- tf_listener.waitForTransform("/map", point.header.frame_id,
- ros::Time::now(), ros::Duration(5.0));
- tf_listener.transformPoint("/map", point, mpoint);
- std::vector<homer_mapnav_msgs::RegionOfInterest>::iterator it;
- bool inside = false;
- for (it = m_Rois.begin(); it != m_Rois.end(); it++) {
- inside = false;
- int i, j;
- // code idea from
- // http://www.ecse.rpi.edu/Homepages/wrf/Research/Short_Notes/pnpoly.html
- j = it->points.size() - 1;
- for (i = 0; i < it->points.size(); i++) {
- if (((it->points[i].y > mpoint.point.y) !=
- (it->points[j].y > mpoint.point.y)) &&
- (mpoint.point.x <
- (it->points[j].x - it->points[i].x) *
- (float)(mpoint.point.y - it->points[i].y) /
- (it->points[j].y - it->points[i].y) +
- it->points[i].x)) {
- inside = !inside;
- }
- j = i;
- }
- if (inside) {
- rois.push_back(*it);
- }
+RoiManager::pointInsideRegionOfInterest(const geometry_msgs::PointStamped point)
+{
+ std::vector<homer_mapnav_msgs::RegionOfInterest> rois;
+ geometry_msgs::PointStamped mpoint;
+ tf_listener.waitForTransform("/map", point.header.frame_id, ros::Time::now(),
+ ros::Duration(5.0));
+ tf_listener.transformPoint("/map", point, mpoint);
+ std::vector<homer_mapnav_msgs::RegionOfInterest>::iterator it;
+ bool inside = false;
+ for (it = m_Rois.begin(); it != m_Rois.end(); it++)
+ {
+ inside = false;
+ int i, j;
+ // code idea from
+ // http://www.ecse.rpi.edu/Homepages/wrf/Research/Short_Notes/pnpoly.html
+ j = it->points.size() - 1;
+ for (i = 0; i < it->points.size(); i++)
+ {
+ if (((it->points[i].y > mpoint.point.y) !=
+ (it->points[j].y > mpoint.point.y)) &&
+ (mpoint.point.x < (it->points[j].x - it->points[i].x) *
+ (float)(mpoint.point.y - it->points[i].y) /
+ (it->points[j].y - it->points[i].y) +
+ it->points[i].x))
+ {
+ inside = !inside;
+ }
+ j = i;
}
- return rois;
+ if (inside)
+ {
+ rois.push_back(*it);
+ }
+ }
+ return rois;
}
bool RoiManager::addRegionOfInterest(
- const homer_mapnav_msgs::RegionOfInterest::ConstPtr &roi) {
- ROS_INFO_STREAM("Recieved new roi.");
- if (roiExists(roi->id)) {
- ROS_INFO_STREAM("id exists");
- std::ostringstream stream;
- stream << "Roi with ID " << roi->id << " (name: " << roi->name
- << ") already exists! Modifiying Roi.";
- ROS_WARN_STREAM(stream.str());
- return modifyRegionOfInterest(roi);
- } else {
- ROS_INFO_STREAM("new id");
- // copy roi & assigning new id
- homer_mapnav_msgs::RegionOfInterest new_roi = *roi;
- new_roi.id = (m_highest_id + 1);
-
- ROS_INFO_STREAM("Adding ROI '" << new_roi.name << "' with ID "
- << new_roi.id << ".");
-
- // insert into list
- m_Rois.push_back(new_roi);
-
- setHighestId();
- broadcastRoiList();
- return true;
- }
+ const homer_mapnav_msgs::RegionOfInterest::ConstPtr &roi)
+{
+ ROS_INFO_STREAM("Recieved new roi.");
+ if (roiExists(roi->id))
+ {
+ ROS_INFO_STREAM("id exists");
+ std::ostringstream stream;
+ stream << "Roi with ID " << roi->id << " (name: " << roi->name
+ << ") already exists! Modifiying Roi.";
+ ROS_WARN_STREAM(stream.str());
+ return modifyRegionOfInterest(roi);
+ }
+ else
+ {
+ ROS_INFO_STREAM("new id");
+ // copy roi & assigning new id
+ homer_mapnav_msgs::RegionOfInterest new_roi = *roi;
+ new_roi.id = (m_highest_id + 1);
+
+ ROS_INFO_STREAM("Adding ROI '" << new_roi.name << "' with ID " << new_roi.id
+ << ".");
+
+ // insert into list
+ m_Rois.push_back(new_roi);
+
+ setHighestId();
+ broadcastRoiList();
+ return true;
+ }
}
bool RoiManager::modifyRegionOfInterest(
- const homer_mapnav_msgs::RegionOfInterest::ConstPtr &roi) {
- std::vector<homer_mapnav_msgs::RegionOfInterest>::iterator it;
+ const homer_mapnav_msgs::RegionOfInterest::ConstPtr &roi)
+{
+ std::vector<homer_mapnav_msgs::RegionOfInterest>::iterator it;
- for (it = m_Rois.begin(); it != m_Rois.end(); it++) {
- if (it->id == roi->id) {
- *it = *roi;
- setHighestId();
- broadcastRoiList();
- return true;
- }
+ for (it = m_Rois.begin(); it != m_Rois.end(); it++)
+ {
+ if (it->id == roi->id)
+ {
+ *it = *roi;
+ setHighestId();
+ broadcastRoiList();
+ return true;
}
+ }
- ROS_ERROR_STREAM("Cannot modify: ROI does not exist!");
+ ROS_ERROR_STREAM("Cannot modify: ROI does not exist!");
- return false;
+ return false;
}
void RoiManager::replaceROIList(
- std::vector<homer_mapnav_msgs::RegionOfInterest> roilist) {
- m_Rois = roilist;
- setHighestId();
- broadcastRoiList();
+ std::vector<homer_mapnav_msgs::RegionOfInterest> roilist)
+{
+ m_Rois = roilist;
+ setHighestId();
+ broadcastRoiList();
}
-bool RoiManager::roiExists(int id) {
- ROS_INFO_STREAM("ID: " << id);
- ROS_INFO_STREAM("roi exists?");
- ROS_INFO_STREAM("Number Rois: ");
- ROS_INFO_STREAM(m_Rois.size());
- if (m_Rois.size() != 0) {
- std::vector<homer_mapnav_msgs::RegionOfInterest>::iterator it;
-
- for (it = m_Rois.begin(); it != m_Rois.end(); it++) {
- if (it->id == id) {
- return true;
- }
- }
- }
- ROS_INFO_STREAM("Return false");
- return false;
-}
+bool RoiManager::roiExists(int id)
+{
+ ROS_INFO_STREAM("ID: " << id);
+ ROS_INFO_STREAM("roi exists?");
+ ROS_INFO_STREAM("Number Rois: ");
+ ROS_INFO_STREAM(m_Rois.size());
+ if (m_Rois.size() != 0)
+ {
+ std::vector<homer_mapnav_msgs::RegionOfInterest>::iterator it;
-bool RoiManager::roiExists(std::string name) {
- ROS_INFO_STREAM("name: " << name);
- ROS_INFO_STREAM("roi exists?");
- ROS_INFO_STREAM("Number Rois: ");
- ROS_INFO_STREAM(m_Rois.size());
- if (m_Rois.size() != 0) {
- std::vector<homer_mapnav_msgs::RegionOfInterest>::iterator it;
-
- for (it = m_Rois.begin(); it != m_Rois.end(); it++) {
- if (it->name == name) {
- return true;
- }
- }
+ for (it = m_Rois.begin(); it != m_Rois.end(); it++)
+ {
+ if (it->id == id)
+ {
+ return true;
+ }
}
- ROS_INFO_STREAM("Return false");
- return false;
+ }
+ ROS_INFO_STREAM("Return false");
+ return false;
}
-bool RoiManager::deleteRegionOfInterest(std::string name) {
- ROS_INFO_STREAM("Delete Roi with name: " << name);
+bool RoiManager::roiExists(std::string name)
+{
+ ROS_INFO_STREAM("name: " << name);
+ ROS_INFO_STREAM("roi exists?");
+ ROS_INFO_STREAM("Number Rois: ");
+ ROS_INFO_STREAM(m_Rois.size());
+ if (m_Rois.size() != 0)
+ {
std::vector<homer_mapnav_msgs::RegionOfInterest>::iterator it;
- bool modified = false;
- for (it = m_Rois.begin(); it != m_Rois.end();) {
- if (it->name == name) {
- ROS_INFO_STREAM("Erasing all ROIs with name " << name << ".");
- it = m_Rois.erase(it);
- modified = true;
- } else {
- it++;
- }
+ for (it = m_Rois.begin(); it != m_Rois.end(); it++)
+ {
+ if (it->name == name)
+ {
+ return true;
+ }
}
+ }
+ ROS_INFO_STREAM("Return false");
+ return false;
+}
- if (modified) {
- ROS_INFO_STREAM("modified");
- broadcastRoiList();
- return true;
+bool RoiManager::deleteRegionOfInterest(std::string name)
+{
+ ROS_INFO_STREAM("Delete Roi with name: " << name);
+ std::vector<homer_mapnav_msgs::RegionOfInterest>::iterator it;
+
+ bool modified = false;
+ for (it = m_Rois.begin(); it != m_Rois.end();)
+ {
+ if (it->name == name)
+ {
+ ROS_INFO_STREAM("Erasing all ROIs with name " << name << ".");
+ it = m_Rois.erase(it);
+ modified = true;
+ }
+ else
+ {
+ it++;
}
+ }
- ROS_ERROR_STREAM("ROI " << name << " does not exist.");
+ if (modified)
+ {
+ ROS_INFO_STREAM("modified");
+ broadcastRoiList();
+ return true;
+ }
+
+ ROS_ERROR_STREAM("ROI " << name << " does not exist.");
- return false;
+ return false;
}
-bool RoiManager::deleteRegionOfInterest(int id) {
- std::vector<homer_mapnav_msgs::RegionOfInterest>::iterator it;
+bool RoiManager::deleteRegionOfInterest(int id)
+{
+ std::vector<homer_mapnav_msgs::RegionOfInterest>::iterator it;
- for (it = m_Rois.begin(); it != m_Rois.end(); it++) {
- if (it->id == id) {
- ROS_INFO_STREAM("Erasing ROI with ID " << id << ".");
- it = m_Rois.erase(it);
- broadcastRoiList();
- return true;
- }
+ for (it = m_Rois.begin(); it != m_Rois.end(); it++)
+ {
+ if (it->id == id)
+ {
+ ROS_INFO_STREAM("Erasing ROI with ID " << id << ".");
+ it = m_Rois.erase(it);
+ broadcastRoiList();
+ return true;
}
+ }
- ROS_ERROR_STREAM("ROI with ID " << id << " does not exist.");
+ ROS_ERROR_STREAM("ROI with ID " << id << " does not exist.");
- return false;
+ return false;
}
-void RoiManager::broadcastRoiList() {
- std::ostringstream stream;
- ROS_INFO_STREAM("Broadcast ROI.");
- // print the current list
- std::vector<homer_mapnav_msgs::RegionOfInterest>::iterator it;
- stream << "Contents of ROI list:\n";
- homer_mapnav_msgs::RegionsOfInterest roiMsg;
- for (it = m_Rois.begin(); it != m_Rois.end(); it++) {
- stream << " ROI '" << it->name << "', '" << it->type << "', [ "
- << it->points[0].x << " " << it->points[0].y << " ; "
- << " " << it->points[1].x << " " << it->points[1].y << " ; "
- << " " << it->points[2].x << " " << it->points[2].y << " ; "
- << " " << it->points[3].x << " " << it->points[3].y << " ], '"
- << it->remarks << "'\n";
- }
- // ROS_INFO_STREAM( stream.str() );
- roiMsg.rois = m_Rois;
- ROS_INFO_STREAM("roiMsg.rois");
- m_ROIsPublisher.publish(roiMsg);
- ROS_DEBUG_STREAM(stream.str());
+void RoiManager::broadcastRoiList()
+{
+ std::ostringstream stream;
+ ROS_INFO_STREAM("Broadcast ROI.");
+ // print the current list
+ std::vector<homer_mapnav_msgs::RegionOfInterest>::iterator it;
+ stream << "Contents of ROI list:\n";
+ homer_mapnav_msgs::RegionsOfInterest roiMsg;
+ for (it = m_Rois.begin(); it != m_Rois.end(); it++)
+ {
+ stream << " ROI '" << it->name << "', '" << it->type << "', [ "
+ << it->points[0].x << " " << it->points[0].y << " ; "
+ << " " << it->points[1].x << " " << it->points[1].y << " ; "
+ << " " << it->points[2].x << " " << it->points[2].y << " ; "
+ << " " << it->points[3].x << " " << it->points[3].y << " ], '"
+ << it->remarks << "'\n";
+ }
+ // ROS_INFO_STREAM( stream.str() );
+ roiMsg.rois = m_Rois;
+ ROS_INFO_STREAM("roiMsg.rois");
+ m_ROIsPublisher.publish(roiMsg);
+ ROS_DEBUG_STREAM(stream.str());
}
-void RoiManager::setHighestId() {
- ROS_INFO_STREAM("Set global variable highest_id.");
- ROS_INFO_STREAM("Find highest id of all ROIs.");
- ROS_INFO_STREAM("current highest id: " << m_highest_id);
- std::vector<homer_mapnav_msgs::RegionOfInterest>::iterator it;
- for (it = m_Rois.begin(); it != m_Rois.end(); it++) {
- ROS_INFO_STREAM("Roi: " << it->name << ", " << it->id);
- if (it->id >= m_highest_id) {
- m_highest_id = it->id;
- ROS_INFO_STREAM("set new highest id: " << m_highest_id);
- }
+void RoiManager::setHighestId()
+{
+ ROS_INFO_STREAM("Set global variable highest_id.");
+ ROS_INFO_STREAM("Find highest id of all ROIs.");
+ ROS_INFO_STREAM("current highest id: " << m_highest_id);
+ std::vector<homer_mapnav_msgs::RegionOfInterest>::iterator it;
+ for (it = m_Rois.begin(); it != m_Rois.end(); it++)
+ {
+ ROS_INFO_STREAM("Roi: " << it->name << ", " << it->id);
+ if (it->id >= m_highest_id)
+ {
+ m_highest_id = it->id;
+ ROS_INFO_STREAM("set new highest id: " << m_highest_id);
}
+ }
}
diff --git a/homer_map_manager/src/MapIO/image_loader.cpp b/homer_map_manager/src/MapIO/image_loader.cpp
index 46f36923bb6f59bb28a1da98f3dfcc64713a34bb..c910d027f42dd866ef5c1914379e475a5bfcfd01 100644
--- a/homer_map_manager/src/MapIO/image_loader.cpp
+++ b/homer_map_manager/src/MapIO/image_loader.cpp
@@ -1,10 +1,10 @@
/*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
- *
+ *
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
- *
+ *
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
@@ -13,7 +13,7 @@
* * Neither the name of the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
- *
+ *
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
@@ -29,15 +29,15 @@
/*
* This file contains helper functions for loading images as maps.
- *
+ *
* Author: Brian Gerkey
*/
#include <cstring>
#include <stdexcept>
-#include <stdlib.h>
#include <stdio.h>
+#include <stdlib.h>
// We use SDL_image to load the image from disk
#include <SDL/SDL_image.h>
@@ -50,28 +50,26 @@
namespace map_server
{
-
-void
-loadMapFromFile(nav_msgs::OccupancyGrid* map,
- const char* fname, double res, bool negate,
- double occ_th, double free_th, double* origin)
+void loadMapFromFile(nav_msgs::OccupancyGrid* map, const char* fname,
+ double res, bool negate, double occ_th, double free_th,
+ double* origin)
{
SDL_Surface* img;
unsigned char* pixels;
unsigned char* p;
int rowstride, n_channels;
- unsigned int i,j;
+ unsigned int i, j;
int k;
double occ;
int color_sum;
double color_avg;
// Load the image using SDL. If we get NULL back, the image load failed.
- if(!(img = IMG_Load(fname)))
+ if (!(img = IMG_Load(fname)))
{
- std::string errmsg = std::string("failed to open image file \"") +
- std::string(fname) + std::string("\"");
+ std::string errmsg = std::string("failed to open image file \"") +
+ std::string(fname) + std::string("\"");
throw std::runtime_error(errmsg);
}
@@ -80,10 +78,10 @@ loadMapFromFile(nav_msgs::OccupancyGrid* map,
map->info.height = img->h;
map->info.resolution = res;
map->info.origin.position.x = *(origin);
- map->info.origin.position.y = *(origin+1);
+ map->info.origin.position.y = *(origin + 1);
map->info.origin.position.z = 0.0;
tf::Quaternion q;
- q.setRPY(0,0, *(origin+2));
+ q.setRPY(0, 0, *(origin + 2));
map->info.origin.orientation.x = q.x();
map->info.origin.orientation.y = q.y();
map->info.origin.orientation.z = q.z();
@@ -98,37 +96,36 @@ loadMapFromFile(nav_msgs::OccupancyGrid* map,
// Copy pixel data into the map structure
pixels = (unsigned char*)(img->pixels);
- for(j = 0; j < map->info.height; j++)
+ for (j = 0; j < map->info.height; j++)
{
for (i = 0; i < map->info.width; i++)
{
// Compute mean of RGB for this pixel
- p = pixels + j*rowstride + i*n_channels;
+ p = pixels + j * rowstride + i * n_channels;
color_sum = 0;
- for(k=0;k<n_channels;k++)
+ for (k = 0; k < n_channels; k++)
color_sum += *(p + (k));
color_avg = color_sum / (double)n_channels;
// If negate is true, we consider blacker pixels free, and whiter
// pixels free. Otherwise, it's vice versa.
- if(negate)
+ if (negate)
occ = color_avg / 255.0;
else
occ = (255 - color_avg) / 255.0;
-
+
// Apply thresholds to RGB means to determine occupancy values for
// map. Note that we invert the graphics-ordering of the pixels to
// produce a map with cell (0,0) in the lower-left corner.
- if(occ > occ_th)
- map->data[MAP_IDX(map->info.width,i,map->info.height - j - 1)] = 99;
- else if(occ < free_th)
- map->data[MAP_IDX(map->info.width,i,map->info.height - j - 1)] = 0;
+ if (occ > occ_th)
+ map->data[MAP_IDX(map->info.width, i, map->info.height - j - 1)] = 99;
+ else if (occ < free_th)
+ map->data[MAP_IDX(map->info.width, i, map->info.height - j - 1)] = 0;
else
- map->data[MAP_IDX(map->info.width,i,map->info.height - j - 1)] = -1;
+ map->data[MAP_IDX(map->info.width, i, map->info.height - j - 1)] = -1;
}
}
SDL_FreeSurface(img);
}
-
}
diff --git a/homer_map_manager/src/MapIO/map_loader.cpp b/homer_map_manager/src/MapIO/map_loader.cpp
index 5b01067701540a5691692a9ab180039a8dabd8b9..e4c10fc913c300406971e88763daee5a1122aaba 100644
--- a/homer_map_manager/src/MapIO/map_loader.cpp
+++ b/homer_map_manager/src/MapIO/map_loader.cpp
@@ -29,154 +29,173 @@
/* Author: Brian Gerkey */
-
+#include <libgen.h>
#include <stdio.h>
#include <stdlib.h>
-#include <libgen.h>
#include <fstream>
-#include "ros/ros.h"
-#include "ros/console.h"
#include <homer_map_manager/MapIO/image_loader.h>
#include "nav_msgs/MapMetaData.h"
-#include "yaml-cpp/yaml.h"
+#include "ros/console.h"
+#include "ros/ros.h"
#include "tf/transform_datatypes.h"
+#include "yaml-cpp/yaml.h"
#include <homer_map_manager/MapIO/map_loader.h>
/** Trivial constructor */
-MapServer::MapServer(const std::string fname, bool &success)
+MapServer::MapServer(const std::string fname, bool& success)
{
- success = false;
- std::string slammapfname = "";
- std::string maskingmapfname = "";
- double origin[3];
- int negate;
- double res, occ_th, free_th;
- std::string frame_id;
- frame_id = "map";
- //mapfname = fname + ".pgm";
- //std::ifstream fin((fname + ".yaml").c_str());
- std::ifstream fin(fname.c_str());
- if (fin.fail()) {
- ROS_ERROR("Map_server could not open %s.", fname.c_str());
- return;
- }
+ success = false;
+ std::string slammapfname = "";
+ std::string maskingmapfname = "";
+ double origin[3];
+ int negate;
+ double res, occ_th, free_th;
+ std::string frame_id;
+ frame_id = "map";
+ // mapfname = fname + ".pgm";
+ // std::ifstream fin((fname + ".yaml").c_str());
+ std::ifstream fin(fname.c_str());
+ if (fin.fail())
+ {
+ ROS_ERROR("Map_server could not open %s.", fname.c_str());
+ return;
+ }
- YAML::Node doc = YAML::LoadFile(fname);
+ YAML::Node doc = YAML::LoadFile(fname);
- try {
- res = doc["resolution"].as<double>();
- } catch (YAML::InvalidScalar) {
- ROS_ERROR("The map does not contain a resolution tag or it is invalid.");
- return;
- }
- try {
- negate = doc["negate"].as<int>();
- } catch (YAML::InvalidScalar) {
- ROS_ERROR("The map does not contain a negate tag or it is invalid.");
- return;
- }
- try {
- occ_th = doc["occupied_thresh"].as<double>();
- } catch (YAML::InvalidScalar) {
- ROS_ERROR("The map does not contain an occupied_thresh tag or it is invalid.");
- return;
- }
- try {
- free_th = doc["free_thresh"].as<double>();
- } catch (YAML::InvalidScalar) {
- ROS_ERROR("The map does not contain a free_thresh tag or it is invalid.");
- return;
- }
- try {
- origin[0] = doc["origin"][0].as<double>();
- origin[1] = doc["origin"][1].as<double>();
- origin[2] = doc["origin"][2].as<double>();
- } catch (YAML::InvalidScalar) {
- ROS_ERROR("The map does not contain an origin tag or it is invalid.");
- return;
- }
- try {
- slammapfname = doc["image"].as<std::string>();
- // TODO: make this path-handling more robust
- if(slammapfname.size() == 0)
- {
- ROS_ERROR("The image tag cannot be an empty string.");
- return;
- }
- if(slammapfname[0] != '/')
- {
- // dirname can modify what you pass it
- char* fname_copy = strdup(fname.c_str());
- slammapfname = std::string(dirname(fname_copy)) + '/' + slammapfname;
- free(fname_copy);
- }
- } catch (YAML::InvalidScalar) {
- ROS_ERROR("The map does not contain an image tag or it is invalid.");
- return;
- }
- //get masking map image path if available
- if(doc["mask_image"])
- {
- maskingmapfname = doc["mask_image"].as<std::string>();
- // TODO: make this path-handling more robust
- if(maskingmapfname.size() == 0)
- {
- ROS_ERROR("The image tag cannot be an empty string.");
- return;
- }
- if(maskingmapfname[0] != '/')
- {
- // // dirname can modify what you pass it
- char* fname_copy = strdup(fname.c_str());
- maskingmapfname = std::string(dirname(fname_copy)) + '/' + maskingmapfname;
- free(fname_copy);
- }
- }
+ try
+ {
+ res = doc["resolution"].as<double>();
+ }
+ catch (YAML::InvalidScalar)
+ {
+ ROS_ERROR("The map does not contain a resolution tag or it is invalid.");
+ return;
+ }
+ try
+ {
+ negate = doc["negate"].as<int>();
+ }
+ catch (YAML::InvalidScalar)
+ {
+ ROS_ERROR("The map does not contain a negate tag or it is invalid.");
+ return;
+ }
+ try
+ {
+ occ_th = doc["occupied_thresh"].as<double>();
+ }
+ catch (YAML::InvalidScalar)
+ {
+ ROS_ERROR("The map does not contain an occupied_thresh tag or it is "
+ "invalid.");
+ return;
+ }
+ try
+ {
+ free_th = doc["free_thresh"].as<double>();
+ }
+ catch (YAML::InvalidScalar)
+ {
+ ROS_ERROR("The map does not contain a free_thresh tag or it is invalid.");
+ return;
+ }
+ try
+ {
+ origin[0] = doc["origin"][0].as<double>();
+ origin[1] = doc["origin"][1].as<double>();
+ origin[2] = doc["origin"][2].as<double>();
+ }
+ catch (YAML::InvalidScalar)
+ {
+ ROS_ERROR("The map does not contain an origin tag or it is invalid.");
+ return;
+ }
+ try
+ {
+ slammapfname = doc["image"].as<std::string>();
+ // TODO: make this path-handling more robust
+ if (slammapfname.size() == 0)
+ {
+ ROS_ERROR("The image tag cannot be an empty string.");
+ return;
+ }
+ if (slammapfname[0] != '/')
+ {
+ // dirname can modify what you pass it
+ char* fname_copy = strdup(fname.c_str());
+ slammapfname = std::string(dirname(fname_copy)) + '/' + slammapfname;
+ free(fname_copy);
+ }
+ }
+ catch (YAML::InvalidScalar)
+ {
+ ROS_ERROR("The map does not contain an image tag or it is invalid.");
+ return;
+ }
+ // get masking map image path if available
+ if (doc["mask_image"])
+ {
+ maskingmapfname = doc["mask_image"].as<std::string>();
+ // TODO: make this path-handling more robust
+ if (maskingmapfname.size() == 0)
+ {
+ ROS_ERROR("The image tag cannot be an empty string.");
+ return;
+ }
+ if (maskingmapfname[0] != '/')
+ {
+ // // dirname can modify what you pass it
+ char* fname_copy = strdup(fname.c_str());
+ maskingmapfname =
+ std::string(dirname(fname_copy)) + '/' + maskingmapfname;
+ free(fname_copy);
+ }
+ }
- //get POIs if existent
- if(doc["pois"])
- {
- ROS_INFO_STREAM("Found " << doc["pois"].size() << " pois");
- for(size_t i = 0; i < doc["pois"].size(); ++i)
- {
+ // get POIs if existent
+ if (doc["pois"])
+ {
+ ROS_INFO_STREAM("Found " << doc["pois"].size() << " pois");
+ for (size_t i = 0; i < doc["pois"].size(); ++i)
+ {
ROS_INFO_STREAM("load one poi." << i);
- std::string name;
- int type;
- float posX;
- float posY;
- float theta;
- std::string remarks;
- name = doc["pois"][i]["name"].as<std::string>() ;
- type = doc["pois"][i]["type"].as<int>() ;
- posX = doc["pois"][i]["x"].as<double>() ;
- posY = doc["pois"][i]["y"].as<double>() ;
- theta = doc["pois"][i]["theta"].as<double>() ;
- remarks = doc["pois"][i]["remarks"].as<std::string>("");
+ std::string name;
+ int type;
+ float posX;
+ float posY;
+ float theta;
+ std::string remarks;
+ name = doc["pois"][i]["name"].as<std::string>();
+ type = doc["pois"][i]["type"].as<int>();
+ posX = doc["pois"][i]["x"].as<double>();
+ posY = doc["pois"][i]["y"].as<double>();
+ theta = doc["pois"][i]["theta"].as<double>();
+ remarks = doc["pois"][i]["remarks"].as<std::string>("");
- homer_mapnav_msgs::PointOfInterest poi;
- poi.name = name;
- poi.type = type;
- poi.pose.position.x = posX;
- poi.pose.position.y = posY;
- poi.pose.position.z = 0.0;
- poi.pose.orientation = tf::createQuaternionMsgFromYaw(theta);
- poi.remarks = remarks;
+ homer_mapnav_msgs::PointOfInterest poi;
+ poi.name = name;
+ poi.type = type;
+ poi.pose.position.x = posX;
+ poi.pose.position.y = posY;
+ poi.pose.position.z = 0.0;
+ poi.pose.orientation = tf::createQuaternionMsgFromYaw(theta);
+ poi.remarks = remarks;
ROS_INFO_STREAM("Done, Saving in mapnav_msgs");
- poiList.push_back(poi);
+ poiList.push_back(poi);
ROS_INFO_STREAM("loaded one poi");
}
ROS_INFO_STREAM("Done. Loaded all POIs.");
}
-
- //get ROIs if existent
- if(doc["rois"])
+ // get ROIs if existent
+ if (doc["rois"])
{
- ROS_INFO_STREAM("Found " << doc["rois"].size() << " rois");
- for(size_t i = 0; i < doc["rois"].size(); ++i)
+ ROS_INFO_STREAM("Found " << doc["rois"].size() << " rois");
+ for (size_t i = 0; i < doc["rois"].size(); ++i)
{
std::string name;
int type;
@@ -192,18 +211,18 @@ MapServer::MapServer(const std::string fname, bool &success)
std::string remarks;
// Read from file
- name = doc["rois"][i]["name"].as<std::string>() ;
- type = doc["rois"][i]["type"].as<int>() ;
- posX1 = doc["rois"][i]["x1"].as<double>() ;
- posY1 = doc["rois"][i]["y1"].as<double>() ;
- posX2 = doc["rois"][i]["x2"].as<double>() ;
- posY2 = doc["rois"][i]["y2"].as<double>() ;
- posX3 = doc["rois"][i]["x3"].as<double>() ;
- posY3 = doc["rois"][i]["y3"].as<double>() ;
- posX4 = doc["rois"][i]["x4"].as<double>() ;
- posY4 = doc["rois"][i]["y4"].as<double>() ;
- id = doc["rois"][i]["id"].as<int>() ;
- remarks = doc["rois"][i]["remarks"].as<std::string>("-") ;
+ name = doc["rois"][i]["name"].as<std::string>();
+ type = doc["rois"][i]["type"].as<int>();
+ posX1 = doc["rois"][i]["x1"].as<double>();
+ posY1 = doc["rois"][i]["y1"].as<double>();
+ posX2 = doc["rois"][i]["x2"].as<double>();
+ posY2 = doc["rois"][i]["y2"].as<double>();
+ posX3 = doc["rois"][i]["x3"].as<double>();
+ posY3 = doc["rois"][i]["y3"].as<double>();
+ posX4 = doc["rois"][i]["x4"].as<double>();
+ posY4 = doc["rois"][i]["y4"].as<double>();
+ id = doc["rois"][i]["id"].as<int>();
+ remarks = doc["rois"][i]["remarks"].as<std::string>("-");
// save in roi-type
homer_mapnav_msgs::RegionOfInterest roi;
@@ -228,48 +247,46 @@ MapServer::MapServer(const std::string fname, bool &success)
}
}
- ROS_INFO("Loading SLAM map from image \"%s\"", slammapfname.c_str());
- map_server::loadMapFromFile(&m_SLAMMap,slammapfname.c_str(),res,negate,occ_th,free_th, origin);
- m_SLAMMap.info.map_load_time = ros::Time::now();
- m_SLAMMap.header.frame_id = frame_id;
- m_SLAMMap.header.stamp = ros::Time::now();
- ROS_INFO("Read a %d X %d SLAM map @ %.3lf m/cell",
- m_SLAMMap.info.width,
- m_SLAMMap.info.height,
- m_SLAMMap.info.resolution);
+ ROS_INFO("Loading SLAM map from image \"%s\"", slammapfname.c_str());
+ map_server::loadMapFromFile(&m_SLAMMap, slammapfname.c_str(), res, negate,
+ occ_th, free_th, origin);
+ m_SLAMMap.info.map_load_time = ros::Time::now();
+ m_SLAMMap.header.frame_id = frame_id;
+ m_SLAMMap.header.stamp = ros::Time::now();
+ ROS_INFO("Read a %d X %d SLAM map @ %.3lf m/cell", m_SLAMMap.info.width,
+ m_SLAMMap.info.height, m_SLAMMap.info.resolution);
- if(maskingmapfname != "")
- {
- ROS_INFO("Loading masking map from image \"%s\"", maskingmapfname.c_str());
- map_server::loadMapFromFile(&m_MaskingMap,maskingmapfname.c_str(),res,negate,occ_th,free_th, origin);
- m_MaskingMap.info.map_load_time = ros::Time::now();
- m_MaskingMap.header.frame_id = frame_id;
- m_MaskingMap.header.stamp = ros::Time::now();
- ROS_INFO("Read a %d X %d masking map @ %.3lf m/cell",
- m_MaskingMap.info.width,
- m_MaskingMap.info.height,
- m_MaskingMap.info.resolution);
- }
- success = true;
+ if (maskingmapfname != "")
+ {
+ ROS_INFO("Loading masking map from image \"%s\"", maskingmapfname.c_str());
+ map_server::loadMapFromFile(&m_MaskingMap, maskingmapfname.c_str(), res,
+ negate, occ_th, free_th, origin);
+ m_MaskingMap.info.map_load_time = ros::Time::now();
+ m_MaskingMap.header.frame_id = frame_id;
+ m_MaskingMap.header.stamp = ros::Time::now();
+ ROS_INFO("Read a %d X %d masking map @ %.3lf m/cell",
+ m_MaskingMap.info.width, m_MaskingMap.info.height,
+ m_MaskingMap.info.resolution);
+ }
+ success = true;
}
nav_msgs::OccupancyGrid MapServer::getSLAMMap()
{
- return m_SLAMMap;
+ return m_SLAMMap;
}
nav_msgs::OccupancyGrid MapServer::getMaskingMap()
{
- return m_MaskingMap;
+ return m_MaskingMap;
}
std::vector<homer_mapnav_msgs::PointOfInterest> MapServer::getPois()
{
- return poiList;
+ return poiList;
}
std::vector<homer_mapnav_msgs::RegionOfInterest> MapServer::getRois()
{
return roiList;
}
-
diff --git a/homer_map_manager/src/MapIO/map_saver.cpp b/homer_map_manager/src/MapIO/map_saver.cpp
index 7aeff6a34056dbb2cbf3d38c44c019b6f64dfb0a..4764aecbe3b2e17ad600db6cd162179cb7ce3753 100644
--- a/homer_map_manager/src/MapIO/map_saver.cpp
+++ b/homer_map_manager/src/MapIO/map_saver.cpp
@@ -2,10 +2,10 @@
* map_saver
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
- *
+ *
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
- *
+ *
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
@@ -14,7 +14,7 @@
* * Neither the name of the <ORGANIZATION> nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
- *
+ *
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
@@ -30,84 +30,94 @@
#include <homer_map_manager/MapIO/map_saver.h>
-#include <sys/types.h>
#include <sys/stat.h>
+#include <sys/types.h>
#include <tf/tf.h>
using namespace std;
-
-
- MapGenerator::MapGenerator(const std::string mapname)
- {
- m_Mapname = mapname;
- }
-
- void MapGenerator::saveMapLayer(const nav_msgs::OccupancyGridConstPtr &map, std::string fileName)
- {
- ROS_INFO("Writing map occupancy data to %s", fileName.c_str());
- FILE* out = fopen(fileName.c_str(), "w");
- if (!out)
- {
- ROS_ERROR("Couldn't save map file to %s", fileName.c_str());
- return;
- }
-
- fprintf(out, "P5\n# CREATOR: map_saver.cpp %.3f m/pix\n%d %d\n255\n",
- map->info.resolution, map->info.width, map->info.height);
- for(unsigned int y = 0; y < map->info.height; y++) {
- for(unsigned int x = 0; x < map->info.width; x++) {
- unsigned int i = x + (map->info.height - y - 1) * map->info.width;
- if (map->data[i] == -1) {
- fputc(205, out);
- } else if (map->data[i] < 20) { //occ [0,0.2)
- fputc(254, out);
- } else if (map->data[i] > 65) { //occ (0.65,1]
- fputc(000, out);
- } else { //occ [0.2,0.65]
- fputc(205, out);
- }
- }
- }
- fclose(out);
- }
- void MapGenerator::save(const nav_msgs::OccupancyGridConstPtr& SLAMMap,
- const nav_msgs::OccupancyGridConstPtr& maskingMap,
- std::vector<homer_mapnav_msgs::PointOfInterest> poiList,
- std::vector<homer_mapnav_msgs::RegionOfInterest> roiList)
+MapGenerator::MapGenerator(const std::string mapname)
+{
+ m_Mapname = mapname;
+}
+
+void MapGenerator::saveMapLayer(const nav_msgs::OccupancyGridConstPtr& map,
+ std::string fileName)
+{
+ ROS_INFO("Writing map occupancy data to %s", fileName.c_str());
+ FILE* out = fopen(fileName.c_str(), "w");
+ if (!out)
+ {
+ ROS_ERROR("Couldn't save map file to %s", fileName.c_str());
+ return;
+ }
+
+ fprintf(out, "P5\n# CREATOR: map_saver.cpp %.3f m/pix\n%d %d\n255\n",
+ map->info.resolution, map->info.width, map->info.height);
+ for (unsigned int y = 0; y < map->info.height; y++)
+ {
+ for (unsigned int x = 0; x < map->info.width; x++)
{
- if (access(m_Mapname.c_str(),W_OK) != (-1))
+ unsigned int i = x + (map->info.height - y - 1) * map->info.width;
+ if (map->data[i] == -1)
{
- // Map "Directory" is an accessible File
- size_t sp = m_Mapname.rfind('/');
- m_Mapname = m_Mapname.substr(0,sp);
+ fputc(205, out);
}
- size_t a = m_Mapname.rfind('/');
- std::string filename = "";
- if (a == std::string::npos)
- {
- filename = m_Mapname;
+ else if (map->data[i] < 20)
+ { // occ [0,0.2)
+ fputc(254, out);
}
- else
- {
- filename = m_Mapname.substr(a+1, m_Mapname.size()-1);
+ else if (map->data[i] > 65)
+ { // occ (0.65,1]
+ fputc(000, out);
}
- int status;
- status = mkdir(m_Mapname.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
- std::string SLAMMapdatafile = filename + "_SLAM.pgm";
- std::string maskingMapdatafile = "";
- saveMapLayer(SLAMMap, m_Mapname + "/" + SLAMMapdatafile);
- if(maskingMap != NULL)
- {
- maskingMapdatafile = filename + "_mask.pgm";
- saveMapLayer(maskingMap, m_Mapname + "/" + maskingMapdatafile);
+ else
+ { // occ [0.2,0.65]
+ fputc(205, out);
}
- std::string mapmetadatafile = m_Mapname + "/" + filename + ".yaml";
- ROS_INFO("Writing map metadata to %s", mapmetadatafile.c_str());
- FILE* yaml = fopen(mapmetadatafile.c_str(), "w");
-
- /*
+ }
+ }
+ fclose(out);
+}
+
+void MapGenerator::save(
+ const nav_msgs::OccupancyGridConstPtr& SLAMMap,
+ const nav_msgs::OccupancyGridConstPtr& maskingMap,
+ std::vector<homer_mapnav_msgs::PointOfInterest> poiList,
+ std::vector<homer_mapnav_msgs::RegionOfInterest> roiList)
+{
+ if (access(m_Mapname.c_str(), W_OK) != (-1))
+ {
+ // Map "Directory" is an accessible File
+ size_t sp = m_Mapname.rfind('/');
+ m_Mapname = m_Mapname.substr(0, sp);
+ }
+ size_t a = m_Mapname.rfind('/');
+ std::string filename = "";
+ if (a == std::string::npos)
+ {
+ filename = m_Mapname;
+ }
+ else
+ {
+ filename = m_Mapname.substr(a + 1, m_Mapname.size() - 1);
+ }
+ int status;
+ status = mkdir(m_Mapname.c_str(), S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
+ std::string SLAMMapdatafile = filename + "_SLAM.pgm";
+ std::string maskingMapdatafile = "";
+ saveMapLayer(SLAMMap, m_Mapname + "/" + SLAMMapdatafile);
+ if (maskingMap != NULL)
+ {
+ maskingMapdatafile = filename + "_mask.pgm";
+ saveMapLayer(maskingMap, m_Mapname + "/" + maskingMapdatafile);
+ }
+ std::string mapmetadatafile = m_Mapname + "/" + filename + ".yaml";
+ ROS_INFO("Writing map metadata to %s", mapmetadatafile.c_str());
+ FILE* yaml = fopen(mapmetadatafile.c_str(), "w");
+
+ /*
resolution: 0.100000
origin: [0.000000, 0.000000, 0.000000]
#
@@ -115,75 +125,84 @@ negate: 0
occupied_thresh: 0.65
free_thresh: 0.196
- */
-
- geometry_msgs::Quaternion orientation = SLAMMap->info.origin.orientation;
- //btMatrix3x3 mat(btQuaternion(orientation.x, orientation.y, orientation.z, orientation.w));
- tf::Quaternion quat_tf;
- tf::quaternionMsgToTF(orientation, quat_tf);
- double yaw = tf::getYaw(quat_tf);
- //mat.getEulerYPR(yaw, pitch, roll);
-
- stringstream pois;
- if(!poiList.empty())
+ */
+
+ geometry_msgs::Quaternion orientation = SLAMMap->info.origin.orientation;
+ // btMatrix3x3 mat(btQuaternion(orientation.x, orientation.y, orientation.z,
+ // orientation.w));
+ tf::Quaternion quat_tf;
+ tf::quaternionMsgToTF(orientation, quat_tf);
+ double yaw = tf::getYaw(quat_tf);
+ // mat.getEulerYPR(yaw, pitch, roll);
+
+ stringstream pois;
+ if (!poiList.empty())
+ {
+ pois << "pois:\n";
+ std::vector<homer_mapnav_msgs::PointOfInterest>::iterator it;
+ for (it = poiList.begin(); it != poiList.end(); it++)
+ {
+ pois << " - name: " << it->name << "\n";
+ pois << " type: " << it->type << "\n";
+ pois << " x: " << it->pose.position.x << "\n";
+ pois << " y: " << it->pose.position.y << "\n";
+ pois << " theta: " << tf::getYaw(it->pose.orientation) << "\n";
+ if (it->remarks == "-")
{
- pois << "pois:\n";
- std::vector< homer_mapnav_msgs::PointOfInterest >::iterator it;
- for ( it = poiList.begin(); it != poiList.end(); it++ ) {
- pois << " - name: " << it->name << "\n";
- pois << " type: " << it->type << "\n";
- pois << " x: " << it->pose.position.x << "\n";
- pois << " y: " << it->pose.position.y << "\n";
- pois << " theta: " << tf::getYaw(it->pose.orientation) << "\n";
- if(it->remarks == "-")
- {
- pois << " remarks: " << "\n";
- }else{
- pois << " remarks: " << it->remarks << "\n";
- }
- //pois << " remarks: " << it->remarks << "\n";
- }
+ pois << " remarks: "
+ << "\n";
}
- string poiStr = pois.str();
-
- stringstream rois;
- if(!roiList.empty())
+ else
{
- rois << "rois:\n";
- std::vector< homer_mapnav_msgs::RegionOfInterest >::iterator it;
- for ( it = roiList.begin(); it != roiList.end(); it++ ) {
-
- rois << " - name: " << it->name << "\n";
- rois << " type: " << it->type << "\n";
- rois << " x1: " << it->points[0].x << "\n";
- rois << " y1: " << it->points[0].y << "\n";
- rois << " x2: " << it->points[1].x << "\n";
- rois << " y2: " << it->points[1].y << "\n";
- rois << " x3: " << it->points[2].x << "\n";
- rois << " y3: " << it->points[2].y << "\n";
- rois << " x4: " << it->points[3].x << "\n";
- rois << " y4: " << it->points[3].y << "\n";
- rois << " id: " << it->id << "\n";
- //rois << " remarks: " << it->remarks << "\n";
- if(it->remarks == "-")
- {
- rois << " remarks: " << "\n";
- }else{
- rois << " remarks: " << it->remarks << "\n";
- }
- }
+ pois << " remarks: " << it->remarks << "\n";
}
- string roiStr = rois.str();
-
- string maskImage = "";
- if(maskingMapdatafile != "")
- maskImage = "\nmask_image: ";
- fprintf(yaml, "image: %s%s%s\nresolution: %f\norigin: [%f, %f, %f]\nnegate: 0\noccupied_thresh: 0.65\nfree_thresh: 0.195\n\n%s\n%s",
- SLAMMapdatafile.c_str(), maskImage.c_str(), maskingMapdatafile.c_str(), SLAMMap->info.resolution, SLAMMap->info.origin.position.x, SLAMMap->info.origin.position.y, yaw, poiStr.c_str(), roiStr.c_str() );
- fclose(yaml);
-
- ROS_INFO("Done\n");
+ // pois << " remarks: " << it->remarks << "\n";
}
-
-
-
+ }
+ string poiStr = pois.str();
+
+ stringstream rois;
+ if (!roiList.empty())
+ {
+ rois << "rois:\n";
+ std::vector<homer_mapnav_msgs::RegionOfInterest>::iterator it;
+ for (it = roiList.begin(); it != roiList.end(); it++)
+ {
+ rois << " - name: " << it->name << "\n";
+ rois << " type: " << it->type << "\n";
+ rois << " x1: " << it->points[0].x << "\n";
+ rois << " y1: " << it->points[0].y << "\n";
+ rois << " x2: " << it->points[1].x << "\n";
+ rois << " y2: " << it->points[1].y << "\n";
+ rois << " x3: " << it->points[2].x << "\n";
+ rois << " y3: " << it->points[2].y << "\n";
+ rois << " x4: " << it->points[3].x << "\n";
+ rois << " y4: " << it->points[3].y << "\n";
+ rois << " id: " << it->id << "\n";
+ // rois << " remarks: " << it->remarks << "\n";
+ if (it->remarks == "-")
+ {
+ rois << " remarks: "
+ << "\n";
+ }
+ else
+ {
+ rois << " remarks: " << it->remarks << "\n";
+ }
+ }
+ }
+ string roiStr = rois.str();
+
+ string maskImage = "";
+ if (maskingMapdatafile != "")
+ maskImage = "\nmask_image: ";
+ fprintf(yaml, "image: %s%s%s\nresolution: %f\norigin: [%f, %f, %f]\nnegate: "
+ "0\noccupied_thresh: 0.65\nfree_thresh: 0.195\n\n%s\n%s",
+ SLAMMapdatafile.c_str(), maskImage.c_str(),
+ maskingMapdatafile.c_str(), SLAMMap->info.resolution,
+ SLAMMap->info.origin.position.x, SLAMMap->info.origin.position.y, yaw,
+ poiStr.c_str(), roiStr.c_str());
+ fclose(yaml);
+
+ ROS_INFO("Done\n");
+}
diff --git a/homer_map_manager/src/map_manager_node.cpp b/homer_map_manager/src/map_manager_node.cpp
index babc26f262d16443b18ac1ce61d6d9975dcccc31..acdb6bc32e7f65f854b654c79394604c771863b7 100644
--- a/homer_map_manager/src/map_manager_node.cpp
+++ b/homer_map_manager/src/map_manager_node.cpp
@@ -1,455 +1,509 @@
#include <homer_map_manager/map_manager_node.h>
//#include <gperftools/profiler.h>
-MapManagerNode::MapManagerNode(ros::NodeHandle* nh) {
- m_LastLaserTime = ros::Time::now();
-
- int mapSize = 1;
- float resolution = 1;
- ros::param::param("/map_manager/roi_updates", m_roi_polling, (bool)false);
- ros::param::param("/map_manager/roi_polling_time", m_roi_polling_time,
- (float)0.5);
- m_lastROIPoll = ros::Time::now();
- m_MapManager = new MapManager(nh);
- m_POIManager = new PoiManager(nh);
- m_ROIManager = new RoiManager(nh);
-
- nav_msgs::MapMetaData mapInfo;
- mapInfo.width = mapSize / resolution;
- mapInfo.height = mapSize / resolution;
- mapInfo.resolution = resolution;
- mapInfo.origin.position.x = 0;
- mapInfo.origin.position.y = 0;
- mapInfo.origin.position.z = 0;
- mapInfo.origin.orientation.x = 0;
- mapInfo.origin.orientation.y = 0;
- mapInfo.origin.orientation.z = 0;
- mapInfo.origin.orientation.w = 1;
-
- m_MaskingManager = new MaskingManager(mapInfo);
-
- // subscriptions of MapManagerModule
- m_RapidMapSubscriber = nh->subscribe(
- "/rapid_mapping/map", 1, &MapManagerNode::callbackRapidMap, this);
- m_OctomapSubscriber = nh->subscribe(
- "/projected_map", 1, &MapManagerNode::callbackOctomapMap, this);
- m_SLAMMapSubscriber = nh->subscribe("/homer_mapping/slam_map", 1,
- &MapManagerNode::callbackSLAMMap, this);
- m_SaveMapSubscriber = nh->subscribe("/map_manager/save_map", 1,
- &MapManagerNode::callbackSaveMap, this);
- m_LoadMapSubscriber = nh->subscribe("/map_manager/load_map", 1,
- &MapManagerNode::callbackLoadMap, this);
- m_MapVisibilitySubscriber =
- nh->subscribe("/map_manager/toggle_map_visibility", 1,
- &MapManagerNode::callbackMapVisibility, this);
- m_LaserScanSubscriber =
- nh->subscribe("/scan", 1, &MapManagerNode::callbackLaserScan, this);
- m_BackLaserScanSubscriber = nh->subscribe(
- "/back_scan", 1, &MapManagerNode::callbackBackLaser, this);
- m_FrontLaserScanSubscriber = nh->subscribe(
- "/front_scan", 1, &MapManagerNode::callbackFrontLaser, this);
- m_PoseSubscriber =
- nh->subscribe("/pose", 1, &MapManagerNode::poseCallback, this);
-
- // subscriptions of PoiManager
- m_AddPOISubscriber = nh->subscribe("/map_manager/add_POI", 20,
- &MapManagerNode::callbackAddPOI, this);
- m_ModifyPOISubscriber =
- nh->subscribe("/map_manager/modify_POI", 100,
- &MapManagerNode::callbackModifyPOI, this);
- m_DeletePOISubscriber =
- nh->subscribe("/map_manager/delete_POI", 100,
- &MapManagerNode::callbackDeletePOI, this);
- m_GetPOIsService = nh->advertiseService(
- "/map_manager/get_pois", &MapManagerNode::getPOIsService, this);
-
- // Services for Map Handling
- m_SaveMapService = nh->advertiseService(
- "/map_manager/save_map", &MapManagerNode::saveMapService, this);
- m_ResetMapService = nh->advertiseService(
- "/map_manager/reset_map", &MapManagerNode::resetMapService, this);
- m_LoadMapService = nh->advertiseService(
- "/map_manager/load_map", &MapManagerNode::loadMapService, this);
-
- // subscriptions of RoiManager
- m_AddROISubscriber = nh->subscribe("/map_manager/add_ROI", 20,
- &MapManagerNode::callbackAddROI, this);
- m_ModifyROISubscriber =
- nh->subscribe("/map_manager/modify_ROI", 100,
- &MapManagerNode::callbackModifyROI, this);
- m_DeleteROIByNameSubscriber =
- nh->subscribe("/map_manager/delete_ROI_by_name", 100,
- &MapManagerNode::callbackDeleteROIbyName, this);
- m_DeleteROIByIDSubscriber =
- nh->subscribe("/map_manager/delete_ROI_by_id", 100,
- &MapManagerNode::callbackDeleteROIbyID, this);
- m_GetROIsService = nh->advertiseService(
- "/map_manager/get_rois", &MapManagerNode::getROIsService, this);
- m_GetROINameService = nh->advertiseService(
- "/map_manager/get_roi_name", &MapManagerNode::getROINameService, this);
- m_PoiInsideROISService =
- nh->advertiseService("/map_manager/point_inside_rois",
- &MapManagerNode::pointInsideRoisService, this);
- if (m_roi_polling) {
- m_RoiPollPublisher = nh->advertise<homer_mapnav_msgs::RoiChange>(
- "/map_manager/roi_change", 1);
- }
-
- // subscribtions of MaskingMapModule
- m_ModifyMapSubscriber = nh->subscribe(
- "/map_manager/modify_map", 1, &MapManagerNode::callbackModifyMap, this);
- m_ResetMapsSubscriber = nh->subscribe(
- "/map_manager/reset_maps", 1, &MapManagerNode::callbackResetMaps, this);
-
- // loaded map publisher
- m_LoadedMapPublisher =
- nh->advertise<nav_msgs::OccupancyGrid>("/map_manager/loaded_map", 1);
-
- // mask slam publisher
- m_MaskSlamMapPublisher =
- nh->advertise<nav_msgs::OccupancyGrid>("/map_manager/mask_slam", 1);
-
- // load map file from config if present
- std::string mapfile;
- ros::param::get("/map_manager/default_mapfile", mapfile);
- if (mapfile != "") {
- std_msgs::String::Ptr mapfileMsg(new std_msgs::String);
- mapfileMsg->data = mapfile;
- callbackLoadMap(mapfileMsg);
- }
- m_POIManager->broadcastPoiList();
- m_ROIManager->broadcastRoiList();
+MapManagerNode::MapManagerNode(ros::NodeHandle* nh)
+{
+ m_LastLaserTime = ros::Time::now();
+
+ int mapSize = 1;
+ float resolution = 1;
+ ros::param::param("/map_manager/roi_updates", m_roi_polling, (bool)false);
+ ros::param::param("/map_manager/roi_polling_time", m_roi_polling_time,
+ (float)0.5);
+ m_lastROIPoll = ros::Time::now();
+ m_MapManager = new MapManager(nh);
+ m_POIManager = new PoiManager(nh);
+ m_ROIManager = new RoiManager(nh);
+
+ nav_msgs::MapMetaData mapInfo;
+ mapInfo.width = mapSize / resolution;
+ mapInfo.height = mapSize / resolution;
+ mapInfo.resolution = resolution;
+ mapInfo.origin.position.x = 0;
+ mapInfo.origin.position.y = 0;
+ mapInfo.origin.position.z = 0;
+ mapInfo.origin.orientation.x = 0;
+ mapInfo.origin.orientation.y = 0;
+ mapInfo.origin.orientation.z = 0;
+ mapInfo.origin.orientation.w = 1;
+
+ m_MaskingManager = new MaskingManager(mapInfo);
+
+ // subscriptions of MapManagerModule
+ m_RapidMapSubscriber = nh->subscribe("/rapid_mapping/map", 1,
+ &MapManagerNode::callbackRapidMap, this);
+ m_OctomapSubscriber = nh->subscribe(
+ "/projected_map", 1, &MapManagerNode::callbackOctomapMap, this);
+ m_SLAMMapSubscriber = nh->subscribe("/homer_mapping/slam_map", 1,
+ &MapManagerNode::callbackSLAMMap, this);
+ m_SaveMapSubscriber = nh->subscribe("/map_manager/save_map", 1,
+ &MapManagerNode::callbackSaveMap, this);
+ m_LoadMapSubscriber = nh->subscribe("/map_manager/load_map", 1,
+ &MapManagerNode::callbackLoadMap, this);
+ m_MapVisibilitySubscriber =
+ nh->subscribe("/map_manager/toggle_map_visibility", 1,
+ &MapManagerNode::callbackMapVisibility, this);
+ m_LaserScanSubscriber =
+ nh->subscribe("/scan", 1, &MapManagerNode::callbackLaserScan, this);
+ m_BackLaserScanSubscriber =
+ nh->subscribe("/back_scan", 1, &MapManagerNode::callbackBackLaser, this);
+ m_FrontLaserScanSubscriber = nh->subscribe(
+ "/front_scan", 1, &MapManagerNode::callbackFrontLaser, this);
+ m_PoseSubscriber =
+ nh->subscribe("/pose", 1, &MapManagerNode::poseCallback, this);
+
+ // subscriptions of PoiManager
+ m_AddPOISubscriber = nh->subscribe("/map_manager/add_POI", 20,
+ &MapManagerNode::callbackAddPOI, this);
+ m_ModifyPOISubscriber = nh->subscribe(
+ "/map_manager/modify_POI", 100, &MapManagerNode::callbackModifyPOI, this);
+ m_DeletePOISubscriber = nh->subscribe(
+ "/map_manager/delete_POI", 100, &MapManagerNode::callbackDeletePOI, this);
+ m_GetPOIsService = nh->advertiseService(
+ "/map_manager/get_pois", &MapManagerNode::getPOIsService, this);
+
+ // Services for Map Handling
+ m_SaveMapService = nh->advertiseService(
+ "/map_manager/save_map", &MapManagerNode::saveMapService, this);
+ m_ResetMapService = nh->advertiseService(
+ "/map_manager/reset_map", &MapManagerNode::resetMapService, this);
+ m_LoadMapService = nh->advertiseService(
+ "/map_manager/load_map", &MapManagerNode::loadMapService, this);
+
+ // subscriptions of RoiManager
+ m_AddROISubscriber = nh->subscribe("/map_manager/add_ROI", 20,
+ &MapManagerNode::callbackAddROI, this);
+ m_ModifyROISubscriber = nh->subscribe(
+ "/map_manager/modify_ROI", 100, &MapManagerNode::callbackModifyROI, this);
+ m_DeleteROIByNameSubscriber =
+ nh->subscribe("/map_manager/delete_ROI_by_name", 100,
+ &MapManagerNode::callbackDeleteROIbyName, this);
+ m_DeleteROIByIDSubscriber =
+ nh->subscribe("/map_manager/delete_ROI_by_id", 100,
+ &MapManagerNode::callbackDeleteROIbyID, this);
+ m_GetROIsService = nh->advertiseService(
+ "/map_manager/get_rois", &MapManagerNode::getROIsService, this);
+ m_GetROINameService = nh->advertiseService(
+ "/map_manager/get_roi_name", &MapManagerNode::getROINameService, this);
+ m_PoiInsideROISService =
+ nh->advertiseService("/map_manager/point_inside_rois",
+ &MapManagerNode::pointInsideRoisService, this);
+ if (m_roi_polling)
+ {
+ m_RoiPollPublisher = nh->advertise<homer_mapnav_msgs::RoiChange>(
+ "/map_manager/roi_change", 1);
+ }
+
+ // subscribtions of MaskingMapModule
+ m_ModifyMapSubscriber = nh->subscribe(
+ "/map_manager/modify_map", 1, &MapManagerNode::callbackModifyMap, this);
+ m_ResetMapsSubscriber = nh->subscribe(
+ "/map_manager/reset_maps", 1, &MapManagerNode::callbackResetMaps, this);
+
+ // loaded map publisher
+ m_LoadedMapPublisher =
+ nh->advertise<nav_msgs::OccupancyGrid>("/map_manager/loaded_map", 1);
+
+ // mask slam publisher
+ m_MaskSlamMapPublisher =
+ nh->advertise<nav_msgs::OccupancyGrid>("/map_manager/mask_slam", 1);
+
+ // load map file from config if present
+ std::string mapfile;
+ ros::param::get("/map_manager/default_mapfile", mapfile);
+ if (mapfile != "")
+ {
+ std_msgs::String::Ptr mapfileMsg(new std_msgs::String);
+ mapfileMsg->data = mapfile;
+ callbackLoadMap(mapfileMsg);
+ }
+ m_POIManager->broadcastPoiList();
+ m_ROIManager->broadcastRoiList();
}
-MapManagerNode::~MapManagerNode() {
- if (m_MapManager) delete m_MapManager;
- if (m_POIManager) delete m_POIManager;
- if (m_ROIManager) delete m_ROIManager;
- if (m_MaskingManager) delete m_MaskingManager;
+MapManagerNode::~MapManagerNode()
+{
+ if (m_MapManager)
+ delete m_MapManager;
+ if (m_POIManager)
+ delete m_POIManager;
+ if (m_ROIManager)
+ delete m_ROIManager;
+ if (m_MaskingManager)
+ delete m_MaskingManager;
}
void MapManagerNode::callbackSLAMMap(
- const nav_msgs::OccupancyGrid::ConstPtr& msg) {
- nav_msgs::OccupancyGrid::ConstPtr maskingMap = m_MaskingManager->updateMapInfo(msg->info);
- m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER, maskingMap);
- m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::SLAM_LAYER, msg);
+ const nav_msgs::OccupancyGrid::ConstPtr& msg)
+{
+ nav_msgs::OccupancyGrid::ConstPtr maskingMap =
+ m_MaskingManager->updateMapInfo(msg->info);
+ m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER,
+ maskingMap);
+ m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::SLAM_LAYER, msg);
}
void MapManagerNode::callbackRapidMap(
- const nav_msgs::OccupancyGrid::ConstPtr& msg) {
- m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::RAPID_MAP, msg);
+ const nav_msgs::OccupancyGrid::ConstPtr& msg)
+{
+ m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::RAPID_MAP, msg);
}
void MapManagerNode::callbackOctomapMap(
- const nav_msgs::OccupancyGrid::ConstPtr& msg) {
- m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::KINECT_LAYER,
- msg);
-
- // nav_msgs::OccupancyGrid mergedMap;
- //
- // int width = 701;
- // int height = 701;
- // float resolution = 0.05;
- //
- // int ByteSize = width * height;
- //
- // mergedMap.header.stamp = ros::Time::now();
- // mergedMap.header.frame_id = "map";
- // mergedMap.info.width = width;
- // mergedMap.info.height = height;
- // mergedMap.info.resolution = resolution;
- // mergedMap.info.origin.position.x = -height*resolution/2;
- // mergedMap.info.origin.position.y = -width*resolution/2;
- // mergedMap.info.origin.orientation.w = 1.0;
- // mergedMap.info.origin.orientation.x = 0.0;
- // mergedMap.info.origin.orientation.y = 0.0;
- // mergedMap.info.origin.orientation.z = 0.0;
- // mergedMap.data.resize(ByteSize,-1);
-
- // for ( int y = 0; y < msg->info.height; y++ )
- // {
- // for ( int x = 0; x < msg->info.width; x++ )
- // {
- // int i = x + y * msg->info.width;
-
- //
- // //if cell is occupied by kinect obstacle merge cell with merged
- //map
- // if(msg->data[i] ==
- //homer_mapnav_msgs::ModifyMap::BLOCKED)
- // {
-
- // Eigen::Vector2i point(x,y);
- // geometry_msgs::Point tmp = map_tools::fromMapCoords( point
- //,msg->info.origin, msg->info.resolution);
- // point = map_tools::toMapCoords(tmp , mergedMap.info.origin,
- //mergedMap.info.resolution);
- // int k = point.y() * mergedMap.info.width +
- //point.x();
- // mergedMap.data[k] =
- //homer_mapnav_msgs::ModifyMap::DEPTH;
- // }
- // }
- // }
- //
- //
- // m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::KINECT_LAYER,
- //boost::make_shared<nav_msgs::OccupancyGrid>(mergedMap));
+ const nav_msgs::OccupancyGrid::ConstPtr& msg)
+{
+ m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::KINECT_LAYER, msg);
+
+ // nav_msgs::OccupancyGrid mergedMap;
+ //
+ // int width = 701;
+ // int height = 701;
+ // float resolution = 0.05;
+ //
+ // int ByteSize = width * height;
+ //
+ // mergedMap.header.stamp = ros::Time::now();
+ // mergedMap.header.frame_id = "map";
+ // mergedMap.info.width = width;
+ // mergedMap.info.height = height;
+ // mergedMap.info.resolution = resolution;
+ // mergedMap.info.origin.position.x = -height*resolution/2;
+ // mergedMap.info.origin.position.y = -width*resolution/2;
+ // mergedMap.info.origin.orientation.w = 1.0;
+ // mergedMap.info.origin.orientation.x = 0.0;
+ // mergedMap.info.origin.orientation.y = 0.0;
+ // mergedMap.info.origin.orientation.z = 0.0;
+ // mergedMap.data.resize(ByteSize,-1);
+
+ // for ( int y = 0; y < msg->info.height; y++ )
+ // {
+ // for ( int x = 0; x < msg->info.width; x++ )
+ // {
+ // int i = x + y * msg->info.width;
+
+ //
+ // //if cell is occupied by kinect obstacle merge cell with merged
+ // map
+ // if(msg->data[i] ==
+ // homer_mapnav_msgs::ModifyMap::BLOCKED)
+ // {
+
+ // Eigen::Vector2i point(x,y);
+ // geometry_msgs::Point tmp = map_tools::fromMapCoords( point
+ //,msg->info.origin, msg->info.resolution);
+ // point = map_tools::toMapCoords(tmp , mergedMap.info.origin,
+ // mergedMap.info.resolution);
+ // int k = point.y() * mergedMap.info.width +
+ // point.x();
+ // mergedMap.data[k] =
+ // homer_mapnav_msgs::ModifyMap::DEPTH;
+ // }
+ // }
+ // }
+ //
+ //
+ // m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::KINECT_LAYER,
+ // boost::make_shared<nav_msgs::OccupancyGrid>(mergedMap));
}
-void MapManagerNode::callbackSaveMap(const std_msgs::String::ConstPtr& msg) {
- MapGenerator map_saver(msg->data);
- nav_msgs::OccupancyGrid::ConstPtr SLAMMap =
- m_MapManager->getMapLayer(homer_mapnav_msgs::MapLayers::SLAM_LAYER);
- nav_msgs::OccupancyGrid::ConstPtr maskingMap =
- m_MapManager->getMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER);
- map_saver.save(SLAMMap, maskingMap, m_POIManager->getList(),
- m_ROIManager->getList());
+void MapManagerNode::callbackSaveMap(const std_msgs::String::ConstPtr& msg)
+{
+ MapGenerator map_saver(msg->data);
+ nav_msgs::OccupancyGrid::ConstPtr SLAMMap =
+ m_MapManager->getMapLayer(homer_mapnav_msgs::MapLayers::SLAM_LAYER);
+ nav_msgs::OccupancyGrid::ConstPtr maskingMap =
+ m_MapManager->getMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER);
+ map_saver.save(SLAMMap, maskingMap, m_POIManager->getList(),
+ m_ROIManager->getList());
}
-void MapManagerNode::callbackLoadMap(const std_msgs::String::ConstPtr& msg) {
- m_MapManager->clearMapLayers();
- ROS_INFO_STREAM("Loading map from file " << msg->data);
- bool success;
- MapServer map_loader(msg->data, success);
- if (success) {
- ros::Rate poll_rate(10);
- while (m_LoadedMapPublisher.getNumSubscribers() == 0) {
- poll_rate.sleep();
- }
- m_LoadedMapPublisher.publish(map_loader.getSLAMMap());
- m_MapManager->updateMapLayer(
- homer_mapnav_msgs::MapLayers::SLAM_LAYER,
- boost::make_shared<nav_msgs::OccupancyGrid>(
- map_loader.getSLAMMap()));
- nav_msgs::OccupancyGrid::ConstPtr maskingMap =
- boost::make_shared<nav_msgs::OccupancyGrid>(
- map_loader.getMaskingMap());
- m_MaskingManager->replaceMap(map_loader.getMaskingMap());
- if (maskingMap->data.size() != 0) {
- m_MapManager->updateMapLayer(
- homer_mapnav_msgs::MapLayers::MASKING_LAYER, maskingMap);
- }
- m_POIManager->replacePOIList(map_loader.getPois());
- m_POIManager->broadcastPoiList();
- m_ROIManager->replaceROIList(map_loader.getRois());
- m_ROIManager->broadcastRoiList();
- } else {
- ROS_ERROR_STREAM("[MapManager] Could not open mapfile!!");
+void MapManagerNode::callbackLoadMap(const std_msgs::String::ConstPtr& msg)
+{
+ m_MapManager->clearMapLayers();
+ ROS_INFO_STREAM("Loading map from file " << msg->data);
+ bool success;
+ MapServer map_loader(msg->data, success);
+ if (success)
+ {
+ ros::Rate poll_rate(10);
+ while (m_LoadedMapPublisher.getNumSubscribers() == 0)
+ {
+ poll_rate.sleep();
+ }
+ m_LoadedMapPublisher.publish(map_loader.getSLAMMap());
+ m_MapManager->updateMapLayer(
+ homer_mapnav_msgs::MapLayers::SLAM_LAYER,
+ boost::make_shared<nav_msgs::OccupancyGrid>(map_loader.getSLAMMap()));
+ nav_msgs::OccupancyGrid::ConstPtr maskingMap =
+ boost::make_shared<nav_msgs::OccupancyGrid>(map_loader.getMaskingMap());
+ m_MaskingManager->replaceMap(map_loader.getMaskingMap());
+ if (maskingMap->data.size() != 0)
+ {
+ m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER,
+ maskingMap);
}
+ m_POIManager->replacePOIList(map_loader.getPois());
+ m_POIManager->broadcastPoiList();
+ m_ROIManager->replaceROIList(map_loader.getRois());
+ m_ROIManager->broadcastRoiList();
+ }
+ else
+ {
+ ROS_ERROR_STREAM("[MapManager] Could not open mapfile!!");
+ }
}
void MapManagerNode::callbackAddPOI(
- const homer_mapnav_msgs::PointOfInterest::ConstPtr& msg) {
- ROS_INFO_STREAM("callbackAddPOI");
- m_POIManager->addPointOfInterest(msg);
+ const homer_mapnav_msgs::PointOfInterest::ConstPtr& msg)
+{
+ ROS_INFO_STREAM("callbackAddPOI");
+ m_POIManager->addPointOfInterest(msg);
}
void MapManagerNode::callbackModifyPOI(
- const homer_mapnav_msgs::ModifyPOI::ConstPtr& msg) {
- m_POIManager->modifyPointOfInterest(msg);
+ const homer_mapnav_msgs::ModifyPOI::ConstPtr& msg)
+{
+ m_POIManager->modifyPointOfInterest(msg);
}
-void MapManagerNode::callbackDeletePOI(const std_msgs::String::ConstPtr& msg) {
- m_POIManager->deletePointOfInterest(msg->data);
+void MapManagerNode::callbackDeletePOI(const std_msgs::String::ConstPtr& msg)
+{
+ m_POIManager->deletePointOfInterest(msg->data);
}
void MapManagerNode::callbackAddROI(
- const homer_mapnav_msgs::RegionOfInterest::ConstPtr& msg) {
- m_ROIManager->addRegionOfInterest(msg);
+ const homer_mapnav_msgs::RegionOfInterest::ConstPtr& msg)
+{
+ m_ROIManager->addRegionOfInterest(msg);
}
void MapManagerNode::callbackModifyROI(
- const homer_mapnav_msgs::RegionOfInterest::ConstPtr& msg) {
- m_ROIManager->modifyRegionOfInterest(msg);
+ const homer_mapnav_msgs::RegionOfInterest::ConstPtr& msg)
+{
+ m_ROIManager->modifyRegionOfInterest(msg);
}
void MapManagerNode::callbackDeleteROIbyName(
- const std_msgs::String::ConstPtr& msg) {
- ROS_INFO_STREAM("Recieved /map_manager/delete_ROI_by_name");
- if (m_ROIManager->deleteRegionOfInterest(msg->data)) {
- ROS_INFO_STREAM("ROI deleted.");
- } else {
- ROS_WARN_STREAM("Could not delete ROI.");
- }
+ const std_msgs::String::ConstPtr& msg)
+{
+ ROS_INFO_STREAM("Recieved /map_manager/delete_ROI_by_name");
+ if (m_ROIManager->deleteRegionOfInterest(msg->data))
+ {
+ ROS_INFO_STREAM("ROI deleted.");
+ }
+ else
+ {
+ ROS_WARN_STREAM("Could not delete ROI.");
+ }
}
-void MapManagerNode::callbackDeleteROIbyID(
- const std_msgs::Int32::ConstPtr& msg) {
- m_ROIManager->deleteRegionOfInterest(msg->data);
+void MapManagerNode::callbackDeleteROIbyID(const std_msgs::Int32::ConstPtr& msg)
+{
+ m_ROIManager->deleteRegionOfInterest(msg->data);
}
void MapManagerNode::callbackMapVisibility(
- const homer_mapnav_msgs::MapLayers::ConstPtr& msg) {
- m_MapManager->toggleMapVisibility(msg->layer, msg->state);
+ const homer_mapnav_msgs::MapLayers::ConstPtr& msg)
+{
+ m_MapManager->toggleMapVisibility(msg->layer, msg->state);
}
void MapManagerNode::callbackModifyMap(
- const homer_mapnav_msgs::ModifyMap::ConstPtr& msg) {
- nav_msgs::OccupancyGrid::ConstPtr maskingMap =
- m_MaskingManager->modifyMap(msg);
- if (msg->mapLayer == homer_mapnav_msgs::MapLayers::SLAM_LAYER ||
- msg->maskAction == homer_mapnav_msgs::ModifyMap::HIGH_SENSITIV) {
- m_MaskSlamMapPublisher.publish(maskingMap);
- m_highSensitiveMap = maskingMap;
- } else {
- m_MapManager->updateMapLayer(
- homer_mapnav_msgs::MapLayers::MASKING_LAYER, maskingMap);
- }
-}
-
-void MapManagerNode::callbackResetMaps(const std_msgs::Empty::ConstPtr& msg) {
- nav_msgs::OccupancyGrid::ConstPtr maskingMap = m_MaskingManager->resetMap();
+ const homer_mapnav_msgs::ModifyMap::ConstPtr& msg)
+{
+ nav_msgs::OccupancyGrid::ConstPtr maskingMap =
+ m_MaskingManager->modifyMap(msg);
+ if (msg->mapLayer == homer_mapnav_msgs::MapLayers::SLAM_LAYER ||
+ msg->maskAction == homer_mapnav_msgs::ModifyMap::HIGH_SENSITIV)
+ {
+ m_MaskSlamMapPublisher.publish(maskingMap);
+ m_highSensitiveMap = maskingMap;
+ }
+ else
+ {
m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER,
maskingMap);
+ }
+}
+
+void MapManagerNode::callbackResetMaps(const std_msgs::Empty::ConstPtr& msg)
+{
+ nav_msgs::OccupancyGrid::ConstPtr maskingMap = m_MaskingManager->resetMap();
+ m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER,
+ maskingMap);
}
void MapManagerNode::callbackLaserScan(
- const sensor_msgs::LaserScan::ConstPtr& msg) {
- m_MapManager->updateLaser(homer_mapnav_msgs::MapLayers::SICK_LAYER, msg);
+ const sensor_msgs::LaserScan::ConstPtr& msg)
+{
+ m_MapManager->updateLaser(homer_mapnav_msgs::MapLayers::SICK_LAYER, msg);
}
void MapManagerNode::callbackBackLaser(
- const sensor_msgs::LaserScan::ConstPtr& msg) {
- m_MapManager->updateLaser(homer_mapnav_msgs::MapLayers::HOKUYO_BACK, msg);
+ const sensor_msgs::LaserScan::ConstPtr& msg)
+{
+ m_MapManager->updateLaser(homer_mapnav_msgs::MapLayers::HOKUYO_BACK, msg);
}
void MapManagerNode::callbackFrontLaser(
- const sensor_msgs::LaserScan::ConstPtr& msg) {
- m_MapManager->updateLaser(homer_mapnav_msgs::MapLayers::HOKUYO_FRONT, msg);
+ const sensor_msgs::LaserScan::ConstPtr& msg)
+{
+ m_MapManager->updateLaser(homer_mapnav_msgs::MapLayers::HOKUYO_FRONT, msg);
}
bool MapManagerNode::getPOIsService(
homer_mapnav_msgs::GetPointsOfInterest::Request& req,
- homer_mapnav_msgs::GetPointsOfInterest::Response& res) {
- res.poi_list.pois = m_POIManager->getList();
- return true;
+ homer_mapnav_msgs::GetPointsOfInterest::Response& res)
+{
+ res.poi_list.pois = m_POIManager->getList();
+ return true;
}
bool MapManagerNode::getROIsService(
homer_mapnav_msgs::GetRegionsOfInterest::Request& req,
- homer_mapnav_msgs::GetRegionsOfInterest::Response& res) {
- res.roi_list.rois = m_ROIManager->getList();
- return true;
+ homer_mapnav_msgs::GetRegionsOfInterest::Response& res)
+{
+ res.roi_list.rois = m_ROIManager->getList();
+ return true;
}
bool MapManagerNode::pointInsideRoisService(
homer_mapnav_msgs::PointInsideRois::Request& req,
- homer_mapnav_msgs::PointInsideRois::Response& res) {
- res.rois = m_ROIManager->pointInsideRegionOfInterest(req.point);
- return true;
+ homer_mapnav_msgs::PointInsideRois::Response& res)
+{
+ res.rois = m_ROIManager->pointInsideRegionOfInterest(req.point);
+ return true;
}
bool MapManagerNode::getROINameService(
homer_mapnav_msgs::GetRegionOfInterestName::Request& req,
- homer_mapnav_msgs::GetRegionOfInterestName::Response& res) {
- res.name = m_ROIManager->getROIName(req.roi_id);
- return true;
+ homer_mapnav_msgs::GetRegionOfInterestName::Response& res)
+{
+ res.name = m_ROIManager->getROIName(req.roi_id);
+ return true;
}
void MapManagerNode::poseCallback(
- const geometry_msgs::PoseStamped::ConstPtr& msg) {
- m_MapManager->updatePose(msg);
- if (msg->header.stamp - m_lastROIPoll > ros::Duration(m_roi_polling_time) &&
- m_roi_polling) {
- m_lastROIPoll = msg->header.stamp;
- geometry_msgs::PointStamped posePoint;
- posePoint.header.frame_id = "/map";
- posePoint.header.stamp = msg->header.stamp;
- posePoint.point = msg->pose.position;
- std::vector<homer_mapnav_msgs::RegionOfInterest> rois;
- rois = m_ROIManager->pointInsideRegionOfInterest(posePoint);
- bool found = false;
- for (int i = 0; i < m_ids.size(); i++) {
- found = false;
- for (int j = 0; j < rois.size(); j++) {
- if (m_ids[i] == rois[j].id) {
- rois.erase(rois.begin() + j);
- found = true;
- break;
- }
- }
- if (!found) {
- homer_mapnav_msgs::RoiChange change;
- change.id = m_ids[i];
- change.name = m_ROIManager->getROIName(m_ids[i]);
- change.action = false;
- m_RoiPollPublisher.publish(change);
- m_ids.erase(m_ids.begin() + i);
- i--;
- }
- }
- for (int i = 0; i < rois.size(); i++) {
- homer_mapnav_msgs::RoiChange change;
- change.id = rois[i].id;
- change.name = m_ROIManager->getROIName(change.id);
- change.action = true;
- m_RoiPollPublisher.publish(change);
- m_ids.push_back(rois[i].id);
+ const geometry_msgs::PoseStamped::ConstPtr& msg)
+{
+ m_MapManager->updatePose(msg);
+ if (msg->header.stamp - m_lastROIPoll > ros::Duration(m_roi_polling_time) &&
+ m_roi_polling)
+ {
+ m_lastROIPoll = msg->header.stamp;
+ geometry_msgs::PointStamped posePoint;
+ posePoint.header.frame_id = "/map";
+ posePoint.header.stamp = msg->header.stamp;
+ posePoint.point = msg->pose.position;
+ std::vector<homer_mapnav_msgs::RegionOfInterest> rois;
+ rois = m_ROIManager->pointInsideRegionOfInterest(posePoint);
+ bool found = false;
+ for (int i = 0; i < m_ids.size(); i++)
+ {
+ found = false;
+ for (int j = 0; j < rois.size(); j++)
+ {
+ if (m_ids[i] == rois[j].id)
+ {
+ rois.erase(rois.begin() + j);
+ found = true;
+ break;
}
+ }
+ if (!found)
+ {
+ homer_mapnav_msgs::RoiChange change;
+ change.id = m_ids[i];
+ change.name = m_ROIManager->getROIName(m_ids[i]);
+ change.action = false;
+ m_RoiPollPublisher.publish(change);
+ m_ids.erase(m_ids.begin() + i);
+ i--;
+ }
}
+ for (int i = 0; i < rois.size(); i++)
+ {
+ homer_mapnav_msgs::RoiChange change;
+ change.id = rois[i].id;
+ change.name = m_ROIManager->getROIName(change.id);
+ change.action = true;
+ m_RoiPollPublisher.publish(change);
+ m_ids.push_back(rois[i].id);
+ }
+ }
}
bool MapManagerNode::saveMapService(homer_mapnav_msgs::SaveMap::Request& req,
- homer_mapnav_msgs::SaveMap::Response& res) {
- // ROS_INFO_STREAM("Saving map "<<req->folder);
- MapGenerator map_saver(std::string(req.folder.data));
- nav_msgs::OccupancyGrid::ConstPtr SLAMMap =
- m_MapManager->getMapLayer(homer_mapnav_msgs::MapLayers::SLAM_LAYER);
- nav_msgs::OccupancyGrid::ConstPtr maskingMap =
- m_MapManager->getMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER);
- map_saver.save(SLAMMap, maskingMap, m_POIManager->getList(),
- m_ROIManager->getList());
+ homer_mapnav_msgs::SaveMap::Response& res)
+{
+ // ROS_INFO_STREAM("Saving map "<<req->folder);
+ MapGenerator map_saver(std::string(req.folder.data));
+ nav_msgs::OccupancyGrid::ConstPtr SLAMMap =
+ m_MapManager->getMapLayer(homer_mapnav_msgs::MapLayers::SLAM_LAYER);
+ nav_msgs::OccupancyGrid::ConstPtr maskingMap =
+ m_MapManager->getMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER);
+ map_saver.save(SLAMMap, maskingMap, m_POIManager->getList(),
+ m_ROIManager->getList());
}
bool MapManagerNode::loadMapService(homer_mapnav_msgs::LoadMap::Request& req,
- homer_mapnav_msgs::LoadMap::Response& res) {
- // load map file from config if present
- std::string mapfile = std::string(req.filename.data);
- if (mapfile != "") {
- ROS_INFO_STREAM("Loading map with filename: " << mapfile);
- std_msgs::String::Ptr mapfileMsg(new std_msgs::String);
- mapfileMsg->data = mapfile;
- callbackLoadMap(mapfileMsg);
- } else {
- ROS_ERROR_STREAM("Map filename is empty. Could not load map");
- }
+ homer_mapnav_msgs::LoadMap::Response& res)
+{
+ // load map file from config if present
+ std::string mapfile = std::string(req.filename.data);
+ if (mapfile != "")
+ {
+ ROS_INFO_STREAM("Loading map with filename: " << mapfile);
+ std_msgs::String::Ptr mapfileMsg(new std_msgs::String);
+ mapfileMsg->data = mapfile;
+ callbackLoadMap(mapfileMsg);
+ }
+ else
+ {
+ ROS_ERROR_STREAM("Map filename is empty. Could not load map");
+ }
}
bool MapManagerNode::resetMapService(std_srvs::Empty::Request& req,
- std_srvs::Empty::Response& res) {
- ROS_INFO_STREAM("Resetting current map");
- nav_msgs::OccupancyGrid::ConstPtr maskingMap = m_MaskingManager->resetMap();
- m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER,
- maskingMap);
+ std_srvs::Empty::Response& res)
+{
+ ROS_INFO_STREAM("Resetting current map");
+ nav_msgs::OccupancyGrid::ConstPtr maskingMap = m_MaskingManager->resetMap();
+ m_MapManager->updateMapLayer(homer_mapnav_msgs::MapLayers::MASKING_LAYER,
+ maskingMap);
}
-int main(int argc, char** argv) {
- ros::init(argc, argv, "map_manager");
- ros::NodeHandle nh;
-
- /* char* pprofile = std::getenv("MAPMANAGER_PROFILE");
- if (pprofile)
- {
- ProfilerStart(pprofile);
- }
- */
-
- MapManagerNode node(&nh);
-
- ros::Rate loop_rate(10);
- while (ros::ok()) {
- try {
- ros::spinOnce();
- loop_rate.sleep();
- } catch (exception& e) {
- std::cout << "Exception in main loop" << e.what() << std::endl;
- }
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "map_manager");
+ ros::NodeHandle nh;
+
+ /* char* pprofile = std::getenv("MAPMANAGER_PROFILE");
+ if (pprofile)
+ {
+ ProfilerStart(pprofile);
+ }
+ */
+
+ MapManagerNode node(&nh);
+
+ ros::Rate loop_rate(10);
+ while (ros::ok())
+ {
+ try
+ {
+ ros::spinOnce();
+ loop_rate.sleep();
}
- /* if (pprofile)
- {
- ProfilerStop();
- }
- */
- return 0;
+ catch (exception& e)
+ {
+ std::cout << "Exception in main loop" << e.what() << std::endl;
+ }
+ }
+ /* if (pprofile)
+ {
+ ProfilerStop();
+ }
+ */
+ return 0;
}
diff --git a/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h b/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h
index 63f51b978d6c4d74dab3ad59095d7aeb6b20acd6..75c71a0cedd02e96296b989dd245e3f23177cb63 100644
--- a/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h
+++ b/homer_mapping/include/homer_mapping/OccupancyMap/OccupancyMap.h
@@ -1,19 +1,19 @@
#ifndef OCCUPANCYMAP_H
#define OCCUPANCYMAP_H
-#include <vector>
+#include <iostream>
#include <list>
#include <string>
-#include <iostream>
+#include <vector>
#include <Eigen/Geometry>
-#include <homer_nav_libs/Math/Pose.h>
-#include <homer_nav_libs/Math/Point2D.h>
#include <homer_nav_libs/Math/Box2D.h>
+#include <homer_nav_libs/Math/Point2D.h>
+#include <homer_nav_libs/Math/Pose.h>
-#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/MapMetaData.h>
+#include <nav_msgs/OccupancyGrid.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/LaserScan.h>
@@ -23,10 +23,14 @@ class QImage;
using namespace std;
/**
- * Structure to store the start and end point of each laser range in the current scan
- * @param sensorPos position of the laser in the current scan (in base_link frame)
- * @param endPos position of the end point of the laser frame in the current scan (in base_link frame)
- * @param free indicates if the laser range hit an obstacle (false) or not (true)
+ * Structure to store the start and end point of each laser range in the current
+ * scan
+ * @param sensorPos position of the laser in the current scan (in base_link
+ * frame)
+ * @param endPos position of the end point of the laser frame in the current
+ * scan (in base_link frame)
+ * @param free indicates if the laser range hit an obstacle (false) or not
+ * (true)
*/
struct RangeMeasurement
{
@@ -36,7 +40,8 @@ struct RangeMeasurement
};
/**
- * Used in struct MeasurePoint to specify if a measurement point is at the border of a scan segment
+ * Used in struct MeasurePoint to specify if a measurement point is at the
+ * border of a scan segment
*/
enum BorderType
{
@@ -49,8 +54,10 @@ enum BorderType
* Structure to store a measurement point for computeLaserScanProbability()
* @param hitPos Position of measured obstacle (robot coordinates)
* @param frontPos Position to check for NOT_KNOWN terrain
- * This is needed to assure that front- and backside of obstacles can be distinguished
- * @param border specifies if the measurement point is at the border of a scan segment
+ * This is needed to assure that front- and backside of
+ * obstacles can be distinguished
+ * @param border specifies if the measurement point is at the border of a scan
+ * segment
*/
struct MeasurePoint
{
@@ -62,309 +69,354 @@ struct MeasurePoint
/**
* @class OccupancyMap
*
- * @author Malte Knauf, Stephan Wirth, Susanne Maur (RX), David Gossow (RX), Susanne Thierfelder (R16)
+ * @author Malte Knauf, Stephan Wirth, Susanne Maur (RX), David Gossow (RX),
+ * Susanne Thierfelder (R16)
*
* @brief This class holds and manages an occupancy map.
*
- * An occupancy map is a map where free space and occupied space are marked. This map stores values
- * for free and occupied space in an (2D-)unsigned char array. This array can be seen as a graylevel image.
- * The darker a cell, the higher the probability that this cell is occupied by an obstacle.
- * The size of the map and the size of one cell can be defined in the setup file with the values
- * MAP_SIZE and MAP_CELL_SIZE. The origin of the coordinate system of the map is the center of the array.
- * The x-axis is heading front, the y-axis points to the left (like the robot's coordinate system).
+ * An occupancy map is a map where free space and occupied space are marked.
+ * This map stores values
+ * for free and occupied space in an (2D-)unsigned char array. This array can be
+ * seen as a graylevel image.
+ * The darker a cell, the higher the probability that this cell is occupied by
+ * an obstacle.
+ * The size of the map and the size of one cell can be defined in the setup file
+ * with the values
+ * MAP_SIZE and MAP_CELL_SIZE. The origin of the coordinate system of the map is
+ * the center of the array.
+ * The x-axis is heading front, the y-axis points to the left (like the robot's
+ * coordinate system).
* The mapping data has to be inserted via the method insertLaserData().
*/
-class OccupancyMap {
-
- public:
- static const int8_t INACCESSIBLE = 100;
- static const int8_t OBSTACLE = 99;
- static const int8_t OCCUPIED = 98;
- static const int8_t UNKNOWN = 50;
- static const int8_t NOT_SEEN_YET = -1;
- static const int8_t FREE = 0;
-
- /**
- * The default constructor calls initMembers().
- */
- OccupancyMap();
-
- /**
- * Constructor for a loaded map.
- */
- OccupancyMap(float*& occupancyProbability, geometry_msgs::Pose origin, float resolution, int width, int height, Box2D<int> exploredRegion);
-
- /**
- * Copy constructor, copies all members inclusive the arrays that lie behind the pointers.
- * @param occupancyMap Source for copying
- */
- OccupancyMap(const OccupancyMap& occupancyMap);
-
- /**
- * Method to init all members with default values from the configuration file. All arrays are initialized.
- */
- void initMembers();
-
- /**
- * Assignment operator, copies all members (deep copy!)
- * @param source Source to copy from
- * @return Reference to copied OccupancyMap
- */
- OccupancyMap& operator=(const OccupancyMap& source);
-
- /**
- * Deletes all dynamically allocated memory.
- */
- ~OccupancyMap();
-
- /*
- /**
- * @return The resolution of the map in m.
- */
-// int resolution() const;
-
- geometry_msgs::Pose origin() const;
-
- /**
- * @return Width of the map.
- */
- int width() const;
-
- /**
- * @return Height of the map.
- */
- int height() const;
-
- /**
- * This method is used to reset all HighSensitive areas
- */
- void resetHighSensitive();
-
- /**
- * @return Probability of pixel p being occupied.
- */
- float getOccupancyProbability(Eigen::Vector2i p);
-
- /**
- * @brief This function inserts the data of a laserscan into the map.
- *
- * With the given data, start and end cells of a laser beam are computed and given to the
- * method markLineFree().
- * If the measurement is smaller than VALID_MAX_RANGE, markOccupied() is called for the endpoint.
- * @param laserData The laser data msg.
- */
- void insertLaserData( sensor_msgs::LaserScan::ConstPtr laserData, tf::Transform transform);
-
- void insertRanges( vector<RangeMeasurement> ranges , ros::Time stamp = ros::Time::now() );
-
- /**
- * @brief gives a list specially processed coordinates to be used for computeLaserScanProbability
- */
- std::vector<MeasurePoint> getMeasurePoints( sensor_msgs::LaserScanConstPtr laserData );
-
- /**
- * This method computes a score that describes how good the given hypothesis matches with the map
- * @param robotPose The pose of the robot
- * @return The "fitting factor". The higher the factor, the better the fitting.
- * This factor is NOT normalized, it is a positive float between 0 and FLOAT_MAX
- */
- float computeScore( Pose robotPose, std::vector<MeasurePoint> measurePoints );
-
- /**
- * @return QImage of size m_metaData.width, m_metaData.height with values of m_OccupancyProbability scaled to 0-254
- */
- QImage getProbabilityQImage(int trancparencyThreshold, bool showInaccessible) const;
-
- //puma2::ColorImageRGB8* getUpdateImage( bool withMap=true ); TODO
-
- /**
- * Returns an "image" of the obstacles e.g. seen in the 3D scans
- * @returns image with dark red dots in areas where the obstacles were seen
- */
- //puma2::ColorImageRGB8* getObstacleImage ( ); TODO
-
- /**
- * Returns an "image" of occupancy probability image.
- * @param[out] data vector containing occupancy probabilities. 0 = free, 100 = occupied, -1 = NOT_KNOWN
- * @param[out] width Width of data array
- * @param[out] height Height of data array
- * @param[out] resolution Resolution of the map (m_metaData.resolution)
- */
- void getOccupancyProbabilityImage(vector<int8_t> &data, nav_msgs::MapMetaData& metaData);
-
- /**
- * This method marks free the position of the robot (according to its dimensions).
- */
- void markRobotPositionFree();
-
- /**
- * @brief Computes the contrast of a single pixel.
- * @param prob probability value (100=occupied, 50=NOT_KNOWN, 0=free) of a pixel.
- * @return Contrast value from 0 (no contrast) to 1 (max. contrast) of this pixel
- */
- double contrastFromProbability (int8_t prob);
-
- /**
- * @brief This method computes the sharpness of the occupancy grid
- * @return Contrast value from 0 (no contrast) to 1 (max. contrast) of the map
- */
- double evaluateByContrast();
-
- /// GETTERS
-
- Box2D<int> getExploredRegion() { return m_ExploredRegion; }
- Box2D<int> getChangedRegion() { return m_ChangedRegion; }
-
- /**
- * Sets cells of this map to free or occupied according to maskMap
- */
- void applyMasking(const nav_msgs::OccupancyGrid::ConstPtr &msg);
-
- void changeMapSize(int x_add_left, int y_add_up, int x_add_right, int y_add_down );
-
-
- protected:
-
- /**
- * This method increments m_MeasurementCount for pixel p.
- * @param p Pixel that has been measured.
- */
- void incrementMeasurementCount(Eigen::Vector2i p);
-
- /**
- * This method increments the occupancy count int m_OccupancyCount for pixel p.
- * @param p Occupied pixel.
- */
- void incrementOccupancyCount(Eigen::Vector2i p);
-
- /**
- * This method increments m_MeasurementCount and if neccessary m_OccupancyCount for all pixels.
- */
- void applyChanges();
-
- void clearChanges();
-
- /**
- * This method scales the counts of all pixels down to the given value.
- * @param maxCount Maximum value to which all counts are set.
- */
- void scaleDownCounts(int maxCount);
-
- /**
- * This function paints a line from a start pixel to an end pixel.
- * The computation is made with the Bresenham algorithm.
- * @param data array on which the line shall be painted
- * @param startPixel starting coordinates of the beam
- * @param endPixel ending coordinates of the beam
- * @param value The value with which the lines are marked.
- */
- template<class DataT>
- void drawLine(DataT* data, Eigen::Vector2i& startPixel, Eigen::Vector2i& endPixel, char value);
-
- /**
- * This method computes the values for m_OccupancyProbabilities from m_MeasurementCount and m_OccupancyCount.
- */
- void computeOccupancyProbabilities();
-
- /**
- * This method sets all values of m_CurrentChanges to NO_CHANGE.
- */
- void clearCurrentChanges();
-
- /**
- * This method resets all values of m_MinChangeX, m_MaxChangeX, m_MinChangeY and m_MaxChangeY.
- * This means that no current changes are assumed.
- */
- void resetChangedRegion();
-
- /**
- * This method updates the values of m_MinChangeX, m_MaxChangeX, m_MinChangeY and m_MaxChangeY to current changes.
- * The area around the current robot pose will be included to the changed region.
- * @param robotPose The current pose of the robot.
- */
- void updateChangedRegion(Pose robotPose);
-
- /**
- * This method sets all values of m_MinChangeX, m_MaxChangeX, m_MinChangeY and m_MaxChangeY
- * to initial values so that the complete map will be processed.
- */
- void maximizeChangedRegion();
-
- /**
- * This method resets all values of m_ExploredX, m_MaxExploredX, m_MinExploredY and m_MaxExploredY.
- */
- void resetExploredRegion();
-
- /**
- * Deletes all allocated members.
- */
- void cleanUp();
-
- /**
- * Stores the metadata of the map
- */
- nav_msgs::MapMetaData m_metaData;
-
-
- /**
- * Stores the size of the map arrays, i.e. m_metaData.width * m_metaData.height
- * for faster computation.
- */
- unsigned m_ByteSize;
-
- /**
- * Array to store occupancy probability values.
- * Values between 0 and 1.
- */
- float* m_OccupancyProbability;
-
- // Counts how often a pixel is hit by a measurement.
- unsigned short* m_MeasurementCount;
-
- // Counts how often a pixel is hit by a measurement that says the pixel is occupied.
- unsigned short* m_OccupancyCount;
-
- // Used for setting flags for cells, that have been modified during the current update.
- unsigned char* m_CurrentChanges;
-
- // Used for high Sensitive areas
- unsigned short* m_HighSensitive;
-
- /**
- * Store values from config files.
- */
- //minimum range classified as free in case of errorneous laser measurement
- float m_FreeReadingDistance;
- //enables checking to avoid matching front- and backside of an obstacle, e.g. wall
- bool m_BacksideChecking;
- //leaves a small border around obstacles unchanged when inserting a laser scan
- bool m_ObstacleBorders;
- //minimum distance in m between two samples for probability calculation
- float m_MeasureSamplingStep;
-
- //bool to reset high_sensitive Areas on the next iteration
- bool m_reset_high;
-
- /**
- * Defines a bounding box around the changes in the current map.
- */
- Box2D<int> m_ChangedRegion;
-
- /**
- * Defines a bounding box around the area in the map, which is already explored.
- */
- Box2D<int> m_ExploredRegion;
-
- /**
- * ros transform listener
- */
- tf::TransformListener m_tfListener;
-
- /**
- * ros transformation laser to base_link
- */
- tf::StampedTransform m_laserTransform;
- tf::Transform m_latestMapTransform;
-
+class OccupancyMap
+{
+public:
+ static const int8_t INACCESSIBLE = 100;
+ static const int8_t OBSTACLE = 99;
+ static const int8_t OCCUPIED = 98;
+ static const int8_t UNKNOWN = 50;
+ static const int8_t NOT_SEEN_YET = -1;
+ static const int8_t FREE = 0;
+
+ /**
+ * The default constructor calls initMembers().
+ */
+ OccupancyMap();
+
+ /**
+ * Constructor for a loaded map.
+ */
+ OccupancyMap(float*& occupancyProbability, geometry_msgs::Pose origin,
+ float resolution, int width, int height,
+ Box2D<int> exploredRegion);
+
+ /**
+ * Copy constructor, copies all members inclusive the arrays that lie behind
+ * the pointers.
+ * @param occupancyMap Source for copying
+ */
+ OccupancyMap(const OccupancyMap& occupancyMap);
+
+ /**
+ * Method to init all members with default values from the configuration file.
+ * All arrays are initialized.
+ */
+ void initMembers();
+
+ /**
+ * Assignment operator, copies all members (deep copy!)
+ * @param source Source to copy from
+ * @return Reference to copied OccupancyMap
+ */
+ OccupancyMap& operator=(const OccupancyMap& source);
+
+ /**
+ * Deletes all dynamically allocated memory.
+ */
+ ~OccupancyMap();
+
+ /*
+ /**
+ * @return The resolution of the map in m.
+ */
+ // int resolution() const;
+
+ geometry_msgs::Pose origin() const;
+
+ /**
+ * @return Width of the map.
+ */
+ int width() const;
+
+ /**
+ * @return Height of the map.
+ */
+ int height() const;
+
+ /**
+ * This method is used to reset all HighSensitive areas
+ */
+ void resetHighSensitive();
+
+ /**
+ * @return Probability of pixel p being occupied.
+ */
+ float getOccupancyProbability(Eigen::Vector2i p);
+
+ /**
+ * @brief This function inserts the data of a laserscan into the map.
+ *
+ * With the given data, start and end cells of a laser beam are computed and
+ * given to the
+ * method markLineFree().
+ * If the measurement is smaller than VALID_MAX_RANGE, markOccupied() is
+ * called for the endpoint.
+ * @param laserData The laser data msg.
+ */
+ void insertLaserData(sensor_msgs::LaserScan::ConstPtr laserData,
+ tf::Transform transform);
+
+ void insertRanges(vector<RangeMeasurement> ranges,
+ ros::Time stamp = ros::Time::now());
+
+ /**
+ * @brief gives a list specially processed coordinates to be used for
+ * computeLaserScanProbability
+ */
+ std::vector<MeasurePoint>
+ getMeasurePoints(sensor_msgs::LaserScanConstPtr laserData);
+
+ /**
+ * This method computes a score that describes how good the given hypothesis
+ * matches with the map
+ * @param robotPose The pose of the robot
+ * @return The "fitting factor". The higher the factor, the better the
+ * fitting.
+ * This factor is NOT normalized, it is a positive float between 0 and
+ * FLOAT_MAX
+ */
+ float computeScore(Pose robotPose, std::vector<MeasurePoint> measurePoints);
+
+ /**
+ * @return QImage of size m_metaData.width, m_metaData.height with values of
+ * m_OccupancyProbability scaled to 0-254
+ */
+ QImage getProbabilityQImage(int trancparencyThreshold,
+ bool showInaccessible) const;
+
+ // puma2::ColorImageRGB8* getUpdateImage( bool withMap=true ); TODO
+
+ /**
+ * Returns an "image" of the obstacles e.g. seen in the 3D scans
+ * @returns image with dark red dots in areas where the obstacles were seen
+ */
+ // puma2::ColorImageRGB8* getObstacleImage ( ); TODO
+
+ /**
+ * Returns an "image" of occupancy probability image.
+ * @param[out] data vector containing occupancy probabilities. 0 = free, 100 =
+ * occupied, -1 = NOT_KNOWN
+ * @param[out] width Width of data array
+ * @param[out] height Height of data array
+ * @param[out] resolution Resolution of the map (m_metaData.resolution)
+ */
+ void getOccupancyProbabilityImage(vector<int8_t>& data,
+ nav_msgs::MapMetaData& metaData);
+
+ /**
+ * This method marks free the position of the robot (according to its
+ * dimensions).
+ */
+ void markRobotPositionFree();
+
+ /**
+ * @brief Computes the contrast of a single pixel.
+ * @param prob probability value (100=occupied, 50=NOT_KNOWN, 0=free) of a
+ * pixel.
+ * @return Contrast value from 0 (no contrast) to 1 (max. contrast) of this
+ * pixel
+ */
+ double contrastFromProbability(int8_t prob);
+
+ /**
+ * @brief This method computes the sharpness of the occupancy grid
+ * @return Contrast value from 0 (no contrast) to 1 (max. contrast) of the map
+ */
+ double evaluateByContrast();
+
+ /// GETTERS
+
+ Box2D<int> getExploredRegion()
+ {
+ return m_ExploredRegion;
+ }
+ Box2D<int> getChangedRegion()
+ {
+ return m_ChangedRegion;
+ }
+
+ /**
+ * Sets cells of this map to free or occupied according to maskMap
+ */
+ void applyMasking(const nav_msgs::OccupancyGrid::ConstPtr& msg);
+
+ void changeMapSize(int x_add_left, int y_add_up, int x_add_right,
+ int y_add_down);
+
+protected:
+ /**
+ * This method increments m_MeasurementCount for pixel p.
+ * @param p Pixel that has been measured.
+ */
+ void incrementMeasurementCount(Eigen::Vector2i p);
+
+ /**
+ * This method increments the occupancy count int m_OccupancyCount for pixel
+ * p.
+ * @param p Occupied pixel.
+ */
+ void incrementOccupancyCount(Eigen::Vector2i p);
+
+ /**
+ * This method increments m_MeasurementCount and if neccessary
+ * m_OccupancyCount for all pixels.
+ */
+ void applyChanges();
+
+ void clearChanges();
+
+ /**
+ * This method scales the counts of all pixels down to the given value.
+ * @param maxCount Maximum value to which all counts are set.
+ */
+ void scaleDownCounts(int maxCount);
+
+ /**
+ * This function paints a line from a start pixel to an end pixel.
+ * The computation is made with the Bresenham algorithm.
+ * @param data array on which the line shall be painted
+ * @param startPixel starting coordinates of the beam
+ * @param endPixel ending coordinates of the beam
+ * @param value The value with which the lines are marked.
+ */
+ template <class DataT>
+ void drawLine(DataT* data, Eigen::Vector2i& startPixel,
+ Eigen::Vector2i& endPixel, char value);
+
+ /**
+ * This method computes the values for m_OccupancyProbabilities from
+ * m_MeasurementCount and m_OccupancyCount.
+ */
+ void computeOccupancyProbabilities();
+
+ /**
+ * This method sets all values of m_CurrentChanges to NO_CHANGE.
+ */
+ void clearCurrentChanges();
+
+ /**
+ * This method resets all values of m_MinChangeX, m_MaxChangeX, m_MinChangeY
+ * and m_MaxChangeY.
+ * This means that no current changes are assumed.
+ */
+ void resetChangedRegion();
+
+ /**
+ * This method updates the values of m_MinChangeX, m_MaxChangeX, m_MinChangeY
+ * and m_MaxChangeY to current changes.
+ * The area around the current robot pose will be included to the changed
+ * region.
+ * @param robotPose The current pose of the robot.
+ */
+ void updateChangedRegion(Pose robotPose);
+
+ /**
+ * This method sets all values of m_MinChangeX, m_MaxChangeX, m_MinChangeY and
+ * m_MaxChangeY
+ * to initial values so that the complete map will be processed.
+ */
+ void maximizeChangedRegion();
+
+ /**
+ * This method resets all values of m_ExploredX, m_MaxExploredX,
+ * m_MinExploredY and m_MaxExploredY.
+ */
+ void resetExploredRegion();
+
+ /**
+ * Deletes all allocated members.
+ */
+ void cleanUp();
+
+ /**
+ * Stores the metadata of the map
+ */
+ nav_msgs::MapMetaData m_metaData;
+
+ /**
+ * Stores the size of the map arrays, i.e. m_metaData.width *
+ * m_metaData.height
+ * for faster computation.
+ */
+ unsigned m_ByteSize;
+
+ /**
+ * Array to store occupancy probability values.
+ * Values between 0 and 1.
+ */
+ float* m_OccupancyProbability;
+
+ // Counts how often a pixel is hit by a measurement.
+ unsigned short* m_MeasurementCount;
+
+ // Counts how often a pixel is hit by a measurement that says the pixel is
+ // occupied.
+ unsigned short* m_OccupancyCount;
+
+ // Used for setting flags for cells, that have been modified during the
+ // current update.
+ unsigned char* m_CurrentChanges;
+
+ // Used for high Sensitive areas
+ unsigned short* m_HighSensitive;
+
+ /**
+ * Store values from config files.
+ */
+ // minimum range classified as free in case of errorneous laser measurement
+ float m_FreeReadingDistance;
+ // enables checking to avoid matching front- and backside of an obstacle, e.g.
+ // wall
+ bool m_BacksideChecking;
+ // leaves a small border around obstacles unchanged when inserting a laser
+ // scan
+ bool m_ObstacleBorders;
+ // minimum distance in m between two samples for probability calculation
+ float m_MeasureSamplingStep;
+
+ // bool to reset high_sensitive Areas on the next iteration
+ bool m_reset_high;
+
+ /**
+ * Defines a bounding box around the changes in the current map.
+ */
+ Box2D<int> m_ChangedRegion;
+
+ /**
+ * Defines a bounding box around the area in the map, which is already
+ * explored.
+ */
+ Box2D<int> m_ExploredRegion;
+
+ /**
+ * ros transform listener
+ */
+ tf::TransformListener m_tfListener;
+
+ /**
+ * ros transformation laser to base_link
+ */
+ tf::StampedTransform m_laserTransform;
+ tf::Transform m_latestMapTransform;
};
#endif
diff --git a/homer_mapping/include/homer_mapping/ParticleFilter/HyperSlamFilter.h b/homer_mapping/include/homer_mapping/ParticleFilter/HyperSlamFilter.h
old mode 100755
new mode 100644
index c85da226e7e2db741d950381f3f887a46b1b163a..6b5d52751441fd0d9be4fe5b78ad691d37a93cea
--- a/homer_mapping/include/homer_mapping/ParticleFilter/HyperSlamFilter.h
+++ b/homer_mapping/include/homer_mapping/ParticleFilter/HyperSlamFilter.h
@@ -1,12 +1,12 @@
#ifndef HYPERSLAMFILTER_H
#define HYPERSLAMFILTER_H
-#include <vector>
+#include <homer_mapping/OccupancyMap/OccupancyMap.h>
#include <homer_mapping/ParticleFilter/ParticleFilter.h>
-#include <homer_mapping/ParticleFilter/SlamParticle.h>
#include <homer_mapping/ParticleFilter/SlamFilter.h>
+#include <homer_mapping/ParticleFilter/SlamParticle.h>
#include <homer_nav_libs/Math/Pose.h>
-#include <homer_mapping/OccupancyMap/OccupancyMap.h>
+#include <vector>
#include <sensor_msgs/LaserScan.h>
@@ -17,147 +17,160 @@ class OccupancyMap;
*
* @author Malte Knauf, Stephan Wirth, Susanne Maur
*
- * @brief This class is used to determine the robot's most likely pose with given map and given laser data.
+ * @brief This class is used to determine the robot's most likely pose with
+ * given map and given laser data.
*
- * A particle filter is a descrete method to describe and compute with a probability distribution.
- * This particle filter uses an occupancy map to determine the probability of robot states.
- * The robot states are stored in a particle together with their weight @see SlamParticle.
+ * A particle filter is a descrete method to describe and compute with a
+ * probability distribution.
+ * This particle filter uses an occupancy map to determine the probability of
+ * robot states.
+ * The robot states are stored in a particle together with their weight @see
+ * SlamParticle.
*
* @see SlamParticle
* @see ParticleFilter
* @see OccupancyMap
*/
-class HyperSlamFilter {
-
- public:
-
- /**
- * This constructor initializes the random number generator and sets the member variables to the given values.
- * @param particleNum Number of particleFilters to use.
- */
- HyperSlamFilter(int particleFilterNum, int particleNum);
-
- /**
- * The destructor releases the OccupancyMap and the particles.
- */
- ~HyperSlamFilter();
-
- /**
- * This method runs the filter routine.
- * The given odometry measurement is used as movement hypothesis, the laserData-vector is used
- * as measurement and is used to weight the particles.
- * @param currentPoseOdometry Odometry data of time t.
- * @param laserData msg containing the laser measurement.
- * @param measurementTime Time stamp of the measurement.
- * @param filterDurationTime Returns the time in ms that the filtering needed
- */
- void filter(Pose currentPoseOdometry, sensor_msgs::LaserScanConstPtr laserData, ros::Time measurementTime,
- ros::Duration &filterDuration);
-
- /**
- * Computes and sets the new value for m_Alpha1.
- * @param percent Rotation error while rotating (see class constructor for details)
- */
- void setRotationErrorRotating(float percent);
-
- /**
- * Computes and sets the new value for m_Alpha2.
- * @param degreesPerMeter Rotation error while translating (see class constructor for details)
- */
- void setRotationErrorTranslating(float degreesPerMeter);
-
- /**
- * Computes and sets the new value for m_Alpha3.
- * @param percent Translation error while translating (see class constructor for details)
- */
- void setTranslationErrorTranslating(float percent);
-
- /**
- * Computes and sets the new value for m_Alpha4.
- * @param mPerDegree Translation error while rotating (see class constructor for details)
- */
- void setTranslationErrorRotating(float mPerDegree);
-
- /**
- * Computes and sets the new value for m_Alpha5.
- * @param mPerDegree Move jitter while turning (see class constructor for details)
- */
- void setMoveJitterWhileTurning(float mPerDegree);
-
- /**
- * Sets a new minimal size of a cluster of scan points which is considered in scan matching.
- * @param clusterSize Minimal size of a cluster in mm of scan points which is considered in scan matching.
- */
- void setScanMatchingClusterSize(float clusterSize);
-
- /**
- * Sets whether the map is updated or just used for self-localization.
- * @param doMapping True if robot shall do mapping, false otherwise.
- */
- void setMapping(bool doMapping);
-
- /**
- * Deletes the current occupancy map and copies a new one into the system.
- * @param occupancyMap The occupancy map to load into the system (copies are being made)
- */
- void setOccupancyMap(OccupancyMap* occupancyMap);
-
- /**
- * Sets the robot pose in the current occupancy map.
- * @param Robot pose.
- * @param scatterVariance if not equal to 0 the poses are equally scattered around the pose
- */
- void setRobotPose(Pose pose, double scatterVarXY=0.0, double scatterVarTheta=0.0);
-
- /**
- *Returns the best SlamFilter
- */
- SlamFilter* getBestSlamFilter();
-
- void resetHigh();
-
- /**
- * Factor (default 0.98) of the contrast of the best particle.
- * If the contrast of the worst particle is below this threshold
- * it will be replaced by the best particle
- * @param deletionThreshold see above
- */
- void setDeletionThreshold(double deletionThreshold);
-
- /**
- * applies masking to map of slam filter set in GUI
- * @param msg masking message received from GUI
- */
- void applyMasking(const nav_msgs::OccupancyGrid::ConstPtr &msg)
+class HyperSlamFilter
+{
+public:
+ /**
+ * This constructor initializes the random number generator and sets the
+ * member variables to the given values.
+ * @param particleNum Number of particleFilters to use.
+ */
+ HyperSlamFilter(int particleFilterNum, int particleNum);
+
+ /**
+ * The destructor releases the OccupancyMap and the particles.
+ */
+ ~HyperSlamFilter();
+
+ /**
+ * This method runs the filter routine.
+ * The given odometry measurement is used as movement hypothesis, the
+ * laserData-vector is used
+ * as measurement and is used to weight the particles.
+ * @param currentPoseOdometry Odometry data of time t.
+ * @param laserData msg containing the laser measurement.
+ * @param measurementTime Time stamp of the measurement.
+ * @param filterDurationTime Returns the time in ms that the filtering needed
+ */
+ void filter(Pose currentPoseOdometry,
+ sensor_msgs::LaserScanConstPtr laserData,
+ ros::Time measurementTime, ros::Duration& filterDuration);
+
+ /**
+ * Computes and sets the new value for m_Alpha1.
+ * @param percent Rotation error while rotating (see class constructor for
+ * details)
+ */
+ void setRotationErrorRotating(float percent);
+
+ /**
+ * Computes and sets the new value for m_Alpha2.
+ * @param degreesPerMeter Rotation error while translating (see class
+ * constructor for details)
+ */
+ void setRotationErrorTranslating(float degreesPerMeter);
+
+ /**
+ * Computes and sets the new value for m_Alpha3.
+ * @param percent Translation error while translating (see class constructor
+ * for details)
+ */
+ void setTranslationErrorTranslating(float percent);
+
+ /**
+ * Computes and sets the new value for m_Alpha4.
+ * @param mPerDegree Translation error while rotating (see class constructor
+ * for details)
+ */
+ void setTranslationErrorRotating(float mPerDegree);
+
+ /**
+ * Computes and sets the new value for m_Alpha5.
+ * @param mPerDegree Move jitter while turning (see class constructor for
+ * details)
+ */
+ void setMoveJitterWhileTurning(float mPerDegree);
+
+ /**
+ * Sets a new minimal size of a cluster of scan points which is considered in
+ * scan matching.
+ * @param clusterSize Minimal size of a cluster in mm of scan points which is
+ * considered in scan matching.
+ */
+ void setScanMatchingClusterSize(float clusterSize);
+
+ /**
+ * Sets whether the map is updated or just used for self-localization.
+ * @param doMapping True if robot shall do mapping, false otherwise.
+ */
+ void setMapping(bool doMapping);
+
+ /**
+ * Deletes the current occupancy map and copies a new one into the system.
+ * @param occupancyMap The occupancy map to load into the system (copies are
+ * being made)
+ */
+ void setOccupancyMap(OccupancyMap* occupancyMap);
+
+ /**
+ * Sets the robot pose in the current occupancy map.
+ * @param Robot pose.
+ * @param scatterVariance if not equal to 0 the poses are equally scattered
+ * around the pose
+ */
+ void setRobotPose(Pose pose, double scatterVarXY = 0.0,
+ double scatterVarTheta = 0.0);
+
+ /**
+ *Returns the best SlamFilter
+ */
+ SlamFilter* getBestSlamFilter();
+
+ void resetHigh();
+
+ /**
+ * Factor (default 0.98) of the contrast of the best particle.
+ * If the contrast of the worst particle is below this threshold
+ * it will be replaced by the best particle
+ * @param deletionThreshold see above
+ */
+ void setDeletionThreshold(double deletionThreshold);
+
+ /**
+ * applies masking to map of slam filter set in GUI
+ * @param msg masking message received from GUI
+ */
+ void applyMasking(const nav_msgs::OccupancyGrid::ConstPtr& msg)
+ {
+ for (unsigned i = 0; i < m_ParticleFilterNum; ++i)
{
- for(unsigned i = 0; i < m_ParticleFilterNum; ++i)
- {
- m_SlamFilters[i]->applyMasking(msg);
- }
+ m_SlamFilters[i]->applyMasking(msg);
}
+ }
- private:
-
- /** Used SlamFilters */
- std::vector <SlamFilter*> m_SlamFilters;
+private:
+ /** Used SlamFilters */
+ std::vector<SlamFilter*> m_SlamFilters;
- /** Number of SlamFilters */
- unsigned m_ParticleFilterNum;
+ /** Number of SlamFilters */
+ unsigned m_ParticleFilterNum;
- /** Number of Particles of SlamFilter */
- unsigned m_ParticleNum;
+ /** Number of Particles of SlamFilter */
+ unsigned m_ParticleNum;
- /** */
- double m_DeletionThreshold;
+ /** */
+ double m_DeletionThreshold;
- /** Best SLAM Filter */
- SlamFilter* m_BestSlamFilter;
+ /** Best SLAM Filter */
+ SlamFilter* m_BestSlamFilter;
- /** Worst SlamFilter */
- SlamFilter* m_WorstSlamFilter;
-
- bool m_DoMapping;
+ /** Worst SlamFilter */
+ SlamFilter* m_WorstSlamFilter;
+ bool m_DoMapping;
};
#endif
-
diff --git a/homer_mapping/include/homer_mapping/ParticleFilter/Particle.h b/homer_mapping/include/homer_mapping/ParticleFilter/Particle.h
index bd96aa857da906c16c47914e38c7bbe000178e2b..425667d4a1f2b9c187f3e75b6a2ba855052684a0 100644
--- a/homer_mapping/include/homer_mapping/ParticleFilter/Particle.h
+++ b/homer_mapping/include/homer_mapping/ParticleFilter/Particle.h
@@ -1,64 +1,74 @@
#ifndef PARTICLE_H
-#define PARTICLE_H
+#define PARTICLE_H
-#include <iostream>
#include <fstream>
+#include <iostream>
-/**
+/**
* @class Particle
*
* @author Malte Knauf, Stephan Wirth
*
* @brief This class is an implementation of a "particle".
*
- * A particle as it is used in particle filters is a set of one state and one importance factor (=weight).
- * A set of Particles is a discrete representation of a probability distribution.
- *
+ * A particle as it is used in particle filters is a set of one state and one
+ * importance factor (=weight).
+ * A set of Particles is a discrete representation of a probability
+ * distribution.
+ *
* @see ParticleFilter
*/
-class Particle {
-
- public:
- /**
- * This constructor assigns the given weight to the member m_Weight.
- * @param weight The weight of the particle.
- */
- Particle(float weight = 0.0, int id = 0);
+class Particle
+{
+public:
+ /**
+ * This constructor assigns the given weight to the member m_Weight.
+ * @param weight The weight of the particle.
+ */
+ Particle(float weight = 0.0, int id = 0);
- /**
- * The destructor does nothing so far.
- */
- virtual ~Particle();
+ /**
+ * The destructor does nothing so far.
+ */
+ virtual ~Particle();
- /**
- * This method returns the importance factor of the particle.
- * @return The importance factor (=weight) of the particle.
- */
- inline float getWeight() const { return m_Weight; }
+ /**
+ * This method returns the importance factor of the particle.
+ * @return The importance factor (=weight) of the particle.
+ */
+ inline float getWeight() const
+ {
+ return m_Weight;
+ }
- /**
- * Method to set the weight of the particle.
- * @param newWeight New weight for the particle.
- */
- inline void setWeight(float newWeight) { m_Weight=newWeight; }
+ /**
+ * Method to set the weight of the particle.
+ * @param newWeight New weight for the particle.
+ */
+ inline void setWeight(float newWeight)
+ {
+ m_Weight = newWeight;
+ }
- /**
- * @return id of the particle that is stored in m_Id
- */
- inline int getId() { return m_Id; }
+ /**
+ * @return id of the particle that is stored in m_Id
+ */
+ inline int getId()
+ {
+ return m_Id;
+ }
- private:
- /**
- * Stores the importance factor (=weight) of the particle. This should be a value between 0 and 1.
- */
- float m_Weight;
-
- /**
- * Stores the id of the particle (for testing purpose)
- */
- int m_Id;
+private:
+ /**
+ * Stores the importance factor (=weight) of the particle. This should be a
+ * value between 0 and 1.
+ */
+ float m_Weight;
+ /**
+ * Stores the id of the particle (for testing purpose)
+ */
+ int m_Id;
};
#endif
-
diff --git a/homer_mapping/include/homer_mapping/ParticleFilter/ParticleFilter.h b/homer_mapping/include/homer_mapping/ParticleFilter/ParticleFilter.h
index 651f758714e8b1b2ca8a03338b2f46beea05c195..dbb4b0a519039c9e1a38beb32fde4244f7d6bf24 100644
--- a/homer_mapping/include/homer_mapping/ParticleFilter/ParticleFilter.h
+++ b/homer_mapping/include/homer_mapping/ParticleFilter/ParticleFilter.h
@@ -1,10 +1,10 @@
#ifndef PARTICLEFILTER_H
#define PARTICLEFILTER_H
-#include <iostream>
-#include <cmath>
#include <limits.h>
#include <omp.h>
+#include <cmath>
+#include <iostream>
#include <ros/ros.h>
@@ -19,117 +19,135 @@ const float MIN_EFFECTIVE_PARTICLE_WEIGHT = 0.2;
*
* @brief This class is a template class for a particle filter.
*
- * A particle filter is a descrete method to describe and compute with a probability distribution.
- * This template class implements the basic methods for a particle filter: sort() and resample().
- * Use this class do derivate your custom particle filter from it. Use a self-defined subclass of
+ * A particle filter is a descrete method to describe and compute with a
+ * probability distribution.
+ * This template class implements the basic methods for a particle filter:
+ * sort() and resample().
+ * Use this class do derivate your custom particle filter from it. Use a
+ * self-defined subclass of
* Particle as ParticleType.
*
* @see Particle
*/
template <class ParticleType>
-class ParticleFilter {
-
- public:
- /**
- * The constructor initializes the random number generator and allocates the memory for the particle lists.
- * The lists will have particleNum elements.
- * @param particleNum Number of particles for the filter.
- */
- ParticleFilter<ParticleType>(int particleNum);
-
- /**
- * The destructor releases the particle lists.
- */
- virtual ~ParticleFilter();
-
- /**
- * @return Number of particles used in this filter
- */
- int getParticleNum();
-
- /**
- * @return The number of effective particles (according to "Improving Grid-based SLAM with Rao-Blackwellized Particle
- * Filters by Adaptive Proposals and Selective Resampling (2005)" by Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
- */
- int getEffectiveParticleNum() const;
- int getEffectiveParticleNum2() const;
-
- /**
- * @return Pointer to the particle that has the highest weight.
- */
- ParticleType* getBestParticle() const;
-
-
- protected:
-
- /**
- * This method generates a random variable in the interval [0,1].
- * @param init The initial value for the static random base number. When running the constructor of this
- * class, this method is run once with the C-function time() as parameter to initialize it.
- * Then you should use it without parameter.
- * @return Random value between 0 and 1
- */
- double random01(unsigned long init = 0) const;
-
- /**
- * This method sorts the particles in m_CurrentList from leftIndex to rightIndex according to their weight.
- * The particle with the highest weight is at position 0 after calling this function. The algorithm used here is
- * known as quicksort and works recursively.
- * @param leftIndex Left index of area to sort
- * @param rightIndex Right index of area to sort
- */
- void sort(int leftIndex, int rightIndex);
-
- /**
- * This method normalizes the weights of the particles. After calling this function, the sum of the weights of
- * all particles in m_CurrentList equals 1.0.
- * In this function the sum of all weights of the particles of m_CurrentList is computed and each weight of each
- * particle is devided through this sum.
- */
- void normalize();
-
- /**
- * This method selects a new set of particles out of an old set according to their weight
- * (importance resampling). The particles from the list m_CurrentList points to are used as source,
- * m_LastList points to the destination list. The pointers m_CurrentList and m_LastList are switched.
- * The higher the weight of a particle, the more particles are drawn (copied) from this particle.
- * The weight remains untouched, because measure() will be called afterwards.
- * This method only works on a sorted m_CurrentList, therefore sort() is called first.
- */
- void resample();
-
- /**
- * This method drifts the particles (second step of a filter process).
- * Has to be implemented in sub-classes (pure virtual function).
- */
- virtual void drift() = 0;
-
- /**
- * This method has to be implemented in sub-classes. It is used to determine the weight of each particle.
- */
- virtual void measure() = 0;
-
- /**
- * These two pointers point to m_ParticleListOne and to m_ParticleListTwo.
- * The particles are drawn from m_LastList to m_CurrentList to avoid new and delete commands.
- * In each run, the pointers are switched in resample().
- */
- ParticleType** m_CurrentList;
- ParticleType** m_LastList;
-
- /**
- * Stores the number of particles.
- */
- int m_ParticleNum;
-
- /**
- * Stores the number of effective particles.
- */
- int m_EffectiveParticleNum;
+class ParticleFilter
+{
+public:
+ /**
+ * The constructor initializes the random number generator and allocates the
+ * memory for the particle lists.
+ * The lists will have particleNum elements.
+ * @param particleNum Number of particles for the filter.
+ */
+ ParticleFilter<ParticleType>(int particleNum);
+
+ /**
+ * The destructor releases the particle lists.
+ */
+ virtual ~ParticleFilter();
+
+ /**
+ * @return Number of particles used in this filter
+ */
+ int getParticleNum();
+
+ /**
+ * @return The number of effective particles (according to "Improving
+ * Grid-based SLAM with Rao-Blackwellized Particle
+ * Filters by Adaptive Proposals and Selective Resampling (2005)" by Giorgio
+ * Grisetti, Cyrill Stachniss, Wolfram Burgard
+ */
+ int getEffectiveParticleNum() const;
+ int getEffectiveParticleNum2() const;
+
+ /**
+ * @return Pointer to the particle that has the highest weight.
+ */
+ ParticleType* getBestParticle() const;
+
+protected:
+ /**
+ * This method generates a random variable in the interval [0,1].
+ * @param init The initial value for the static random base number. When
+ * running the constructor of this
+ * class, this method is run once with the C-function time() as parameter to
+ * initialize it.
+ * Then you should use it without parameter.
+ * @return Random value between 0 and 1
+ */
+ double random01(unsigned long init = 0) const;
+
+ /**
+ * This method sorts the particles in m_CurrentList from leftIndex to
+ * rightIndex according to their weight.
+ * The particle with the highest weight is at position 0 after calling this
+ * function. The algorithm used here is
+ * known as quicksort and works recursively.
+ * @param leftIndex Left index of area to sort
+ * @param rightIndex Right index of area to sort
+ */
+ void sort(int leftIndex, int rightIndex);
+
+ /**
+ * This method normalizes the weights of the particles. After calling this
+ * function, the sum of the weights of
+ * all particles in m_CurrentList equals 1.0.
+ * In this function the sum of all weights of the particles of m_CurrentList
+ * is computed and each weight of each
+ * particle is devided through this sum.
+ */
+ void normalize();
+
+ /**
+ * This method selects a new set of particles out of an old set according to
+ * their weight
+ * (importance resampling). The particles from the list m_CurrentList points
+ * to are used as source,
+ * m_LastList points to the destination list. The pointers m_CurrentList and
+ * m_LastList are switched.
+ * The higher the weight of a particle, the more particles are drawn (copied)
+ * from this particle.
+ * The weight remains untouched, because measure() will be called afterwards.
+ * This method only works on a sorted m_CurrentList, therefore sort() is
+ * called first.
+ */
+ void resample();
+
+ /**
+ * This method drifts the particles (second step of a filter process).
+ * Has to be implemented in sub-classes (pure virtual function).
+ */
+ virtual void drift() = 0;
+
+ /**
+ * This method has to be implemented in sub-classes. It is used to determine
+ * the weight of each particle.
+ */
+ virtual void measure() = 0;
+
+ /**
+ * These two pointers point to m_ParticleListOne and to m_ParticleListTwo.
+ * The particles are drawn from m_LastList to m_CurrentList to avoid new and
+ * delete commands.
+ * In each run, the pointers are switched in resample().
+ */
+ ParticleType** m_CurrentList;
+ ParticleType** m_LastList;
+
+ /**
+ * Stores the number of particles.
+ */
+ int m_ParticleNum;
+
+ /**
+ * Stores the number of effective particles.
+ */
+ int m_EffectiveParticleNum;
};
template <class ParticleType>
-ParticleFilter<ParticleType>::ParticleFilter(int particleNum) {
+ParticleFilter<ParticleType>::ParticleFilter(int particleNum)
+{
// initialize particle lists
m_CurrentList = new ParticleType*[particleNum];
m_LastList = new ParticleType*[particleNum];
@@ -140,41 +158,46 @@ ParticleFilter<ParticleType>::ParticleFilter(int particleNum) {
m_ParticleNum = particleNum;
}
-
template <class ParticleType>
-ParticleFilter<ParticleType>::~ParticleFilter() {
- if (m_CurrentList) {
+ParticleFilter<ParticleType>::~ParticleFilter()
+{
+ if (m_CurrentList)
+ {
delete[] m_CurrentList;
m_CurrentList = 0;
}
- if (m_LastList) {
+ if (m_LastList)
+ {
delete[] m_LastList;
m_LastList = 0;
}
}
template <class ParticleType>
-int ParticleFilter<ParticleType>::getParticleNum() {
+int ParticleFilter<ParticleType>::getParticleNum()
+{
return m_ParticleNum;
}
template <class ParticleType>
-double ParticleFilter<ParticleType>::random01(unsigned long init) const {
+double ParticleFilter<ParticleType>::random01(unsigned long init) const
+{
static unsigned long n;
- if (init > 0) {
+ if (init > 0)
+ {
n = init;
}
n = 1664525 * n + 1013904223;
// create double from unsigned long
- return (double)(n/2) / (double)LONG_MAX;
+ return (double)(n / 2) / (double)LONG_MAX;
}
-
template <class ParticleType>
-void ParticleFilter<ParticleType>::sort(int indexLeft, int indexRight) {
-
+void ParticleFilter<ParticleType>::sort(int indexLeft, int indexRight)
+{
// SOMETHING LEFT TO SORT?
- if (indexLeft >= indexRight) {
+ if (indexLeft >= indexRight)
+ {
// ready!
return;
}
@@ -184,17 +207,22 @@ void ParticleFilter<ParticleType>::sort(int indexLeft, int indexRight) {
int ri = indexRight;
int first = le;
int pivot = ri--;
- while(le <= ri) {
+ while (le <= ri)
+ {
// skip from left
- while(m_CurrentList[le]->getWeight() > m_CurrentList[pivot]->getWeight()) {
+ while (m_CurrentList[le]->getWeight() > m_CurrentList[pivot]->getWeight())
+ {
le++;
}
// skip from right
- while((ri >= first) && (m_CurrentList[ri]->getWeight() <= m_CurrentList[pivot]->getWeight())) {
+ while ((ri >= first) && (m_CurrentList[ri]->getWeight() <=
+ m_CurrentList[pivot]->getWeight()))
+ {
ri--;
}
// now we have two elements to swap
- if(le < ri) {
+ if (le < ri)
+ {
// swap
ParticleType* temp = m_CurrentList[le];
m_CurrentList[le] = m_CurrentList[ri];
@@ -203,7 +231,8 @@ void ParticleFilter<ParticleType>::sort(int indexLeft, int indexRight) {
}
}
- if(le != pivot) {
+ if (le != pivot)
+ {
// swap
ParticleType* temp = m_CurrentList[le];
m_CurrentList[le] = m_CurrentList[pivot];
@@ -214,107 +243,114 @@ void ParticleFilter<ParticleType>::sort(int indexLeft, int indexRight) {
sort(indexLeft, le - 1);
// sort right side
sort(le + 1, indexRight);
-
}
template <class ParticleType>
-void ParticleFilter<ParticleType>::normalize() {
-
+void ParticleFilter<ParticleType>::normalize()
+{
float weightSum = 0.0;
- for (int i = 0; i < m_ParticleNum; i++) {
+ for (int i = 0; i < m_ParticleNum; i++)
+ {
weightSum += m_CurrentList[i]->getWeight();
}
// only normalize if weightSum is big enough to divide
- if (weightSum > 0.000001)
+ if (weightSum > 0.000001)
{
-
- //calculating parallel on 8 threats
+ // calculating parallel on 8 threats
omp_set_num_threads(8);
int i = 0;
- // #pragma omp parallel for
- for ( i = 0; i < m_ParticleNum; i++)
+ // #pragma omp parallel for
+ for (i = 0; i < m_ParticleNum; i++)
{
float newWeight = m_CurrentList[i]->getWeight() / weightSum;
m_CurrentList[i]->setWeight(newWeight);
}
}
- else
+ else
{
- ROS_WARN_STREAM( "Particle weights VERY small: " << weightSum << ". Got "<< m_ParticleNum << " particles.");
+ ROS_WARN_STREAM("Particle weights VERY small: "
+ << weightSum << ". Got " << m_ParticleNum << " particles.");
}
}
template <class ParticleType>
-void ParticleFilter<ParticleType>::resample()
+void ParticleFilter<ParticleType>::resample()
{
- // swap pointers
- ParticleType** help = m_LastList;
- m_LastList = m_CurrentList;
- m_CurrentList = help;
- // now we copy from m_LastList to m_CurrentList
-
- int drawIndex = 0;
- // index of the particle where we are drawing to
- int targetIndex = 0;
-
- int numToDraw = 0;
- do {
- numToDraw = static_cast<int>(round((m_ParticleNum * m_LastList[drawIndex]->getWeight()) + 0.5));
- for (int i = 0; i < numToDraw; i++) {
- *m_CurrentList[targetIndex++] = *m_LastList[drawIndex];
- // don't draw too much
- if (targetIndex >= m_ParticleNum) {
- break;
- }
- }
- drawIndex++;
- } while (numToDraw > 0 && targetIndex < m_ParticleNum);
-
- // fill the rest of the particle list
- for (int i = targetIndex; i < m_ParticleNum; i++) {
- float particlePos = random01();
- float weightSum = 0.0;
- drawIndex = 0;
- weightSum += m_LastList[drawIndex]->getWeight();
- while (weightSum < particlePos) {
- weightSum += m_LastList[++drawIndex]->getWeight();
+ // swap pointers
+ ParticleType** help = m_LastList;
+ m_LastList = m_CurrentList;
+ m_CurrentList = help;
+ // now we copy from m_LastList to m_CurrentList
+
+ int drawIndex = 0;
+ // index of the particle where we are drawing to
+ int targetIndex = 0;
+
+ int numToDraw = 0;
+ do
+ {
+ numToDraw = static_cast<int>(
+ round((m_ParticleNum * m_LastList[drawIndex]->getWeight()) + 0.5));
+ for (int i = 0; i < numToDraw; i++)
+ {
+ *m_CurrentList[targetIndex++] = *m_LastList[drawIndex];
+ // don't draw too much
+ if (targetIndex >= m_ParticleNum)
+ {
+ break;
}
- *m_CurrentList[i] = *m_LastList[drawIndex];
}
-}
+ drawIndex++;
+ } while (numToDraw > 0 && targetIndex < m_ParticleNum);
+ // fill the rest of the particle list
+ for (int i = targetIndex; i < m_ParticleNum; i++)
+ {
+ float particlePos = random01();
+ float weightSum = 0.0;
+ drawIndex = 0;
+ weightSum += m_LastList[drawIndex]->getWeight();
+ while (weightSum < particlePos)
+ {
+ weightSum += m_LastList[++drawIndex]->getWeight();
+ }
+ *m_CurrentList[i] = *m_LastList[drawIndex];
+ }
+}
template <class ParticleType>
-int ParticleFilter<ParticleType>::getEffectiveParticleNum() const {
+int ParticleFilter<ParticleType>::getEffectiveParticleNum() const
+{
// does not work with normalized particle weights
// does not work with our weights at all (algorithm of Grisetti)
float squareSum = 0;
- for (int i = 0; i < m_ParticleNum; i++) {
+ for (int i = 0; i < m_ParticleNum; i++)
+ {
float weight = m_CurrentList[i]->getWeight();
squareSum += weight * weight;
}
return static_cast<int>(1.0f / squareSum);
}
-
template <class ParticleType>
-int ParticleFilter<ParticleType>::getEffectiveParticleNum2() const {
- // does not work with normalized particle weights
+int ParticleFilter<ParticleType>::getEffectiveParticleNum2() const
+{
+ // does not work with normalized particle weights
int effectiveParticleNum = 0;
- for (int i = 0; i < m_ParticleNum; i++) {
- if (m_CurrentList[i]->getWeight() > MIN_EFFECTIVE_PARTICLE_WEIGHT) {
- effectiveParticleNum ++;
+ for (int i = 0; i < m_ParticleNum; i++)
+ {
+ if (m_CurrentList[i]->getWeight() > MIN_EFFECTIVE_PARTICLE_WEIGHT)
+ {
+ effectiveParticleNum++;
}
}
return effectiveParticleNum;
}
-
template <class ParticleType>
-ParticleType* ParticleFilter<ParticleType>::getBestParticle() const {
+ParticleType* ParticleFilter<ParticleType>::getBestParticle() const
+{
return m_CurrentList[0];
}
-
#endif
-
diff --git a/homer_mapping/include/homer_mapping/ParticleFilter/SlamFilter.h b/homer_mapping/include/homer_mapping/ParticleFilter/SlamFilter.h
index 89026615dcd140e77e3fb11159c6ebce03f500d0..19915e592de38d68340b36d16e2c46b3afb64d62 100644
--- a/homer_mapping/include/homer_mapping/ParticleFilter/SlamFilter.h
+++ b/homer_mapping/include/homer_mapping/ParticleFilter/SlamFilter.h
@@ -1,15 +1,15 @@
#ifndef SLAMFILTER_H
#define SLAMFILTER_H
-#include <vector>
+#include <homer_mapping/OccupancyMap/OccupancyMap.h>
#include <homer_mapping/ParticleFilter/ParticleFilter.h>
#include <homer_mapping/ParticleFilter/SlamParticle.h>
#include <homer_nav_libs/Math/Pose.h>
-#include <homer_mapping/OccupancyMap/OccupancyMap.h>
+#include <vector>
-#include <sensor_msgs/LaserScan.h>
-#include <homer_nav_libs/Math/Transformation2D.h>
#include <homer_nav_libs/Math/Math.h>
+#include <homer_nav_libs/Math/Transformation2D.h>
+#include <sensor_msgs/LaserScan.h>
#include <tf/transform_broadcaster.h>
@@ -19,7 +19,6 @@
#include "ros/ros.h"
-
class OccupancyMap;
/**
@@ -27,303 +26,344 @@ class OccupancyMap;
*
* @author Malte Knauf, Stephan Wirth, Susanne Maur
*
- * @brief This class is used to determine the robot's most likely pose with given map and given laser data.
+ * @brief This class is used to determine the robot's most likely pose with
+ * given map and given laser data.
*
- * A particle filter is a descrete method to describe and compute with a probability distribution.
- * This particle filter uses an occupancy map to determine the probability of robot states.
- * The robot states are stored in a particle together with their weight @see SlamParticle.
+ * A particle filter is a descrete method to describe and compute with a
+ * probability distribution.
+ * This particle filter uses an occupancy map to determine the probability of
+ * robot states.
+ * The robot states are stored in a particle together with their weight @see
+ * SlamParticle.
*
* @see SlamParticle
* @see ParticleFilter
* @see OccupancyMap
*/
-class SlamFilter : public ParticleFilter<SlamParticle> {
-
- public:
-
- /**
- * This constructor initializes the random number generator and sets the member variables to the given values.
- * @param particleNum Number of particles to use.
- */
- SlamFilter(int particleNum);
-
- /// @brief copy constructor
- SlamFilter( SlamFilter& slamFilter );
-
- /**
- * The destructor releases the OccupancyMap and the particles.
- */
- ~SlamFilter();
-
- /**
- * This method runs the filter routine.
- * The given odometry measurement is used as movement hypothesis, the laserData-vector is used
- * as measurement and is used to weight the particles.
- * @param currentPoseOdometry Odometry data of time t.
- * @param laserData msg containing the laser measurement.
- * @param measurementTime Time stamp of the measurement.
- * @param filterDurationTime Returns the time that the filtering needed
- */
- void filter(Pose currentPoseOdometry, sensor_msgs::LaserScanConstPtr laserData, ros::Time measurementTime,
- ros::Duration &filterDuration);
-
- /**
- * @return The Pose of the most important particle (particle with highest weight).
- */
- Pose getLikeliestPose(ros::Time poseTime = ros::Time::now());
-
- /**
- * This method can be used to retrieve the most likely map that is stored by the particle filter.
- * @return Pointer to the most likely occupancy map.
- */
- OccupancyMap* getLikeliestMap() const;
-
- /**
- * This function prints out the list of particles to stdout via cout.
- */
- void printParticles() const;
-
- void resetHigh();
-
- /**
- * Computes and sets the new value for m_Alpha1.
- * @param percent Rotation error while rotating (see class constructor for details)
- */
- void setRotationErrorRotating(float percent);
-
- /**
- * Computes and sets the new value for m_Alpha2.
- * @param degreesPerMeter Rotation error while translating (see class constructor for details)
- */
- void setRotationErrorTranslating(float degreesPerMeter);
-
- /**
- * Computes and sets the new value for m_Alpha3.
- * @param percent Translation error while translating (see class constructor for details)
- */
- void setTranslationErrorTranslating(float percent);
-
- /**
- * Computes and sets the new value for m_Alpha4.
- * @param mPerDegree Translation error while rotating (see class constructor for details)
- */
- void setTranslationErrorRotating(float mPerDegree);
-
- /**
- * Computes and sets the new value for m_Alpha5.
- * @param mPerDegree Move jitter while turning (see class constructor for details)
- */
- void setMoveJitterWhileTurning(float mPerDegree);
-
- /**
- * Sets a new minimal size of a cluster of scan points which is considered in scan matching.
- * @param clusterSize Minimal size of a cluster in mm of scan points which is considered in scan matching.
- */
- void setScanMatchingClusterSize(float clusterSize);
-
- /**
- * Sets whether the map is updated or just used for self-localization.
- * @param doMapping True if robot shall do mapping, false otherwise.
- */
- void setMapping(bool doMapping);
-
- /**
- * Deletes the current occupancy map and copies a new one into the system.
- * @param occupancyMap The occupancy map to load into the system (is being copied)
- */
- void setOccupancyMap(OccupancyMap* occupancyMap);
-
- /**
- * Sets the robot pose in the current occupancy map.
- * @param Robot pose.
- * @param scatterVariance if not equal to 0 the poses are equally scattered around the pose
- */
- void setRobotPose(Pose pose, double scatterVarXY=0.0, double scatterVarTheta=0.0);
-
- /**
- * @return Vector of current particle poses. The vector is sorted according to the weights of the
- * particles. The pose of the particle with the highest value is the first element of the vector.
- */
- std::vector<Pose>* getParticlePoses() const;
-
- /**
- * @return vector of all particles
+class SlamFilter : public ParticleFilter<SlamParticle>
+{
+public:
+ /**
+ * This constructor initializes the random number generator and sets the
+ * member variables to the given values.
+ * @param particleNum Number of particles to use.
*/
- std::vector<SlamParticle*>* getParticles() const;
-
- /**
- * @return Vector of current particle weights. The vector is sorted by weight, highest weight first.
- */
- std::vector<float> getParticleWeights() const;
-
- /**
- * Calculates and returns the variance of the current likeliest particle poses.
- * The orientation of the particle is neglected.
- * @param The number of treated particles.
- * @param[out] poseVarianceX The variance of particle poses in x direction.
- * @param[out] poseVarianceY The variance of particle poses in y direction.
- */
- void getPoseVariances(int particleNum, float& poseVarianceX, float& poseVarianceY);
-
- /**
- * This method reduces the number of particles used in this SlamFilter to the given value.
- * @param newParticleNumber The new number of particles
- */
- void reduceParticleNumber(int newParticleNumber);
-
- /**
- * This method returns the contrast of the occupancy grid
- * @return Contrast value from 0 (no contrast) to 1 (max. contrast) of the map
- */
- double evaluateByContrast();
-
- /**
- * This method passes a masking map to to the underlying occupancy map
- */
- void applyMasking(const nav_msgs::OccupancyGrid::ConstPtr &msg);
-
-
- private:
-
- /**
- * This method filter outliers in the given laser scan
- * @param rawData the laser scan to check
- * @param maxDiff maximal difference between two adjacent ranges
- * @return filtered scan without outliers
- */
- vector<float> filterOutliers(sensor_msgs::LaserScanConstPtr rawData, float maxDiff );
-
- /**
- * This method generates Gauss-distributed random variables with the given variance. The computation
- * method is the Polar Method that is described in the book "A First Course on Probability" by Sheldon Ross.
- * @param variance The variance for the Gauss-distribution that is used to generate the random variable.
- * @return A random variable that is N(0, variance) distributed.
- */
- double randomGauss(float variance = 1.0) const;
-
- /**
- * This method drifts the particles according to the last two odometry readings (time t-1 and time t).
- */
- void drift();
-
- /**
- * This method weightens each particle according to the given laser measurement in m_LaserData.
- */
- void measure();
-
- /**
- * This method updates the map by inserting the current laser measurement at the pose of the likeliest particle.
- */
- void updateMap();
-
- /**
- * For weightening the particles, the filter needs a map.
- * This variable holds a pointer to a map.
- * @see OccupancyMap
- */
- OccupancyMap* m_OccupancyMap;
-
- /**
- * threshold values for when the map will be updated.
- * The map is only updated when the robot has turned a minimal angle (m_UpdateMinMoveAngle in radiants),
- * has moved a minimal distance (m_UpdateMinMoveDistance in m) or a maximal time has passed (m_MaxUpdateInterval)
- */
- float m_UpdateMinMoveAngle;
- float m_UpdateMinMoveDistance;
- ros::Duration m_MaxUpdateInterval;
-
- /**
- * This variable holds the rotation error that the robot makes while it is rotating.
- * Has to be given in percent. Example: robot makes errors of 3 degrees while making a 60 degrees
- * move -> error is 5% -> rotationErrorRotating = 5)
- */
- float m_Alpha1;
-
- /**
- * This variable holds the rotation error that the robot makes while it is translating
- * (moving forward or backwards). Has to be given in degrees per meter.
- */
- float m_Alpha2;
-
- /**
- * This variable holds the translation error that the robot makes while it is translating.
- * Has to be given in percent.
- */
- float m_Alpha3;
-
- /**
- * This variable holds the translation error that the robot makes while it is rotating.
- * This error only carries weight, if a translation es performed at the same time.
- * See also m_Alpha5.
- * Has to be given in milimeters per degree. Example: Robot makes a turn of 10 degrees and moves its
- * center unintentional 15 mm. -> translationErrorRotating = 15.0 / 10.0 = 1.5
- */
- float m_Alpha4;
-
- /**
- * This variable holds a move jitter that is considered if the robot is turning.
- * Has to be given in milimeters per degree.
- */
- float m_Alpha5;
-
- /**
- * The maximal rotation if mapping is performed. If the rotation is bigger, mapping is interrupted.
- * This value may depend on the computing power, because it is influenced by the size of time intervals of mapping.
- */
- float m_MaxRotationPerSecond;
-
- /**
- * Last laser data.
- */
- sensor_msgs::LaserScanPtr m_CurrentLaserData;
-
- /**
- * Last two odometry measurements.
- */
- Pose m_ReferencePoseOdometry;
- Pose m_CurrentPoseOdometry;
-
- /**
- * Time stamp of the last sensor measurement.
- */
- ros::Time m_ReferenceMeasurementTime;
-
- /**
- * True if it is the first run of SlamFilter, false otherwise.
- */
- bool m_FirstRun;
-
- /**
- * This variabe is true, if the SlamFilter is used for mapping and updates the map,
- * false if it is just used for self-localization.
- */
- bool m_DoMapping;
-
- /** Points used in last measure() step */
- vector<MeasurePoint> m_MeasurePoints;
-
- /// Pose of robot during last map update
- Pose m_LastUpdatePose;
-
- tf::Transform m_latestTransform;
-
- /**
- * Time stamp of the last particle filter step
- */
- ros::Time m_LastUpdateTime;
-
- ros::Time m_LastMoveTime;
-
- /**
- * Calculates the square of given input f
- * @param f input
- * @return square of input
- */
- template<class T>
- T sqr(T f)
- {
- return f * f;
- }
+ SlamFilter(int particleNum);
+
+ /// @brief copy constructor
+ SlamFilter(SlamFilter& slamFilter);
+
+ /**
+ * The destructor releases the OccupancyMap and the particles.
+ */
+ ~SlamFilter();
+
+ /**
+ * This method runs the filter routine.
+ * The given odometry measurement is used as movement hypothesis, the
+ * laserData-vector is used
+ * as measurement and is used to weight the particles.
+ * @param currentPoseOdometry Odometry data of time t.
+ * @param laserData msg containing the laser measurement.
+ * @param measurementTime Time stamp of the measurement.
+ * @param filterDurationTime Returns the time that the filtering needed
+ */
+ void filter(Pose currentPoseOdometry,
+ sensor_msgs::LaserScanConstPtr laserData,
+ ros::Time measurementTime, ros::Duration& filterDuration);
+
+ /**
+ * @return The Pose of the most important particle (particle with highest
+ * weight).
+ */
+ Pose getLikeliestPose(ros::Time poseTime = ros::Time::now());
+
+ /**
+ * This method can be used to retrieve the most likely map that is stored by
+ * the particle filter.
+ * @return Pointer to the most likely occupancy map.
+ */
+ OccupancyMap* getLikeliestMap() const;
+
+ /**
+ * This function prints out the list of particles to stdout via cout.
+ */
+ void printParticles() const;
+
+ void resetHigh();
+
+ /**
+ * Computes and sets the new value for m_Alpha1.
+ * @param percent Rotation error while rotating (see class constructor for
+ * details)
+ */
+ void setRotationErrorRotating(float percent);
+
+ /**
+ * Computes and sets the new value for m_Alpha2.
+ * @param degreesPerMeter Rotation error while translating (see class
+ * constructor for details)
+ */
+ void setRotationErrorTranslating(float degreesPerMeter);
+
+ /**
+ * Computes and sets the new value for m_Alpha3.
+ * @param percent Translation error while translating (see class constructor
+ * for details)
+ */
+ void setTranslationErrorTranslating(float percent);
+
+ /**
+ * Computes and sets the new value for m_Alpha4.
+ * @param mPerDegree Translation error while rotating (see class constructor
+ * for details)
+ */
+ void setTranslationErrorRotating(float mPerDegree);
+
+ /**
+ * Computes and sets the new value for m_Alpha5.
+ * @param mPerDegree Move jitter while turning (see class constructor for
+ * details)
+ */
+ void setMoveJitterWhileTurning(float mPerDegree);
+
+ /**
+ * Sets a new minimal size of a cluster of scan points which is considered in
+ * scan matching.
+ * @param clusterSize Minimal size of a cluster in mm of scan points which is
+ * considered in scan matching.
+ */
+ void setScanMatchingClusterSize(float clusterSize);
+
+ /**
+ * Sets whether the map is updated or just used for self-localization.
+ * @param doMapping True if robot shall do mapping, false otherwise.
+ */
+ void setMapping(bool doMapping);
+
+ /**
+ * Deletes the current occupancy map and copies a new one into the system.
+ * @param occupancyMap The occupancy map to load into the system (is being
+ * copied)
+ */
+ void setOccupancyMap(OccupancyMap* occupancyMap);
+
+ /**
+ * Sets the robot pose in the current occupancy map.
+ * @param Robot pose.
+ * @param scatterVariance if not equal to 0 the poses are equally scattered
+ * around the pose
+ */
+ void setRobotPose(Pose pose, double scatterVarXY = 0.0,
+ double scatterVarTheta = 0.0);
+
+ /**
+ * @return Vector of current particle poses. The vector is sorted according to
+ * the weights of the
+ * particles. The pose of the particle with the highest value is the first
+ * element of the vector.
+ */
+ std::vector<Pose>* getParticlePoses() const;
+
+ /**
+ * @return vector of all particles
+ */
+ std::vector<SlamParticle*>* getParticles() const;
+
+ /**
+ * @return Vector of current particle weights. The vector is sorted by weight,
+ * highest weight first.
+ */
+ std::vector<float> getParticleWeights() const;
+
+ /**
+ * Calculates and returns the variance of the current likeliest particle
+ * poses.
+ * The orientation of the particle is neglected.
+ * @param The number of treated particles.
+ * @param[out] poseVarianceX The variance of particle poses in x direction.
+ * @param[out] poseVarianceY The variance of particle poses in y direction.
+ */
+ void getPoseVariances(int particleNum, float& poseVarianceX,
+ float& poseVarianceY);
+
+ /**
+ * This method reduces the number of particles used in this SlamFilter to the
+ * given value.
+ * @param newParticleNumber The new number of particles
+ */
+ void reduceParticleNumber(int newParticleNumber);
+
+ /**
+ * This method returns the contrast of the occupancy grid
+ * @return Contrast value from 0 (no contrast) to 1 (max. contrast) of the map
+ */
+ double evaluateByContrast();
+
+ /**
+ * This method passes a masking map to to the underlying occupancy map
+ */
+ void applyMasking(const nav_msgs::OccupancyGrid::ConstPtr& msg);
+
+private:
+ /**
+ * This method filter outliers in the given laser scan
+ * @param rawData the laser scan to check
+ * @param maxDiff maximal difference between two adjacent ranges
+ * @return filtered scan without outliers
+ */
+ vector<float> filterOutliers(sensor_msgs::LaserScanConstPtr rawData,
+ float maxDiff);
+
+ /**
+ * This method generates Gauss-distributed random variables with the given
+ * variance. The computation
+ * method is the Polar Method that is described in the book "A First Course on
+ * Probability" by Sheldon Ross.
+ * @param variance The variance for the Gauss-distribution that is used to
+ * generate the random variable.
+ * @return A random variable that is N(0, variance) distributed.
+ */
+ double randomGauss(float variance = 1.0) const;
+
+ /**
+ * This method drifts the particles according to the last two odometry
+ * readings (time t-1 and time t).
+ */
+ void drift();
+
+ /**
+ * This method weightens each particle according to the given laser
+ * measurement in m_LaserData.
+ */
+ void measure();
+
+ /**
+ * This method updates the map by inserting the current laser measurement at
+ * the pose of the likeliest particle.
+ */
+ void updateMap();
+
+ /**
+ * For weightening the particles, the filter needs a map.
+ * This variable holds a pointer to a map.
+ * @see OccupancyMap
+ */
+ OccupancyMap* m_OccupancyMap;
+
+ /**
+ * threshold values for when the map will be updated.
+ * The map is only updated when the robot has turned a minimal angle
+ * (m_UpdateMinMoveAngle in radiants),
+ * has moved a minimal distance (m_UpdateMinMoveDistance in m) or a maximal
+ * time has passed (m_MaxUpdateInterval)
+ */
+ float m_UpdateMinMoveAngle;
+ float m_UpdateMinMoveDistance;
+ ros::Duration m_MaxUpdateInterval;
+
+ /**
+ * This variable holds the rotation error that the robot makes while it is
+ * rotating.
+ * Has to be given in percent. Example: robot makes errors of 3 degrees while
+ * making a 60 degrees
+ * move -> error is 5% -> rotationErrorRotating = 5)
+ */
+ float m_Alpha1;
+
+ /**
+ * This variable holds the rotation error that the robot makes while it is
+ * translating
+ * (moving forward or backwards). Has to be given in degrees per meter.
+ */
+ float m_Alpha2;
+
+ /**
+ * This variable holds the translation error that the robot makes while it is
+ * translating.
+ * Has to be given in percent.
+ */
+ float m_Alpha3;
+
+ /**
+ * This variable holds the translation error that the robot makes while it is
+ * rotating.
+ * This error only carries weight, if a translation es performed at the same
+ * time.
+ * See also m_Alpha5.
+ * Has to be given in milimeters per degree. Example: Robot makes a turn of 10
+ * degrees and moves its
+ * center unintentional 15 mm. -> translationErrorRotating = 15.0 / 10.0 = 1.5
+ */
+ float m_Alpha4;
+
+ /**
+ * This variable holds a move jitter that is considered if the robot is
+ * turning.
+ * Has to be given in milimeters per degree.
+ */
+ float m_Alpha5;
+
+ /**
+ * The maximal rotation if mapping is performed. If the rotation is bigger,
+ * mapping is interrupted.
+ * This value may depend on the computing power, because it is influenced by
+ * the size of time intervals of mapping.
+ */
+ float m_MaxRotationPerSecond;
+
+ /**
+ * Last laser data.
+ */
+ sensor_msgs::LaserScanPtr m_CurrentLaserData;
+
+ /**
+ * Last two odometry measurements.
+ */
+ Pose m_ReferencePoseOdometry;
+ Pose m_CurrentPoseOdometry;
+
+ /**
+ * Time stamp of the last sensor measurement.
+ */
+ ros::Time m_ReferenceMeasurementTime;
+
+ /**
+ * True if it is the first run of SlamFilter, false otherwise.
+ */
+ bool m_FirstRun;
+
+ /**
+ * This variabe is true, if the SlamFilter is used for mapping and updates the
+ * map,
+ * false if it is just used for self-localization.
+ */
+ bool m_DoMapping;
+
+ /** Points used in last measure() step */
+ vector<MeasurePoint> m_MeasurePoints;
+
+ /// Pose of robot during last map update
+ Pose m_LastUpdatePose;
+
+ tf::Transform m_latestTransform;
+
+ /**
+ * Time stamp of the last particle filter step
+ */
+ ros::Time m_LastUpdateTime;
+
+ ros::Time m_LastMoveTime;
+
+ /**
+ * Calculates the square of given input f
+ * @param f input
+ * @return square of input
+ */
+ template <class T>
+ T sqr(T f)
+ {
+ return f * f;
+ }
};
#endif
-
diff --git a/homer_mapping/include/homer_mapping/ParticleFilter/SlamParticle.h b/homer_mapping/include/homer_mapping/ParticleFilter/SlamParticle.h
index dc05b056bcabf524b5a9132a665315beddd2dd03..fd7e738bcb4a05cd132ef4a636c8ff62d2eff626 100644
--- a/homer_mapping/include/homer_mapping/ParticleFilter/SlamParticle.h
+++ b/homer_mapping/include/homer_mapping/ParticleFilter/SlamParticle.h
@@ -1,8 +1,8 @@
#ifndef SLAMPARTICLE_H
#define SLAMPARTICLE_H
-#include <iostream>
#include <fstream>
+#include <iostream>
#include <homer_mapping/ParticleFilter/Particle.h>
@@ -13,7 +13,8 @@
*
* @brief This class defines a particle for the SlamFilter.
*
- * This particle contains a weight (inherited from base class) and a Pose (position + orientation).
+ * This particle contains a weight (inherited from base class) and a Pose
+ * (position + orientation).
* The Pose describes a possible position and orientation of the robot.
*
* @see SlamFilter
@@ -21,52 +22,50 @@
*/
class SlamParticle : public Particle
{
+public:
+ /**
+ * This constructor assigns the given weight to the member m_Weight.
+ * @param weight The weight of the particle.
+ * @param robotX X-Position of the robot (world coordinates in m).
+ * @param robotY Y-Position of the robot (world coordinates in m).
+ * @param robotTheta Orientation of the robot (radiants).
+ */
+ SlamParticle(float weight = 1.0, float robotX = 0.0, float robotY = 0.0,
+ float robotTheta = 0.0);
- public:
- /**
- * This constructor assigns the given weight to the member m_Weight.
- * @param weight The weight of the particle.
- * @param robotX X-Position of the robot (world coordinates in m).
- * @param robotY Y-Position of the robot (world coordinates in m).
- * @param robotTheta Orientation of the robot (radiants).
- */
- SlamParticle ( float weight = 1.0, float robotX = 0.0, float robotY = 0.0, float robotTheta = 0.0 );
-
- ///@brief copy contructor
- SlamParticle ( SlamParticle& slamParticle );
-
- /**
- * The destructor does nothing so far.
- */
- ~SlamParticle();
+ ///@brief copy contructor
+ SlamParticle(SlamParticle& slamParticle);
- /**
- * Sets the three members m_RobotPositionX, m_RobotPositionY, m_RobotOrientation.
- * @param robotX X-Position of the robot (world coordinates in m).
- * @param robotY Y-Position of the robot (world coordinates in m).
- * @param robotTheta Orientation of the robot (radiants).
- */
- void setRobotPose ( float robotX, float robotY, float robotTheta );
+ /**
+ * The destructor does nothing so far.
+ */
+ ~SlamParticle();
- /**
- * Returns the content of the three members m_RobotPositionX, m_RobotPositionY, m_RobotOrientation.
- * @param[out] robotX X-Position of the robot (world coordinates in m).
- * @param[out] robotY Y-Position of the robot (world coordinates in m).
- * @param[out] robotTheta Orientation of the robot (radiants).
- */
- void getRobotPose ( float& robotX, float& robotY, float& robotTheta );
+ /**
+ * Sets the three members m_RobotPositionX, m_RobotPositionY,
+ * m_RobotOrientation.
+ * @param robotX X-Position of the robot (world coordinates in m).
+ * @param robotY Y-Position of the robot (world coordinates in m).
+ * @param robotTheta Orientation of the robot (radiants).
+ */
+ void setRobotPose(float robotX, float robotY, float robotTheta);
+ /**
+ * Returns the content of the three members m_RobotPositionX,
+ * m_RobotPositionY, m_RobotOrientation.
+ * @param[out] robotX X-Position of the robot (world coordinates in m).
+ * @param[out] robotY Y-Position of the robot (world coordinates in m).
+ * @param[out] robotTheta Orientation of the robot (radiants).
+ */
+ void getRobotPose(float& robotX, float& robotY, float& robotTheta);
- private:
-
- /**
- * These members store the pose of the robot.
- */
- float m_RobotPositionX;
- float m_RobotPositionY;
- float m_RobotOrientation;
-
+private:
+ /**
+ * These members store the pose of the robot.
+ */
+ float m_RobotPositionX;
+ float m_RobotPositionY;
+ float m_RobotOrientation;
};
#endif
-
diff --git a/homer_mapping/include/homer_mapping/slam_node.h b/homer_mapping/include/homer_mapping/slam_node.h
index 5e4081a4a76ad6282cdbf92b55cbe2d40260fdec..733e90ae9a04f2bc0e41301b6c702f69f1ce07f4 100644
--- a/homer_mapping/include/homer_mapping/slam_node.h
+++ b/homer_mapping/include/homer_mapping/slam_node.h
@@ -1,36 +1,36 @@
#ifndef SLAM_NODE_H
#define SLAM_NODE_H
-#include <vector>
+#include <stdlib.h>
+#include <cmath>
+#include <fstream>
+#include <iostream>
#include <map>
#include <sstream>
-#include <vector>
-#include <iostream>
-#include <fstream>
#include <sstream>
-#include <cmath>
-#include <stdlib.h>
+#include <vector>
+#include <vector>
#include <homer_nav_libs/Math/Pose.h>
#include <tf/transform_broadcaster.h>
-#include <sensor_msgs/LaserScan.h>
-#include <nav_msgs/Odometry.h>
-#include <nav_msgs/OccupancyGrid.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseWithCovariance.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
-#include <std_msgs/Empty.h>
-#include <std_msgs/Bool.h>
-#include <nav_msgs/Odometry.h>
#include <nav_msgs/OccupancyGrid.h>
+#include <nav_msgs/OccupancyGrid.h>
+#include <nav_msgs/Odometry.h>
+#include <nav_msgs/Odometry.h>
+#include <sensor_msgs/LaserScan.h>
+#include <std_msgs/Bool.h>
+#include <std_msgs/Empty.h>
#include <tf/tf.h>
-#include <homer_mapping/ParticleFilter/SlamFilter.h>
+#include <homer_mapping/OccupancyMap/OccupancyMap.h>
#include <homer_mapping/ParticleFilter/HyperSlamFilter.h>
+#include <homer_mapping/ParticleFilter/SlamFilter.h>
#include <homer_nav_libs/Math/Box2D.h>
-#include <homer_mapping/OccupancyMap/OccupancyMap.h>
class OccupancyMap;
class SlamFilter;
@@ -51,136 +51,134 @@ class HyperSlamFilter;
*/
class SlamNode
{
-
public:
-
- /**
- * The constructor adds the message types and prepares the module for receiving them.
- */
- SlamNode(ros::NodeHandle *nh);
-
- /**
- * This method initializes the member variables.
- */
- virtual void init();
-
- /**
- * The destructor deletes the filter thread instance.
- */
- virtual ~SlamNode();
+ /**
+ * The constructor adds the message types and prepares the module for
+ * receiving them.
+ */
+ SlamNode(ros::NodeHandle* nh);
+
+ /**
+ * This method initializes the member variables.
+ */
+ virtual void init();
+
+ /**
+ * The destructor deletes the filter thread instance.
+ */
+ virtual ~SlamNode();
private:
-
- /**
- * Callback methods for all incoming messages
- */
- void callbackLaserScan( const sensor_msgs::LaserScan::ConstPtr& msg );
- void callbackOdometry( const nav_msgs::Odometry::ConstPtr& msg );
- void callbackUserDefPose( const geometry_msgs::Pose::ConstPtr& msg );
- void callbackDoMapping( const std_msgs::Bool::ConstPtr& msg );
- void callbackResetMap( const std_msgs::Empty::ConstPtr& msg );
- void callbackLoadedMap( const nav_msgs::OccupancyGrid::ConstPtr& msg );
- void callbackMasking( const nav_msgs::OccupancyGrid::ConstPtr& msg );
- void callbackResetHigh(const std_msgs::Empty::ConstPtr& msg);
- void callbackInitialPose(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg);
-
- /**
- * This function resets the current maps to the initial state.
- */
- void resetMaps();
-
- /**
- * This function processes the current odometry data in combination with the
- * last send odometry and laser informations to pass on corresponding data
- * to the filter threads.
-
- /**
- * This method retrieves the current map of the slam filter and sends a map
- * data message containing the map.
- */
- void sendMapDataMessage(ros::Time mapTime = ros::Time::now());
-
- /**
- * This variables stores the last odometry measurement as reference that is used
- * to compute the pose of the robot during a specific laserscan.
- */
- Pose m_ReferenceOdometryPose;
-
- Pose m_LastLikeliestPose;
-
- /**
- * This variable stores the time of the last odometry measurement as reference
- * which is used to compute the pose of the robot during a specific laserscan.
- */
- ros::Time m_ReferenceOdometryTime;
-
- /**
- * This variable stores the time the last map message was sent to be able to
- * compute the time for the next map send.
- */
- ros::Time m_LastMapSendTime;
- ros::Time m_LastPositionSendTime;
-
- /**
- * This variable stores the last laser measurement.
- */
- sensor_msgs::LaserScan::ConstPtr m_LastLaserData;
-
- /**
- * time stamp of last particle filter step
- */
- ros::Time m_LastMappingTime;
-
-
- /**
- * This variable stores a pointer to the hyper slam filter
- */
- HyperSlamFilter* m_HyperSlamFilter;
-
- /**
- * Scatter variances in self localization.
- */
- double m_ScatterVarXY;
- double m_ScatterVarTheta;
-
- /**
- * This variabe is true, if the slam algorithm is used for mapping and
- * keeps updating the map, false otherwise.
- */
- bool m_DoMapping;
-
- /**
- * Vectors used to queue laser and odom messages to find a fit
- */
- std::vector<sensor_msgs::LaserScan::ConstPtr> m_laser_queue;
- std::vector<nav_msgs::Odometry::ConstPtr> m_odom_queue;
-
- /**
- * duration to wait between two particle filter steps
- */
- ros::Duration m_WaitDuration;
-
- /**
- * Broadcasts the transform map -> base_link
- */
- tf::TransformBroadcaster m_tfBroadcaster;
-
- /**
- * subscribers and publishers
- */
- ros::Subscriber m_LaserScanSubscriber;
- ros::Subscriber m_OdometrySubscriber;
- ros::Subscriber m_UserDefPoseSubscriber;
- ros::Subscriber m_DoMappingSubscriber;
- ros::Subscriber m_ResetMapSubscriber;
- ros::Subscriber m_LoadMapSubscriber;
- ros::Subscriber m_MaskingSubscriber;
- ros::Subscriber m_ResetHighSubscriber;
- ros::Subscriber m_InitialPoseSubscriber;
-
- ros::Publisher m_PoseStampedPublisher;
- ros::Publisher m_SLAMMapPublisher;
-
+ /**
+ * Callback methods for all incoming messages
+ */
+ void callbackLaserScan(const sensor_msgs::LaserScan::ConstPtr& msg);
+ void callbackOdometry(const nav_msgs::Odometry::ConstPtr& msg);
+ void callbackUserDefPose(const geometry_msgs::Pose::ConstPtr& msg);
+ void callbackDoMapping(const std_msgs::Bool::ConstPtr& msg);
+ void callbackResetMap(const std_msgs::Empty::ConstPtr& msg);
+ void callbackLoadedMap(const nav_msgs::OccupancyGrid::ConstPtr& msg);
+ void callbackMasking(const nav_msgs::OccupancyGrid::ConstPtr& msg);
+ void callbackResetHigh(const std_msgs::Empty::ConstPtr& msg);
+ void callbackInitialPose(
+ const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg);
+
+ /**
+ * This function resets the current maps to the initial state.
+ */
+ void resetMaps();
+
+ /**
+ * This function processes the current odometry data in combination with the
+ * last send odometry and laser informations to pass on corresponding data
+ * to the filter threads.
+
+ /**
+ * This method retrieves the current map of the slam filter and sends a map
+ * data message containing the map.
+ */
+ void sendMapDataMessage(ros::Time mapTime = ros::Time::now());
+
+ /**
+ * This variables stores the last odometry measurement as reference that is
+ * used
+ * to compute the pose of the robot during a specific laserscan.
+ */
+ Pose m_ReferenceOdometryPose;
+
+ Pose m_LastLikeliestPose;
+
+ /**
+ * This variable stores the time of the last odometry measurement as reference
+ * which is used to compute the pose of the robot during a specific laserscan.
+ */
+ ros::Time m_ReferenceOdometryTime;
+
+ /**
+ * This variable stores the time the last map message was sent to be able to
+ * compute the time for the next map send.
+ */
+ ros::Time m_LastMapSendTime;
+ ros::Time m_LastPositionSendTime;
+
+ /**
+ * This variable stores the last laser measurement.
+ */
+ sensor_msgs::LaserScan::ConstPtr m_LastLaserData;
+
+ /**
+ * time stamp of last particle filter step
+ */
+ ros::Time m_LastMappingTime;
+
+ /**
+ * This variable stores a pointer to the hyper slam filter
+ */
+ HyperSlamFilter* m_HyperSlamFilter;
+
+ /**
+ * Scatter variances in self localization.
+ */
+ double m_ScatterVarXY;
+ double m_ScatterVarTheta;
+
+ /**
+ * This variabe is true, if the slam algorithm is used for mapping and
+ * keeps updating the map, false otherwise.
+ */
+ bool m_DoMapping;
+
+ /**
+ * Vectors used to queue laser and odom messages to find a fit
+ */
+ std::vector<sensor_msgs::LaserScan::ConstPtr> m_laser_queue;
+ std::vector<nav_msgs::Odometry::ConstPtr> m_odom_queue;
+
+ /**
+ * duration to wait between two particle filter steps
+ */
+ ros::Duration m_WaitDuration;
+
+ /**
+ * Broadcasts the transform map -> base_link
+ */
+ tf::TransformBroadcaster m_tfBroadcaster;
+
+ /**
+ * subscribers and publishers
+ */
+ ros::Subscriber m_LaserScanSubscriber;
+ ros::Subscriber m_OdometrySubscriber;
+ ros::Subscriber m_UserDefPoseSubscriber;
+ ros::Subscriber m_DoMappingSubscriber;
+ ros::Subscriber m_ResetMapSubscriber;
+ ros::Subscriber m_LoadMapSubscriber;
+ ros::Subscriber m_MaskingSubscriber;
+ ros::Subscriber m_ResetHighSubscriber;
+ ros::Subscriber m_InitialPoseSubscriber;
+
+ ros::Publisher m_PoseStampedPublisher;
+ ros::Publisher m_SLAMMapPublisher;
};
#endif
diff --git a/homer_mapping/src/OccupancyMap/OccupancyMap.cpp b/homer_mapping/src/OccupancyMap/OccupancyMap.cpp
index 515128a22715094ec3338dcd8a4bbde13989cdfb..f8952872e297ae9aa6cfbe7e6fa702cb1d049ede 100644
--- a/homer_mapping/src/OccupancyMap/OccupancyMap.cpp
+++ b/homer_mapping/src/OccupancyMap/OccupancyMap.cpp
@@ -2,11 +2,11 @@
#include <homer_nav_libs/Math/Math.h>
+#include <QtGui/QImage>
#include <cmath>
-#include <vector>
#include <fstream>
#include <sstream>
-#include <QtGui/QImage>
+#include <vector>
#include <Eigen/Geometry>
@@ -16,7 +16,7 @@
#include <homer_mapnav_msgs/ModifyMap.h>
#include <homer_nav_libs/tools.h>
-//uncomment this to get extended information on the tracer
+// uncomment this to get extended information on the tracer
//#define TRACER_OUTPUT
using namespace std;
@@ -27,28 +27,29 @@ const float UNKNOWN_LIKELIHOOD = 0.3;
const char NO_CHANGE = 0;
const char OCCUPIED = 1;
const char FREE = 2;
-//the safety border around occupied pixels which is left unchanged
+// the safety border around occupied pixels which is left unchanged
const char SAFETY_BORDER = 3;
///////////////////////////////
-//assumed laser measure count for loaded maps
+// assumed laser measure count for loaded maps
const int LOADED_MEASURECOUNT = 10;
-
OccupancyMap::OccupancyMap()
{
float mapSize, resolution;
ros::param::get("/homer_mapping/size", mapSize);
ros::param::get("/homer_mapping/resolution", resolution);
- //add one safety pixel
+ // add one safety pixel
m_metaData.resolution = resolution;
m_metaData.width = mapSize / m_metaData.resolution + 1;
m_metaData.height = mapSize / m_metaData.resolution + 1;
m_ByteSize = m_metaData.width * m_metaData.height;
- m_metaData.origin.position.x = - (m_metaData.width * m_metaData.resolution / 2.0);
- m_metaData.origin.position.y = - (m_metaData.height * m_metaData.resolution / 2.0);
+ m_metaData.origin.position.x =
+ -(m_metaData.width * m_metaData.resolution / 2.0);
+ m_metaData.origin.position.y =
+ -(m_metaData.height * m_metaData.resolution / 2.0);
m_metaData.origin.orientation.w = 1.0;
m_metaData.origin.orientation.x = 0.0;
m_metaData.origin.orientation.y = 0.0;
@@ -56,36 +57,37 @@ OccupancyMap::OccupancyMap()
initMembers();
}
-OccupancyMap::OccupancyMap(float *&occupancyProbability, geometry_msgs::Pose origin, float resolution, int width, int height, Box2D<int> exploredRegion)
+OccupancyMap::OccupancyMap(float*& occupancyProbability,
+ geometry_msgs::Pose origin, float resolution,
+ int width, int height, Box2D<int> exploredRegion)
{
- m_metaData.origin = origin;
- m_metaData.resolution = resolution;
- m_metaData.width = width;
- m_metaData.height = height;
- m_ByteSize = m_metaData.width * m_metaData.height;
- initMembers();
-
+ m_metaData.origin = origin;
+ m_metaData.resolution = resolution;
+ m_metaData.width = width;
+ m_metaData.height = height;
+ m_ByteSize = m_metaData.width * m_metaData.height;
+ initMembers();
- m_ExploredRegion = exploredRegion;
- m_ChangedRegion = exploredRegion;
+ m_ExploredRegion = exploredRegion;
+ m_ChangedRegion = exploredRegion;
- if ( m_OccupancyProbability )
+ if (m_OccupancyProbability)
+ {
+ delete[] m_OccupancyProbability;
+ }
+ m_OccupancyProbability = occupancyProbability;
+ for (unsigned i = 0; i < m_ByteSize; i++)
+ {
+ if (m_OccupancyProbability[i] != 0.5)
{
- delete[] m_OccupancyProbability;
- }
- m_OccupancyProbability = occupancyProbability;
- for(unsigned i = 0; i < m_ByteSize; i++)
- {
- if(m_OccupancyProbability[i] != 0.5)
- {
- m_MeasurementCount[i] = LOADED_MEASURECOUNT;
- m_OccupancyCount[i] = m_OccupancyProbability[i] * (float)LOADED_MEASURECOUNT;
- }
+ m_MeasurementCount[i] = LOADED_MEASURECOUNT;
+ m_OccupancyCount[i] =
+ m_OccupancyProbability[i] * (float)LOADED_MEASURECOUNT;
}
+ }
}
-
-OccupancyMap::OccupancyMap ( const OccupancyMap& occupancyMap )
+OccupancyMap::OccupancyMap(const OccupancyMap& occupancyMap)
{
m_OccupancyProbability = 0;
m_MeasurementCount = 0;
@@ -103,58 +105,64 @@ OccupancyMap::~OccupancyMap()
void OccupancyMap::initMembers()
{
-
ros::param::get("/homer_mapping/backside_checking", m_BacksideChecking);
ros::param::get("/homer_mapping/obstacle_borders", m_ObstacleBorders);
- ros::param::get("/homer_mapping/measure_sampling_step", m_MeasureSamplingStep);
- ros::param::get("/homer_mapping/laser_scanner/free_reading_distance", m_FreeReadingDistance);
+ ros::param::get("/homer_mapping/measure_sampling_step",
+ m_MeasureSamplingStep);
+ ros::param::get("/homer_mapping/laser_scanner/free_reading_distance",
+ m_FreeReadingDistance);
m_OccupancyProbability = new float[m_ByteSize];
m_MeasurementCount = new unsigned short[m_ByteSize];
m_OccupancyCount = new unsigned short[m_ByteSize];
m_CurrentChanges = new unsigned char[m_ByteSize];
m_HighSensitive = new unsigned short[m_ByteSize];
- for ( unsigned i=0; i<m_ByteSize; i++ )
+ for (unsigned i = 0; i < m_ByteSize; i++)
{
- m_OccupancyProbability[i]=UNKNOWN_LIKELIHOOD;
- m_OccupancyCount[i]=0;
- m_MeasurementCount[i]=0;
- m_CurrentChanges[i]=NO_CHANGE;
+ m_OccupancyProbability[i] = UNKNOWN_LIKELIHOOD;
+ m_OccupancyCount[i] = 0;
+ m_MeasurementCount[i] = 0;
+ m_CurrentChanges[i] = NO_CHANGE;
m_HighSensitive[i] = 0;
}
- m_ExploredRegion=Box2D<int> ( m_metaData.width/2.1, m_metaData.height/2.1, m_metaData.width/1.9, m_metaData.height/1.9 );
+ m_ExploredRegion =
+ Box2D<int>(m_metaData.width / 2.1, m_metaData.height / 2.1,
+ m_metaData.width / 1.9, m_metaData.height / 1.9);
maximizeChangedRegion();
-
+
try
{
- bool got_transform = m_tfListener.waitForTransform("/base_link","/laser", ros::Time(0), ros::Duration(1));
- while(!got_transform);
- {
- got_transform = m_tfListener.waitForTransform("/base_link","/laser", ros::Time(0), ros::Duration(1));
- if(!got_transform)
- {
- ROS_ERROR_STREAM("need transformation from base_link to laser!");
- }
- }
-
- m_tfListener.lookupTransform("/base_link", "/laser", ros::Time(0), m_laserTransform);
+ bool got_transform = m_tfListener.waitForTransform(
+ "/base_link", "/laser", ros::Time(0), ros::Duration(1));
+ while (!got_transform)
+ ;
+ {
+ got_transform = m_tfListener.waitForTransform(
+ "/base_link", "/laser", ros::Time(0), ros::Duration(1));
+ if (!got_transform)
+ {
+ ROS_ERROR_STREAM("need transformation from base_link to laser!");
+ }
+ }
+
+ m_tfListener.lookupTransform("/base_link", "/laser", ros::Time(0),
+ m_laserTransform);
}
- catch (tf::TransformException ex) {
- ROS_ERROR_STREAM(ex.what());
+ catch (tf::TransformException ex)
+ {
+ ROS_ERROR_STREAM(ex.what());
}
-
}
-
-OccupancyMap& OccupancyMap::operator= ( const OccupancyMap & occupancyMap )
+OccupancyMap& OccupancyMap::operator=(const OccupancyMap& occupancyMap)
{
// free allocated memory
cleanUp();
m_metaData = occupancyMap.m_metaData;
- m_ExploredRegion = occupancyMap.m_ExploredRegion;
+ m_ExploredRegion = occupancyMap.m_ExploredRegion;
m_ByteSize = occupancyMap.m_ByteSize;
ros::param::get("/homer_mapping/backside_checking", m_BacksideChecking);
@@ -167,19 +175,24 @@ OccupancyMap& OccupancyMap::operator= ( const OccupancyMap & occupancyMap )
m_HighSensitive = new unsigned short[m_ByteSize];
// copy array data
- memcpy ( m_OccupancyProbability, occupancyMap.m_OccupancyProbability, m_ByteSize * sizeof ( *m_OccupancyProbability ) );
- memcpy ( m_MeasurementCount, occupancyMap.m_MeasurementCount, m_ByteSize * sizeof ( *m_MeasurementCount ) );
- memcpy ( m_OccupancyCount, occupancyMap.m_OccupancyCount, m_ByteSize * sizeof ( *m_OccupancyCount ) );
- memcpy ( m_CurrentChanges, occupancyMap.m_CurrentChanges, m_ByteSize * sizeof ( *m_CurrentChanges ) );
- memcpy ( m_HighSensitive, occupancyMap.m_HighSensitive, m_ByteSize * sizeof ( *m_HighSensitive) );
-
+ memcpy(m_OccupancyProbability, occupancyMap.m_OccupancyProbability,
+ m_ByteSize * sizeof(*m_OccupancyProbability));
+ memcpy(m_MeasurementCount, occupancyMap.m_MeasurementCount,
+ m_ByteSize * sizeof(*m_MeasurementCount));
+ memcpy(m_OccupancyCount, occupancyMap.m_OccupancyCount,
+ m_ByteSize * sizeof(*m_OccupancyCount));
+ memcpy(m_CurrentChanges, occupancyMap.m_CurrentChanges,
+ m_ByteSize * sizeof(*m_CurrentChanges));
+ memcpy(m_HighSensitive, occupancyMap.m_HighSensitive,
+ m_ByteSize * sizeof(*m_HighSensitive));
return *this;
}
-void OccupancyMap::changeMapSize( int x_add_left, int y_add_up, int x_add_right, int y_add_down)
+void OccupancyMap::changeMapSize(int x_add_left, int y_add_up, int x_add_right,
+ int y_add_down)
{
- int new_width = m_metaData.width + x_add_left + x_add_right;
+ int new_width = m_metaData.width + x_add_left + x_add_right;
int new_height = m_metaData.height + y_add_up + y_add_down;
m_ByteSize = new_width * new_height;
@@ -190,55 +203,51 @@ void OccupancyMap::changeMapSize( int x_add_left, int y_add_up, int x_add_right,
unsigned char* CurrentChanges = new unsigned char[m_ByteSize];
unsigned short* HighSensitive = new unsigned short[m_ByteSize];
- for ( unsigned i=0; i<m_ByteSize; i++ )
+ for (unsigned i = 0; i < m_ByteSize; i++)
{
- OccupancyProbability[i]=UNKNOWN_LIKELIHOOD;
- OccupancyCount[i]=0;
- MeasurementCount[i]=0;
- CurrentChanges[i]=NO_CHANGE;
+ OccupancyProbability[i] = UNKNOWN_LIKELIHOOD;
+ OccupancyCount[i] = 0;
+ MeasurementCount[i] = 0;
+ CurrentChanges[i] = NO_CHANGE;
HighSensitive[i] = 0;
}
- for(int y = 0; y < m_metaData.height; y++)
+ for (int y = 0; y < m_metaData.height; y++)
{
- for(int x = 0; x < m_metaData.width; x++)
- {
- int i = y * m_metaData.width + x;
- int in = (y + y_add_up) * new_width + (x + x_add_left);
- OccupancyProbability[in] = m_OccupancyProbability[i];
- MeasurementCount[in] = m_MeasurementCount[i];
- OccupancyCount[in] = m_OccupancyCount[i];
- CurrentChanges[in] = m_CurrentChanges[i];
- HighSensitive[in] = m_HighSensitive[i];
- }
+ for (int x = 0; x < m_metaData.width; x++)
+ {
+ int i = y * m_metaData.width + x;
+ int in = (y + y_add_up) * new_width + (x + x_add_left);
+ OccupancyProbability[in] = m_OccupancyProbability[i];
+ MeasurementCount[in] = m_MeasurementCount[i];
+ OccupancyCount[in] = m_OccupancyCount[i];
+ CurrentChanges[in] = m_CurrentChanges[i];
+ HighSensitive[in] = m_HighSensitive[i];
+ }
}
- m_ExploredRegion.setMinX( m_ExploredRegion.minX() + x_add_left);
- m_ExploredRegion.setMaxX( m_ExploredRegion.maxX() + x_add_left);
- m_ExploredRegion.setMinY( m_ExploredRegion.minY() + y_add_up);
- m_ExploredRegion.setMaxY( m_ExploredRegion.maxY() + y_add_up);
+ m_ExploredRegion.setMinX(m_ExploredRegion.minX() + x_add_left);
+ m_ExploredRegion.setMaxX(m_ExploredRegion.maxX() + x_add_left);
+ m_ExploredRegion.setMinY(m_ExploredRegion.minY() + y_add_up);
+ m_ExploredRegion.setMaxY(m_ExploredRegion.maxY() + y_add_up);
- m_ChangedRegion.setMinX( m_ChangedRegion.minX() + x_add_left);
- m_ChangedRegion.setMaxX( m_ChangedRegion.maxX() + x_add_left);
- m_ChangedRegion.setMinY( m_ChangedRegion.minY() + y_add_up);
- m_ChangedRegion.setMaxY( m_ChangedRegion.maxY() + y_add_up);
+ m_ChangedRegion.setMinX(m_ChangedRegion.minX() + x_add_left);
+ m_ChangedRegion.setMaxX(m_ChangedRegion.maxX() + x_add_left);
+ m_ChangedRegion.setMinY(m_ChangedRegion.minY() + y_add_up);
+ m_ChangedRegion.setMaxY(m_ChangedRegion.maxY() + y_add_up);
-
- m_metaData.width = new_width;
+ m_metaData.width = new_width;
m_metaData.height = new_height;
- m_metaData.origin.position.x -= (x_add_left ) * m_metaData.resolution;
- m_metaData.origin.position.y -= (y_add_up ) * m_metaData.resolution;
+ m_metaData.origin.position.x -= (x_add_left)*m_metaData.resolution;
+ m_metaData.origin.position.y -= (y_add_up)*m_metaData.resolution;
cleanUp();
m_OccupancyProbability = OccupancyProbability;
- m_MeasurementCount = MeasurementCount;
- m_OccupancyCount = OccupancyCount;
- m_CurrentChanges = CurrentChanges;
- m_HighSensitive = HighSensitive;
-
-
-
+ m_MeasurementCount = MeasurementCount;
+ m_OccupancyCount = OccupancyCount;
+ m_CurrentChanges = CurrentChanges;
+ m_HighSensitive = HighSensitive;
}
int OccupancyMap::width() const
@@ -251,60 +260,61 @@ int OccupancyMap::height() const
return m_metaData.height;
}
-float OccupancyMap::getOccupancyProbability ( Eigen::Vector2i p )
+float OccupancyMap::getOccupancyProbability(Eigen::Vector2i p)
{
- if(p.y() >= m_metaData.height || p.x() >= m_metaData.width)
+ if (p.y() >= m_metaData.height || p.x() >= m_metaData.width)
{
- return UNKNOWN_LIKELIHOOD;
+ return UNKNOWN_LIKELIHOOD;
}
unsigned offset = m_metaData.width * p.y() + p.x();
- return m_OccupancyProbability[ offset ];
+ return m_OccupancyProbability[offset];
}
void OccupancyMap::resetHighSensitive()
{
- ROS_INFO_STREAM("High sensitive Areas reseted");
- m_reset_high = true;
+ ROS_INFO_STREAM("High sensitive Areas reseted");
+ m_reset_high = true;
}
void OccupancyMap::computeOccupancyProbabilities()
{
- for ( int y = m_ChangedRegion.minY(); y <= m_ChangedRegion.maxY(); y++ )
+ for (int y = m_ChangedRegion.minY(); y <= m_ChangedRegion.maxY(); y++)
{
int yOffset = m_metaData.width * y;
- for ( int x = m_ChangedRegion.minX(); x <= m_ChangedRegion.maxX(); x++ )
+ for (int x = m_ChangedRegion.minX(); x <= m_ChangedRegion.maxX(); x++)
{
int i = x + yOffset;
- if ( m_MeasurementCount[i] > 0 )
+ if (m_MeasurementCount[i] > 0)
{
- //int maxCount = 100; //TODO param
- //if(m_MeasurementCount[i] > maxCount * 2 -1)
- //{
- //int scalingFactor = m_MeasurementCount[i] / maxCount;
- //if ( scalingFactor != 0 )
- //{
- //m_MeasurementCount[i] /= scalingFactor;
- //m_OccupancyCount[i] /= scalingFactor;
- //}
- //}
- m_OccupancyProbability[i] = m_OccupancyCount[i] / static_cast<float> ( m_MeasurementCount[i] );
- if (m_HighSensitive[i] == 1)
- {
- if(m_reset_high == true)
- {
- m_OccupancyCount[i] = 0;
- m_OccupancyProbability[i] = 0;
- }
- if(m_MeasurementCount[i] > 20 )
- {
- m_MeasurementCount[i] = 10;
- m_OccupancyCount[i] = 10 * m_OccupancyProbability[i];
- }
- if(m_OccupancyProbability[i] > 0.3)
- {
- m_OccupancyProbability[i] = 1 ;
- }
- }
+ // int maxCount = 100; //TODO param
+ // if(m_MeasurementCount[i] > maxCount * 2 -1)
+ //{
+ // int scalingFactor = m_MeasurementCount[i] / maxCount;
+ // if ( scalingFactor != 0 )
+ //{
+ // m_MeasurementCount[i] /= scalingFactor;
+ // m_OccupancyCount[i] /= scalingFactor;
+ //}
+ //}
+ m_OccupancyProbability[i] =
+ m_OccupancyCount[i] / static_cast<float>(m_MeasurementCount[i]);
+ if (m_HighSensitive[i] == 1)
+ {
+ if (m_reset_high == true)
+ {
+ m_OccupancyCount[i] = 0;
+ m_OccupancyProbability[i] = 0;
+ }
+ if (m_MeasurementCount[i] > 20)
+ {
+ m_MeasurementCount[i] = 10;
+ m_OccupancyCount[i] = 10 * m_OccupancyProbability[i];
+ }
+ if (m_OccupancyProbability[i] > 0.3)
+ {
+ m_OccupancyProbability[i] = 1;
+ }
+ }
}
else
{
@@ -312,240 +322,249 @@ void OccupancyMap::computeOccupancyProbabilities()
}
}
}
- if(m_reset_high == true)
+ if (m_reset_high == true)
{
- m_reset_high = false;
+ m_reset_high = false;
}
}
-void OccupancyMap::insertLaserData ( sensor_msgs::LaserScan::ConstPtr laserData, tf::Transform transform)
+void OccupancyMap::insertLaserData(sensor_msgs::LaserScan::ConstPtr laserData,
+ tf::Transform transform)
{
m_latestMapTransform = transform;
markRobotPositionFree();
std::vector<RangeMeasurement> ranges;
- ranges.reserve ( laserData->ranges.size() );
+ ranges.reserve(laserData->ranges.size());
- bool errorFound=false;
- int lastValidIndex=-1;
- float lastValidRange=m_FreeReadingDistance;
+ bool errorFound = false;
+ int lastValidIndex = -1;
+ float lastValidRange = m_FreeReadingDistance;
RangeMeasurement rangeMeasurement;
rangeMeasurement.sensorPos.x = m_laserTransform.getOrigin().getX();
rangeMeasurement.sensorPos.y = m_laserTransform.getOrigin().getY();
-
- for ( unsigned int i = 0; i < laserData->ranges.size(); i++ )
+
+ for (unsigned int i = 0; i < laserData->ranges.size(); i++)
{
- if ( ( laserData->ranges[i] >= laserData->range_min ) && ( laserData->ranges[i] <= laserData->range_max ) )
+ if ((laserData->ranges[i] >= laserData->range_min) &&
+ (laserData->ranges[i] <= laserData->range_max))
{
- //if we're at the end of an errorneous segment, interpolate
- //between last valid point and current point
- if ( errorFound )
+ // if we're at the end of an errorneous segment, interpolate
+ // between last valid point and current point
+ if (errorFound)
{
- //smaller of the two ranges belonging to end points
- float range = Math::min ( lastValidRange, laserData->ranges[i] );
+ // smaller of the two ranges belonging to end points
+ float range = Math::min(lastValidRange, laserData->ranges[i]);
range -= m_metaData.resolution * 2;
- if ( range < m_FreeReadingDistance )
+ if (range < m_FreeReadingDistance)
{
range = m_FreeReadingDistance;
}
- else
- if ( range > laserData->range_max )
- {
- range = laserData->range_max;
- }
- //choose smaller range
- for ( unsigned j=lastValidIndex+1; j<i; j++ )
- {
- float alpha = laserData->angle_min + j * laserData->angle_increment;
- tf::Vector3 pin;
- tf::Vector3 pout;
- pin.setX( cos(alpha) * range);
- pin.setY( sin(alpha) * range);
- pin.setZ(0);
- pout = m_laserTransform * pin;
- rangeMeasurement.endPos.x = pout.x();
- rangeMeasurement.endPos.y = pout.y();
- rangeMeasurement.free = true;
- ranges.push_back ( rangeMeasurement );
- }
+ else if (range > laserData->range_max)
+ {
+ range = laserData->range_max;
+ }
+ // choose smaller range
+ for (unsigned j = lastValidIndex + 1; j < i; j++)
+ {
+ float alpha = laserData->angle_min + j * laserData->angle_increment;
+ tf::Vector3 pin;
+ tf::Vector3 pout;
+ pin.setX(cos(alpha) * range);
+ pin.setY(sin(alpha) * range);
+ pin.setZ(0);
+ pout = m_laserTransform * pin;
+ rangeMeasurement.endPos.x = pout.x();
+ rangeMeasurement.endPos.y = pout.y();
+ rangeMeasurement.free = true;
+ ranges.push_back(rangeMeasurement);
+ }
}
- float alpha = laserData->angle_min + i * laserData->angle_increment;
- tf::Vector3 pin;
- tf::Vector3 pout;
- pin.setX( cos(alpha) * laserData->ranges[i]);
- pin.setY( sin(alpha) * laserData->ranges[i]);
- pin.setZ(0);
- pout = m_laserTransform * pin;
- rangeMeasurement.endPos.x = pout.x();
- rangeMeasurement.endPos.y = pout.y();
+ float alpha = laserData->angle_min + i * laserData->angle_increment;
+ tf::Vector3 pin;
+ tf::Vector3 pout;
+ pin.setX(cos(alpha) * laserData->ranges[i]);
+ pin.setY(sin(alpha) * laserData->ranges[i]);
+ pin.setZ(0);
+ pout = m_laserTransform * pin;
+ rangeMeasurement.endPos.x = pout.x();
+ rangeMeasurement.endPos.y = pout.y();
rangeMeasurement.free = false;
- ranges.push_back ( rangeMeasurement );
- errorFound=false;
- lastValidIndex=i;
- lastValidRange=laserData->ranges[i];
+ ranges.push_back(rangeMeasurement);
+ errorFound = false;
+ lastValidIndex = i;
+ lastValidRange = laserData->ranges[i];
}
else
{
- errorFound=true;
+ errorFound = true;
}
}
- insertRanges ( ranges , laserData->header.stamp);
-
+ insertRanges(ranges, laserData->header.stamp);
}
-
-void OccupancyMap::insertRanges ( vector<RangeMeasurement> ranges, ros::Time stamp )
+void OccupancyMap::insertRanges(vector<RangeMeasurement> ranges,
+ ros::Time stamp)
{
- if(ranges.size() < 1)
- {
- return;
- }
+ if (ranges.size() < 1)
+ {
+ return;
+ }
clearChanges();
Eigen::Vector2i lastEndPixel;
int need_x_left = 0;
- int need_x_right = 0;
+ int need_x_right = 0;
int need_y_up = 0;
int need_y_down = 0;
std::vector<Eigen::Vector2i> map_pixel;
- for(int i = 0; i < ranges.size(); i++)
+ for (int i = 0; i < ranges.size(); i++)
{
- geometry_msgs::Point endPosWorld = map_tools::transformPoint(ranges[i].endPos, m_latestMapTransform);
- map_pixel.push_back(map_tools::toMapCoords(endPosWorld, m_metaData.origin, m_metaData.resolution));
+ geometry_msgs::Point endPosWorld =
+ map_tools::transformPoint(ranges[i].endPos, m_latestMapTransform);
+ map_pixel.push_back(map_tools::toMapCoords(endPosWorld, m_metaData.origin,
+ m_metaData.resolution));
}
- geometry_msgs::Point sensorPosWorld = map_tools::transformPoint(ranges[0].sensorPos, m_latestMapTransform);
- Eigen::Vector2i sensorPixel = map_tools::toMapCoords(sensorPosWorld, m_metaData.origin, m_metaData.resolution);
- m_ChangedRegion.enclose ( sensorPixel.x(), sensorPixel.y() );
+ geometry_msgs::Point sensorPosWorld =
+ map_tools::transformPoint(ranges[0].sensorPos, m_latestMapTransform);
+ Eigen::Vector2i sensorPixel = map_tools::toMapCoords(
+ sensorPosWorld, m_metaData.origin, m_metaData.resolution);
+ m_ChangedRegion.enclose(sensorPixel.x(), sensorPixel.y());
////paint safety borders
- //if ( m_ObstacleBorders )
+ // if ( m_ObstacleBorders )
//{
- //for ( unsigned i=0; i<ranges.size(); i++ )
- //{
- //Eigen::Vector2i endPixel = map_pixel[i];
-
- //for ( int y=endPixel.y()-1; y <= endPixel.y() +1; y++ )
- //{
- //if(y > m_metaData.height) continue;
- //for ( int x=endPixel.x()-1; x <= endPixel.x() +1; x++ )
- //{
- //if(x > m_metaData.width) continue;
- //unsigned offset=x+m_metaData.width*y;
- //if ( offset < unsigned ( m_ByteSize ) )
- //{
- //m_CurrentChanges[ offset ] = SAFETY_BORDER;
- //}
- //}
- //}
- //}
+ // for ( unsigned i=0; i<ranges.size(); i++ )
+ //{
+ // Eigen::Vector2i endPixel = map_pixel[i];
+
+ // for ( int y=endPixel.y()-1; y <= endPixel.y() +1; y++ )
+ //{
+ // if(y > m_metaData.height) continue;
+ // for ( int x=endPixel.x()-1; x <= endPixel.x() +1; x++ )
+ //{
+ // if(x > m_metaData.width) continue;
+ // unsigned offset=x+m_metaData.width*y;
+ // if ( offset < unsigned ( m_ByteSize ) )
+ //{
+ // m_CurrentChanges[ offset ] = SAFETY_BORDER;
+ //}
+ //}
+ //}
+ //}
//}
////paint safety ranges
- //for ( unsigned i=0; i<ranges.size(); i++ )
+ // for ( unsigned i=0; i<ranges.size(); i++ )
//{
- //Eigen::Vector2i startPixel = map_pixel[i];
- //geometry_msgs::Point endPos;
- //endPos.x = ranges[i].endPos.x * 4;
- //endPos.y = ranges[i].endPos.y * 4;
-
- //geometry_msgs::Point endPosWorld = map_tools::transformPoint(endPos, m_latestMapTransform);
- //Eigen::Vector2i endPixel = map_tools::toMapCoords(endPosWorld, m_metaData.origin, m_metaData.resolution);
-
-
- //if(endPixel.x() < 0) endPixel.x() = 0;
- //if(endPixel.y() < 0) endPixel.y() = 0;
- //if(endPixel.x() >= m_metaData.width) endPixel.x() = m_metaData.width - 1;
- //if(endPixel.y() >= m_metaData.height) endPixel.y() = m_metaData.height - 1;
-
- //drawLine ( m_CurrentChanges, startPixel, endPixel, SAFETY_BORDER );
+ // Eigen::Vector2i startPixel = map_pixel[i];
+ // geometry_msgs::Point endPos;
+ // endPos.x = ranges[i].endPos.x * 4;
+ // endPos.y = ranges[i].endPos.y * 4;
+
+ // geometry_msgs::Point endPosWorld = map_tools::transformPoint(endPos,
+ // m_latestMapTransform);
+ // Eigen::Vector2i endPixel = map_tools::toMapCoords(endPosWorld,
+ // m_metaData.origin, m_metaData.resolution);
+
+ // if(endPixel.x() < 0) endPixel.x() = 0;
+ // if(endPixel.y() < 0) endPixel.y() = 0;
+ // if(endPixel.x() >= m_metaData.width) endPixel.x() = m_metaData.width - 1;
+ // if(endPixel.y() >= m_metaData.height) endPixel.y() = m_metaData.height - 1;
+
+ // drawLine ( m_CurrentChanges, startPixel, endPixel, SAFETY_BORDER );
//}
- //paint end pixels
- for ( unsigned i=0; i<ranges.size(); i++ )
+ // paint end pixels
+ for (unsigned i = 0; i < ranges.size(); i++)
{
- Eigen::Vector2i endPixel = map_pixel[i];
-
- if ( endPixel != lastEndPixel )
- {
- bool outside = false;
- if(endPixel.x() >= m_metaData.width)
- {
- need_x_right = std::max((int)( endPixel.x() - m_metaData.width + 10), need_x_right);
- outside = true;
- }
- if(endPixel.x() < 0)
- {
- need_x_left = std::max((int)(-endPixel.x()) + 10, need_x_left);
- outside = true;
- }
- if(endPixel.y() >= m_metaData.height)
- {
- need_y_down = std::max((int)(endPixel.y() - m_metaData.height) + 10, need_y_down);
- outside = true;
- }
- if(endPixel.y() < 0)
- {
- need_y_up = std::max((int)(- endPixel.y()) + 10, need_y_up);
- outside = true;
- }
- if(outside)
- {
- continue;
- }
- m_ChangedRegion.enclose ( endPixel.x(), endPixel.y() );
- //paint free ranges
- drawLine ( m_CurrentChanges, sensorPixel, endPixel, ::FREE );
-
- if ( !ranges[i].free )
- {
- unsigned offset = endPixel.x() + m_metaData.width * endPixel.y();
- m_CurrentChanges[ offset ] = ::OCCUPIED;
- }
- }
- lastEndPixel=endPixel;
+ Eigen::Vector2i endPixel = map_pixel[i];
+
+ if (endPixel != lastEndPixel)
+ {
+ bool outside = false;
+ if (endPixel.x() >= m_metaData.width)
+ {
+ need_x_right =
+ std::max((int)(endPixel.x() - m_metaData.width + 10), need_x_right);
+ outside = true;
+ }
+ if (endPixel.x() < 0)
+ {
+ need_x_left = std::max((int)(-endPixel.x()) + 10, need_x_left);
+ outside = true;
+ }
+ if (endPixel.y() >= m_metaData.height)
+ {
+ need_y_down =
+ std::max((int)(endPixel.y() - m_metaData.height) + 10, need_y_down);
+ outside = true;
+ }
+ if (endPixel.y() < 0)
+ {
+ need_y_up = std::max((int)(-endPixel.y()) + 10, need_y_up);
+ outside = true;
+ }
+ if (outside)
+ {
+ continue;
+ }
+ m_ChangedRegion.enclose(endPixel.x(), endPixel.y());
+ // paint free ranges
+ drawLine(m_CurrentChanges, sensorPixel, endPixel, ::FREE);
+
+ if (!ranges[i].free)
+ {
+ unsigned offset = endPixel.x() + m_metaData.width * endPixel.y();
+ m_CurrentChanges[offset] = ::OCCUPIED;
+ }
+ }
+ lastEndPixel = endPixel;
}
- m_ChangedRegion.clip ( Box2D<int> ( 0,0,m_metaData.width-1,m_metaData.height-1 ) );
- m_ExploredRegion.enclose ( m_ChangedRegion );
+ m_ChangedRegion.clip(
+ Box2D<int>(0, 0, m_metaData.width - 1, m_metaData.height - 1));
+ m_ExploredRegion.enclose(m_ChangedRegion);
applyChanges();
computeOccupancyProbabilities();
- if(need_x_left + need_x_right + need_y_down + need_y_up > 0)
+ if (need_x_left + need_x_right + need_y_down + need_y_up > 0)
{
- //keep square aspect ration till homer_gui can handle other maps
- //int need_x = need_x_left + need_x_right;
- //int need_y = need_y_up + need_y_down;
- //if(need_x > need_y)
- //{
- //need_y_down += need_x - need_y;
- //}
- //else if (need_y > need_x)
- //{
- //need_x_right += need_y - need_x;
- //}
-
- ROS_INFO_STREAM("new map size!");
- ROS_INFO_STREAM(" "<<need_x_left<<" "<<need_y_up<<" "<<need_x_right<<" "<<need_y_down);
- changeMapSize(need_x_left, need_y_up, need_x_right, need_y_down);
+ // keep square aspect ration till homer_gui can handle other maps
+ // int need_x = need_x_left + need_x_right;
+ // int need_y = need_y_up + need_y_down;
+ // if(need_x > need_y)
+ //{
+ // need_y_down += need_x - need_y;
+ //}
+ // else if (need_y > need_x)
+ //{
+ // need_x_right += need_y - need_x;
+ //}
+
+ ROS_INFO_STREAM("new map size!");
+ ROS_INFO_STREAM(" " << need_x_left << " " << need_y_up << " "
+ << need_x_right << " " << need_y_down);
+ changeMapSize(need_x_left, need_y_up, need_x_right, need_y_down);
}
}
-double OccupancyMap::contrastFromProbability ( int8_t prob )
+double OccupancyMap::contrastFromProbability(int8_t prob)
{
// range from 0..126 (=127 values) and 128..255 (=128 values)
- double diff = ( ( double ) prob - UNKNOWN );
+ double diff = ((double)prob - UNKNOWN);
double contrast;
- if ( prob <= UNKNOWN )
+ if (prob <= UNKNOWN)
{
- contrast = ( diff / UNKNOWN ); // 0..1
+ contrast = (diff / UNKNOWN); // 0..1
}
else
{
- contrast = ( diff / ( UNKNOWN+1 ) ); // 0..1
+ contrast = (diff / (UNKNOWN + 1)); // 0..1
}
- return ( contrast * contrast );
+ return (contrast * contrast);
}
double OccupancyMap::evaluateByContrast()
@@ -553,79 +572,79 @@ double OccupancyMap::evaluateByContrast()
double contrastSum = 0.0;
unsigned int contrastCnt = 0;
- for ( int y = m_ExploredRegion.minY(); y <= m_ExploredRegion.maxY(); y++ )
+ for (int y = m_ExploredRegion.minY(); y <= m_ExploredRegion.maxY(); y++)
{
- for ( int x = m_ExploredRegion.minX(); x <= m_ExploredRegion.maxX(); x++ )
+ for (int x = m_ExploredRegion.minX(); x <= m_ExploredRegion.maxX(); x++)
{
int i = x + y * m_metaData.width;
- if ( m_MeasurementCount [i] > 1 )
+ if (m_MeasurementCount[i] > 1)
{
int prob = m_OccupancyProbability[i] * 100;
- if ( prob != NOT_SEEN_YET ) // ignore not yet seen cells
+ if (prob != NOT_SEEN_YET) // ignore not yet seen cells
{
- contrastSum += contrastFromProbability ( prob );
+ contrastSum += contrastFromProbability(prob);
contrastCnt++;
}
}
}
}
- if ( ( contrastCnt ) > 0 )
+ if ((contrastCnt) > 0)
{
- return ( ( contrastSum / contrastCnt ) * 100 );
+ return ((contrastSum / contrastCnt) * 100);
}
- return ( 0 );
+ return (0);
}
-
-
-vector<MeasurePoint> OccupancyMap::getMeasurePoints (sensor_msgs::LaserScanConstPtr laserData)
+vector<MeasurePoint>
+OccupancyMap::getMeasurePoints(sensor_msgs::LaserScanConstPtr laserData)
{
vector<MeasurePoint> result;
- result.reserve ( laserData->ranges.size() );
+ result.reserve(laserData->ranges.size());
double minDist = m_MeasureSamplingStep;
Point2D lastHitPos;
Point2D lastUsedHitPos;
- //extract points for measuring
- for ( unsigned int i=0; i < laserData->ranges.size(); i++ )
+ // extract points for measuring
+ for (unsigned int i = 0; i < laserData->ranges.size(); i++)
{
- if ( laserData->ranges[i] <= laserData->range_max && laserData->ranges[i] >= laserData->range_min )
+ if (laserData->ranges[i] <= laserData->range_max &&
+ laserData->ranges[i] >= laserData->range_min)
{
- float alpha = laserData->angle_min + i * laserData->angle_increment;
- tf::Vector3 pin;
- tf::Vector3 pout;
- pin.setX( cos(alpha) * laserData->ranges[i]);
- pin.setY( sin(alpha) * laserData->ranges[i]);
- pin.setZ(0);
-
- pout = m_laserTransform * pin;
-
- Point2D hitPos(pout.x(), pout.y());
-
- if ( hitPos.distance ( lastUsedHitPos ) >= minDist )
+ float alpha = laserData->angle_min + i * laserData->angle_increment;
+ tf::Vector3 pin;
+ tf::Vector3 pout;
+ pin.setX(cos(alpha) * laserData->ranges[i]);
+ pin.setY(sin(alpha) * laserData->ranges[i]);
+ pin.setZ(0);
+
+ pout = m_laserTransform * pin;
+
+ Point2D hitPos(pout.x(), pout.y());
+
+ if (hitPos.distance(lastUsedHitPos) >= minDist)
{
MeasurePoint p;
- //preserve borders of segments
- if ( ( i!=0 ) &&
- ( lastUsedHitPos.distance ( lastHitPos ) > m_metaData.resolution*0.5 ) &&
- ( hitPos.distance ( lastHitPos ) >= minDist*1.5 ) )
+ // preserve borders of segments
+ if ((i != 0) && (lastUsedHitPos.distance(lastHitPos) >
+ m_metaData.resolution * 0.5) &&
+ (hitPos.distance(lastHitPos) >= minDist * 1.5))
{
p.hitPos = lastHitPos;
p.borderType = RightBorder;
- result.push_back ( p );
+ result.push_back(p);
p.hitPos = hitPos;
p.borderType = LeftBorder;
- result.push_back ( p );
+ result.push_back(p);
lastUsedHitPos = hitPos;
}
else
{
- //save current point
+ // save current point
p.hitPos = hitPos;
p.borderType = NoBorder;
- result.push_back ( p );
+ result.push_back(p);
lastUsedHitPos = hitPos;
}
}
@@ -633,34 +652,34 @@ vector<MeasurePoint> OccupancyMap::getMeasurePoints (sensor_msgs::LaserScanConst
}
}
- //the first and last one are border pixels
- if ( result.size() > 0 )
+ // the first and last one are border pixels
+ if (result.size() > 0)
{
result[0].borderType = LeftBorder;
- result[result.size()-1].borderType = RightBorder;
+ result[result.size() - 1].borderType = RightBorder;
}
- //calculate front check points
- for ( unsigned i=0; i < result.size(); i++ )
+ // calculate front check points
+ for (unsigned i = 0; i < result.size(); i++)
{
CVec2 diff;
- switch ( result[i].borderType )
+ switch (result[i].borderType)
{
case NoBorder:
- diff = result[i-1].hitPos - result[i+1].hitPos;
+ diff = result[i - 1].hitPos - result[i + 1].hitPos;
break;
case LeftBorder:
- diff = result[i].hitPos - result[i+1].hitPos;
+ diff = result[i].hitPos - result[i + 1].hitPos;
break;
case RightBorder:
- diff = result[i-1].hitPos - result[i].hitPos;
+ diff = result[i - 1].hitPos - result[i].hitPos;
break;
}
- CVec2 normal = diff.rotate ( Math::Pi * 0.5 );
+ CVec2 normal = diff.rotate(Math::Pi * 0.5);
normal.normalize();
- normal *= m_metaData.resolution * sqrt ( 2.0 ) * 10.0;
+ normal *= m_metaData.resolution * sqrt(2.0) * 10.0;
result[i].frontPos = result[i].hitPos + normal;
}
@@ -668,68 +687,75 @@ vector<MeasurePoint> OccupancyMap::getMeasurePoints (sensor_msgs::LaserScanConst
return result;
}
-
-float OccupancyMap::computeScore ( Pose robotPose, std::vector<MeasurePoint> measurePoints )
+float OccupancyMap::computeScore(Pose robotPose,
+ std::vector<MeasurePoint> measurePoints)
{
// This is a very simple implementation, using only the end point of the beam.
// For every beam the end cell is computed and tested if the cell is occupied.
- unsigned lastOffset=0;
- unsigned hitOffset=0;
- unsigned frontOffset=0;
+ unsigned lastOffset = 0;
+ unsigned hitOffset = 0;
+ unsigned frontOffset = 0;
float fittingFactor = 0.0;
- float sinTheta = sin ( robotPose.theta() );
- float cosTheta = cos ( robotPose.theta() );
+ float sinTheta = sin(robotPose.theta());
+ float cosTheta = cos(robotPose.theta());
- for ( unsigned int i = 0; i < measurePoints.size(); i++ )
+ for (unsigned int i = 0; i < measurePoints.size(); i++)
{
- //fast variant:
- float x = cosTheta * measurePoints[i].hitPos.x() - sinTheta * measurePoints[i].hitPos.y() + robotPose.x();
- float y = sinTheta * measurePoints[i].hitPos.x() + cosTheta * measurePoints[i].hitPos.y() + robotPose.y();
+ // fast variant:
+ float x = cosTheta * measurePoints[i].hitPos.x() -
+ sinTheta * measurePoints[i].hitPos.y() + robotPose.x();
+ float y = sinTheta * measurePoints[i].hitPos.x() +
+ cosTheta * measurePoints[i].hitPos.y() + robotPose.y();
geometry_msgs::Point hitPos;
hitPos.x = x;
hitPos.y = y;
- Eigen::Vector2i hitPixel = map_tools::toMapCoords(hitPos, m_metaData.origin, m_metaData.resolution);
- hitOffset = hitPixel.x() + m_metaData.width*hitPixel.y();
+ Eigen::Vector2i hitPixel = map_tools::toMapCoords(hitPos, m_metaData.origin,
+ m_metaData.resolution);
+ hitOffset = hitPixel.x() + m_metaData.width * hitPixel.y();
- //avoid multiple measuring of same pixel or unknown pixel
- if ( ( hitOffset == lastOffset ) || ( hitOffset >= unsigned ( m_ByteSize ) ) || ( m_MeasurementCount[hitOffset] == 0 ) )
+ // avoid multiple measuring of same pixel or unknown pixel
+ if ((hitOffset == lastOffset) || (hitOffset >= unsigned(m_ByteSize)) ||
+ (m_MeasurementCount[hitOffset] == 0))
{
continue;
}
- if ( m_BacksideChecking )
+ if (m_BacksideChecking)
{
- //avoid matching of back and front pixels of obstacles
- x = cosTheta * measurePoints[i].frontPos.x() - sinTheta * measurePoints[i].frontPos.y() + robotPose.x();
- y = sinTheta * measurePoints[i].frontPos.x() + cosTheta * measurePoints[i].frontPos.y() + robotPose.y();
+ // avoid matching of back and front pixels of obstacles
+ x = cosTheta * measurePoints[i].frontPos.x() -
+ sinTheta * measurePoints[i].frontPos.y() + robotPose.x();
+ y = sinTheta * measurePoints[i].frontPos.x() +
+ cosTheta * measurePoints[i].frontPos.y() + robotPose.y();
geometry_msgs::Point frontPos;
frontPos.x = x;
frontPos.y = y;
- Eigen::Vector2i frontPixel = map_tools::toMapCoords(frontPos, m_metaData.origin, m_metaData.resolution);
- frontOffset = frontPixel.x() + m_metaData.width*frontPixel.y();
+ Eigen::Vector2i frontPixel = map_tools::toMapCoords(
+ frontPos, m_metaData.origin, m_metaData.resolution);
+ frontOffset = frontPixel.x() + m_metaData.width * frontPixel.y();
- if ( ( frontOffset >= unsigned ( m_ByteSize ) ) || ( m_MeasurementCount[frontOffset] == 0 ) )
+ if ((frontOffset >= unsigned(m_ByteSize)) ||
+ (m_MeasurementCount[frontOffset] == 0))
{
continue;
}
}
- lastOffset=hitOffset;
- //fittingFactor += m_SmoothOccupancyProbability[ offset ];
- fittingFactor += m_OccupancyProbability[ hitOffset ];
+ lastOffset = hitOffset;
+ // fittingFactor += m_SmoothOccupancyProbability[ offset ];
+ fittingFactor += m_OccupancyProbability[hitOffset];
}
return fittingFactor;
}
-
-template<class DataT>
-void OccupancyMap::drawLine ( DataT* data, Eigen::Vector2i& startPixel, Eigen::Vector2i& endPixel, char value )
+template <class DataT>
+void OccupancyMap::drawLine(DataT* data, Eigen::Vector2i& startPixel,
+ Eigen::Vector2i& endPixel, char value)
{
-
- //bresenham algorithm
+ // bresenham algorithm
int xstart = startPixel.x();
int ystart = startPixel.y();
int xend = endPixel.x();
@@ -741,7 +767,7 @@ void OccupancyMap::drawLine ( DataT* data, Eigen::Vector2i& startPixel, Eigen::V
dy = yend - ystart;
// compute increment
- if ( dx < 0 )
+ if (dx < 0)
{
incx = -1;
dx = -dx;
@@ -751,7 +777,7 @@ void OccupancyMap::drawLine ( DataT* data, Eigen::Vector2i& startPixel, Eigen::V
incx = dx ? 1 : 0;
}
- if ( dy < 0 )
+ if (dy < 0)
{
incy = -1;
dy = -dy;
@@ -762,7 +788,7 @@ void OccupancyMap::drawLine ( DataT* data, Eigen::Vector2i& startPixel, Eigen::V
}
// which distance is greater?
- dist = ( dx > dy ) ? dx : dy;
+ dist = (dx > dy) ? dx : dy;
// initializing
x = xstart;
y = ystart;
@@ -770,31 +796,31 @@ void OccupancyMap::drawLine ( DataT* data, Eigen::Vector2i& startPixel, Eigen::V
yerr = dy;
// compute cells
- for ( t = 0; t < dist; t++ )
+ for (t = 0; t < dist; t++)
{
int index = x + m_metaData.width * y;
// set flag to free if no flag is set
// (do not overwrite occupied cells)
- if(index < 0 || index > m_ByteSize)
- {
- continue;
- }
- if ( data[index] == NO_CHANGE )
+ if (index < 0 || index > m_ByteSize)
{
- data[index] = value;
+ continue;
}
-/* if ( data[index] == OCCUPIED || data[index] == SAFETY_BORDER )
+ if (data[index] == NO_CHANGE)
{
- return;
- }*/
+ data[index] = value;
+ }
+ /* if ( data[index] == OCCUPIED || data[index] == SAFETY_BORDER )
+ {
+ return;
+ }*/
xerr += dx;
yerr += dy;
- if ( xerr > dist )
+ if (xerr > dist)
{
xerr -= dist;
x += incx;
}
- if ( yerr > dist )
+ if (yerr > dist)
{
yerr -= dist;
y += incy;
@@ -802,104 +828,112 @@ void OccupancyMap::drawLine ( DataT* data, Eigen::Vector2i& startPixel, Eigen::V
}
}
-
void OccupancyMap::applyChanges()
{
- for ( int y = m_ChangedRegion.minY()+1; y <= m_ChangedRegion.maxY()-1; y++ )
+ for (int y = m_ChangedRegion.minY() + 1; y <= m_ChangedRegion.maxY() - 1; y++)
{
int yOffset = m_metaData.width * y;
- for ( int x = m_ChangedRegion.minX()+1; x <= m_ChangedRegion.maxX()-1; x++ )
+ for (int x = m_ChangedRegion.minX() + 1; x <= m_ChangedRegion.maxX() - 1;
+ x++)
{
int i = x + yOffset;
- if ( ( m_CurrentChanges[i] == ::FREE || m_CurrentChanges[i] == ::OCCUPIED ) && m_MeasurementCount[i] < SHRT_MAX )
+ if ((m_CurrentChanges[i] == ::FREE ||
+ m_CurrentChanges[i] == ::OCCUPIED) &&
+ m_MeasurementCount[i] < SHRT_MAX)
{
m_MeasurementCount[i]++;
}
- if ( m_CurrentChanges[i] == ::OCCUPIED && m_OccupancyCount[i] < USHRT_MAX )
+ if (m_CurrentChanges[i] == ::OCCUPIED && m_OccupancyCount[i] < USHRT_MAX)
{
-
- //if(m_MeasurementCount[x + m_metaData.width * (y+1)] > 1)
- //m_MeasurementCount[x + m_metaData.width * (y+1)]++;
- //if(m_MeasurementCount[x + m_metaData.width * (y-1)] > 1)
- //m_MeasurementCount[x + m_metaData.width * (y-1)]++;
- //if(m_MeasurementCount[i-1] > 1)
- //m_MeasurementCount[i-1]++;
- //if(m_MeasurementCount[i+1] > 1)
- //m_MeasurementCount[i+1]++;
- m_OccupancyCount[i]++ ;
+ // if(m_MeasurementCount[x + m_metaData.width * (y+1)] > 1)
+ // m_MeasurementCount[x + m_metaData.width * (y+1)]++;
+ // if(m_MeasurementCount[x + m_metaData.width * (y-1)] > 1)
+ // m_MeasurementCount[x + m_metaData.width * (y-1)]++;
+ // if(m_MeasurementCount[i-1] > 1)
+ // m_MeasurementCount[i-1]++;
+ // if(m_MeasurementCount[i+1] > 1)
+ // m_MeasurementCount[i+1]++;
+ m_OccupancyCount[i]++;
}
}
}
- for ( int y = m_ChangedRegion.minY()+1; y <= m_ChangedRegion.maxY()-1; y++ )
+ for (int y = m_ChangedRegion.minY() + 1; y <= m_ChangedRegion.maxY() - 1; y++)
{
int yOffset = m_metaData.width * y;
- for ( int x = m_ChangedRegion.minX()+1; x <= m_ChangedRegion.maxX()-1; x++ )
+ for (int x = m_ChangedRegion.minX() + 1; x <= m_ChangedRegion.maxX() - 1;
+ x++)
{
int i = x + yOffset;
- if(m_OccupancyCount[i]> m_MeasurementCount[i])
- m_OccupancyCount[i]=m_MeasurementCount[i];
-}}
+ if (m_OccupancyCount[i] > m_MeasurementCount[i])
+ m_OccupancyCount[i] = m_MeasurementCount[i];
+ }
+ }
}
void OccupancyMap::clearChanges()
{
- m_ChangedRegion.expand ( 2 );
- m_ChangedRegion.clip ( Box2D<int> ( 0,0,m_metaData.width-1,m_metaData.height-1 ) );
- for ( int y = m_ChangedRegion.minY(); y <= m_ChangedRegion.maxY(); y++ )
+ m_ChangedRegion.expand(2);
+ m_ChangedRegion.clip(
+ Box2D<int>(0, 0, m_metaData.width - 1, m_metaData.height - 1));
+ for (int y = m_ChangedRegion.minY(); y <= m_ChangedRegion.maxY(); y++)
{
int yOffset = m_metaData.width * y;
- for ( int x = m_ChangedRegion.minX(); x <= m_ChangedRegion.maxX(); x++ )
+ for (int x = m_ChangedRegion.minX(); x <= m_ChangedRegion.maxX(); x++)
{
int i = x + yOffset;
m_CurrentChanges[i] = NO_CHANGE;
}
}
- m_ChangedRegion=Box2D<int> ( m_metaData.width - 1, m_metaData.height - 1, 0, 0 );
+ m_ChangedRegion =
+ Box2D<int>(m_metaData.width - 1, m_metaData.height - 1, 0, 0);
}
-void OccupancyMap::incrementMeasurementCount ( Eigen::Vector2i p )
+void OccupancyMap::incrementMeasurementCount(Eigen::Vector2i p)
{
- if(p.x() >= m_metaData.width || p.y() >= m_metaData.height)
- {
- return;
- }
- unsigned index = p.x() + m_metaData.width * p.y();
- if ( m_CurrentChanges[index] == NO_CHANGE && m_MeasurementCount[index] < USHRT_MAX )
- {
- m_CurrentChanges[index] = ::FREE;
- m_MeasurementCount[index]++;
- }
+ if (p.x() >= m_metaData.width || p.y() >= m_metaData.height)
+ {
+ return;
+ }
+ unsigned index = p.x() + m_metaData.width * p.y();
+ if (m_CurrentChanges[index] == NO_CHANGE &&
+ m_MeasurementCount[index] < USHRT_MAX)
+ {
+ m_CurrentChanges[index] = ::FREE;
+ m_MeasurementCount[index]++;
+ }
}
-void OccupancyMap::incrementOccupancyCount ( Eigen::Vector2i p )
+void OccupancyMap::incrementOccupancyCount(Eigen::Vector2i p)
{
- if(p.x() >= m_metaData.width || p.y() >= m_metaData.height)
- {
- return;
- }
- unsigned index = p.x() + m_metaData.width * p.y();
- if ( ( m_CurrentChanges[index] == NO_CHANGE || m_CurrentChanges[index] == ::FREE ) && m_MeasurementCount[index] < USHRT_MAX )
- {
- m_CurrentChanges[index] = ::OCCUPIED;
- m_OccupancyCount[index]++;
- }
+ if (p.x() >= m_metaData.width || p.y() >= m_metaData.height)
+ {
+ return;
+ }
+ unsigned index = p.x() + m_metaData.width * p.y();
+ if ((m_CurrentChanges[index] == NO_CHANGE ||
+ m_CurrentChanges[index] == ::FREE) &&
+ m_MeasurementCount[index] < USHRT_MAX)
+ {
+ m_CurrentChanges[index] = ::OCCUPIED;
+ m_OccupancyCount[index]++;
+ }
}
-void OccupancyMap::scaleDownCounts ( int maxCount )
+void OccupancyMap::scaleDownCounts(int maxCount)
{
clearChanges();
- if ( maxCount <= 0 )
+ if (maxCount <= 0)
{
ROS_WARN("WARNING: argument maxCount is choosen to small, resetting map.");
- memset ( m_MeasurementCount, 0, m_ByteSize );
- memset ( m_OccupancyCount, 0, m_ByteSize );
+ memset(m_MeasurementCount, 0, m_ByteSize);
+ memset(m_OccupancyCount, 0, m_ByteSize);
}
else
{
- for ( unsigned i = 0; i < m_ByteSize; i++ )
+ for (unsigned i = 0; i < m_ByteSize; i++)
{
int scalingFactor = m_MeasurementCount[i] / maxCount;
- if ( scalingFactor != 0 )
+ if (scalingFactor != 0)
{
m_MeasurementCount[i] /= scalingFactor;
m_OccupancyCount[i] /= scalingFactor;
@@ -911,75 +945,82 @@ void OccupancyMap::scaleDownCounts ( int maxCount )
computeOccupancyProbabilities();
}
-
void OccupancyMap::markRobotPositionFree()
{
geometry_msgs::Point point;
point.x = 0;
point.y = 0;
point.z = 0;
- geometry_msgs::Point endPosWorld = map_tools::transformPoint(point, m_latestMapTransform);
- Eigen::Vector2i robotPixel = map_tools::toMapCoords(endPosWorld, m_metaData.origin, m_metaData.resolution);
+ geometry_msgs::Point endPosWorld =
+ map_tools::transformPoint(point, m_latestMapTransform);
+ Eigen::Vector2i robotPixel = map_tools::toMapCoords(
+ endPosWorld, m_metaData.origin, m_metaData.resolution);
int width = 0.35 / m_metaData.resolution;
- for ( int i = robotPixel.y() - width; i <= robotPixel.y() + width; i++ )
+ for (int i = robotPixel.y() - width; i <= robotPixel.y() + width; i++)
{
- for ( int j = robotPixel.x() - width; j <= robotPixel.x() + width; j++ )
+ for (int j = robotPixel.x() - width; j <= robotPixel.x() + width; j++)
{
- incrementMeasurementCount ( Eigen::Vector2i ( j, i ) );
+ incrementMeasurementCount(Eigen::Vector2i(j, i));
}
}
- Box2D<int> robotBox ( robotPixel.x()-width, robotPixel.y()-width, robotPixel.x() +width, robotPixel.y() +width );
- m_ChangedRegion.enclose ( robotBox );
- m_ExploredRegion.enclose ( robotBox );
+ Box2D<int> robotBox(robotPixel.x() - width, robotPixel.y() - width,
+ robotPixel.x() + width, robotPixel.y() + width);
+ m_ChangedRegion.enclose(robotBox);
+ m_ExploredRegion.enclose(robotBox);
}
-
-QImage OccupancyMap::getProbabilityQImage ( int trancparencyThreshold, bool showInaccessible ) const
+QImage OccupancyMap::getProbabilityQImage(int trancparencyThreshold,
+ bool showInaccessible) const
{
- QImage retImage ( m_metaData.width, m_metaData.height, QImage::Format_RGB32 );
- for ( int y = 0; y < m_metaData.height; y++ )
+ QImage retImage(m_metaData.width, m_metaData.height, QImage::Format_RGB32);
+ for (int y = 0; y < m_metaData.height; y++)
{
- for ( int x = 0; x < m_metaData.width; x++ )
+ for (int x = 0; x < m_metaData.width; x++)
{
int index = x + y * m_metaData.width;
int value = UNKNOWN;
- if ( m_MeasurementCount[index] > 0 )
+ if (m_MeasurementCount[index] > 0)
{
- value = static_cast<int> ( ( 1.0 - m_OccupancyProbability[index] ) * 255 );
- if ( m_MeasurementCount[index] < trancparencyThreshold )
+ value = static_cast<int>((1.0 - m_OccupancyProbability[index]) * 255);
+ if (m_MeasurementCount[index] < trancparencyThreshold)
{
- value = static_cast<int> ( ( 0.75 + 0.025 * m_MeasurementCount[index] ) * ( 1.0 - m_OccupancyProbability[index] ) * 255 );
+ value = static_cast<int>((0.75 + 0.025 * m_MeasurementCount[index]) *
+ (1.0 - m_OccupancyProbability[index]) * 255);
}
}
- retImage.setPixel ( x, y, qRgb ( value, value, value ) );
+ retImage.setPixel(x, y, qRgb(value, value, value));
}
}
return retImage;
}
-void OccupancyMap::getOccupancyProbabilityImage ( vector<int8_t>& data, nav_msgs::MapMetaData& metaData)
+void OccupancyMap::getOccupancyProbabilityImage(vector<int8_t>& data,
+ nav_msgs::MapMetaData& metaData)
{
metaData = m_metaData;
data.resize(m_metaData.width * m_metaData.height);
- std::fill(data.begin(), data.end(), (int8_t)NOT_SEEN_YET); //note: linker error without strange cast from int8_t to int8_t
- for ( int y = m_ExploredRegion.minY(); y <= m_ExploredRegion.maxY(); y++ )
+ std::fill(data.begin(), data.end(),
+ (int8_t)NOT_SEEN_YET); // note: linker error without strange cast
+ // from int8_t to int8_t
+ for (int y = m_ExploredRegion.minY(); y <= m_ExploredRegion.maxY(); y++)
{
int yOffset = m_metaData.width * y;
- for ( int x = m_ExploredRegion.minX(); x <= m_ExploredRegion.maxX(); x++ )
+ for (int x = m_ExploredRegion.minX(); x <= m_ExploredRegion.maxX(); x++)
{
int i = x + yOffset;
- if ( m_MeasurementCount[i] < 1 )
+ if (m_MeasurementCount[i] < 1)
{
continue;
}
- if(m_OccupancyProbability[i] == UNKNOWN_LIKELIHOOD)
+ if (m_OccupancyProbability[i] == UNKNOWN_LIKELIHOOD)
{
- data[i] = NOT_SEEN_YET;
+ data[i] = NOT_SEEN_YET;
}
else
{
- data[i] = (int)(m_OccupancyProbability[i] * 99); //TODO maybe - 2 (or *0.99 or smth)
+ data[i] = (int)(m_OccupancyProbability[i] *
+ 99); // TODO maybe - 2 (or *0.99 or smth)
}
}
}
@@ -987,71 +1028,74 @@ void OccupancyMap::getOccupancyProbabilityImage ( vector<int8_t>& data, nav_msgs
void OccupancyMap::maximizeChangedRegion()
{
- m_ChangedRegion=m_ExploredRegion;
+ m_ChangedRegion = m_ExploredRegion;
}
-void OccupancyMap::applyMasking(const nav_msgs::OccupancyGrid::ConstPtr &msg)
+void OccupancyMap::applyMasking(const nav_msgs::OccupancyGrid::ConstPtr& msg)
{
- if(msg->data.size() != m_ByteSize)
- {
- ROS_ERROR_STREAM("Size Mismatch between SLAM map (" << m_ByteSize << ") and masking map (" << msg->data.size() << ")");
- return;
- }
- for(size_t y = 0; y < msg->info.height; y++)
+ if (msg->data.size() != m_ByteSize)
+ {
+ ROS_ERROR_STREAM("Size Mismatch between SLAM map ("
+ << m_ByteSize << ") and masking map (" << msg->data.size()
+ << ")");
+ return;
+ }
+ for (size_t y = 0; y < msg->info.height; y++)
+ {
+ int yOffset = msg->info.width * y;
+ for (size_t x = 0; x < msg->info.width; x++)
{
- int yOffset = msg->info.width * y;
- for(size_t x = 0; x < msg->info.width; x++)
- {
- int i = yOffset + x;
-
- switch(msg->data[i])
- {
- case homer_mapnav_msgs::ModifyMap::BLOCKED:
- case homer_mapnav_msgs::ModifyMap::OBSTACLE:
- //increase measure count of cells which were not yet visible to be able to modify unknown areas
- if(m_MeasurementCount[i] == 0)
- m_MeasurementCount[i] = 10;
-
- m_OccupancyCount[i] = m_MeasurementCount[i];
- m_OccupancyProbability[i] = 1.0;
- m_ExploredRegion.enclose(x, y);
- break;
- case homer_mapnav_msgs::ModifyMap::FREE:
- //see comment above
- if(m_MeasurementCount[i] == 0)
- m_MeasurementCount[i] = 10;
-
- m_OccupancyCount[i] = 0;
- m_OccupancyProbability[i] = 0.0;
- m_ExploredRegion.enclose(x, y);
- break;
- case homer_mapnav_msgs::ModifyMap::HIGH_SENSITIV:
- m_HighSensitive[i] = 1;
- break;
- }
- }
+ int i = yOffset + x;
+
+ switch (msg->data[i])
+ {
+ case homer_mapnav_msgs::ModifyMap::BLOCKED:
+ case homer_mapnav_msgs::ModifyMap::OBSTACLE:
+ // increase measure count of cells which were not yet visible to be
+ // able to modify unknown areas
+ if (m_MeasurementCount[i] == 0)
+ m_MeasurementCount[i] = 10;
+
+ m_OccupancyCount[i] = m_MeasurementCount[i];
+ m_OccupancyProbability[i] = 1.0;
+ m_ExploredRegion.enclose(x, y);
+ break;
+ case homer_mapnav_msgs::ModifyMap::FREE:
+ // see comment above
+ if (m_MeasurementCount[i] == 0)
+ m_MeasurementCount[i] = 10;
+
+ m_OccupancyCount[i] = 0;
+ m_OccupancyProbability[i] = 0.0;
+ m_ExploredRegion.enclose(x, y);
+ break;
+ case homer_mapnav_msgs::ModifyMap::HIGH_SENSITIV:
+ m_HighSensitive[i] = 1;
+ break;
+ }
}
+ }
}
void OccupancyMap::cleanUp()
{
- if ( m_OccupancyProbability )
+ if (m_OccupancyProbability)
{
delete[] m_OccupancyProbability;
}
- if ( m_MeasurementCount )
+ if (m_MeasurementCount)
{
delete[] m_MeasurementCount;
}
- if ( m_OccupancyCount )
+ if (m_OccupancyCount)
{
delete[] m_OccupancyCount;
}
- if ( m_CurrentChanges )
+ if (m_CurrentChanges)
{
delete[] m_CurrentChanges;
}
- if ( m_HighSensitive )
+ if (m_HighSensitive)
{
delete[] m_HighSensitive;
}
diff --git a/homer_mapping/src/ParticleFilter/HyperSlamFilter.cpp b/homer_mapping/src/ParticleFilter/HyperSlamFilter.cpp
old mode 100755
new mode 100644
index 7265b0ead89933bb6abfc422dd448d0c7c5a4889..4f5aec7d9be187c44247b43aee7d615668479b24
--- a/homer_mapping/src/ParticleFilter/HyperSlamFilter.cpp
+++ b/homer_mapping/src/ParticleFilter/HyperSlamFilter.cpp
@@ -1,36 +1,36 @@
#include <homer_mapping/ParticleFilter/HyperSlamFilter.h>
-#include <vector>
+#include <stdlib.h>
#include <cmath>
#include <fstream>
#include <sstream>
-#include <stdlib.h>
+#include <vector>
#include "ros/ros.h"
using namespace std;
-HyperSlamFilter::HyperSlamFilter (int particleFilterNum, int particleNum )
+HyperSlamFilter::HyperSlamFilter(int particleFilterNum, int particleNum)
{
m_ParticleFilterNum = particleFilterNum;
- if ( m_ParticleFilterNum < 1 )
- {
- m_ParticleFilterNum = 1;
- }
- ROS_DEBUG( "Using %d Hyper Particles.", particleFilterNum);
+ if (m_ParticleFilterNum < 1)
+ {
+ m_ParticleFilterNum = 1;
+ }
+ ROS_DEBUG("Using %d Hyper Particles.", particleFilterNum);
m_ParticleNum = particleNum;
- m_DoMapping = true;
+ m_DoMapping = true;
m_DeletionThreshold = 0.98;
- for ( unsigned i=0; i < m_ParticleFilterNum; i++ )
+ for (unsigned i = 0; i < m_ParticleFilterNum; i++)
{
ostringstream stream;
stream << "SlamFilter " << i;
- SlamFilter *slamFilter = new SlamFilter ( particleNum );
- m_SlamFilters.push_back ( slamFilter );
+ SlamFilter* slamFilter = new SlamFilter(particleNum);
+ m_SlamFilters.push_back(slamFilter);
}
m_BestSlamFilter = m_SlamFilters[0];
@@ -40,7 +40,7 @@ HyperSlamFilter::~HyperSlamFilter()
{
for (unsigned i = 0; i < m_ParticleFilterNum; i++)
{
- if( m_SlamFilters[i] )
+ if (m_SlamFilters[i])
{
delete m_SlamFilters[i];
m_SlamFilters[i] = 0;
@@ -48,147 +48,155 @@ HyperSlamFilter::~HyperSlamFilter()
}
}
-void HyperSlamFilter::setRotationErrorRotating ( float percent )
+void HyperSlamFilter::setRotationErrorRotating(float percent)
{
- for ( unsigned i=0; i < m_SlamFilters.size(); i++ )
- {
- m_SlamFilters[i]->setRotationErrorRotating(percent / 100.0);
- }
+ for (unsigned i = 0; i < m_SlamFilters.size(); i++)
+ {
+ m_SlamFilters[i]->setRotationErrorRotating(percent / 100.0);
+ }
}
-void HyperSlamFilter::setRotationErrorTranslating ( float degreePerMeter )
+void HyperSlamFilter::setRotationErrorTranslating(float degreePerMeter)
{
- for ( unsigned int i=0; i < m_SlamFilters.size(); i++ )
+ for (unsigned int i = 0; i < m_SlamFilters.size(); i++)
{
- m_SlamFilters[i]->setRotationErrorTranslating(degreePerMeter / 180.0 * M_PI);
+ m_SlamFilters[i]->setRotationErrorTranslating(degreePerMeter / 180.0 *
+ M_PI);
}
}
-void HyperSlamFilter::setTranslationErrorTranslating ( float percent )
+void HyperSlamFilter::setTranslationErrorTranslating(float percent)
{
- for ( unsigned int i=0; i < m_SlamFilters.size(); i++ )
+ for (unsigned int i = 0; i < m_SlamFilters.size(); i++)
{
m_SlamFilters[i]->setTranslationErrorTranslating(percent / 100.0);
}
}
-void HyperSlamFilter::setTranslationErrorRotating ( float mPerDegree )
+void HyperSlamFilter::setTranslationErrorRotating(float mPerDegree)
{
- for ( unsigned int i=0; i < m_SlamFilters.size(); i++ )
+ for (unsigned int i = 0; i < m_SlamFilters.size(); i++)
{
- m_SlamFilters[i]->setTranslationErrorRotating( mPerDegree / 180.0 * M_PI );
+ m_SlamFilters[i]->setTranslationErrorRotating(mPerDegree / 180.0 * M_PI);
}
}
-void HyperSlamFilter::setMoveJitterWhileTurning ( float mPerDegree )
+void HyperSlamFilter::setMoveJitterWhileTurning(float mPerDegree)
{
- for ( unsigned int i=0; i < m_SlamFilters.size(); i++ )
+ for (unsigned int i = 0; i < m_SlamFilters.size(); i++)
{
- m_SlamFilters[i]->setMoveJitterWhileTurning( mPerDegree / 180.0 * M_PI );
+ m_SlamFilters[i]->setMoveJitterWhileTurning(mPerDegree / 180.0 * M_PI);
}
}
-void HyperSlamFilter::setScanMatchingClusterSize ( float minClusterSize )
+void HyperSlamFilter::setScanMatchingClusterSize(float minClusterSize)
{
- for ( unsigned int i=0; i < m_SlamFilters.size(); i++ )
+ for (unsigned int i = 0; i < m_SlamFilters.size(); i++)
{
- m_SlamFilters[i]->setScanMatchingClusterSize( minClusterSize );
+ m_SlamFilters[i]->setScanMatchingClusterSize(minClusterSize);
}
}
void HyperSlamFilter::resetHigh()
{
- for ( unsigned int i=0; i < m_SlamFilters.size(); i++ )
+ for (unsigned int i = 0; i < m_SlamFilters.size(); i++)
{
m_SlamFilters[i]->resetHigh();
}
}
-void HyperSlamFilter::setMapping ( bool doMapping )
+void HyperSlamFilter::setMapping(bool doMapping)
{
- m_DoMapping = doMapping;
+ m_DoMapping = doMapping;
}
-void HyperSlamFilter:: setOccupancyMap ( OccupancyMap* occupancyMap )
+void HyperSlamFilter::setOccupancyMap(OccupancyMap* occupancyMap)
{
- for ( unsigned int i=0; i < m_SlamFilters.size(); i++ )
+ for (unsigned int i = 0; i < m_SlamFilters.size(); i++)
{
- m_SlamFilters[i]->setOccupancyMap( occupancyMap );
+ m_SlamFilters[i]->setOccupancyMap(occupancyMap);
}
}
-void HyperSlamFilter::setRobotPose ( Pose pose, double scatterVarXY, double scatterVarTheta )
+void HyperSlamFilter::setRobotPose(Pose pose, double scatterVarXY,
+ double scatterVarTheta)
{
- for ( unsigned int i=0; i < m_SlamFilters.size(); i++ )
+ for (unsigned int i = 0; i < m_SlamFilters.size(); i++)
{
m_SlamFilters[i]->setRobotPose(pose, scatterVarXY, scatterVarTheta);
}
}
-void HyperSlamFilter::filter ( Pose currentPose, sensor_msgs::LaserScanConstPtr laserData, ros::Time measurementTime, ros::Duration &filterDuration)
+void HyperSlamFilter::filter(Pose currentPose,
+ sensor_msgs::LaserScanConstPtr laserData,
+ ros::Time measurementTime,
+ ros::Duration& filterDuration)
{
- //call filter methods of all particle filters
- for ( unsigned int i=0; i < m_SlamFilters.size(); i++ )
+ // call filter methods of all particle filters
+ for (unsigned int i = 0; i < m_SlamFilters.size(); i++)
{
bool randOnOff;
- if(m_SlamFilters.size() == 1)
- {
- randOnOff = true;
- }
- else
- {
- //if mapping is on, switch on with 80% probability to introduce some randomness in different particle filters
- randOnOff = (rand() % 100) < 80;
- }
- m_SlamFilters[i]->setMapping( m_DoMapping && randOnOff );
- m_SlamFilters[i]->filter(currentPose, laserData, measurementTime, filterDuration);
-
+ if (m_SlamFilters.size() == 1)
+ {
+ randOnOff = true;
+ }
+ else
+ {
+ // if mapping is on, switch on with 80% probability to introduce some
+ // randomness in different particle filters
+ randOnOff = (rand() % 100) < 80;
+ }
+ m_SlamFilters[i]->setMapping(m_DoMapping && randOnOff);
+ m_SlamFilters[i]->filter(currentPose, laserData, measurementTime,
+ filterDuration);
}
- if(m_SlamFilters.size() != 1)
+ if (m_SlamFilters.size() != 1)
{
- //determine which map has the best/worst contrast
- double bestContrast = 0.0;
- static unsigned int bestFilter = 0;
- double worstContrast = 100.0;
- static unsigned int worstFilter = 0;
-
- for ( unsigned int i=0; i < m_SlamFilters.size(); i++ )
- {
- double contrast = m_SlamFilters[i]->evaluateByContrast();
- {
- if( contrast > bestContrast )
- {
- bestContrast = contrast;
- bestFilter = i;
- }
- if ( contrast < worstContrast )
- {
- worstContrast = contrast;
- worstFilter = i;
- }
- }
- }
-
- // set best filter
- SlamFilter* lastBestFilter = m_BestSlamFilter;
- m_BestSlamFilter = m_SlamFilters[bestFilter];
-
- if ( m_BestSlamFilter != lastBestFilter )
- {
- ROS_INFO( "Switched to best filter %d (bestContrast: %f) -- the worst filter is %d (worstContrast: %f)", bestFilter, bestContrast, worstFilter, worstContrast); //TODO
- }
-
- if ( bestFilter != worstFilter )
- {
- if ( worstContrast < ( bestContrast * m_DeletionThreshold ) )
- {
- // replace the worst filter by the one with the best contrast
- delete m_SlamFilters[worstFilter];
- m_SlamFilters[worstFilter] = new SlamFilter ( * m_SlamFilters [bestFilter] );
- }
- }
- }
+ // determine which map has the best/worst contrast
+ double bestContrast = 0.0;
+ static unsigned int bestFilter = 0;
+ double worstContrast = 100.0;
+ static unsigned int worstFilter = 0;
+
+ for (unsigned int i = 0; i < m_SlamFilters.size(); i++)
+ {
+ double contrast = m_SlamFilters[i]->evaluateByContrast();
+ {
+ if (contrast > bestContrast)
+ {
+ bestContrast = contrast;
+ bestFilter = i;
+ }
+ if (contrast < worstContrast)
+ {
+ worstContrast = contrast;
+ worstFilter = i;
+ }
+ }
+ }
+
+ // set best filter
+ SlamFilter* lastBestFilter = m_BestSlamFilter;
+ m_BestSlamFilter = m_SlamFilters[bestFilter];
+
+ if (m_BestSlamFilter != lastBestFilter)
+ {
+ ROS_INFO("Switched to best filter %d (bestContrast: %f) -- the worst "
+ "filter is %d (worstContrast: %f)",
+ bestFilter, bestContrast, worstFilter, worstContrast); // TODO
+ }
+
+ if (bestFilter != worstFilter)
+ {
+ if (worstContrast < (bestContrast * m_DeletionThreshold))
+ {
+ // replace the worst filter by the one with the best contrast
+ delete m_SlamFilters[worstFilter];
+ m_SlamFilters[worstFilter] = new SlamFilter(*m_SlamFilters[bestFilter]);
+ }
+ }
+ }
}
SlamFilter* HyperSlamFilter::getBestSlamFilter()
diff --git a/homer_mapping/src/ParticleFilter/Particle.cpp b/homer_mapping/src/ParticleFilter/Particle.cpp
index 1d64cf733efb608db16e4a43112fcd56f54485e2..d1a6f759cf17f2d626b8a783b3c29a74d114e5f1 100644
--- a/homer_mapping/src/ParticleFilter/Particle.cpp
+++ b/homer_mapping/src/ParticleFilter/Particle.cpp
@@ -1,10 +1,11 @@
#include <homer_mapping/ParticleFilter/Particle.h>
-Particle::Particle(float weight, int id) {
+Particle::Particle(float weight, int id)
+{
m_Weight = weight;
m_Id = id;
}
-Particle::~Particle() {
+Particle::~Particle()
+{
}
-
diff --git a/homer_mapping/src/ParticleFilter/SlamFilter.cpp b/homer_mapping/src/ParticleFilter/SlamFilter.cpp
index 2f6508c36aec1c280dcdd75624d45dc05a0f42fc..60a585a8d7ce21537bf9e5c81f865635016cf16d 100644
--- a/homer_mapping/src/ParticleFilter/SlamFilter.cpp
+++ b/homer_mapping/src/ParticleFilter/SlamFilter.cpp
@@ -9,369 +9,427 @@ const float MIN_TURN_DISTANCE2 = 0.01 * 0.01;
const float M_2PI = 2 * M_PI;
SlamFilter::SlamFilter(int particleNum)
- : ParticleFilter<SlamParticle>(particleNum) {
- m_OccupancyMap = new OccupancyMap();
- // generate initial particles
- for (int i = 0; i < m_ParticleNum; i++) {
- m_CurrentList[i] = new SlamParticle();
- m_LastList[i] = new SlamParticle();
- }
-
- float rotationErrorRotating = 0.0;
- ros::param::get("/particlefilter/error_values/rotation_error_rotating",
- rotationErrorRotating);
- float rotationErrorTranslating = 0.0;
- ros::param::get("/particlefilter/error_values/rotation_error_translating",
- rotationErrorTranslating);
- float translationErrorTranslating = 0.0;
- ros::param::get(
- "/particlefilter/error_values/translation_error_translating",
- translationErrorTranslating);
- float translationErrorRotating = 0.0;
- ros::param::get(
- "/particlefilter/error_values/translation_error_translating",
- translationErrorRotating);
- float moveJitterWhileTurning = 0.0;
- ros::param::get("/particlefilter/error_values/move_jitter_while_turning",
- moveJitterWhileTurning);
- ros::param::get("/particlefilter/max_rotation_per_second",
- m_MaxRotationPerSecond);
-
- int updateMinMoveAngleDegrees;
- ros::param::get("/particlefilter/update_min_move_angle",
- updateMinMoveAngleDegrees);
- m_UpdateMinMoveAngle = Math::deg2Rad(updateMinMoveAngleDegrees);
- ros::param::get("/particlefilter/update_min_move_dist",
- m_UpdateMinMoveDistance);
- double maxUpdateInterval;
- ros::param::get("/particlefilter/max_update_interval", maxUpdateInterval);
- m_MaxUpdateInterval = ros::Duration(maxUpdateInterval);
-
- setRotationErrorRotating(rotationErrorRotating);
- setRotationErrorTranslating(rotationErrorTranslating);
- setTranslationErrorTranslating(translationErrorTranslating);
- setTranslationErrorRotating(translationErrorRotating);
- setMoveJitterWhileTurning(moveJitterWhileTurning);
-
- m_FirstRun = true;
- m_DoMapping = true;
-
- m_EffectiveParticleNum = m_ParticleNum;
- m_LastUpdateTime = ros::Time(0);
- m_LastMoveTime = ros::Time::now();
+ : ParticleFilter<SlamParticle>(particleNum)
+{
+ m_OccupancyMap = new OccupancyMap();
+ // generate initial particles
+ for (int i = 0; i < m_ParticleNum; i++)
+ {
+ m_CurrentList[i] = new SlamParticle();
+ m_LastList[i] = new SlamParticle();
+ }
+
+ float rotationErrorRotating = 0.0;
+ ros::param::get("/particlefilter/error_values/rotation_error_rotating",
+ rotationErrorRotating);
+ float rotationErrorTranslating = 0.0;
+ ros::param::get("/particlefilter/error_values/rotation_error_translating",
+ rotationErrorTranslating);
+ float translationErrorTranslating = 0.0;
+ ros::param::get("/particlefilter/error_values/translation_error_translating",
+ translationErrorTranslating);
+ float translationErrorRotating = 0.0;
+ ros::param::get("/particlefilter/error_values/translation_error_translating",
+ translationErrorRotating);
+ float moveJitterWhileTurning = 0.0;
+ ros::param::get("/particlefilter/error_values/move_jitter_while_turning",
+ moveJitterWhileTurning);
+ ros::param::get("/particlefilter/max_rotation_per_second",
+ m_MaxRotationPerSecond);
+
+ int updateMinMoveAngleDegrees;
+ ros::param::get("/particlefilter/update_min_move_angle",
+ updateMinMoveAngleDegrees);
+ m_UpdateMinMoveAngle = Math::deg2Rad(updateMinMoveAngleDegrees);
+ ros::param::get("/particlefilter/update_min_move_dist",
+ m_UpdateMinMoveDistance);
+ double maxUpdateInterval;
+ ros::param::get("/particlefilter/max_update_interval", maxUpdateInterval);
+ m_MaxUpdateInterval = ros::Duration(maxUpdateInterval);
+
+ setRotationErrorRotating(rotationErrorRotating);
+ setRotationErrorTranslating(rotationErrorTranslating);
+ setTranslationErrorTranslating(translationErrorTranslating);
+ setTranslationErrorRotating(translationErrorRotating);
+ setMoveJitterWhileTurning(moveJitterWhileTurning);
+
+ m_FirstRun = true;
+ m_DoMapping = true;
+
+ m_EffectiveParticleNum = m_ParticleNum;
+ m_LastUpdateTime = ros::Time(0);
+ m_LastMoveTime = ros::Time::now();
}
SlamFilter::SlamFilter(SlamFilter& slamFilter)
- : ParticleFilter<SlamParticle>(slamFilter.m_ParticleNum) {
- m_OccupancyMap = new OccupancyMap(*(slamFilter.m_OccupancyMap));
- // generate initial particles
- for (int i = 0; i < m_ParticleNum; i++) {
- if (slamFilter.m_CurrentList[i] == 0) {
- m_CurrentList[i] = 0;
- } else {
- m_CurrentList[i] = new SlamParticle(*(slamFilter.m_CurrentList[i]));
- }
- if (slamFilter.m_LastList[i] == 0) {
- m_LastList[i] = 0;
- } else {
- m_LastList[i] = new SlamParticle(*(slamFilter.m_LastList[i]));
- }
+ : ParticleFilter<SlamParticle>(slamFilter.m_ParticleNum)
+{
+ m_OccupancyMap = new OccupancyMap(*(slamFilter.m_OccupancyMap));
+ // generate initial particles
+ for (int i = 0; i < m_ParticleNum; i++)
+ {
+ if (slamFilter.m_CurrentList[i] == 0)
+ {
+ m_CurrentList[i] = 0;
}
-
- float rotationErrorRotating = 0.0;
- ros::param::get("/particlefilter/error_values/rotation_error_rotating",
- rotationErrorRotating);
- float rotationErrorTranslating = 0.0;
- ros::param::get("/particlefilter/error_values/rotation_error_translating",
- rotationErrorTranslating);
- float translationErrorTranslating = 0.0;
- ros::param::get(
- "/particlefilter/error_values/translation_error_translating",
- translationErrorTranslating);
- float translationErrorRotating = 0.0;
- ros::param::get(
- "/particlefilter/error_values/translation_error_translating",
- translationErrorRotating);
- float moveJitterWhileTurning = 0.0;
- ros::param::get("/particlefilter/error_values/move_jitter_while_turning",
- moveJitterWhileTurning);
- ros::param::get("/particlefilter/max_rotation_per_second",
- m_MaxRotationPerSecond);
-
- int updateMinMoveAngleDegrees;
- ros::param::get("/particlefilter/update_min_move_angle",
- updateMinMoveAngleDegrees);
- m_UpdateMinMoveAngle = Math::deg2Rad(updateMinMoveAngleDegrees);
- ros::param::get("/particlefilter/update_min_move_dist",
- m_UpdateMinMoveDistance);
- double maxUpdateInterval;
- ros::param::get("/particlefilter/max_update_interval", maxUpdateInterval);
- m_MaxUpdateInterval = ros::Duration(maxUpdateInterval);
-
- setRotationErrorRotating(rotationErrorRotating);
- setRotationErrorTranslating(rotationErrorTranslating);
- setTranslationErrorTranslating(translationErrorTranslating);
- setTranslationErrorRotating(translationErrorRotating);
- setMoveJitterWhileTurning(moveJitterWhileTurning);
-
- m_FirstRun = slamFilter.m_FirstRun;
- m_DoMapping = slamFilter.m_DoMapping;
-
- m_EffectiveParticleNum = slamFilter.m_EffectiveParticleNum;
-
- m_LastUpdateTime = slamFilter.m_LastUpdateTime;
-
- m_ReferencePoseOdometry = slamFilter.m_ReferencePoseOdometry;
- m_ReferenceMeasurementTime = slamFilter.m_ReferenceMeasurementTime;
+ else
+ {
+ m_CurrentList[i] = new SlamParticle(*(slamFilter.m_CurrentList[i]));
+ }
+ if (slamFilter.m_LastList[i] == 0)
+ {
+ m_LastList[i] = 0;
+ }
+ else
+ {
+ m_LastList[i] = new SlamParticle(*(slamFilter.m_LastList[i]));
+ }
+ }
+
+ float rotationErrorRotating = 0.0;
+ ros::param::get("/particlefilter/error_values/rotation_error_rotating",
+ rotationErrorRotating);
+ float rotationErrorTranslating = 0.0;
+ ros::param::get("/particlefilter/error_values/rotation_error_translating",
+ rotationErrorTranslating);
+ float translationErrorTranslating = 0.0;
+ ros::param::get("/particlefilter/error_values/translation_error_translating",
+ translationErrorTranslating);
+ float translationErrorRotating = 0.0;
+ ros::param::get("/particlefilter/error_values/translation_error_translating",
+ translationErrorRotating);
+ float moveJitterWhileTurning = 0.0;
+ ros::param::get("/particlefilter/error_values/move_jitter_while_turning",
+ moveJitterWhileTurning);
+ ros::param::get("/particlefilter/max_rotation_per_second",
+ m_MaxRotationPerSecond);
+
+ int updateMinMoveAngleDegrees;
+ ros::param::get("/particlefilter/update_min_move_angle",
+ updateMinMoveAngleDegrees);
+ m_UpdateMinMoveAngle = Math::deg2Rad(updateMinMoveAngleDegrees);
+ ros::param::get("/particlefilter/update_min_move_dist",
+ m_UpdateMinMoveDistance);
+ double maxUpdateInterval;
+ ros::param::get("/particlefilter/max_update_interval", maxUpdateInterval);
+ m_MaxUpdateInterval = ros::Duration(maxUpdateInterval);
+
+ setRotationErrorRotating(rotationErrorRotating);
+ setRotationErrorTranslating(rotationErrorTranslating);
+ setTranslationErrorTranslating(translationErrorTranslating);
+ setTranslationErrorRotating(translationErrorRotating);
+ setMoveJitterWhileTurning(moveJitterWhileTurning);
+
+ m_FirstRun = slamFilter.m_FirstRun;
+ m_DoMapping = slamFilter.m_DoMapping;
+
+ m_EffectiveParticleNum = slamFilter.m_EffectiveParticleNum;
+
+ m_LastUpdateTime = slamFilter.m_LastUpdateTime;
+
+ m_ReferencePoseOdometry = slamFilter.m_ReferencePoseOdometry;
+ m_ReferenceMeasurementTime = slamFilter.m_ReferenceMeasurementTime;
}
-SlamFilter::~SlamFilter() {
- if (m_OccupancyMap) {
- delete m_OccupancyMap;
+SlamFilter::~SlamFilter()
+{
+ if (m_OccupancyMap)
+ {
+ delete m_OccupancyMap;
+ }
+ for (int i = 0; i < m_ParticleNum; i++)
+ {
+ if (m_CurrentList[i])
+ {
+ delete m_CurrentList[i];
+ m_CurrentList[i] = 0;
}
- for (int i = 0; i < m_ParticleNum; i++) {
- if (m_CurrentList[i]) {
- delete m_CurrentList[i];
- m_CurrentList[i] = 0;
- }
- if (m_LastList[i]) {
- delete m_LastList[i];
- m_LastList[i] = 0;
- }
+ if (m_LastList[i])
+ {
+ delete m_LastList[i];
+ m_LastList[i] = 0;
}
+ }
}
-void SlamFilter::setRotationErrorRotating(float percent) {
- m_Alpha1 = percent / 100.0;
+void SlamFilter::setRotationErrorRotating(float percent)
+{
+ m_Alpha1 = percent / 100.0;
}
-void SlamFilter::resetHigh() { m_OccupancyMap->resetHighSensitive(); }
+void SlamFilter::resetHigh()
+{
+ m_OccupancyMap->resetHighSensitive();
+}
-void SlamFilter::setRotationErrorTranslating(float degreePerMeter) {
- m_Alpha2 = degreePerMeter / 180.0 * M_PI;
+void SlamFilter::setRotationErrorTranslating(float degreePerMeter)
+{
+ m_Alpha2 = degreePerMeter / 180.0 * M_PI;
}
-void SlamFilter::setTranslationErrorTranslating(float percent) {
- m_Alpha3 = percent / 100.0;
+void SlamFilter::setTranslationErrorTranslating(float percent)
+{
+ m_Alpha3 = percent / 100.0;
}
-void SlamFilter::setTranslationErrorRotating(float mPerDegree) {
- m_Alpha4 = mPerDegree / 180.0 * M_PI;
+void SlamFilter::setTranslationErrorRotating(float mPerDegree)
+{
+ m_Alpha4 = mPerDegree / 180.0 * M_PI;
}
-void SlamFilter::setMoveJitterWhileTurning(float mPerDegree) {
- m_Alpha5 = mPerDegree / 180.0 * M_PI;
+void SlamFilter::setMoveJitterWhileTurning(float mPerDegree)
+{
+ m_Alpha5 = mPerDegree / 180.0 * M_PI;
}
-void SlamFilter::setScanMatchingClusterSize(float minClusterSize) {
- minClusterSize = minClusterSize;
+void SlamFilter::setScanMatchingClusterSize(float minClusterSize)
+{
+ minClusterSize = minClusterSize;
}
-void SlamFilter::setMapping(bool doMapping) { m_DoMapping = doMapping; }
+void SlamFilter::setMapping(bool doMapping)
+{
+ m_DoMapping = doMapping;
+}
-void SlamFilter::setOccupancyMap(OccupancyMap* occupancyMap) {
- // delete old
- if (m_OccupancyMap) {
- delete m_OccupancyMap;
- }
- // copy
- m_OccupancyMap = occupancyMap;
+void SlamFilter::setOccupancyMap(OccupancyMap* occupancyMap)
+{
+ // delete old
+ if (m_OccupancyMap)
+ {
+ delete m_OccupancyMap;
+ }
+ // copy
+ m_OccupancyMap = occupancyMap;
}
-vector<Pose>* SlamFilter::getParticlePoses() const {
- vector<Pose>* particlePoses = new vector<Pose>();
- for (int i = 0; i < m_ParticleNum; i++) {
- float robotX, robotY, robotTheta;
- SlamParticle* particle = m_CurrentList[i];
- particle->getRobotPose(robotX, robotY, robotTheta);
- particlePoses->push_back(Pose(robotX, robotY, robotTheta));
- }
- return particlePoses;
+vector<Pose>* SlamFilter::getParticlePoses() const
+{
+ vector<Pose>* particlePoses = new vector<Pose>();
+ for (int i = 0; i < m_ParticleNum; i++)
+ {
+ float robotX, robotY, robotTheta;
+ SlamParticle* particle = m_CurrentList[i];
+ particle->getRobotPose(robotX, robotY, robotTheta);
+ particlePoses->push_back(Pose(robotX, robotY, robotTheta));
+ }
+ return particlePoses;
}
-vector<SlamParticle*>* SlamFilter::getParticles() const {
- vector<SlamParticle*>* particles = new vector<SlamParticle*>();
- for (int i = 0; i < m_ParticleNum; i++) {
- SlamParticle* particle = m_CurrentList[i];
- particles->push_back(particle);
- }
- return particles;
+vector<SlamParticle*>* SlamFilter::getParticles() const
+{
+ vector<SlamParticle*>* particles = new vector<SlamParticle*>();
+ for (int i = 0; i < m_ParticleNum; i++)
+ {
+ SlamParticle* particle = m_CurrentList[i];
+ particles->push_back(particle);
+ }
+ return particles;
}
void SlamFilter::setRobotPose(Pose pose, double scatterVarXY,
- double scatterVarTheta) {
- // set first particle to exact position
- m_CurrentList[0]->setRobotPose(pose.x(), pose.y(), pose.theta());
- m_LastList[0]->setRobotPose(pose.x(), pose.y(), pose.theta());
- // scatter remaining particles
- for (int i = 1; i < m_ParticleNum; ++i) {
- const double scatterX = randomGauss() * scatterVarXY;
- const double scatterY = randomGauss() * scatterVarXY;
- const double scatterTheta = randomGauss() * scatterVarTheta;
-
- m_CurrentList[i]->setRobotPose(pose.x() + scatterX, pose.y() + scatterY,
- pose.theta() + scatterTheta);
- m_LastList[i]->setRobotPose(pose.x() + scatterX, pose.y() + scatterY,
- pose.theta() + scatterTheta);
- }
+ double scatterVarTheta)
+{
+ // set first particle to exact position
+ m_CurrentList[0]->setRobotPose(pose.x(), pose.y(), pose.theta());
+ m_LastList[0]->setRobotPose(pose.x(), pose.y(), pose.theta());
+ // scatter remaining particles
+ for (int i = 1; i < m_ParticleNum; ++i)
+ {
+ const double scatterX = randomGauss() * scatterVarXY;
+ const double scatterY = randomGauss() * scatterVarXY;
+ const double scatterTheta = randomGauss() * scatterVarTheta;
+
+ m_CurrentList[i]->setRobotPose(pose.x() + scatterX, pose.y() + scatterY,
+ pose.theta() + scatterTheta);
+ m_LastList[i]->setRobotPose(pose.x() + scatterX, pose.y() + scatterY,
+ pose.theta() + scatterTheta);
+ }
}
-vector<float> SlamFilter::getParticleWeights() const {
- vector<float> particleWeights(m_ParticleNum);
- for (int i = 0; i < m_ParticleNum; i++) {
- particleWeights[i] = m_CurrentList[i]->getWeight();
- }
- return particleWeights;
+vector<float> SlamFilter::getParticleWeights() const
+{
+ vector<float> particleWeights(m_ParticleNum);
+ for (int i = 0; i < m_ParticleNum; i++)
+ {
+ particleWeights[i] = m_CurrentList[i]->getWeight();
+ }
+ return particleWeights;
}
-double SlamFilter::randomGauss(float variance) const {
- if (variance < 0) {
- variance = -variance;
- }
- double x1, x2, w, y1;
- do {
- x1 = 2.0 * random01() - 1.0;
- x2 = 2.0 * random01() - 1.0;
- w = x1 * x1 + x2 * x2;
- } while (w >= 1.0);
-
- w = sqrt((-2.0 * log(w)) / w);
- y1 = x1 * w;
- // now y1 is uniformly distributed
- return sqrt(variance) * y1;
+double SlamFilter::randomGauss(float variance) const
+{
+ if (variance < 0)
+ {
+ variance = -variance;
+ }
+ double x1, x2, w, y1;
+ do
+ {
+ x1 = 2.0 * random01() - 1.0;
+ x2 = 2.0 * random01() - 1.0;
+ w = x1 * x1 + x2 * x2;
+ } while (w >= 1.0);
+
+ w = sqrt((-2.0 * log(w)) / w);
+ y1 = x1 * w;
+ // now y1 is uniformly distributed
+ return sqrt(variance) * y1;
}
vector<float> SlamFilter::filterOutliers(sensor_msgs::LaserScanConstPtr rawData,
- float maxDiff) {
- if (rawData->ranges.size() < 2) {
- return rawData->ranges;
- }
- vector<float> filteredData = rawData->ranges;
- for (unsigned int i = 1; i < filteredData.size() - 1; i++) {
- if (abs((float)(rawData->ranges[i - 1] - rawData->ranges[i] * 2 +
- rawData->ranges[i + 1])) > maxDiff * 2) {
- filteredData[i] = 0;
- }
- }
- if (fabs(rawData->ranges[0] - rawData->ranges[1]) > maxDiff) {
- filteredData[0] = 0;
- }
- if (fabs(rawData->ranges[rawData->ranges.size() - 1] -
- rawData->ranges[rawData->ranges.size() - 2]) > maxDiff) {
- filteredData[rawData->ranges.size() - 1] = 0;
+ float maxDiff)
+{
+ if (rawData->ranges.size() < 2)
+ {
+ return rawData->ranges;
+ }
+ vector<float> filteredData = rawData->ranges;
+ for (unsigned int i = 1; i < filteredData.size() - 1; i++)
+ {
+ if (abs((float)(rawData->ranges[i - 1] - rawData->ranges[i] * 2 +
+ rawData->ranges[i + 1])) > maxDiff * 2)
+ {
+ filteredData[i] = 0;
}
-
- return filteredData;
+ }
+ if (fabs(rawData->ranges[0] - rawData->ranges[1]) > maxDiff)
+ {
+ filteredData[0] = 0;
+ }
+ if (fabs(rawData->ranges[rawData->ranges.size() - 1] -
+ rawData->ranges[rawData->ranges.size() - 2]) > maxDiff)
+ {
+ filteredData[rawData->ranges.size() - 1] = 0;
+ }
+
+ return filteredData;
}
void SlamFilter::filter(Pose currentPose,
sensor_msgs::LaserScanConstPtr laserData,
ros::Time measurementTime,
- ros::Duration& FilterDuration) {
- // if first run, initialize data
- if (m_FirstRun) {
- m_FirstRun = false;
- // only do mapping, save first pose as reference
- if (m_DoMapping) {
- tf::Transform transform(tf::createQuaternionFromYaw(0.0),
- tf::Vector3(0.0, 0.0, 0.0));
- m_OccupancyMap->insertLaserData(laserData, transform);
- }
- m_CurrentLaserData = boost::make_shared<sensor_msgs::LaserScan>(
- *laserData); // copy const ptr to be able to change values; //test
- m_ReferencePoseOdometry = currentPose;
- m_ReferenceMeasurementTime = measurementTime;
-
- measure();
- ROS_INFO_STREAM("first run!");
- normalize();
- sort(0, m_ParticleNum - 1);
- return;
+ ros::Duration& FilterDuration)
+{
+ // if first run, initialize data
+ if (m_FirstRun)
+ {
+ m_FirstRun = false;
+ // only do mapping, save first pose as reference
+ if (m_DoMapping)
+ {
+ tf::Transform transform(tf::createQuaternionFromYaw(0.0),
+ tf::Vector3(0.0, 0.0, 0.0));
+ m_OccupancyMap->insertLaserData(laserData, transform);
}
- // m_CurrentLaserConfig = laserConf;
- m_CurrentPoseOdometry = currentPose;
m_CurrentLaserData = boost::make_shared<sensor_msgs::LaserScan>(
- *laserData); // copy const ptr to be able to change values
- m_CurrentLaserData->ranges = filterOutliers(laserData, 0.3);
-
- Transformation2D trans = m_CurrentPoseOdometry - m_ReferencePoseOdometry;
-
- bool moving = sqr(trans.x()) + sqr(trans.y()) > 0 || sqr(trans.theta()) > 0;
+ *laserData); // copy const ptr to be able to change values; //test
+ m_ReferencePoseOdometry = currentPose;
+ m_ReferenceMeasurementTime = measurementTime;
- // do not resample if move to small and last move is min 0.5 sec away
- // if (sqr(trans.x()) + sqr(trans.y()) < MIN_MOVE_DISTANCE2 &&
- // sqr(trans.theta()) < MIN_TURN_DISTANCE2 &&
- //(ros::Time::now() - m_LastMoveTime).toSec() > 1.0)
- if (!moving && (ros::Time::now() - m_LastMoveTime).toSec() > 1.0)
- // if(false)
+ measure();
+ ROS_INFO_STREAM("first run!");
+ normalize();
+ sort(0, m_ParticleNum - 1);
+ return;
+ }
+ // m_CurrentLaserConfig = laserConf;
+ m_CurrentPoseOdometry = currentPose;
+ m_CurrentLaserData = boost::make_shared<sensor_msgs::LaserScan>(
+ *laserData); // copy const ptr to be able to change values
+ m_CurrentLaserData->ranges = filterOutliers(laserData, 0.3);
+
+ Transformation2D trans = m_CurrentPoseOdometry - m_ReferencePoseOdometry;
+
+ bool moving = sqr(trans.x()) + sqr(trans.y()) > 0 || sqr(trans.theta()) > 0;
+
+ // do not resample if move to small and last move is min 0.5 sec away
+ // if (sqr(trans.x()) + sqr(trans.y()) < MIN_MOVE_DISTANCE2 &&
+ // sqr(trans.theta()) < MIN_TURN_DISTANCE2 &&
+ //(ros::Time::now() - m_LastMoveTime).toSec() > 1.0)
+ if (!moving && (ros::Time::now() - m_LastMoveTime).toSec() > 1.0)
+ // if(false)
+ {
+ ROS_DEBUG_STREAM("Move too small, will not resample.");
+ if (m_EffectiveParticleNum < m_ParticleNum / 10)
{
- ROS_DEBUG_STREAM("Move too small, will not resample.");
- if (m_EffectiveParticleNum < m_ParticleNum / 10) {
- resample();
- ROS_INFO_STREAM("Particles too scattered, resampling.");
- // filter steps
- drift();
- measure();
- normalize();
-
- sort(0, m_ParticleNum - 1);
- }
- } else {
- if (moving) {
- m_LastMoveTime = ros::Time::now();
- }
- resample();
- // filter steps
- drift();
- measure();
- normalize();
-
- sort(0, m_ParticleNum - 1);
+ resample();
+ ROS_INFO_STREAM("Particles too scattered, resampling.");
+ // filter steps
+ drift();
+ measure();
+ normalize();
+
+ sort(0, m_ParticleNum - 1);
}
-
- Pose likeliestPose = getLikeliestPose(measurementTime); // test
- Transformation2D transSinceLastUpdate = likeliestPose - m_LastUpdatePose;
-
- ostringstream stream;
- stream.precision(2);
- stream << "Transformation since last update: angle="
- << Math::rad2Deg(transSinceLastUpdate.theta())
- << " dist=" << transSinceLastUpdate.magnitude() << "m" << endl;
-
- bool update =
- (std::fabs(transSinceLastUpdate.theta()) > m_UpdateMinMoveAngle) ||
- (transSinceLastUpdate.magnitude() > m_UpdateMinMoveDistance) ||
- ((measurementTime - m_LastUpdateTime) > m_MaxUpdateInterval);
-
- if (m_DoMapping && update) {
- stream << "Updating map.";
- double elapsedSeconds =
- (measurementTime - m_ReferenceMeasurementTime).toSec();
- double thetaPerSecond;
- if (elapsedSeconds == 0.0) {
- thetaPerSecond = trans.theta();
- } else {
- thetaPerSecond = trans.theta() / elapsedSeconds;
- }
- float poseVarianceX, poseVarianceY;
- getPoseVariances(50, poseVarianceX, poseVarianceY);
-
- if (std::fabs(thetaPerSecond) < m_MaxRotationPerSecond &&
- poseVarianceX < 0.05 && poseVarianceY < 0.05) {
- updateMap();
- m_LastUpdatePose = likeliestPose;
- m_LastUpdateTime = measurementTime;
- } else {
- ROS_WARN_STREAM("No mapping performed - variance to high");
- }
- } else {
- stream << "No map update performed.";
+ }
+ else
+ {
+ if (moving)
+ {
+ m_LastMoveTime = ros::Time::now();
}
- ROS_DEBUG_STREAM(stream.str());
- // safe last used pose and laserdata as reference
+ resample();
+ // filter steps
+ drift();
+ measure();
+ normalize();
+
+ sort(0, m_ParticleNum - 1);
+ }
+
+ Pose likeliestPose = getLikeliestPose(measurementTime); // test
+ Transformation2D transSinceLastUpdate = likeliestPose - m_LastUpdatePose;
+
+ ostringstream stream;
+ stream.precision(2);
+ stream << "Transformation since last update: angle="
+ << Math::rad2Deg(transSinceLastUpdate.theta())
+ << " dist=" << transSinceLastUpdate.magnitude() << "m" << endl;
+
+ bool update =
+ (std::fabs(transSinceLastUpdate.theta()) > m_UpdateMinMoveAngle) ||
+ (transSinceLastUpdate.magnitude() > m_UpdateMinMoveDistance) ||
+ ((measurementTime - m_LastUpdateTime) > m_MaxUpdateInterval);
+
+ if (m_DoMapping && update)
+ {
+ stream << "Updating map.";
+ double elapsedSeconds =
+ (measurementTime - m_ReferenceMeasurementTime).toSec();
+ double thetaPerSecond;
+ if (elapsedSeconds == 0.0)
+ {
+ thetaPerSecond = trans.theta();
+ }
+ else
+ {
+ thetaPerSecond = trans.theta() / elapsedSeconds;
+ }
+ float poseVarianceX, poseVarianceY;
+ getPoseVariances(50, poseVarianceX, poseVarianceY);
- m_ReferencePoseOdometry = m_CurrentPoseOdometry;
- m_ReferenceMeasurementTime = measurementTime;
+ if (std::fabs(thetaPerSecond) < m_MaxRotationPerSecond &&
+ poseVarianceX < 0.05 && poseVarianceY < 0.05)
+ {
+ updateMap();
+ m_LastUpdatePose = likeliestPose;
+ m_LastUpdateTime = measurementTime;
+ }
+ else
+ {
+ ROS_WARN_STREAM("No mapping performed - variance to high");
+ }
+ }
+ else
+ {
+ stream << "No map update performed.";
+ }
+ ROS_DEBUG_STREAM(stream.str());
+ // safe last used pose and laserdata as reference
+
+ m_ReferencePoseOdometry = m_CurrentPoseOdometry;
+ m_ReferenceMeasurementTime = measurementTime;
}
/**
@@ -390,238 +448,275 @@ void SlamFilter::filter(Pose currentPose,
* The expected value of the errors are zero.
*/
-void SlamFilter::drift() {
- float rx = m_ReferencePoseOdometry.x();
- float ry = m_ReferencePoseOdometry.y();
- float rt = m_ReferencePoseOdometry.theta();
- float cx = m_CurrentPoseOdometry.x();
- float cy = m_CurrentPoseOdometry.y();
- float ct = m_CurrentPoseOdometry.theta();
-
- Transformation2D odoTrans = m_CurrentPoseOdometry - m_ReferencePoseOdometry;
-
- // find out if driving forward or backward
- bool backwardMove = false;
- float scalar = odoTrans.x() * cosf(rt) + odoTrans.y() * sinf(rt);
- if (scalar <= 0) {
- backwardMove = true;
- }
- float distance = sqrt(sqr(odoTrans.x()) + sqr(odoTrans.y()));
- float deltaRot1, deltaTrans, deltaRot2;
- if (distance < sqrt(MIN_MOVE_DISTANCE2)) {
- deltaRot1 = odoTrans.theta();
- deltaTrans = 0.0;
- deltaRot2 = 0.0;
- } else if (backwardMove) {
- deltaRot1 = atan2(ry - cy, rx - cx) - rt;
- deltaTrans = -distance;
- deltaRot2 = ct - rt - deltaRot1;
- } else {
- deltaRot1 = atan2(odoTrans.y(), odoTrans.x()) - rt;
- deltaTrans = distance;
- deltaRot2 = ct - rt - deltaRot1;
- }
-
- while (deltaRot1 >= M_PI) deltaRot1 -= M_2PI;
- while (deltaRot1 < -M_PI) deltaRot1 += M_2PI;
- while (deltaRot2 >= M_PI) deltaRot2 -= M_2PI;
- while (deltaRot2 < -M_PI) deltaRot2 += M_2PI;
-
- // always leave one particle with pure displacement
- SlamParticle* particle = m_CurrentList[0];
+void SlamFilter::drift()
+{
+ float rx = m_ReferencePoseOdometry.x();
+ float ry = m_ReferencePoseOdometry.y();
+ float rt = m_ReferencePoseOdometry.theta();
+ float cx = m_CurrentPoseOdometry.x();
+ float cy = m_CurrentPoseOdometry.y();
+ float ct = m_CurrentPoseOdometry.theta();
+
+ Transformation2D odoTrans = m_CurrentPoseOdometry - m_ReferencePoseOdometry;
+
+ // find out if driving forward or backward
+ bool backwardMove = false;
+ float scalar = odoTrans.x() * cosf(rt) + odoTrans.y() * sinf(rt);
+ if (scalar <= 0)
+ {
+ backwardMove = true;
+ }
+ float distance = sqrt(sqr(odoTrans.x()) + sqr(odoTrans.y()));
+ float deltaRot1, deltaTrans, deltaRot2;
+ if (distance < sqrt(MIN_MOVE_DISTANCE2))
+ {
+ deltaRot1 = odoTrans.theta();
+ deltaTrans = 0.0;
+ deltaRot2 = 0.0;
+ }
+ else if (backwardMove)
+ {
+ deltaRot1 = atan2(ry - cy, rx - cx) - rt;
+ deltaTrans = -distance;
+ deltaRot2 = ct - rt - deltaRot1;
+ }
+ else
+ {
+ deltaRot1 = atan2(odoTrans.y(), odoTrans.x()) - rt;
+ deltaTrans = distance;
+ deltaRot2 = ct - rt - deltaRot1;
+ }
+
+ while (deltaRot1 >= M_PI)
+ deltaRot1 -= M_2PI;
+ while (deltaRot1 < -M_PI)
+ deltaRot1 += M_2PI;
+ while (deltaRot2 >= M_PI)
+ deltaRot2 -= M_2PI;
+ while (deltaRot2 < -M_PI)
+ deltaRot2 += M_2PI;
+
+ // always leave one particle with pure displacement
+ SlamParticle* particle = m_CurrentList[0];
+ // get stored pose
+ float robotX, robotY, robotTheta;
+ particle->getRobotPose(robotX, robotY, robotTheta);
+ Pose pose(robotX, robotY, robotTheta);
+ // move pose
+ float posX = pose.x() + deltaTrans * cos(pose.theta() + deltaRot1);
+ float posY = pose.y() + deltaTrans * sin(pose.theta() + deltaRot1);
+ float theta = pose.theta() + deltaRot1 + deltaRot2;
+ while (theta > M_PI)
+ theta -= M_2PI;
+ while (theta <= -M_PI)
+ theta += M_2PI;
+ // save new pose
+ particle->setRobotPose(posX, posY, theta);
+ int i = 1;
+
+ // calculating parallel on 8 threats
+ // TODO: ERROR ON RESET MAPS
+ // omp_set_num_threads(4);
+ // #pragma omp parallel for
+ for (i = 1; i < m_ParticleNum; i++)
+ {
+ SlamParticle* particle = m_CurrentList[i];
// get stored pose
float robotX, robotY, robotTheta;
particle->getRobotPose(robotX, robotY, robotTheta);
Pose pose(robotX, robotY, robotTheta);
// move pose
- float posX = pose.x() + deltaTrans * cos(pose.theta() + deltaRot1);
- float posY = pose.y() + deltaTrans * sin(pose.theta() + deltaRot1);
- float theta = pose.theta() + deltaRot1 + deltaRot2;
- while (theta > M_PI) theta -= M_2PI;
- while (theta <= -M_PI) theta += M_2PI;
+ float estDeltaRot1 = deltaRot1 - randomGauss(m_Alpha1 * fabs(deltaRot1) +
+ m_Alpha2 * deltaTrans);
+ float estDeltaTrans =
+ deltaTrans -
+ randomGauss(m_Alpha3 * deltaTrans +
+ m_Alpha4 * (fabs(deltaRot1) + fabs(deltaRot2)));
+ float estDeltaRot2 = deltaRot2 - randomGauss(m_Alpha1 * fabs(deltaRot2) +
+ m_Alpha2 * deltaTrans);
+
+ float posX = pose.x() + estDeltaTrans * cos(pose.theta() + estDeltaRot1) +
+ randomGauss(m_Alpha5 * fabs(estDeltaRot1 + estDeltaRot2));
+ float posY = pose.y() + estDeltaTrans * sin(pose.theta() + estDeltaRot1) +
+ randomGauss(m_Alpha5 * fabs(estDeltaRot1 + estDeltaRot2));
+ float theta = pose.theta() + estDeltaRot1 + estDeltaRot2;
+
// save new pose
+ while (theta > M_PI)
+ theta -= M_2PI;
+ while (theta <= -M_PI)
+ theta += M_2PI;
+
particle->setRobotPose(posX, posY, theta);
- int i = 1;
+ }
+}
+
+void SlamFilter::measure()
+{
+ if (m_OccupancyMap)
+ {
+ m_MeasurePoints = m_OccupancyMap->getMeasurePoints(m_CurrentLaserData);
// calculating parallel on 8 threats
- // TODO: ERROR ON RESET MAPS
- // omp_set_num_threads(4);
- // #pragma omp parallel for
- for (i = 1; i < m_ParticleNum; i++) {
- SlamParticle* particle = m_CurrentList[i];
- // get stored pose
+ omp_set_num_threads(8);
+ int i = 0;
+ //#pragma omp parallel for
+ for (i = 0; i < m_ParticleNum; i++)
+ {
+ SlamParticle* particle = m_CurrentList[i];
+ if (!particle)
+ {
+ ROS_ERROR_STREAM("ERROR: Particle is NULL-pointer!");
+ }
+ else
+ {
+ // calculate weights
float robotX, robotY, robotTheta;
particle->getRobotPose(robotX, robotY, robotTheta);
Pose pose(robotX, robotY, robotTheta);
- // move pose
- float estDeltaRot1 =
- deltaRot1 -
- randomGauss(m_Alpha1 * fabs(deltaRot1) + m_Alpha2 * deltaTrans);
- float estDeltaTrans =
- deltaTrans -
- randomGauss(m_Alpha3 * deltaTrans +
- m_Alpha4 * (fabs(deltaRot1) + fabs(deltaRot2)));
- float estDeltaRot2 =
- deltaRot2 -
- randomGauss(m_Alpha1 * fabs(deltaRot2) + m_Alpha2 * deltaTrans);
-
- float posX = pose.x() +
- estDeltaTrans * cos(pose.theta() + estDeltaRot1) +
- randomGauss(m_Alpha5 * fabs(estDeltaRot1 + estDeltaRot2));
- float posY = pose.y() +
- estDeltaTrans * sin(pose.theta() + estDeltaRot1) +
- randomGauss(m_Alpha5 * fabs(estDeltaRot1 + estDeltaRot2));
- float theta = pose.theta() + estDeltaRot1 + estDeltaRot2;
-
- // save new pose
- while (theta > M_PI) theta -= M_2PI;
- while (theta <= -M_PI) theta += M_2PI;
-
- particle->setRobotPose(posX, posY, theta);
+ float weight = m_OccupancyMap->computeScore(pose, m_MeasurePoints);
+ particle->setWeight(weight);
+ }
}
+ }
+ m_EffectiveParticleNum = getEffectiveParticleNum2();
}
-void SlamFilter::measure() {
- if (m_OccupancyMap) {
- m_MeasurePoints = m_OccupancyMap->getMeasurePoints(m_CurrentLaserData);
-
- // calculating parallel on 8 threats
- omp_set_num_threads(8);
- int i = 0;
- //#pragma omp parallel for
- for (i = 0; i < m_ParticleNum; i++) {
- SlamParticle* particle = m_CurrentList[i];
- if (!particle) {
- ROS_ERROR_STREAM("ERROR: Particle is NULL-pointer!");
- } else {
- // calculate weights
- float robotX, robotY, robotTheta;
- particle->getRobotPose(robotX, robotY, robotTheta);
- Pose pose(robotX, robotY, robotTheta);
- float weight =
- m_OccupancyMap->computeScore(pose, m_MeasurePoints);
- particle->setWeight(weight);
- }
- }
- }
- m_EffectiveParticleNum = getEffectiveParticleNum2();
-}
-
-void SlamFilter::updateMap() {
- m_OccupancyMap->insertLaserData(m_CurrentLaserData, m_latestTransform);
+void SlamFilter::updateMap()
+{
+ m_OccupancyMap->insertLaserData(m_CurrentLaserData, m_latestTransform);
}
-void SlamFilter::printParticles() const {
- cout << endl << "### PARTICLE LIST ###" << endl;
- cout << right << fixed;
- cout.width(5);
- for (int i = 0; i < m_ParticleNum; i++) {
- SlamParticle* p_particle = m_CurrentList[i];
- if (p_particle) {
- float robotX, robotY, robotTheta;
- p_particle->getRobotPose(robotX, robotY, robotTheta);
- cout << "Particle " << i << ": (" << robotX << "," << robotY << ","
- << robotTheta * 180.0 / M_PI << "), weight:\t"
- << p_particle->getWeight() << endl;
- }
+void SlamFilter::printParticles() const
+{
+ cout << endl << "### PARTICLE LIST ###" << endl;
+ cout << right << fixed;
+ cout.width(5);
+ for (int i = 0; i < m_ParticleNum; i++)
+ {
+ SlamParticle* p_particle = m_CurrentList[i];
+ if (p_particle)
+ {
+ float robotX, robotY, robotTheta;
+ p_particle->getRobotPose(robotX, robotY, robotTheta);
+ cout << "Particle " << i << ": (" << robotX << "," << robotY << ","
+ << robotTheta * 180.0 / M_PI << "), weight:\t"
+ << p_particle->getWeight() << endl;
}
- cout << "### END OF LIST ###" << endl;
+ }
+ cout << "### END OF LIST ###" << endl;
}
-void SlamFilter::reduceParticleNumber(int newParticleNum) {
- if (newParticleNum < m_ParticleNum) {
- SlamParticle** newCurrentList = new SlamParticle*[newParticleNum];
- SlamParticle** newLastList = new SlamParticle*[newParticleNum];
+void SlamFilter::reduceParticleNumber(int newParticleNum)
+{
+ if (newParticleNum < m_ParticleNum)
+ {
+ SlamParticle** newCurrentList = new SlamParticle*[newParticleNum];
+ SlamParticle** newLastList = new SlamParticle*[newParticleNum];
- for (int i = 0; i < newParticleNum; i++) {
- newCurrentList[i] = m_CurrentList[i];
- newLastList[i] = m_LastList[i];
- }
+ for (int i = 0; i < newParticleNum; i++)
+ {
+ newCurrentList[i] = m_CurrentList[i];
+ newLastList[i] = m_LastList[i];
+ }
- for (int i = newParticleNum + 1; i < m_ParticleNum; i++) {
- delete m_CurrentList[i];
- delete m_LastList[i];
- }
- delete[] m_CurrentList;
- delete[] m_LastList;
+ for (int i = newParticleNum + 1; i < m_ParticleNum; i++)
+ {
+ delete m_CurrentList[i];
+ delete m_LastList[i];
+ }
+ delete[] m_CurrentList;
+ delete[] m_LastList;
- m_CurrentList = newCurrentList;
- m_LastList = newLastList;
+ m_CurrentList = newCurrentList;
+ m_LastList = newLastList;
- m_ParticleNum = newParticleNum;
- normalize();
- }
+ m_ParticleNum = newParticleNum;
+ normalize();
+ }
}
-Pose SlamFilter::getLikeliestPose(ros::Time poseTime) {
- float percentage = 0.4; // TODO param? //test
- float sumX = 0, sumY = 0, sumDirX = 0, sumDirY = 0;
- int numParticles = static_cast<int>(percentage / 100 * m_ParticleNum);
- if (0 == numParticles) {
- numParticles = 1;
- }
- for (int i = 0; i < numParticles; i++) {
- float robotX, robotY, robotTheta;
- m_CurrentList[i]->getRobotPose(robotX, robotY, robotTheta);
- sumX += robotX;
- sumY += robotY;
- // calculate sum of vectors in unit circle
- sumDirX += cos(robotTheta);
- sumDirY += sin(robotTheta);
- }
- float meanTheta = atan2(sumDirY, sumDirX);
- float meanX = sumX / numParticles;
- float meanY = sumY / numParticles;
- // broadcast transform map -> base_link
- tf::Transform transform(
- tf::createQuaternionFromYaw(meanTheta),
- tf::Vector3(sumX / numParticles, sumY / numParticles, 0.0));
- tf::TransformBroadcaster tfBroadcaster;
- m_latestTransform = transform;
-
- tfBroadcaster.sendTransform(tf::StampedTransform(
- m_latestTransform, poseTime, "/map", "/base_link"));
- return Pose(meanX, meanY, meanTheta);
+Pose SlamFilter::getLikeliestPose(ros::Time poseTime)
+{
+ float percentage = 0.4; // TODO param? //test
+ float sumX = 0, sumY = 0, sumDirX = 0, sumDirY = 0;
+ int numParticles = static_cast<int>(percentage / 100 * m_ParticleNum);
+ if (0 == numParticles)
+ {
+ numParticles = 1;
+ }
+ for (int i = 0; i < numParticles; i++)
+ {
+ float robotX, robotY, robotTheta;
+ m_CurrentList[i]->getRobotPose(robotX, robotY, robotTheta);
+ sumX += robotX;
+ sumY += robotY;
+ // calculate sum of vectors in unit circle
+ sumDirX += cos(robotTheta);
+ sumDirY += sin(robotTheta);
+ }
+ float meanTheta = atan2(sumDirY, sumDirX);
+ float meanX = sumX / numParticles;
+ float meanY = sumY / numParticles;
+ // broadcast transform map -> base_link
+ tf::Transform transform(
+ tf::createQuaternionFromYaw(meanTheta),
+ tf::Vector3(sumX / numParticles, sumY / numParticles, 0.0));
+ tf::TransformBroadcaster tfBroadcaster;
+ m_latestTransform = transform;
+
+ tfBroadcaster.sendTransform(
+ tf::StampedTransform(m_latestTransform, poseTime, "/map", "/base_link"));
+ return Pose(meanX, meanY, meanTheta);
}
-OccupancyMap* SlamFilter::getLikeliestMap() const { return m_OccupancyMap; }
+OccupancyMap* SlamFilter::getLikeliestMap() const
+{
+ return m_OccupancyMap;
+}
void SlamFilter::getPoseVariances(int particleNum, float& poseVarianceX,
- float& poseVarianceY) {
- // the particles of m_CurrentList are sorted by their weights
- if (particleNum > m_ParticleNum || particleNum <= 0) {
- particleNum = m_ParticleNum;
- }
- // calculate average pose
- float averagePoseX = 0;
- float averagePoseY = 0;
- float robotX = 0.0;
- float robotY = 0.0;
- float robotTheta = 0.0;
- for (int i = 0; i < particleNum; i++) {
- m_CurrentList[i]->getRobotPose(robotX, robotY, robotTheta);
- averagePoseX += robotX;
- averagePoseY += robotY;
- }
- averagePoseX /= particleNum;
- averagePoseY /= particleNum;
-
- // calculate standard deviation of pose
- poseVarianceX = 0.0;
- poseVarianceY = 0.0;
- for (int i = 0; i < particleNum; i++) {
- m_CurrentList[i]->getRobotPose(robotX, robotY, robotTheta);
- poseVarianceX += (averagePoseX - robotX) * (averagePoseX - robotX);
- poseVarianceY += (averagePoseY - robotY) * (averagePoseY - robotY);
- }
- poseVarianceX /= particleNum;
- poseVarianceY /= particleNum;
+ float& poseVarianceY)
+{
+ // the particles of m_CurrentList are sorted by their weights
+ if (particleNum > m_ParticleNum || particleNum <= 0)
+ {
+ particleNum = m_ParticleNum;
+ }
+ // calculate average pose
+ float averagePoseX = 0;
+ float averagePoseY = 0;
+ float robotX = 0.0;
+ float robotY = 0.0;
+ float robotTheta = 0.0;
+ for (int i = 0; i < particleNum; i++)
+ {
+ m_CurrentList[i]->getRobotPose(robotX, robotY, robotTheta);
+ averagePoseX += robotX;
+ averagePoseY += robotY;
+ }
+ averagePoseX /= particleNum;
+ averagePoseY /= particleNum;
+
+ // calculate standard deviation of pose
+ poseVarianceX = 0.0;
+ poseVarianceY = 0.0;
+ for (int i = 0; i < particleNum; i++)
+ {
+ m_CurrentList[i]->getRobotPose(robotX, robotY, robotTheta);
+ poseVarianceX += (averagePoseX - robotX) * (averagePoseX - robotX);
+ poseVarianceY += (averagePoseY - robotY) * (averagePoseY - robotY);
+ }
+ poseVarianceX /= particleNum;
+ poseVarianceY /= particleNum;
}
-double SlamFilter::evaluateByContrast() {
- return m_OccupancyMap->evaluateByContrast();
+double SlamFilter::evaluateByContrast()
+{
+ return m_OccupancyMap->evaluateByContrast();
}
-void SlamFilter::applyMasking(const nav_msgs::OccupancyGrid::ConstPtr& msg) {
- m_OccupancyMap->applyMasking(msg);
+void SlamFilter::applyMasking(const nav_msgs::OccupancyGrid::ConstPtr& msg)
+{
+ m_OccupancyMap->applyMasking(msg);
}
diff --git a/homer_mapping/src/ParticleFilter/SlamParticle.cpp b/homer_mapping/src/ParticleFilter/SlamParticle.cpp
index c0024dec39ff14883cc39bb0c962a652313c0629..2680b45b66f99509238a48c890cecace8234cd1c 100644
--- a/homer_mapping/src/ParticleFilter/SlamParticle.cpp
+++ b/homer_mapping/src/ParticleFilter/SlamParticle.cpp
@@ -1,30 +1,35 @@
#include <homer_mapping/ParticleFilter/SlamParticle.h>
-SlamParticle::SlamParticle(float weight, float robotX, float robotY, float robotTheta) : Particle(weight) {
+SlamParticle::SlamParticle(float weight, float robotX, float robotY,
+ float robotTheta)
+ : Particle(weight)
+{
m_RobotPositionX = robotX;
m_RobotPositionY = robotY;
m_RobotOrientation = robotTheta;
}
-SlamParticle::SlamParticle( SlamParticle& slamParticle )
+SlamParticle::SlamParticle(SlamParticle& slamParticle)
{
m_RobotPositionX = slamParticle.m_RobotPositionX;
m_RobotPositionY = slamParticle.m_RobotPositionY;
m_RobotOrientation = slamParticle.m_RobotOrientation;
}
-SlamParticle::~SlamParticle() {
+SlamParticle::~SlamParticle()
+{
}
-void SlamParticle::setRobotPose(float robotX, float robotY, float robotTheta) {
+void SlamParticle::setRobotPose(float robotX, float robotY, float robotTheta)
+{
m_RobotPositionX = robotX;
m_RobotPositionY = robotY;
m_RobotOrientation = robotTheta;
}
-void SlamParticle::getRobotPose(float& robotX, float& robotY, float& robotTheta) {
+void SlamParticle::getRobotPose(float& robotX, float& robotY, float& robotTheta)
+{
robotX = m_RobotPositionX;
robotY = m_RobotPositionY;
robotTheta = m_RobotOrientation;
}
-
diff --git a/homer_mapping/src/slam_node.cpp b/homer_mapping/src/slam_node.cpp
index 86014344a5cbb18a80f0138c86dbac93a133d0fc..cb093cff10a505e6dc73e513574da292c66ec1bf 100644
--- a/homer_mapping/src/slam_node.cpp
+++ b/homer_mapping/src/slam_node.cpp
@@ -1,380 +1,413 @@
#include <homer_mapping/slam_node.h>
-SlamNode::SlamNode(ros::NodeHandle* nh) : m_HyperSlamFilter(0) {
- init();
-
- // subscribe to topics
- m_LaserScanSubscriber = nh->subscribe<sensor_msgs::LaserScan>(
- "/scan", 1, &SlamNode::callbackLaserScan, this);
- m_OdometrySubscriber = nh->subscribe<nav_msgs::Odometry>(
- "/odom", 1, &SlamNode::callbackOdometry, this);
- m_UserDefPoseSubscriber = nh->subscribe<geometry_msgs::Pose>(
- "/homer_mapping/userdef_pose", 1, &SlamNode::callbackUserDefPose, this);
- m_InitialPoseSubscriber =
- nh->subscribe<geometry_msgs::PoseWithCovarianceStamped>(
- "/initialpose", 1, &SlamNode::callbackInitialPose, this);
- m_DoMappingSubscriber = nh->subscribe<std_msgs::Bool>(
- "/homer_mapping/do_mapping", 1, &SlamNode::callbackDoMapping, this);
- m_ResetMapSubscriber = nh->subscribe<std_msgs::Empty>(
- "/map_manager/reset_maps", 1, &SlamNode::callbackResetMap, this);
- m_LoadMapSubscriber = nh->subscribe<nav_msgs::OccupancyGrid>(
- "/map_manager/loaded_map", 1, &SlamNode::callbackLoadedMap, this);
- m_MaskingSubscriber = nh->subscribe<nav_msgs::OccupancyGrid>(
- "/map_manager/mask_slam", 1, &SlamNode::callbackMasking, this);
- m_ResetHighSubscriber = nh->subscribe<std_msgs::Empty>(
- "/map_manager/reset_high", 1, &SlamNode::callbackResetHigh, this);
-
- // advertise topics
- m_PoseStampedPublisher =
- nh->advertise<geometry_msgs::PoseStamped>("/pose", 2);
- m_SLAMMapPublisher =
- nh->advertise<nav_msgs::OccupancyGrid>("/homer_mapping/slam_map", 1);
-
- geometry_msgs::PoseStamped poseMsg;
- poseMsg.header.stamp = ros::Time(0);
- poseMsg.header.frame_id = "map";
- poseMsg.pose.position.x = 0.0;
- poseMsg.pose.position.y = 0.0;
- poseMsg.pose.position.z = 0.0;
- tf::Quaternion quatTF = tf::createQuaternionFromYaw(0.0);
- geometry_msgs::Quaternion quatMsg;
- tf::quaternionTFToMsg(quatTF, quatMsg); // conversion from tf::Quaternion
- // to
- poseMsg.pose.orientation = quatMsg;
- m_PoseStampedPublisher.publish(poseMsg);
-
- //// //broadcast transform map -> base_link
- tf::Transform transform(quatTF, tf::Vector3(0.0, 0.0, 0.0));
- m_tfBroadcaster.sendTransform(tf::StampedTransform(
- transform, poseMsg.header.stamp, "map", "base_link"));
- Pose userdef_pose(poseMsg.pose.position.x, poseMsg.pose.position.y,
- tf::getYaw(poseMsg.pose.orientation));
- m_HyperSlamFilter->setRobotPose(userdef_pose, m_ScatterVarXY,
- m_ScatterVarTheta);
+SlamNode::SlamNode(ros::NodeHandle* nh) : m_HyperSlamFilter(0)
+{
+ init();
+
+ // subscribe to topics
+ m_LaserScanSubscriber = nh->subscribe<sensor_msgs::LaserScan>(
+ "/scan", 1, &SlamNode::callbackLaserScan, this);
+ m_OdometrySubscriber = nh->subscribe<nav_msgs::Odometry>(
+ "/odom", 1, &SlamNode::callbackOdometry, this);
+ m_UserDefPoseSubscriber = nh->subscribe<geometry_msgs::Pose>(
+ "/homer_mapping/userdef_pose", 1, &SlamNode::callbackUserDefPose, this);
+ m_InitialPoseSubscriber =
+ nh->subscribe<geometry_msgs::PoseWithCovarianceStamped>(
+ "/initialpose", 1, &SlamNode::callbackInitialPose, this);
+ m_DoMappingSubscriber = nh->subscribe<std_msgs::Bool>(
+ "/homer_mapping/do_mapping", 1, &SlamNode::callbackDoMapping, this);
+ m_ResetMapSubscriber = nh->subscribe<std_msgs::Empty>(
+ "/map_manager/reset_maps", 1, &SlamNode::callbackResetMap, this);
+ m_LoadMapSubscriber = nh->subscribe<nav_msgs::OccupancyGrid>(
+ "/map_manager/loaded_map", 1, &SlamNode::callbackLoadedMap, this);
+ m_MaskingSubscriber = nh->subscribe<nav_msgs::OccupancyGrid>(
+ "/map_manager/mask_slam", 1, &SlamNode::callbackMasking, this);
+ m_ResetHighSubscriber = nh->subscribe<std_msgs::Empty>(
+ "/map_manager/reset_high", 1, &SlamNode::callbackResetHigh, this);
+
+ // advertise topics
+ m_PoseStampedPublisher =
+ nh->advertise<geometry_msgs::PoseStamped>("/pose", 2);
+ m_SLAMMapPublisher =
+ nh->advertise<nav_msgs::OccupancyGrid>("/homer_mapping/slam_map", 1);
+
+ geometry_msgs::PoseStamped poseMsg;
+ poseMsg.header.stamp = ros::Time(0);
+ poseMsg.header.frame_id = "map";
+ poseMsg.pose.position.x = 0.0;
+ poseMsg.pose.position.y = 0.0;
+ poseMsg.pose.position.z = 0.0;
+ tf::Quaternion quatTF = tf::createQuaternionFromYaw(0.0);
+ geometry_msgs::Quaternion quatMsg;
+ tf::quaternionTFToMsg(quatTF, quatMsg); // conversion from tf::Quaternion
+ // to
+ poseMsg.pose.orientation = quatMsg;
+ m_PoseStampedPublisher.publish(poseMsg);
+
+ //// //broadcast transform map -> base_link
+ tf::Transform transform(quatTF, tf::Vector3(0.0, 0.0, 0.0));
+ m_tfBroadcaster.sendTransform(tf::StampedTransform(
+ transform, poseMsg.header.stamp, "map", "base_link"));
+ Pose userdef_pose(poseMsg.pose.position.x, poseMsg.pose.position.y,
+ tf::getYaw(poseMsg.pose.orientation));
+ m_HyperSlamFilter->setRobotPose(userdef_pose, m_ScatterVarXY,
+ m_ScatterVarTheta);
}
-void SlamNode::init() {
- double waitTime;
- ros::param::get("/particlefilter/wait_time", waitTime);
- m_WaitDuration = ros::Duration(waitTime);
- ros::param::get("/selflocalization/scatter_var_xy", m_ScatterVarXY);
- ros::param::get("/selflocalization/scatter_var_theta", m_ScatterVarTheta);
-
- m_DoMapping = true;
-
- int particleNum;
- ros::param::get("/particlefilter/particle_num", particleNum);
- int particleFilterNum;
- ros::param::get("/particlefilter/hyper_slamfilter/particlefilter_num",
- particleFilterNum);
- m_HyperSlamFilter = new HyperSlamFilter(particleFilterNum, particleNum);
-
- m_ReferenceOdometryTime = ros::Time(0);
- m_LastMapSendTime = ros::Time(0);
- m_LastPositionSendTime = ros::Time(0);
- m_LastMappingTime = ros::Time(0);
- m_ReferenceOdometryTime = ros::Time(0);
-
- m_laser_queue.clear();
- m_odom_queue.clear();
+void SlamNode::init()
+{
+ double waitTime;
+ ros::param::get("/particlefilter/wait_time", waitTime);
+ m_WaitDuration = ros::Duration(waitTime);
+ ros::param::get("/selflocalization/scatter_var_xy", m_ScatterVarXY);
+ ros::param::get("/selflocalization/scatter_var_theta", m_ScatterVarTheta);
+
+ m_DoMapping = true;
+
+ int particleNum;
+ ros::param::get("/particlefilter/particle_num", particleNum);
+ int particleFilterNum;
+ ros::param::get("/particlefilter/hyper_slamfilter/particlefilter_num",
+ particleFilterNum);
+ m_HyperSlamFilter = new HyperSlamFilter(particleFilterNum, particleNum);
+
+ m_ReferenceOdometryTime = ros::Time(0);
+ m_LastMapSendTime = ros::Time(0);
+ m_LastPositionSendTime = ros::Time(0);
+ m_LastMappingTime = ros::Time(0);
+ m_ReferenceOdometryTime = ros::Time(0);
+
+ m_laser_queue.clear();
+ m_odom_queue.clear();
}
-SlamNode::~SlamNode() { delete m_HyperSlamFilter; }
-
-void SlamNode::resetMaps() {
- ROS_INFO("Resetting maps..");
-
- delete m_HyperSlamFilter;
- m_HyperSlamFilter = 0;
-
- init();
- geometry_msgs::PoseStamped poseMsg;
- poseMsg.header.stamp = ros::Time::now();
- poseMsg.header.frame_id = "map";
- poseMsg.pose.position.x = 0.0;
- poseMsg.pose.position.y = 0.0;
- poseMsg.pose.position.z = 0.0;
- tf::Quaternion quatTF = tf::createQuaternionFromYaw(0.0);
- geometry_msgs::Quaternion quatMsg;
- tf::quaternionTFToMsg(quatTF, quatMsg); // conversion from tf::Quaternion
- // to
- poseMsg.pose.orientation = quatMsg;
- m_PoseStampedPublisher.publish(poseMsg);
-
- m_LastLikeliestPose.set(0.0, 0.0);
- m_LastLikeliestPose.setTheta(0.0f);
-
- //// //broadcast transform map -> base_link
- tf::Transform transform(quatTF, tf::Vector3(0.0, 0.0, 0.0));
- m_tfBroadcaster.sendTransform(tf::StampedTransform(
- transform, poseMsg.header.stamp, "map", "base_link"));
- Pose userdef_pose(poseMsg.pose.position.x, poseMsg.pose.position.y,
- tf::getYaw(poseMsg.pose.orientation));
- m_HyperSlamFilter->setRobotPose(userdef_pose, m_ScatterVarXY,
- m_ScatterVarTheta);
-
- // sendMapDataMessage();
+SlamNode::~SlamNode()
+{
+ delete m_HyperSlamFilter;
}
-void SlamNode::callbackResetHigh(const std_msgs::Empty::ConstPtr& msg) {
- m_HyperSlamFilter->resetHigh();
+void SlamNode::resetMaps()
+{
+ ROS_INFO("Resetting maps..");
+
+ delete m_HyperSlamFilter;
+ m_HyperSlamFilter = 0;
+
+ init();
+ geometry_msgs::PoseStamped poseMsg;
+ poseMsg.header.stamp = ros::Time::now();
+ poseMsg.header.frame_id = "map";
+ poseMsg.pose.position.x = 0.0;
+ poseMsg.pose.position.y = 0.0;
+ poseMsg.pose.position.z = 0.0;
+ tf::Quaternion quatTF = tf::createQuaternionFromYaw(0.0);
+ geometry_msgs::Quaternion quatMsg;
+ tf::quaternionTFToMsg(quatTF, quatMsg); // conversion from tf::Quaternion
+ // to
+ poseMsg.pose.orientation = quatMsg;
+ m_PoseStampedPublisher.publish(poseMsg);
+
+ m_LastLikeliestPose.set(0.0, 0.0);
+ m_LastLikeliestPose.setTheta(0.0f);
+
+ //// //broadcast transform map -> base_link
+ tf::Transform transform(quatTF, tf::Vector3(0.0, 0.0, 0.0));
+ m_tfBroadcaster.sendTransform(tf::StampedTransform(
+ transform, poseMsg.header.stamp, "map", "base_link"));
+ Pose userdef_pose(poseMsg.pose.position.x, poseMsg.pose.position.y,
+ tf::getYaw(poseMsg.pose.orientation));
+ m_HyperSlamFilter->setRobotPose(userdef_pose, m_ScatterVarXY,
+ m_ScatterVarTheta);
+
+ // sendMapDataMessage();
}
-void SlamNode::sendMapDataMessage(ros::Time mapTime) {
- std::vector<int8_t> mapData;
- nav_msgs::MapMetaData metaData;
+void SlamNode::callbackResetHigh(const std_msgs::Empty::ConstPtr& msg)
+{
+ m_HyperSlamFilter->resetHigh();
+}
+
+void SlamNode::sendMapDataMessage(ros::Time mapTime)
+{
+ std::vector<int8_t> mapData;
+ nav_msgs::MapMetaData metaData;
- OccupancyMap* occMap =
- m_HyperSlamFilter->getBestSlamFilter()->getLikeliestMap();
- occMap->getOccupancyProbabilityImage(mapData, metaData);
+ OccupancyMap* occMap =
+ m_HyperSlamFilter->getBestSlamFilter()->getLikeliestMap();
+ occMap->getOccupancyProbabilityImage(mapData, metaData);
- nav_msgs::OccupancyGrid mapMsg;
- std_msgs::Header header;
- header.stamp = mapTime;
- header.frame_id = "map";
- mapMsg.header = header;
- mapMsg.info = metaData;
- mapMsg.data = mapData;
+ nav_msgs::OccupancyGrid mapMsg;
+ std_msgs::Header header;
+ header.stamp = mapTime;
+ header.frame_id = "map";
+ mapMsg.header = header;
+ mapMsg.info = metaData;
+ mapMsg.data = mapData;
- m_SLAMMapPublisher.publish(mapMsg);
+ m_SLAMMapPublisher.publish(mapMsg);
}
-void SlamNode::callbackUserDefPose(const geometry_msgs::Pose::ConstPtr& msg) {
- Pose userdef_pose(msg->position.x, msg->position.y,
- tf::getYaw(msg->orientation));
- m_HyperSlamFilter->setRobotPose(userdef_pose, m_ScatterVarXY,
- m_ScatterVarTheta);
+void SlamNode::callbackUserDefPose(const geometry_msgs::Pose::ConstPtr& msg)
+{
+ Pose userdef_pose(msg->position.x, msg->position.y,
+ tf::getYaw(msg->orientation));
+ m_HyperSlamFilter->setRobotPose(userdef_pose, m_ScatterVarXY,
+ m_ScatterVarTheta);
}
void SlamNode::callbackInitialPose(
- const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg) {
- Pose userdef_pose(msg->pose.pose.position.x, msg->pose.pose.position.y,
- tf::getYaw(msg->pose.pose.orientation));
- m_HyperSlamFilter->setRobotPose(userdef_pose, m_ScatterVarXY,
- m_ScatterVarTheta);
+ const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
+{
+ Pose userdef_pose(msg->pose.pose.position.x, msg->pose.pose.position.y,
+ tf::getYaw(msg->pose.pose.orientation));
+ m_HyperSlamFilter->setRobotPose(userdef_pose, m_ScatterVarXY,
+ m_ScatterVarTheta);
}
-void SlamNode::callbackLaserScan(const sensor_msgs::LaserScan::ConstPtr& msg) {
- m_laser_queue.push_back(msg);
- if (m_laser_queue.size() > 5) // Todo param
- {
- m_laser_queue.erase(m_laser_queue.begin());
- }
+void SlamNode::callbackLaserScan(const sensor_msgs::LaserScan::ConstPtr& msg)
+{
+ m_laser_queue.push_back(msg);
+ if (m_laser_queue.size() > 5) // Todo param
+ {
+ m_laser_queue.erase(m_laser_queue.begin());
+ }
}
-void SlamNode::callbackOdometry(const nav_msgs::Odometry::ConstPtr& msg) {
- m_odom_queue.push_back(msg);
- static int last_i = 0;
- if (m_odom_queue.size() > 5) // Todo param
+void SlamNode::callbackOdometry(const nav_msgs::Odometry::ConstPtr& msg)
+{
+ m_odom_queue.push_back(msg);
+ static int last_i = 0;
+ if (m_odom_queue.size() > 5) // Todo param
+ {
+ last_i--;
+ if (last_i < 0)
{
- last_i--;
- if (last_i < 0) {
- last_i = 0;
- }
- m_odom_queue.erase(m_odom_queue.begin());
+ last_i = 0;
}
-
- static Pose last_interpolatedPose(0.0, 0.0, 0.0);
- ros::Time currentOdometryTime = msg->header.stamp;
- if ((ros::Time::now() - m_LastMappingTime > m_WaitDuration) &&
- m_odom_queue.size() > 1 && m_laser_queue.size() > 0) {
- int i, j;
- bool got_match = false;
-
- for (i = m_odom_queue.size() - 1; i > 0; i--) {
- // Check if we would repeat a calculation which is already done but
- // still in the queue
- if (m_odom_queue.at(i - 1)->header.stamp ==
- m_ReferenceOdometryTime) {
- break;
- }
-
- for (j = m_laser_queue.size() - 1; j > -1; j--) {
- // find a laserscan in between two odometry readings (or at
- // the same time)
- if ((m_laser_queue.at(j)->header.stamp >=
- m_odom_queue.at(i - 1)->header.stamp) &&
- (m_odom_queue.at(i)->header.stamp >=
- m_laser_queue.at(j)->header.stamp)) {
- got_match = true;
- break;
- }
- }
- if (got_match) {
- break;
- }
- }
-
- if (got_match) {
- last_i = i;
- m_LastLaserData = m_laser_queue.at(j);
- m_LastMappingTime = m_LastLaserData->header.stamp;
-
- float odoX = m_odom_queue.at(i)->pose.pose.position.x;
- float odoY = m_odom_queue.at(i)->pose.pose.position.y;
- float odoTheta =
- tf::getYaw(m_odom_queue.at(i)->pose.pose.orientation);
- Pose currentOdometryPose(odoX, odoY, odoTheta);
- currentOdometryTime = m_odom_queue.at(i)->header.stamp;
-
- odoX = m_odom_queue.at(i - 1)->pose.pose.position.x;
- odoY = m_odom_queue.at(i - 1)->pose.pose.position.y;
- odoTheta =
- tf::getYaw(m_odom_queue.at(i - 1)->pose.pose.orientation);
- Pose lastOdometryPose(odoX, odoY, odoTheta);
- m_ReferenceOdometryPose = lastOdometryPose;
- m_ReferenceOdometryTime = m_odom_queue.at(i - 1)->header.stamp;
-
- ros::Time startTime = ros::Time::now();
- // laserscan in between current odometry reading and
- // m_ReferenceOdometry
- // -> calculate pose of robot during laser scan
- ros::Duration d1 =
- m_LastLaserData->header.stamp - m_ReferenceOdometryTime;
- ros::Duration d2 = currentOdometryTime - m_ReferenceOdometryTime;
-
- float timeFactor;
- if (d1.toSec() == 0.0) {
- timeFactor = 0.0f;
- } else if (d2.toSec() == 0.0) {
- timeFactor = 1.0f;
- } else {
- timeFactor = d1.toSec() / d2.toSec();
- }
- ros::Duration duration = ros::Duration(0);
-
- last_interpolatedPose = m_ReferenceOdometryPose.interpolate(
- currentOdometryPose, timeFactor);
- m_HyperSlamFilter->filter(last_interpolatedPose, m_LastLaserData,
- m_LastLaserData->header.stamp, duration);
- ros::Time finishTime = ros::Time::now();
-
- m_LastLikeliestPose =
- m_HyperSlamFilter->getBestSlamFilter()->getLikeliestPose(
- m_LastLaserData->header.stamp);
-
- // TODO löschen
- m_LastPositionSendTime = m_LastLaserData->header.stamp;
- // send map max. every 500 ms
- if ((m_LastLaserData->header.stamp - m_LastMapSendTime).toSec() >
- 0.5) {
- sendMapDataMessage(m_LastLaserData->header.stamp);
- m_LastMapSendTime = m_LastLaserData->header.stamp;
- }
-
- ROS_DEBUG_STREAM("Pos. data calc time: "
- << (finishTime - startTime).toSec() << "s");
- ROS_DEBUG_STREAM("Map send Interval: "
- << (finishTime - m_LastPositionSendTime).toSec()
- << "s");
+ m_odom_queue.erase(m_odom_queue.begin());
+ }
+
+ static Pose last_interpolatedPose(0.0, 0.0, 0.0);
+ ros::Time currentOdometryTime = msg->header.stamp;
+ if ((ros::Time::now() - m_LastMappingTime > m_WaitDuration) &&
+ m_odom_queue.size() > 1 && m_laser_queue.size() > 0)
+ {
+ int i, j;
+ bool got_match = false;
+
+ for (i = m_odom_queue.size() - 1; i > 0; i--)
+ {
+ // Check if we would repeat a calculation which is already done but
+ // still in the queue
+ if (m_odom_queue.at(i - 1)->header.stamp == m_ReferenceOdometryTime)
+ {
+ break;
+ }
+
+ for (j = m_laser_queue.size() - 1; j > -1; j--)
+ {
+ // find a laserscan in between two odometry readings (or at
+ // the same time)
+ if ((m_laser_queue.at(j)->header.stamp >=
+ m_odom_queue.at(i - 1)->header.stamp) &&
+ (m_odom_queue.at(i)->header.stamp >=
+ m_laser_queue.at(j)->header.stamp))
+ {
+ got_match = true;
+ break;
}
- }
- Pose currentPose = m_LastLikeliestPose;
- // currentPose.setX(currentPose.x() - last_interpolatedPose.x());
- // currentPose.setY(currentPose.y() - last_interpolatedPose.y());
- // currentPose.setTheta(currentPose.theta() -
- // last_interpolatedPose.theta());
- for (int i = (last_i - 1 < 0 ? 0 : last_i - 1); i < m_odom_queue.size();
- i++) {
- currentPose.setX(currentPose.x() +
- m_odom_queue.at(i)->twist.twist.linear.x);
- currentPose.setY(currentPose.y() +
- m_odom_queue.at(i)->twist.twist.linear.y);
- currentPose.setTheta(currentPose.theta() +
- m_odom_queue.at(i)->twist.twist.angular.z);
+ }
+ if (got_match)
+ {
+ break;
+ }
}
- geometry_msgs::PoseStamped poseMsg;
- // header
- poseMsg.header.stamp = msg->header.stamp;
- poseMsg.header.frame_id = "map";
-
- // position and orientation
- poseMsg.pose.position.x = currentPose.x();
- poseMsg.pose.position.y = currentPose.y();
- poseMsg.pose.position.z = 0.0;
- tf::Quaternion quatTF = tf::createQuaternionFromYaw(currentPose.theta());
- geometry_msgs::Quaternion quatMsg;
- tf::quaternionTFToMsg(quatTF, quatMsg); // conversion from tf::Quaternion
- // to geometry_msgs::Quaternion
- poseMsg.pose.orientation = quatMsg;
- m_PoseStampedPublisher.publish(poseMsg);
- // broadcast transform map -> base_link
- // tf::Transform transform(quatTF,tf::Vector3(currentPose.x(),
- // currentPose.y(), 0.0));
- // m_tfBroadcaster.sendTransform(tf::StampedTransform(transform,
- // msg->header.stamp, "map", "base_link"));
+ if (got_match)
+ {
+ last_i = i;
+ m_LastLaserData = m_laser_queue.at(j);
+ m_LastMappingTime = m_LastLaserData->header.stamp;
+
+ float odoX = m_odom_queue.at(i)->pose.pose.position.x;
+ float odoY = m_odom_queue.at(i)->pose.pose.position.y;
+ float odoTheta = tf::getYaw(m_odom_queue.at(i)->pose.pose.orientation);
+ Pose currentOdometryPose(odoX, odoY, odoTheta);
+ currentOdometryTime = m_odom_queue.at(i)->header.stamp;
+
+ odoX = m_odom_queue.at(i - 1)->pose.pose.position.x;
+ odoY = m_odom_queue.at(i - 1)->pose.pose.position.y;
+ odoTheta = tf::getYaw(m_odom_queue.at(i - 1)->pose.pose.orientation);
+ Pose lastOdometryPose(odoX, odoY, odoTheta);
+ m_ReferenceOdometryPose = lastOdometryPose;
+ m_ReferenceOdometryTime = m_odom_queue.at(i - 1)->header.stamp;
+
+ ros::Time startTime = ros::Time::now();
+ // laserscan in between current odometry reading and
+ // m_ReferenceOdometry
+ // -> calculate pose of robot during laser scan
+ ros::Duration d1 =
+ m_LastLaserData->header.stamp - m_ReferenceOdometryTime;
+ ros::Duration d2 = currentOdometryTime - m_ReferenceOdometryTime;
+
+ float timeFactor;
+ if (d1.toSec() == 0.0)
+ {
+ timeFactor = 0.0f;
+ }
+ else if (d2.toSec() == 0.0)
+ {
+ timeFactor = 1.0f;
+ }
+ else
+ {
+ timeFactor = d1.toSec() / d2.toSec();
+ }
+ ros::Duration duration = ros::Duration(0);
+
+ last_interpolatedPose =
+ m_ReferenceOdometryPose.interpolate(currentOdometryPose, timeFactor);
+ m_HyperSlamFilter->filter(last_interpolatedPose, m_LastLaserData,
+ m_LastLaserData->header.stamp, duration);
+ ros::Time finishTime = ros::Time::now();
+
+ m_LastLikeliestPose =
+ m_HyperSlamFilter->getBestSlamFilter()->getLikeliestPose(
+ m_LastLaserData->header.stamp);
+
+ // TODO löschen
+ m_LastPositionSendTime = m_LastLaserData->header.stamp;
+ // send map max. every 500 ms
+ if ((m_LastLaserData->header.stamp - m_LastMapSendTime).toSec() > 0.5)
+ {
+ sendMapDataMessage(m_LastLaserData->header.stamp);
+ m_LastMapSendTime = m_LastLaserData->header.stamp;
+ }
+
+ ROS_DEBUG_STREAM(
+ "Pos. data calc time: " << (finishTime - startTime).toSec() << "s");
+ ROS_DEBUG_STREAM("Map send Interval: "
+ << (finishTime - m_LastPositionSendTime).toSec() << "s");
+ }
+ }
+ Pose currentPose = m_LastLikeliestPose;
+ // currentPose.setX(currentPose.x() - last_interpolatedPose.x());
+ // currentPose.setY(currentPose.y() - last_interpolatedPose.y());
+ // currentPose.setTheta(currentPose.theta() -
+ // last_interpolatedPose.theta());
+ for (int i = (last_i - 1 < 0 ? 0 : last_i - 1); i < m_odom_queue.size(); i++)
+ {
+ currentPose.setX(currentPose.x() +
+ m_odom_queue.at(i)->twist.twist.linear.x);
+ currentPose.setY(currentPose.y() +
+ m_odom_queue.at(i)->twist.twist.linear.y);
+ currentPose.setTheta(currentPose.theta() +
+ m_odom_queue.at(i)->twist.twist.angular.z);
+ }
+
+ geometry_msgs::PoseStamped poseMsg;
+ // header
+ poseMsg.header.stamp = msg->header.stamp;
+ poseMsg.header.frame_id = "map";
+
+ // position and orientation
+ poseMsg.pose.position.x = currentPose.x();
+ poseMsg.pose.position.y = currentPose.y();
+ poseMsg.pose.position.z = 0.0;
+ tf::Quaternion quatTF = tf::createQuaternionFromYaw(currentPose.theta());
+ geometry_msgs::Quaternion quatMsg;
+ tf::quaternionTFToMsg(quatTF, quatMsg); // conversion from tf::Quaternion
+ // to geometry_msgs::Quaternion
+ poseMsg.pose.orientation = quatMsg;
+ m_PoseStampedPublisher.publish(poseMsg);
+ // broadcast transform map -> base_link
+ // tf::Transform transform(quatTF,tf::Vector3(currentPose.x(),
+ // currentPose.y(), 0.0));
+ // m_tfBroadcaster.sendTransform(tf::StampedTransform(transform,
+ // msg->header.stamp, "map", "base_link"));
}
-void SlamNode::callbackDoMapping(const std_msgs::Bool::ConstPtr& msg) {
- m_DoMapping = msg->data;
- m_HyperSlamFilter->setMapping(m_DoMapping);
- ROS_INFO_STREAM("Do mapping is set to " << (m_DoMapping));
+void SlamNode::callbackDoMapping(const std_msgs::Bool::ConstPtr& msg)
+{
+ m_DoMapping = msg->data;
+ m_HyperSlamFilter->setMapping(m_DoMapping);
+ ROS_INFO_STREAM("Do mapping is set to " << (m_DoMapping));
}
-void SlamNode::callbackResetMap(const std_msgs::Empty::ConstPtr& msg) {
- resetMaps();
+void SlamNode::callbackResetMap(const std_msgs::Empty::ConstPtr& msg)
+{
+ resetMaps();
}
-void SlamNode::callbackLoadedMap(const nav_msgs::OccupancyGrid::ConstPtr& msg) {
- float res = msg->info.resolution;
- int height = msg->info.height; // cell size
- int width = msg->info.width; // cell size
- // if(height!=width) {
- // ROS_ERROR("Height != width in loaded map");
- // return;
- //}
-
- // convert map vector from ros format to robbie probability array
- float* map = new float[msg->data.size()];
- // generate exploredRegion
- int minX = INT_MIN;
- int minY = INT_MIN;
- int maxX = INT_MAX;
- int maxY = INT_MAX;
- for (size_t y = 0; y < msg->info.height; y++) {
- int yOffset = msg->info.width * y;
- for (size_t x = 0; x < msg->info.width; x++) {
- int i = yOffset + x;
- if (msg->data[i] == -1)
- map[i] = 0.5;
- else
- map[i] = msg->data[i] / 100.0;
-
- if (map[i] != 0.5) {
- if (minX == INT_MIN || minX > (int)x) minX = (int)x;
- if (minY == INT_MIN || minY > (int)y) minY = (int)y;
- if (maxX == INT_MAX || maxX < (int)x) maxX = (int)x;
- if (maxY == INT_MAX || maxY < (int)y) maxY = (int)y;
- }
- }
+void SlamNode::callbackLoadedMap(const nav_msgs::OccupancyGrid::ConstPtr& msg)
+{
+ float res = msg->info.resolution;
+ int height = msg->info.height; // cell size
+ int width = msg->info.width; // cell size
+ // if(height!=width) {
+ // ROS_ERROR("Height != width in loaded map");
+ // return;
+ //}
+
+ // convert map vector from ros format to robbie probability array
+ float* map = new float[msg->data.size()];
+ // generate exploredRegion
+ int minX = INT_MIN;
+ int minY = INT_MIN;
+ int maxX = INT_MAX;
+ int maxY = INT_MAX;
+ for (size_t y = 0; y < msg->info.height; y++)
+ {
+ int yOffset = msg->info.width * y;
+ for (size_t x = 0; x < msg->info.width; x++)
+ {
+ int i = yOffset + x;
+ if (msg->data[i] == -1)
+ map[i] = 0.5;
+ else
+ map[i] = msg->data[i] / 100.0;
+
+ if (map[i] != 0.5)
+ {
+ if (minX == INT_MIN || minX > (int)x)
+ minX = (int)x;
+ if (minY == INT_MIN || minY > (int)y)
+ minY = (int)y;
+ if (maxX == INT_MAX || maxX < (int)x)
+ maxX = (int)x;
+ if (maxY == INT_MAX || maxY < (int)y)
+ maxY = (int)y;
+ }
}
- Box2D<int> exploredRegion = Box2D<int>(minX, minY, maxX, maxY);
- OccupancyMap* occMap = new OccupancyMap(map, msg->info.origin, res, width,
- height, exploredRegion);
- m_HyperSlamFilter->setOccupancyMap(occMap);
- m_HyperSlamFilter->setMapping(
- false); // is this already done by gui message?
- ROS_INFO_STREAM("Replacing occupancy map");
+ }
+ Box2D<int> exploredRegion = Box2D<int>(minX, minY, maxX, maxY);
+ OccupancyMap* occMap = new OccupancyMap(map, msg->info.origin, res, width,
+ height, exploredRegion);
+ m_HyperSlamFilter->setOccupancyMap(occMap);
+ m_HyperSlamFilter->setMapping(false); // is this already done by gui message?
+ ROS_INFO_STREAM("Replacing occupancy map");
}
-void SlamNode::callbackMasking(const nav_msgs::OccupancyGrid::ConstPtr& msg) {
- m_HyperSlamFilter->applyMasking(msg);
+void SlamNode::callbackMasking(const nav_msgs::OccupancyGrid::ConstPtr& msg)
+{
+ m_HyperSlamFilter->applyMasking(msg);
}
/**
* @brief main function
*/
-int main(int argc, char** argv) {
- ros::init(argc, argv, "homer_mapping");
- ros::NodeHandle nh;
-
- SlamNode slamNode(&nh);
-
- ros::Rate loop_rate(160);
- while (ros::ok()) {
- ros::spinOnce();
- loop_rate.sleep();
- }
- return 0;
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "homer_mapping");
+ ros::NodeHandle nh;
+
+ SlamNode slamNode(&nh);
+
+ ros::Rate loop_rate(160);
+ while (ros::ok())
+ {
+ ros::spinOnce();
+ loop_rate.sleep();
+ }
+ return 0;
}
diff --git a/homer_nav_libs/include/homer_nav_libs/Explorer/Explorer.h b/homer_nav_libs/include/homer_nav_libs/Explorer/Explorer.h
index d39dd791a9000330ffe3a88f0126233e7f768ffb..3b3fc1c6d5bccaf4bb18123d21b11424d808e04f 100644
--- a/homer_nav_libs/include/homer_nav_libs/Explorer/Explorer.h
+++ b/homer_nav_libs/include/homer_nav_libs/Explorer/Explorer.h
@@ -11,7 +11,8 @@
#include <homer_nav_libs/Explorer/GridMap.h>
#include <homer_nav_libs/tools.h>
-namespace ExplorerConstants {
+namespace ExplorerConstants
+{
static int8_t UNKNOWN;
static const int8_t NOT_SEEN_YET = -1;
static const double MAX_DISTANCE = DBL_MAX;
@@ -75,321 +76,327 @@ static const int OBSTACLE = INT_MAX;
* @see GridMap
*
*/
-class Explorer {
- public:
- /**
- * @brief Default constructor.
- * @param minAllowedObstacleDistance,maxAllowedObstacleDistance Range of
- * allowed distances to next obstacle [Pixels]
- * @param minSafeObstacleDistance,maxSafeObstacleDistance Range of distances
- * to next obstacle considered as safe [Pixels]
- * @param safePathWeight Weight for safer path
- */
- Explorer(double minAllowedObstacleDistance,
- double maxAllowedObstacleDistance, double minSafeObstacleDistance,
- double maxSafeObstacleDistance, double safePathWeight,
- double frontierSafenessFactor = 1.0, int unknownThreshold = 50);
-
- /**
- * @brief Destructor deletes all dynamically allocated memory used by the
- * maps
- */
- ~Explorer();
-
- void setUnknownThreshold(int unknownTresh);
- void setAllowedObstacleDistance(double min, double max);
- void setSafeObstacleDistance(double min, double max);
- void setFrontierSafenessFactor(double frontierSafenessFactor);
- void setSafePathWeight(double weight);
- /**
- * @brief Copies and sets the occupancy map.
- * @param width Width of the map
- * @param height Height of the map
- * @param origin Real-world pose of the cell (0,0) in the map
- * @param data GridMap-data (occupancy probabilities: 0 = free, 100 =
- * occupied) of size width * height
- */
- void setOccupancyMap(int width, int height, geometry_msgs::Pose origin,
- int8_t* mapData);
- void setOccupancyMap(const nav_msgs::OccupancyGrid::ConstPtr& cmap);
-
- /** only update occupied areas in current occupancy map */
- void updateObstacles(int width, int height, geometry_msgs::Pose origin,
- int8_t* mapData);
-
- /**
- * @brief Sets the start position for the path finding algorithm.
- * m_Start is set to the given value.
- * If startPixel lies outside the map, m_Start remains untouched.
- * @param startPixel Start position for path finding in pixel (map-)
- * coordinates.
- */
- void setStart(Eigen::Vector2i start);
-
- /**
- * @brief Resets the internal state of the exploration mode.
- * Sets m_DesiredDistance to 0, such that getExplorationTransformPath()
- * triggers
- * a frontier exploration if there is no prior call of setTarget(point,
- * distance).
- * Call this method once before every exploration.
- */
- void resetExploration();
-
- /**
- * Sets the target position for path finding. m_Target is set to the given
- * value.
- * If endPixel lies outside of the map, m_Target remains untouched.
- * computeTargetDistanceMap() is called at the end of this method. m
- * @param targetPixel Target to reach from startPixel
- */
- void setTarget(Eigen::Vector2i targetPixel);
-
- /**
- * Sets the target region for path finding. m_ExplorationMap is set to the
- * given region.
- * If targetPixel lies outside of the map, the exploration map is set empty.
- * @param targetPixel Center of the target region to reach from startPixel
- * @param radius Radius of the target region in pixels
- */
- void setTarget(Eigen::Vector2i targetPixel, int radius);
-
- /**
- * @brief find the nearest position to target that is approachble from the
- * start position
- */
- Eigen::Vector2i getNearestAccessibleTarget(Eigen::Vector2i target);
-
- /**
- * @brief find the nearest position to target surpassing the minimum
- * obstacle distance
- */
- Eigen::Vector2i getNearestWalkablePoint(Eigen::Vector2i target);
-
- /**
- * @brief Returns the map-coordinates of the nearest frontier to m_Start.
- * Uses m_DrivingDistanceMap and m_ObstacleDistanceMap. If there is no
- * frontier left,
- * nextFrontier remains untouched.
- * @param[out] nextFrontier Nearest frontier in map-coordinates.
- * @return true if frontier found and stored in nextFrontier, false if no
- * frontier found (nextFrontier
- * remains untouched).
- */
- bool getNearestFrontier(Eigen::Vector2i& nextFrontier);
-
- /**
- * Computes the path from m_Start to m_Target with path transform.
- * The result is returned. If the returned vector contains no elements,
- * there is no path.
- * @return vector with path points
- */
- std::vector<Eigen::Vector2i> getPath(bool& success);
-
- /**
- * Computes the path from m_Start to the next frontier using exploration
- * transform.
- * The result is returned. If the returned vector contains no elements,
- * there is no path.
- * @return vector with path points
- */
- std::vector<Eigen::Vector2i> getExplorationTransformPath(bool& success);
-
- /**
- * @brief Returns a version of the path that contains less vertices.
- * @note The nearer the next obstacle, the more waypoints are created.
- * @param path List of vertices to be simplified
- * @param treshold[0..1] a lower threshold results in more waypoints
- * (default:1.0)
- * @return Vector of (sampled) waypoints.
- */
- std::vector<Eigen::Vector2i> sampleWaypointsFromPath(
- std::vector<Eigen::Vector2i> path, float threshold = 1.0);
-
- /**
- * Getters for the different transforms (see constructor for description)
- */
- GridMap<int8_t>* getOccupancyMap();
- GridMap<double>* getObstacleTransform();
- GridMap<double>* getCostTransform();
- GridMap<bool>* getTargetMap();
- GridMap<double>* getDrivingDistanceTransform();
- GridMap<double>* getTargetDistanceTransform();
- GridMap<double>* getPathTransform();
- GridMap<double>* getExplorationTransform();
-
- /**
- * @return Start position
- */
- Eigen::Vector2i getStart() const;
-
- /**
- * @return Target position
- */
- Eigen::Vector2i getTarget() const;
-
- private:
- /** @brief Delete the given map and set pointer to 0 */
- template <class T>
- void releaseMap(GridMap<T>*& map) {
- if (map) {
- delete map;
- map = 0;
- }
+class Explorer
+{
+public:
+ /**
+ * @brief Default constructor.
+ * @param minAllowedObstacleDistance,maxAllowedObstacleDistance Range of
+ * allowed distances to next obstacle [Pixels]
+ * @param minSafeObstacleDistance,maxSafeObstacleDistance Range of distances
+ * to next obstacle considered as safe [Pixels]
+ * @param safePathWeight Weight for safer path
+ */
+ Explorer(double minAllowedObstacleDistance, double maxAllowedObstacleDistance,
+ double minSafeObstacleDistance, double maxSafeObstacleDistance,
+ double safePathWeight, double frontierSafenessFactor = 1.0,
+ int unknownThreshold = 50);
+
+ /**
+ * @brief Destructor deletes all dynamically allocated memory used by the
+ * maps
+ */
+ ~Explorer();
+
+ void setUnknownThreshold(int unknownTresh);
+ void setAllowedObstacleDistance(double min, double max);
+ void setSafeObstacleDistance(double min, double max);
+ void setFrontierSafenessFactor(double frontierSafenessFactor);
+ void setSafePathWeight(double weight);
+ /**
+ * @brief Copies and sets the occupancy map.
+ * @param width Width of the map
+ * @param height Height of the map
+ * @param origin Real-world pose of the cell (0,0) in the map
+ * @param data GridMap-data (occupancy probabilities: 0 = free, 100 =
+ * occupied) of size width * height
+ */
+ void setOccupancyMap(int width, int height, geometry_msgs::Pose origin,
+ int8_t* mapData);
+ void setOccupancyMap(const nav_msgs::OccupancyGrid::ConstPtr& cmap);
+
+ /** only update occupied areas in current occupancy map */
+ void updateObstacles(int width, int height, geometry_msgs::Pose origin,
+ int8_t* mapData);
+
+ /**
+ * @brief Sets the start position for the path finding algorithm.
+ * m_Start is set to the given value.
+ * If startPixel lies outside the map, m_Start remains untouched.
+ * @param startPixel Start position for path finding in pixel (map-)
+ * coordinates.
+ */
+ void setStart(Eigen::Vector2i start);
+
+ /**
+ * @brief Resets the internal state of the exploration mode.
+ * Sets m_DesiredDistance to 0, such that getExplorationTransformPath()
+ * triggers
+ * a frontier exploration if there is no prior call of setTarget(point,
+ * distance).
+ * Call this method once before every exploration.
+ */
+ void resetExploration();
+
+ /**
+ * Sets the target position for path finding. m_Target is set to the given
+ * value.
+ * If endPixel lies outside of the map, m_Target remains untouched.
+ * computeTargetDistanceMap() is called at the end of this method. m
+ * @param targetPixel Target to reach from startPixel
+ */
+ void setTarget(Eigen::Vector2i targetPixel);
+
+ /**
+ * Sets the target region for path finding. m_ExplorationMap is set to the
+ * given region.
+ * If targetPixel lies outside of the map, the exploration map is set empty.
+ * @param targetPixel Center of the target region to reach from startPixel
+ * @param radius Radius of the target region in pixels
+ */
+ void setTarget(Eigen::Vector2i targetPixel, int radius);
+
+ /**
+ * @brief find the nearest position to target that is approachble from the
+ * start position
+ */
+ Eigen::Vector2i getNearestAccessibleTarget(Eigen::Vector2i target);
+
+ /**
+ * @brief find the nearest position to target surpassing the minimum
+ * obstacle distance
+ */
+ Eigen::Vector2i getNearestWalkablePoint(Eigen::Vector2i target);
+
+ /**
+ * @brief Returns the map-coordinates of the nearest frontier to m_Start.
+ * Uses m_DrivingDistanceMap and m_ObstacleDistanceMap. If there is no
+ * frontier left,
+ * nextFrontier remains untouched.
+ * @param[out] nextFrontier Nearest frontier in map-coordinates.
+ * @return true if frontier found and stored in nextFrontier, false if no
+ * frontier found (nextFrontier
+ * remains untouched).
+ */
+ bool getNearestFrontier(Eigen::Vector2i& nextFrontier);
+
+ /**
+ * Computes the path from m_Start to m_Target with path transform.
+ * The result is returned. If the returned vector contains no elements,
+ * there is no path.
+ * @return vector with path points
+ */
+ std::vector<Eigen::Vector2i> getPath(bool& success);
+
+ /**
+ * Computes the path from m_Start to the next frontier using exploration
+ * transform.
+ * The result is returned. If the returned vector contains no elements,
+ * there is no path.
+ * @return vector with path points
+ */
+ std::vector<Eigen::Vector2i> getExplorationTransformPath(bool& success);
+
+ /**
+ * @brief Returns a version of the path that contains less vertices.
+ * @note The nearer the next obstacle, the more waypoints are created.
+ * @param path List of vertices to be simplified
+ * @param treshold[0..1] a lower threshold results in more waypoints
+ * (default:1.0)
+ * @return Vector of (sampled) waypoints.
+ */
+ std::vector<Eigen::Vector2i> sampleWaypointsFromPath(
+ std::vector<Eigen::Vector2i> path, float threshold = 1.0);
+
+ /**
+ * Getters for the different transforms (see constructor for description)
+ */
+ GridMap<int8_t>* getOccupancyMap();
+ GridMap<double>* getObstacleTransform();
+ GridMap<double>* getCostTransform();
+ GridMap<bool>* getTargetMap();
+ GridMap<double>* getDrivingDistanceTransform();
+ GridMap<double>* getTargetDistanceTransform();
+ GridMap<double>* getPathTransform();
+ GridMap<double>* getExplorationTransform();
+
+ /**
+ * @return Start position
+ */
+ Eigen::Vector2i getStart() const;
+
+ /**
+ * @return Target position
+ */
+ Eigen::Vector2i getTarget() const;
+
+private:
+ /** @brief Delete the given map and set pointer to 0 */
+ template <class T>
+ void releaseMap(GridMap<T>*& map)
+ {
+ if (map)
+ {
+ delete map;
+ map = 0;
}
-
- /** @brief Delete and re-create given map */
- template <class T>
- void resetMap(GridMap<T>*& map) {
- if (!m_OccupancyMap) {
- ROS_ERROR("Occupancy map is missing.");
- return;
- }
- releaseMap(map);
- map = new GridMap<T>(m_OccupancyMap->width(), m_OccupancyMap->height());
- }
-
- /**
- * @return true if the robot can stand on the given position without
- * touching an obstacle, false otherwise
- * @warning Call computeWalkableMaps before
- */
- inline bool isWalkable(int x, int y) const {
- return (
- (m_OccupancyMap->getValue(x, y) <= ExplorerConstants::UNKNOWN) &&
- (m_ObstacleTransform->getValue(x, y) >
- m_MinAllowedObstacleDistance));
- }
-
- /**
- * @return true if point is approachable from the current start position,
- * false otherwise.
- * @warning m_OccupancyMap, m_ObstacleTransform and
- * m_DrivingDistanceTransform have to be present!
- * @warning Call computeApproachableMaps before
- */
- inline bool isApproachable(int x, int y) const {
- return (m_DrivingDistanceTransform->getValue(x, y) <
- ExplorerConstants::MAX_DISTANCE);
+ }
+
+ /** @brief Delete and re-create given map */
+ template <class T>
+ void resetMap(GridMap<T>*& map)
+ {
+ if (!m_OccupancyMap)
+ {
+ ROS_ERROR("Occupancy map is missing.");
+ return;
}
+ releaseMap(map);
+ map = new GridMap<T>(m_OccupancyMap->width(), m_OccupancyMap->height());
+ }
+
+ /**
+ * @return true if the robot can stand on the given position without
+ * touching an obstacle, false otherwise
+ * @warning Call computeWalkableMaps before
+ */
+ inline bool isWalkable(int x, int y) const
+ {
+ return (
+ (m_OccupancyMap->getValue(x, y) <= ExplorerConstants::UNKNOWN) &&
+ (m_ObstacleTransform->getValue(x, y) > m_MinAllowedObstacleDistance));
+ }
+
+ /**
+ * @return true if point is approachable from the current start position,
+ * false otherwise.
+ * @warning m_OccupancyMap, m_ObstacleTransform and
+ * m_DrivingDistanceTransform have to be present!
+ * @warning Call computeApproachableMaps before
+ */
+ inline bool isApproachable(int x, int y) const
+ {
+ return (m_DrivingDistanceTransform->getValue(x, y) <
+ ExplorerConstants::MAX_DISTANCE);
+ }
+
+ /** @brief Releases all memory of the member maps */
+ void releaseMaps();
+
+ /**
+ * @brief Helper function for computeDistanceTransformation.
+ * @param f 1D-Array for distance transformation
+ * @param n Number of elements in f
+ * @return Distance transformation of f
+ */
+ double* distanceTransform1D(double* f, int n);
+
+ /**
+ * @brief Fills the given map from given start point with distance values to
+ * this point.
+ * The filling will only be performed on cells that are marked as free in
+ * m_OccupancyMap and
+ * that have an obstacle distance value between m_MinimumObstacleDistance
+ * and m_MaximumObstacleDistance.
+ * The map that is passed as argument will be fully overwritten by this
+ * function.
+ * @param map GridMap to fill
+ * @param start Start point for the fill algorithm
+ */
+ void distanceFloodFill(GridMap<double>* map, Eigen::Vector2i start);
+
+ /** @brief Compute map needed for path calculation */
+ void computePathTransform();
+
+ /** @brief Compute map needed for exploration path calculation */
+ void computeExplorationTransform();
+
+ /** @brief Compute the distances to the next obstacle with eucledian
+ * distance transform from m_OccupancyMap. */
+ void computeObstacleTransform();
+
+ /** @brief Compute cost function based on obstacle transform */
+ void computeCostTransform();
+
+ /** @brief Compute the frontiers between free and unknown space. Depends on
+ * OccupancyMap and ObstacleTransform. */
+ void computeFrontierMap();
+
+ /** @brief Compute the target region (a circle of radius m_DesiredDistance
+ * around m_Target). */
+ void computeRegionMap();
+
+ /** @brief Compute the target map, which is either a frontier map or a
+ * region map. */
+ void computeTargetMap();
+
+ /** @brief Compute a map of driving distances from the start point */
+ void computeDrivingDistanceTransform();
+
+ /** @brief Compute a map of driving distances to the target point */
+ void computeTargetDistanceTransform();
+
+ /** @brief Compute maps needed for isWalkable */
+ void computeWalkableMaps();
+
+ /** @brief Compute maps needed for isApproachable */
+ void computeApproachableMaps();
+
+ /** @brief Start point for the way search algorithm. */
+ Eigen::Vector2i m_Start;
+
+ /** @brief Target for the way search algorithm */
+ Eigen::Vector2i m_Target;
+
+ /** @brief Desired distance to target in pixels */
+ int m_DesiredDistance;
+
+ /** @brief Occupancy map */
+ GridMap<int8_t>* m_OccupancyMap;
+
+ /** @see computeObstacleTransform */
+ GridMap<double>* m_ObstacleTransform;
+
+ /** @see computeCostTransform */
+ GridMap<double>* m_CostTransform;
+
+ /** @see computeTargetMap */
+ GridMap<bool>* m_TargetMap;
+
+ /** computeDrivingDistanceTransform */
+ GridMap<double>* m_DrivingDistanceTransform;
+
+ /** @see computeTargetDistanceTransform */
+ GridMap<double>* m_TargetDistanceTransform;
+
+ /** @see computePathTransform */
+ GridMap<double>* m_PathTransform;
+
+ /** @see computeExplorationTransform */
+ GridMap<double>* m_ExplorationTransform;
- /** @brief Releases all memory of the member maps */
- void releaseMaps();
-
- /**
- * @brief Helper function for computeDistanceTransformation.
- * @param f 1D-Array for distance transformation
- * @param n Number of elements in f
- * @return Distance transformation of f
- */
- double* distanceTransform1D(double* f, int n);
-
- /**
- * @brief Fills the given map from given start point with distance values to
- * this point.
- * The filling will only be performed on cells that are marked as free in
- * m_OccupancyMap and
- * that have an obstacle distance value between m_MinimumObstacleDistance
- * and m_MaximumObstacleDistance.
- * The map that is passed as argument will be fully overwritten by this
- * function.
- * @param map GridMap to fill
- * @param start Start point for the fill algorithm
- */
- void distanceFloodFill(GridMap<double>* map, Eigen::Vector2i start);
-
- /** @brief Compute map needed for path calculation */
- void computePathTransform();
-
- /** @brief Compute map needed for exploration path calculation */
- void computeExplorationTransform();
-
- /** @brief Compute the distances to the next obstacle with eucledian
- * distance transform from m_OccupancyMap. */
- void computeObstacleTransform();
-
- /** @brief Compute cost function based on obstacle transform */
- void computeCostTransform();
-
- /** @brief Compute the frontiers between free and unknown space. Depends on
- * OccupancyMap and ObstacleTransform. */
- void computeFrontierMap();
-
- /** @brief Compute the target region (a circle of radius m_DesiredDistance
- * around m_Target). */
- void computeRegionMap();
-
- /** @brief Compute the target map, which is either a frontier map or a
- * region map. */
- void computeTargetMap();
-
- /** @brief Compute a map of driving distances from the start point */
- void computeDrivingDistanceTransform();
-
- /** @brief Compute a map of driving distances to the target point */
- void computeTargetDistanceTransform();
-
- /** @brief Compute maps needed for isWalkable */
- void computeWalkableMaps();
-
- /** @brief Compute maps needed for isApproachable */
- void computeApproachableMaps();
-
- /** @brief Start point for the way search algorithm. */
- Eigen::Vector2i m_Start;
-
- /** @brief Target for the way search algorithm */
- Eigen::Vector2i m_Target;
-
- /** @brief Desired distance to target in pixels */
- int m_DesiredDistance;
-
- /** @brief Occupancy map */
- GridMap<int8_t>* m_OccupancyMap;
-
- /** @see computeObstacleTransform */
- GridMap<double>* m_ObstacleTransform;
-
- /** @see computeCostTransform */
- GridMap<double>* m_CostTransform;
-
- /** @see computeTargetMap */
- GridMap<bool>* m_TargetMap;
-
- /** computeDrivingDistanceTransform */
- GridMap<double>* m_DrivingDistanceTransform;
-
- /** @see computeTargetDistanceTransform */
- GridMap<double>* m_TargetDistanceTransform;
-
- /** @see computePathTransform */
- GridMap<double>* m_PathTransform;
-
- /** @see computeExplorationTransform */
- GridMap<double>* m_ExplorationTransform;
-
- /** @see constructor */
- double m_MinAllowedObstacleDistance;
- double m_MaxAllowedObstacleDistance;
+ /** @see constructor */
+ double m_MinAllowedObstacleDistance;
+ double m_MaxAllowedObstacleDistance;
- double m_MinSafeObstacleDistance;
- double m_MaxSafeObstacleDistance;
+ double m_MinSafeObstacleDistance;
+ double m_MaxSafeObstacleDistance;
- /**
- * Weight for safer path
- */
- double m_SafePathWeight;
+ /**
+ * Weight for safer path
+ */
+ double m_SafePathWeight;
- /**
- * Factor for minObstacleDistance that determines if a frontier pixel is
- * valid
- */
- double m_FrontierSafenessFactor;
+ /**
+ * Factor for minObstacleDistance that determines if a frontier pixel is
+ * valid
+ */
+ double m_FrontierSafenessFactor;
- /**
- * Real-world pose of the point (0,0) in the map
- */
- geometry_msgs::Pose m_Origin;
+ /**
+ * Real-world pose of the point (0,0) in the map
+ */
+ geometry_msgs::Pose m_Origin;
};
#endif
diff --git a/homer_nav_libs/include/homer_nav_libs/Explorer/GridMap.h b/homer_nav_libs/include/homer_nav_libs/Explorer/GridMap.h
index 5ce49ae7aee3a74365671f5ed829197deafffc33..6ee7e7e78dd38180fbc664632b2c0ceb69b993c1 100644
--- a/homer_nav_libs/include/homer_nav_libs/Explorer/GridMap.h
+++ b/homer_nav_libs/include/homer_nav_libs/Explorer/GridMap.h
@@ -20,178 +20,207 @@
*/
template <class DataT>
-class GridMap {
- public:
- /// Initialize empty map
- GridMap();
-
- /**
- * @param width Width of the map.
- * @param height Height of the map.
- * @param data Pointer to map data, must be of size width*height.
- * @param copyData if true, the map data will be copied
- * if false, GridMap takes ownership of the pointer
+class GridMap
+{
+public:
+ /// Initialize empty map
+ GridMap();
+
+ /**
+ * @param width Width of the map.
+ * @param height Height of the map.
+ * @param data Pointer to map data, must be of size width*height.
+ * @param copyData if true, the map data will be copied
+ * if false, GridMap takes ownership of the pointer
* @param cellSize physical size of each map cell [m]
- * @param centerX,centerY center of the map in world coordinates
- */
- GridMap(int width, int height, DataT* data = 0, bool copyData = true,
- float cellSize = 1, float centerX = 0, float centerY = 0);
-
- /// Copy data from given region
- GridMap(int width, int height, DataT* data,
- Eigen::AlignedBox2i extractRegion);
-
- /// Copy data from given map
- GridMap<DataT>(const GridMap<DataT>& other) {
- m_Data = 0;
- *this = other;
- }
-
- /// Copy data from given map
- GridMap<DataT>& operator=(const GridMap<DataT>& other);
-
- ~GridMap();
-
- /// Convert map coordinates to world coordinates
- void mapToWorld(int mapX, int mapY, float& worldX, float& worldY);
-
- /// Convert world coordinates to map coordinates
- void worldToMap(float worldX, float worldY, int& mapX, int& mapY);
-
- /// @brief set value at given position
- inline void setValue(int x, int y, DataT val);
-
- /// @brief replace content with given value
- void fill(DataT val);
-
- /// @brief Draw a filled polygon into the map (world coords)
- void drawPolygon(std::vector<Eigen::Vector2d> vertices, DataT value);
-
- /// @brief Draw a filled circle into the map (world coords)
- void drawCircle(Eigen::Vector2d center, float radius, DataT value);
-
- /// @return Value at the given position.
- inline DataT getValue(int x, int y) const;
-
- /// @return Pointer to given pixel
- inline DataT* getDirectAccess(int x, int y);
-
- /// @return width in grid cells
- int width() const { return m_Width; }
-
- /// @return height in grid cells
- int height() const { return m_Height; }
-
- /// @return center of the map in world coordinates
- Eigen::Vector2d center() const {
- return Eigen::Vector2d(m_CenterX, m_CenterY);
- }
-
- /// @return side length of one cell in mm
- float cellSize() { return m_CellSize; }
-
- private:
- void drawLine(DataT* data, int startX, int startY, int endX, int endY,
- DataT value);
- void fillPolygon(DataT* data, int x, int y, char value);
-
- int m_Width;
- int m_Height;
- int m_DataSize;
- DataT* m_Data;
- float m_CellSize;
- float m_CenterX;
- float m_CenterY;
+ * @param centerX,centerY center of the map in world coordinates
+ */
+ GridMap(int width, int height, DataT* data = 0, bool copyData = true,
+ float cellSize = 1, float centerX = 0, float centerY = 0);
+
+ /// Copy data from given region
+ GridMap(int width, int height, DataT* data,
+ Eigen::AlignedBox2i extractRegion);
+
+ /// Copy data from given map
+ GridMap<DataT>(const GridMap<DataT>& other)
+ {
+ m_Data = 0;
+ *this = other;
+ }
+
+ /// Copy data from given map
+ GridMap<DataT>& operator=(const GridMap<DataT>& other);
+
+ ~GridMap();
+
+ /// Convert map coordinates to world coordinates
+ void mapToWorld(int mapX, int mapY, float& worldX, float& worldY);
+
+ /// Convert world coordinates to map coordinates
+ void worldToMap(float worldX, float worldY, int& mapX, int& mapY);
+
+ /// @brief set value at given position
+ inline void setValue(int x, int y, DataT val);
+
+ /// @brief replace content with given value
+ void fill(DataT val);
+
+ /// @brief Draw a filled polygon into the map (world coords)
+ void drawPolygon(std::vector<Eigen::Vector2d> vertices, DataT value);
+
+ /// @brief Draw a filled circle into the map (world coords)
+ void drawCircle(Eigen::Vector2d center, float radius, DataT value);
+
+ /// @return Value at the given position.
+ inline DataT getValue(int x, int y) const;
+
+ /// @return Pointer to given pixel
+ inline DataT* getDirectAccess(int x, int y);
+
+ /// @return width in grid cells
+ int width() const
+ {
+ return m_Width;
+ }
+
+ /// @return height in grid cells
+ int height() const
+ {
+ return m_Height;
+ }
+
+ /// @return center of the map in world coordinates
+ Eigen::Vector2d center() const
+ {
+ return Eigen::Vector2d(m_CenterX, m_CenterY);
+ }
+
+ /// @return side length of one cell in mm
+ float cellSize()
+ {
+ return m_CellSize;
+ }
+
+private:
+ void drawLine(DataT* data, int startX, int startY, int endX, int endY,
+ DataT value);
+ void fillPolygon(DataT* data, int x, int y, char value);
+
+ int m_Width;
+ int m_Height;
+ int m_DataSize;
+ DataT* m_Data;
+ float m_CellSize;
+ float m_CenterX;
+ float m_CenterY;
};
template <class DataT>
-GridMap<DataT>::GridMap() {
- m_Width = 0;
- m_Height = 0;
- m_DataSize = 0;
- m_Data = 0;
- m_CellSize = 0;
- m_CenterX = 0;
- m_CenterY = 0;
+GridMap<DataT>::GridMap()
+{
+ m_Width = 0;
+ m_Height = 0;
+ m_DataSize = 0;
+ m_Data = 0;
+ m_CellSize = 0;
+ m_CenterX = 0;
+ m_CenterY = 0;
}
template <class DataT>
GridMap<DataT>::GridMap(int width, int height, DataT* data, bool copyData,
- float cellSize, float centerX, float centerY) {
- m_Width = width;
- m_Height = height;
- m_CellSize = cellSize;
- m_DataSize = width * height;
- m_CenterX = centerX;
- m_CenterY = centerY;
- m_Data = 0;
-
- if (data) {
- if (copyData) {
- m_Data = new DataT[m_DataSize];
+ float cellSize, float centerX, float centerY)
+{
+ m_Width = width;
+ m_Height = height;
+ m_CellSize = cellSize;
+ m_DataSize = width * height;
+ m_CenterX = centerX;
+ m_CenterY = centerY;
+ m_Data = 0;
+
+ if (data)
+ {
+ if (copyData)
+ {
+ m_Data = new DataT[m_DataSize];
- for (int i = 0; i < m_DataSize; i++) {
- m_Data[i] = data[i];
- }
- } else {
- m_Data = data;
- }
- } else {
- m_Data = new DataT[m_DataSize];
+ for (int i = 0; i < m_DataSize; i++)
+ {
+ m_Data[i] = data[i];
+ }
+ }
+ else
+ {
+ m_Data = data;
+ }
+ }
+ else
+ {
+ m_Data = new DataT[m_DataSize];
- for (int i = 0; i < m_DataSize; i++) {
- m_Data[i] = 0;
- }
+ for (int i = 0; i < m_DataSize; i++)
+ {
+ m_Data[i] = 0;
}
+ }
}
template <class DataT>
GridMap<DataT>::GridMap(int width, int height, DataT* data,
- Eigen::AlignedBox2i extractRegion) {
- m_Width = extractRegion.sizes().x();
- m_Height = extractRegion.sizes().y();
- m_DataSize = m_Width * m_Height;
- m_Data = new DataT[m_DataSize];
- m_CellSize = 1;
- m_CenterX = 0;
- m_CenterY = 0;
-
- for (int y = extractRegion.min().y(); y <= extractRegion.max().y(); y++) {
- int yOffset = m_Width * y;
-
- for (int x = extractRegion.min().x(); x <= extractRegion.max().x();
- x++) {
- int i = x + yOffset;
- m_Data[i] = data[i];
- }
+ Eigen::AlignedBox2i extractRegion)
+{
+ m_Width = extractRegion.sizes().x();
+ m_Height = extractRegion.sizes().y();
+ m_DataSize = m_Width * m_Height;
+ m_Data = new DataT[m_DataSize];
+ m_CellSize = 1;
+ m_CenterX = 0;
+ m_CenterY = 0;
+
+ for (int y = extractRegion.min().y(); y <= extractRegion.max().y(); y++)
+ {
+ int yOffset = m_Width * y;
+
+ for (int x = extractRegion.min().x(); x <= extractRegion.max().x(); x++)
+ {
+ int i = x + yOffset;
+ m_Data[i] = data[i];
}
+ }
}
template <class DataT>
-inline DataT* GridMap<DataT>::getDirectAccess(int x, int y) {
+inline DataT* GridMap<DataT>::getDirectAccess(int x, int y)
+{
#ifdef GRIDMAP_SAFE_ACCESS
- if (x >= 0 && x < m_Width && y >= 0 && y < m_Height) {
- return &m_Data[y * m_Width + x];
- } else {
- throw;
- }
-#else
+ if (x >= 0 && x < m_Width && y >= 0 && y < m_Height)
+ {
return &m_Data[y * m_Width + x];
+ }
+ else
+ {
+ throw;
+ }
+#else
+ return &m_Data[y * m_Width + x];
#endif
}
template <class DataT>
-GridMap<DataT>& GridMap<DataT>::operator=(const GridMap<DataT>& other) {
- delete[] m_Data;
- m_Width = other.m_Width;
- m_Height = other.m_Height;
- m_DataSize = other.m_DataSize;
- m_Data = new DataT[m_DataSize];
- memcpy(m_Data, other.m_Data, sizeof(DataT) * m_DataSize);
- m_CellSize = other.m_CellSize;
- m_CenterX = other.m_CenterX;
- m_CenterY = other.m_CenterY;
- return *this;
+GridMap<DataT>& GridMap<DataT>::operator=(const GridMap<DataT>& other)
+{
+ delete[] m_Data;
+ m_Width = other.m_Width;
+ m_Height = other.m_Height;
+ m_DataSize = other.m_DataSize;
+ m_Data = new DataT[m_DataSize];
+ memcpy(m_Data, other.m_Data, sizeof(DataT) * m_DataSize);
+ m_CellSize = other.m_CellSize;
+ m_CenterX = other.m_CenterX;
+ m_CenterY = other.m_CenterY;
+ return *this;
}
/* TODO
template<class DataT>
@@ -210,11 +239,13 @@ GridMap<DataT>::GridMap ( ExtendedInStream& strm )
}
*/
template <class DataT>
-GridMap<DataT>::~GridMap() {
- if (m_Data) {
- delete m_Data;
- m_Data = 0;
- }
+GridMap<DataT>::~GridMap()
+{
+ if (m_Data)
+ {
+ delete m_Data;
+ m_Data = 0;
+ }
}
/*
template<class DataT>
@@ -232,73 +263,90 @@ void GridMap<DataT>::storer ( ExtendedOutStream& strm ) const
template <class DataT>
void GridMap<DataT>::mapToWorld(int mapX, int mapY, float& worldX,
- float& worldY) {
- worldX = m_CenterX + m_CellSize * (mapX - m_Width / 2);
- worldY = m_CenterY + m_CellSize * (mapY - m_Height / 2);
+ float& worldY)
+{
+ worldX = m_CenterX + m_CellSize * (mapX - m_Width / 2);
+ worldY = m_CenterY + m_CellSize * (mapY - m_Height / 2);
}
template <class DataT>
void GridMap<DataT>::worldToMap(float worldX, float worldY, int& mapX,
- int& mapY) {
- mapX = float(m_Width) / 2.0 - ((worldY - m_CenterY) / m_CellSize + 0.5);
- mapY = float(m_Height) / 2.0 - ((worldX - m_CenterX) / m_CellSize + 0.5);
+ int& mapY)
+{
+ mapX = float(m_Width) / 2.0 - ((worldY - m_CenterY) / m_CellSize + 0.5);
+ mapY = float(m_Height) / 2.0 - ((worldX - m_CenterX) / m_CellSize + 0.5);
- if (mapX < 0 || mapX >= m_Width || mapY < 0 || mapY >= m_Height) {
- // ROS_WARN_STREAM ( "Index out of bounds: " << mapX << "," << mapY );
- // //TODO
+ if (mapX < 0 || mapX >= m_Width || mapY < 0 || mapY >= m_Height)
+ {
+ // ROS_WARN_STREAM ( "Index out of bounds: " << mapX << "," << mapY );
+ // //TODO
- if (mapX < 0) {
- mapX = 0;
- }
+ if (mapX < 0)
+ {
+ mapX = 0;
+ }
- if (mapX >= m_Width) {
- mapX = m_Width - 1;
- }
+ if (mapX >= m_Width)
+ {
+ mapX = m_Width - 1;
+ }
- if (mapY < 0) {
- mapY = 0;
- }
+ if (mapY < 0)
+ {
+ mapY = 0;
+ }
- if (mapY >= m_Height) {
- mapY = m_Height - 1;
- }
+ if (mapY >= m_Height)
+ {
+ mapY = m_Height - 1;
}
+ }
}
template <class DataT>
-inline void GridMap<DataT>::setValue(int x, int y, DataT val) {
+inline void GridMap<DataT>::setValue(int x, int y, DataT val)
+{
#ifdef GRIDMAP_SAFE_ACCESS
- if (x >= 0 && x < m_Width && y >= 0 && y < m_Height) {
- m_Data[y * m_Width + x] = val;
- } else {
- throw;
- }
-#else
+ if (x >= 0 && x < m_Width && y >= 0 && y < m_Height)
+ {
m_Data[y * m_Width + x] = val;
+ }
+ else
+ {
+ throw;
+ }
+#else
+ m_Data[y * m_Width + x] = val;
#endif
}
template <class DataT>
-inline DataT GridMap<DataT>::getValue(int x, int y) const {
+inline DataT GridMap<DataT>::getValue(int x, int y) const
+{
#ifdef GRIDMAP_SAFE_ACCESS
- if (x >= 0 && x < m_Width && y >= 0 && y < m_Height) {
- return m_Data[y * m_Width + x];
- } else {
- ROS_ERROR_STREAM("Accessing map pixels "
- << x << "," << y << ": out of bounds (0,0,"
- << m_Width - 1 << "," << m_Height - 1 << ")"); // TODO
- throw;
- }
-#else
+ if (x >= 0 && x < m_Width && y >= 0 && y < m_Height)
+ {
return m_Data[y * m_Width + x];
+ }
+ else
+ {
+ ROS_ERROR_STREAM("Accessing map pixels "
+ << x << "," << y << ": out of bounds (0,0," << m_Width - 1
+ << "," << m_Height - 1 << ")"); // TODO
+ throw;
+ }
+#else
+ return m_Data[y * m_Width + x];
#endif
}
template <class DataT>
-void GridMap<DataT>::fill(DataT val) {
- for (int i = 0; i < m_DataSize; i++) {
- m_Data[i] = val;
- }
+void GridMap<DataT>::fill(DataT val)
+{
+ for (int i = 0; i < m_DataSize; i++)
+ {
+ m_Data[i] = val;
+ }
}
/* TODO do we need image representation?
@@ -390,135 +438,158 @@ range;
template <class DataT>
void GridMap<DataT>::drawCircle(Eigen::Vector2d center, float radius,
- DataT value) {
- int centerMapX, centerMapY;
- worldToMap(center.x(), center.y(), centerMapX, centerMapY);
-
- int radiusCells = radius / m_CellSize;
- int radiusCells2 = radiusCells * radiusCells;
-
- Eigen::AlignedBox2i bBox(
- Eigen::Vector2i(centerMapX - radiusCells, centerMapY - radiusCells),
- Eigen::Vector2i(centerMapX + radiusCells, centerMapY + radiusCells));
- Eigen::AlignedBox2i bBoxGrid(Eigen::Vector2i(0, 0),
- Eigen::Vector2i(m_Width - 1, m_Height - 1));
- bBox.clamp(bBoxGrid);
-
- for (int y = bBox.min().y(); y <= bBox.max().y(); y++) {
- for (int x = bBox.min().x(); x <= bBox.max().x(); x++) {
- int xC = x - centerMapX;
- int yC = y - centerMapY;
- if (xC * xC + yC * yC <= radiusCells2) {
- setValue(x, y, value);
- }
- }
+ DataT value)
+{
+ int centerMapX, centerMapY;
+ worldToMap(center.x(), center.y(), centerMapX, centerMapY);
+
+ int radiusCells = radius / m_CellSize;
+ int radiusCells2 = radiusCells * radiusCells;
+
+ Eigen::AlignedBox2i bBox(
+ Eigen::Vector2i(centerMapX - radiusCells, centerMapY - radiusCells),
+ Eigen::Vector2i(centerMapX + radiusCells, centerMapY + radiusCells));
+ Eigen::AlignedBox2i bBoxGrid(Eigen::Vector2i(0, 0),
+ Eigen::Vector2i(m_Width - 1, m_Height - 1));
+ bBox.clamp(bBoxGrid);
+
+ for (int y = bBox.min().y(); y <= bBox.max().y(); y++)
+ {
+ for (int x = bBox.min().x(); x <= bBox.max().x(); x++)
+ {
+ int xC = x - centerMapX;
+ int yC = y - centerMapY;
+ if (xC * xC + yC * yC <= radiusCells2)
+ {
+ setValue(x, y, value);
+ }
}
+ }
}
template <class DataT>
void GridMap<DataT>::drawPolygon(std::vector<Eigen::Vector2d> vertices,
- DataT value) {
- if (vertices.size() == 0) {
- ROS_INFO("No vertices given!");
- return;
- }
- // make temp. map
- DataT* data = new DataT[m_DataSize];
- for (int i = 0; i < m_DataSize; i++) {
- data[i] = 0;
- }
-
- // draw the lines surrounding the polygon
- for (unsigned int i = 0; i < vertices.size(); i++) {
- int i2 = (i + 1) % vertices.size();
- int startX, startY, endX, endY;
- worldToMap(vertices[i].x(), vertices[i].y(), startX, startY);
- worldToMap(vertices[i2].x(), vertices[i2].y(), endX, endY);
- drawLine(data, startX, startY, endX, endY, 1);
- }
- // claculate a point in the middle of the polygon
- float midX = 0;
- float midY = 0;
- for (unsigned int i = 0; i < vertices.size(); i++) {
- midX += vertices[i].x();
- midY += vertices[i].y();
- }
- midX /= vertices.size();
- midY /= vertices.size();
- int midMapX, midMapY;
- worldToMap(midX, midY, midMapX, midMapY);
- // fill polygon
- fillPolygon(data, midMapX, midMapY, 1);
-
- // copy polygon to map
- for (int i = 0; i < m_DataSize; i++) {
- if (data[i] != 0) {
- m_Data[i] = value;
- }
+ DataT value)
+{
+ if (vertices.size() == 0)
+ {
+ ROS_INFO("No vertices given!");
+ return;
+ }
+ // make temp. map
+ DataT* data = new DataT[m_DataSize];
+ for (int i = 0; i < m_DataSize; i++)
+ {
+ data[i] = 0;
+ }
+
+ // draw the lines surrounding the polygon
+ for (unsigned int i = 0; i < vertices.size(); i++)
+ {
+ int i2 = (i + 1) % vertices.size();
+ int startX, startY, endX, endY;
+ worldToMap(vertices[i].x(), vertices[i].y(), startX, startY);
+ worldToMap(vertices[i2].x(), vertices[i2].y(), endX, endY);
+ drawLine(data, startX, startY, endX, endY, 1);
+ }
+ // claculate a point in the middle of the polygon
+ float midX = 0;
+ float midY = 0;
+ for (unsigned int i = 0; i < vertices.size(); i++)
+ {
+ midX += vertices[i].x();
+ midY += vertices[i].y();
+ }
+ midX /= vertices.size();
+ midY /= vertices.size();
+ int midMapX, midMapY;
+ worldToMap(midX, midY, midMapX, midMapY);
+ // fill polygon
+ fillPolygon(data, midMapX, midMapY, 1);
+
+ // copy polygon to map
+ for (int i = 0; i < m_DataSize; i++)
+ {
+ if (data[i] != 0)
+ {
+ m_Data[i] = value;
}
+ }
- delete[] data;
+ delete[] data;
}
template <class DataT>
-void GridMap<DataT>::fillPolygon(DataT* data, int x, int y, char value) {
- int index = x + m_Width * y;
- if (value != data[index]) {
- data[index] = value;
- fillPolygon(data, x + 1, y, value);
- fillPolygon(data, x - 1, y, value);
- fillPolygon(data, x, y + 1, value);
- fillPolygon(data, x, y - 1, value);
- }
+void GridMap<DataT>::fillPolygon(DataT* data, int x, int y, char value)
+{
+ int index = x + m_Width * y;
+ if (value != data[index])
+ {
+ data[index] = value;
+ fillPolygon(data, x + 1, y, value);
+ fillPolygon(data, x - 1, y, value);
+ fillPolygon(data, x, y + 1, value);
+ fillPolygon(data, x, y - 1, value);
+ }
}
template <class DataT>
void GridMap<DataT>::drawLine(DataT* data, int startX, int startY, int endX,
- int endY, DataT value) {
- // bresenham algorithm
- int x, y, t, dist, xerr, yerr, dx, dy, incx, incy;
- // compute distances
- dx = endX - startX;
- dy = endY - startY;
-
- // compute increment
- if (dx < 0) {
- incx = -1;
- dx = -dx;
- } else {
- incx = dx ? 1 : 0;
- }
-
- if (dy < 0) {
- incy = -1;
- dy = -dy;
- } else {
- incy = dy ? 1 : 0;
+ int endY, DataT value)
+{
+ // bresenham algorithm
+ int x, y, t, dist, xerr, yerr, dx, dy, incx, incy;
+ // compute distances
+ dx = endX - startX;
+ dy = endY - startY;
+
+ // compute increment
+ if (dx < 0)
+ {
+ incx = -1;
+ dx = -dx;
+ }
+ else
+ {
+ incx = dx ? 1 : 0;
+ }
+
+ if (dy < 0)
+ {
+ incy = -1;
+ dy = -dy;
+ }
+ else
+ {
+ incy = dy ? 1 : 0;
+ }
+
+ // which distance is greater?
+ dist = (dx > dy) ? dx : dy;
+ // initializing
+ x = startX;
+ y = startY;
+ xerr = dx;
+ yerr = dy;
+
+ // compute cells
+ for (t = 0; t < dist; t++)
+ {
+ data[x + m_Width * y] = value;
+
+ xerr += dx;
+ yerr += dy;
+ if (xerr > dist)
+ {
+ xerr -= dist;
+ x += incx;
}
-
- // which distance is greater?
- dist = (dx > dy) ? dx : dy;
- // initializing
- x = startX;
- y = startY;
- xerr = dx;
- yerr = dy;
-
- // compute cells
- for (t = 0; t < dist; t++) {
- data[x + m_Width * y] = value;
-
- xerr += dx;
- yerr += dy;
- if (xerr > dist) {
- xerr -= dist;
- x += incx;
- }
- if (yerr > dist) {
- yerr -= dist;
- y += incy;
- }
+ if (yerr > dist)
+ {
+ yerr -= dist;
+ y += incy;
}
+ }
}
#endif
diff --git a/homer_nav_libs/include/homer_nav_libs/Math/Box2D.h b/homer_nav_libs/include/homer_nav_libs/Math/Box2D.h
index fc6a5478977767c7274dbc6851ec87bc3cf24c59..6ee1e18dbea5eeda0bfa7097d657d4c25397d8e8 100644
--- a/homer_nav_libs/include/homer_nav_libs/Math/Box2D.h
+++ b/homer_nav_libs/include/homer_nav_libs/Math/Box2D.h
@@ -19,118 +19,171 @@
* @author David Gossow (RX)
* @brief Represents a box given by the upper-left and lower-right corner
*/
-template<class T=float>
+template <class T = float>
class Box2D
{
-
- public:
-
- /** @brief Creates a box given by top-left (minX,minY) and lower-right (maxX,maxY) coordinates */
- Box2D(T minX=0, T minY=0, T maxX=0, T maxY=0);
-
- /** @brief The destructor */
- ~Box2D() {};
-
- inline void setMinX(T value) { m_MinX=value; }
- inline void setMaxX(T value) { m_MaxX=value; }
- inline void setMinY(T value) { m_MinY=value; }
- inline void setMaxY(T value) { m_MaxY=value; }
-
- inline T minX() const { return m_MinX; }
- inline T maxX() const { return m_MaxX; }
- inline T minY() const { return m_MinY; }
- inline T maxY() const { return m_MaxY; }
-
- inline T width() const { return m_MaxX-m_MinX; }
- inline T height() const { return m_MaxY-m_MinY; }
-
- std::vector< Point2D > vertices();
-
- /** @brief Clip the box to fit into clipArea */
- void clip( Box2D<T> clipArea );
-
- /** @return true if the given point is inside the box */
- bool contains( T x, T y );
-
- /** @brief enlarge the box by 'size' units in all directions */
- void expand( T size );
-
- /** @brief shrink the box by 'size' units in all directions */
- void shrink( T size );
-
- /** @brief expand the box so that it contains the given point */
- void enclose( Point2D point );
- void enclose( T x, T y );
-
- template<class OtherT>
- void enclose( Box2D<OtherT> box );
-
- Point2D centerPoint()
- {
- Point2D center;
- center.setX(m_MinX + ( (m_MaxX - m_MinX) / 2 ) );
- center.setY(m_MinY + ( (m_MaxY - m_MinY) / 2 ) );
- return center;
- }
-
- /** @brief area covered by the box */
- T area();
-
- Box2D<T>& operator/= ( T div ) { m_MinX/=div; m_MinY/=div; m_MaxX/=div; m_MaxY/=div; return *this; }
- Box2D<T>& operator*= ( T div ) { m_MinX*=div; m_MinY*=div; m_MaxX*=div; m_MaxY*=div; return *this; }
-
- private:
-
- T m_MinX;
- T m_MaxX;
- T m_MinY;
- T m_MaxY;
-
+public:
+ /** @brief Creates a box given by top-left (minX,minY) and lower-right
+ * (maxX,maxY) coordinates */
+ Box2D(T minX = 0, T minY = 0, T maxX = 0, T maxY = 0);
+
+ /** @brief The destructor */
+ ~Box2D(){};
+
+ inline void setMinX(T value)
+ {
+ m_MinX = value;
+ }
+ inline void setMaxX(T value)
+ {
+ m_MaxX = value;
+ }
+ inline void setMinY(T value)
+ {
+ m_MinY = value;
+ }
+ inline void setMaxY(T value)
+ {
+ m_MaxY = value;
+ }
+
+ inline T minX() const
+ {
+ return m_MinX;
+ }
+ inline T maxX() const
+ {
+ return m_MaxX;
+ }
+ inline T minY() const
+ {
+ return m_MinY;
+ }
+ inline T maxY() const
+ {
+ return m_MaxY;
+ }
+
+ inline T width() const
+ {
+ return m_MaxX - m_MinX;
+ }
+ inline T height() const
+ {
+ return m_MaxY - m_MinY;
+ }
+
+ std::vector<Point2D> vertices();
+
+ /** @brief Clip the box to fit into clipArea */
+ void clip(Box2D<T> clipArea);
+
+ /** @return true if the given point is inside the box */
+ bool contains(T x, T y);
+
+ /** @brief enlarge the box by 'size' units in all directions */
+ void expand(T size);
+
+ /** @brief shrink the box by 'size' units in all directions */
+ void shrink(T size);
+
+ /** @brief expand the box so that it contains the given point */
+ void enclose(Point2D point);
+ void enclose(T x, T y);
+
+ template <class OtherT>
+ void enclose(Box2D<OtherT> box);
+
+ Point2D centerPoint()
+ {
+ Point2D center;
+ center.setX(m_MinX + ((m_MaxX - m_MinX) / 2));
+ center.setY(m_MinY + ((m_MaxY - m_MinY) / 2));
+ return center;
+ }
+
+ /** @brief area covered by the box */
+ T area();
+
+ Box2D<T>& operator/=(T div)
+ {
+ m_MinX /= div;
+ m_MinY /= div;
+ m_MaxX /= div;
+ m_MaxY /= div;
+ return *this;
+ }
+ Box2D<T>& operator*=(T div)
+ {
+ m_MinX *= div;
+ m_MinY *= div;
+ m_MaxX *= div;
+ m_MaxY *= div;
+ return *this;
+ }
+
+private:
+ T m_MinX;
+ T m_MaxX;
+ T m_MinY;
+ T m_MaxY;
};
-template<class T>
+template <class T>
Box2D<T>::Box2D(T minX, T minY, T maxX, T maxY)
{
- m_MinX=minX;
- m_MinY=minY;
- m_MaxX=maxX;
- m_MaxY=maxY;
+ m_MinX = minX;
+ m_MinY = minY;
+ m_MaxX = maxX;
+ m_MaxY = maxY;
}
-template<class T>
-void Box2D<T>::clip( Box2D<T> clipArea )
+template <class T>
+void Box2D<T>::clip(Box2D<T> clipArea)
{
- if (m_MinX < clipArea.minX()) { m_MinX=clipArea.minX(); }
- if (m_MinY < clipArea.minY()) { m_MinY=clipArea.minY(); }
- if (m_MaxX > clipArea.maxX()) { m_MaxX=clipArea.maxX(); }
- if (m_MaxY > clipArea.maxY()) { m_MaxY=clipArea.maxY(); }
+ if (m_MinX < clipArea.minX())
+ {
+ m_MinX = clipArea.minX();
+ }
+ if (m_MinY < clipArea.minY())
+ {
+ m_MinY = clipArea.minY();
+ }
+ if (m_MaxX > clipArea.maxX())
+ {
+ m_MaxX = clipArea.maxX();
+ }
+ if (m_MaxY > clipArea.maxY())
+ {
+ m_MaxY = clipArea.maxY();
+ }
}
-template<class T>
-bool Box2D<T>::contains( T x, T y )
+template <class T>
+bool Box2D<T>::contains(T x, T y)
{
- return ( (x>=m_MinX) && (x<=m_MaxX) && (y>=m_MinY) && (y<=m_MaxY) );
+ return ((x >= m_MinX) && (x <= m_MaxX) && (y >= m_MinY) && (y <= m_MaxY));
}
-template<class T>
- void Box2D<T>::expand( T size )
+template <class T>
+void Box2D<T>::expand(T size)
{
- m_MinX-=size;
- m_MaxX+=size;
- m_MinY-=size;
- m_MaxY+=size;
+ m_MinX -= size;
+ m_MaxX += size;
+ m_MinY -= size;
+ m_MaxY += size;
}
-template<class T>
-void Box2D<T>::shrink( T size )
+template <class T>
+void Box2D<T>::shrink(T size)
{
- m_MinX+=size;
- m_MaxX-=size;
- m_MinY+=size;
- m_MaxY-=size;
+ m_MinX += size;
+ m_MaxX -= size;
+ m_MinY += size;
+ m_MaxY -= size;
}
-template<class T>
+template <class T>
T Box2D<T>::area()
{
T width = m_MaxX - m_MinX;
@@ -138,47 +191,69 @@ T Box2D<T>::area()
T capacity = width * height;
- return (T) capacity;
+ return (T)capacity;
}
-template<class T>
-void Box2D<T>::enclose( Point2D point )
+template <class T>
+void Box2D<T>::enclose(Point2D point)
{
- if ( m_MinX > point.x() ) { m_MinX=point.x(); }
- if ( m_MinY > point.y() ) { m_MinY=point.y(); }
- if ( m_MaxX < point.x() ) { m_MaxX=point.x(); }
- if ( m_MaxY < point.y() ) { m_MaxY=point.y(); }
+ if (m_MinX > point.x())
+ {
+ m_MinX = point.x();
+ }
+ if (m_MinY > point.y())
+ {
+ m_MinY = point.y();
+ }
+ if (m_MaxX < point.x())
+ {
+ m_MaxX = point.x();
+ }
+ if (m_MaxY < point.y())
+ {
+ m_MaxY = point.y();
+ }
}
-template<class T>
-void Box2D<T>::enclose( T x, T y )
+template <class T>
+void Box2D<T>::enclose(T x, T y)
{
- if ( m_MinX > x ) { m_MinX=x; }
- if ( m_MinY > y ) { m_MinY=y; }
- if ( m_MaxX < x ) { m_MaxX=x; }
- if ( m_MaxY < y ) { m_MaxY=y; }
+ if (m_MinX > x)
+ {
+ m_MinX = x;
+ }
+ if (m_MinY > y)
+ {
+ m_MinY = y;
+ }
+ if (m_MaxX < x)
+ {
+ m_MaxX = x;
+ }
+ if (m_MaxY < y)
+ {
+ m_MaxY = y;
+ }
}
-template<class T>
-template<class OtherT>
-void Box2D<T>::enclose( Box2D<OtherT> box )
+template <class T>
+template <class OtherT>
+void Box2D<T>::enclose(Box2D<OtherT> box)
{
- enclose( box.minX(), box.minY() );
- enclose( box.maxX(), box.maxY() );
+ enclose(box.minX(), box.minY());
+ enclose(box.maxX(), box.maxY());
}
-
-template<class T>
-std::vector< Point2D > Box2D<T>::vertices()
+template <class T>
+std::vector<Point2D> Box2D<T>::vertices()
{
std::vector<Point2D> myVertices(5);
- myVertices[0]=Point2D( m_MinX-0.5, m_MinY-0.5 );
- myVertices[1]=Point2D( m_MinX-0.5, m_MaxY+0.5 );
- myVertices[2]=Point2D( m_MaxX+0.5, m_MaxY+0.5 );
- myVertices[3]=Point2D( m_MaxX+0.5, m_MinY-0.5 );
- myVertices[4]=myVertices[0];
+ myVertices[0] = Point2D(m_MinX - 0.5, m_MinY - 0.5);
+ myVertices[1] = Point2D(m_MinX - 0.5, m_MaxY + 0.5);
+ myVertices[2] = Point2D(m_MaxX + 0.5, m_MaxY + 0.5);
+ myVertices[3] = Point2D(m_MaxX + 0.5, m_MinY - 0.5);
+ myVertices[4] = myVertices[0];
return myVertices;
}
-
#endif
diff --git a/homer_nav_libs/include/homer_nav_libs/Math/Line2D.h b/homer_nav_libs/include/homer_nav_libs/Math/Line2D.h
index 9761be66439a30872edcce7a1f91535971a7f179..c7e730fa29fae012611edbf084c20b26b78fd623 100644
--- a/homer_nav_libs/include/homer_nav_libs/Math/Line2D.h
+++ b/homer_nav_libs/include/homer_nav_libs/Math/Line2D.h
@@ -14,9 +14,9 @@
#ifndef LINE2D_H
#define LINE2D_H
-#include <homer_nav_libs/Math/vec2.h>
-#include <homer_nav_libs/Math/mat2.h>
#include <homer_nav_libs/Math/Point2D.h>
+#include <homer_nav_libs/Math/mat2.h>
+#include <homer_nav_libs/Math/vec2.h>
#include <vector>
/**
@@ -27,197 +27,199 @@
*/
class Line2D
{
-
- public:
-
- /**
- * Creates a new line.
- * @param start Start point of the line.
- * @param end End point of the line.
- */
- inline Line2D ( Point2D start, Point2D end )
- {
- m_Start = start;
- m_Vec = end-m_Start;
- }
-
-
- /**
- * Creates a new line.
- * @param start Start point of the line.
- * @param vec The vector from start to end point of the line.
- */
- inline Line2D ( Point2D start, CVec2 vec )
- {
- m_Start = start;
- m_Vec = vec;
- }
-
- /**
- * Destructor does nothing.
- */
- inline ~Line2D() {}
-
- /**
- * Sets the start point of the line to a new value.
- * @param start Start point of the line.
- */
- inline void setStart ( const Point2D start )
- {
- Point2D end = m_Start + m_Vec;
- m_Start = start;
- m_Vec = end-m_Start;
- }
-
- /**
- * Sets the end point of the line to a new value.
- * @param end End point of the line.
- */
-
- inline void setEnd ( const Point2D end )
- {
- m_Vec = end - m_Start;
- }
-
- /**
- * Returns the start point of the line.
- * @return Start point of the line.
- */
- inline Point2D start() const
- {
- return m_Start;
- }
-
- /**
- * Returns the end point of the line.
- * @return End point of the line.
- */
- inline Point2D end() const
- {
- return m_Start + m_Vec;
- }
-
- /**
- * Returns a vector from the start to the end of the line.
- * @return Vector to the end point of the line.
- */
- inline CVec2 vec() const
- {
- return m_Vec;
- }
-
- inline bool operator== ( const Line2D& line ) const
- {
- return ( m_Start == line.start() && end() == line.end() );
- }
-
- /**
- * Returns the gradient of the line.
- * @return Gradient of the line.
- */
- float gradient() const ;
-
- /**
- * Returns the length of the line.
- * @return Length of the line.
- */
- inline float length() const {
- return m_Vec.magnitude();
- }
-
- /**
- * Returns the minimum euclidean distance of the given point to the line.
- * @param point Point of which the distance to the line will be calculated.
- * @return Distance of point to line.
- */
- inline float distance ( Point2D point ) const
- {
- Point2D pointOnLine = getRootPoint ( point );
- return ( point - pointOnLine ).magnitude();
- }
-
- /**
- * Rotates the line round the origin.
- * @param angle The angle of rotation in radiants.
- */
- inline void rotate ( float angle )
- {
- CMat2 rotMat = CMat2 ( angle );
- m_Start = rotMat * m_Start;
- m_Vec = rotMat * m_Vec;
- }
-
- /**
- * Returns the point of the line with the minimal distance to a given point.
- * This algorithm may return a point which lies on the extension of the line and not on itself.
- * See also: getClosestPoint.
- * @param point Point to which the distance is calculated.
- * @return Root point of the line.
- */
- inline Point2D getRootPoint ( Point2D point ) const
- {
- float t = ( point-m_Start ) * m_Vec;
- t /= m_Vec * m_Vec;
- Point2D pointOnLine = m_Start + ( t * m_Vec );
- return pointOnLine;
- }
-
- /**
- * Returns the normal of the line.
- * @return Normal of the line.
- */
- inline CVec2 getNormal() const {
- return m_Vec.getNormal()/m_Vec.magnitude();
- }
-
- /**
- * Returns the point of the line with the minimal distance to a given point.
- * This algorithm returns always a point which lies on the line.
- * Therefor it is not always the root point.
- * See also: getRootPoint.
- * @param point Point to which the distance is calculated.
- * @return Root point of the line.
- */
- Point2D getClosestPoint ( Point2D point ) const;
-
- /**
- * Returns the intersection point of this line with a second line.
- * The intersection point is element of this line.
- * @param line The line with which the intersection is calculated.
- * @return Intersection point.
- */
- Point2D getIntersectionPoint ( Line2D line ) const;
-
- /**
- * Returns the parameter t which identifies the intersection point of this line with a second line.
- * The intersection point is element of this line.
- * @param line The line with which the intersection is calculated.
- * @return Parameter t which identifies the intersection point on the line.
- */
- float getIntersectionPointParameter ( Line2D line ) const;
-
- /**
- * @return Vertices, e.g. for use in a VectorObject
- * @param substeps number of linear interpolation steps between start and end
- */
- std::vector< Point2D > vertices ( unsigned substeps=0 );
-
- /**
- * @overwrite
- */
- std::string toString() const;
-
- private:
-
- /**
- * Start point of the line.
- */
- Point2D m_Start;
-
- /**
- * Vector from the start to the end of the line.
- */
- CVec2 m_Vec;
+public:
+ /**
+ * Creates a new line.
+ * @param start Start point of the line.
+ * @param end End point of the line.
+ */
+ inline Line2D(Point2D start, Point2D end)
+ {
+ m_Start = start;
+ m_Vec = end - m_Start;
+ }
+
+ /**
+ * Creates a new line.
+ * @param start Start point of the line.
+ * @param vec The vector from start to end point of the line.
+ */
+ inline Line2D(Point2D start, CVec2 vec)
+ {
+ m_Start = start;
+ m_Vec = vec;
+ }
+
+ /**
+ * Destructor does nothing.
+ */
+ inline ~Line2D()
+ {
+ }
+
+ /**
+ * Sets the start point of the line to a new value.
+ * @param start Start point of the line.
+ */
+ inline void setStart(const Point2D start)
+ {
+ Point2D end = m_Start + m_Vec;
+ m_Start = start;
+ m_Vec = end - m_Start;
+ }
+
+ /**
+ * Sets the end point of the line to a new value.
+ * @param end End point of the line.
+ */
+
+ inline void setEnd(const Point2D end)
+ {
+ m_Vec = end - m_Start;
+ }
+
+ /**
+ * Returns the start point of the line.
+ * @return Start point of the line.
+ */
+ inline Point2D start() const
+ {
+ return m_Start;
+ }
+
+ /**
+ * Returns the end point of the line.
+ * @return End point of the line.
+ */
+ inline Point2D end() const
+ {
+ return m_Start + m_Vec;
+ }
+
+ /**
+ * Returns a vector from the start to the end of the line.
+ * @return Vector to the end point of the line.
+ */
+ inline CVec2 vec() const
+ {
+ return m_Vec;
+ }
+
+ inline bool operator==(const Line2D& line) const
+ {
+ return (m_Start == line.start() && end() == line.end());
+ }
+
+ /**
+ * Returns the gradient of the line.
+ * @return Gradient of the line.
+ */
+ float gradient() const;
+
+ /**
+ * Returns the length of the line.
+ * @return Length of the line.
+ */
+ inline float length() const
+ {
+ return m_Vec.magnitude();
+ }
+
+ /**
+ * Returns the minimum euclidean distance of the given point to the line.
+ * @param point Point of which the distance to the line will be calculated.
+ * @return Distance of point to line.
+ */
+ inline float distance(Point2D point) const
+ {
+ Point2D pointOnLine = getRootPoint(point);
+ return (point - pointOnLine).magnitude();
+ }
+
+ /**
+ * Rotates the line round the origin.
+ * @param angle The angle of rotation in radiants.
+ */
+ inline void rotate(float angle)
+ {
+ CMat2 rotMat = CMat2(angle);
+ m_Start = rotMat * m_Start;
+ m_Vec = rotMat * m_Vec;
+ }
+
+ /**
+ * Returns the point of the line with the minimal distance to a given point.
+ * This algorithm may return a point which lies on the extension of the line
+ * and not on itself.
+ * See also: getClosestPoint.
+ * @param point Point to which the distance is calculated.
+ * @return Root point of the line.
+ */
+ inline Point2D getRootPoint(Point2D point) const
+ {
+ float t = (point - m_Start) * m_Vec;
+ t /= m_Vec * m_Vec;
+ Point2D pointOnLine = m_Start + (t * m_Vec);
+ return pointOnLine;
+ }
+
+ /**
+ * Returns the normal of the line.
+ * @return Normal of the line.
+ */
+ inline CVec2 getNormal() const
+ {
+ return m_Vec.getNormal() / m_Vec.magnitude();
+ }
+
+ /**
+ * Returns the point of the line with the minimal distance to a given point.
+ * This algorithm returns always a point which lies on the line.
+ * Therefor it is not always the root point.
+ * See also: getRootPoint.
+ * @param point Point to which the distance is calculated.
+ * @return Root point of the line.
+ */
+ Point2D getClosestPoint(Point2D point) const;
+
+ /**
+ * Returns the intersection point of this line with a second line.
+ * The intersection point is element of this line.
+ * @param line The line with which the intersection is calculated.
+ * @return Intersection point.
+ */
+ Point2D getIntersectionPoint(Line2D line) const;
+
+ /**
+ * Returns the parameter t which identifies the intersection point of this
+ * line with a second line.
+ * The intersection point is element of this line.
+ * @param line The line with which the intersection is calculated.
+ * @return Parameter t which identifies the intersection point on the line.
+ */
+ float getIntersectionPointParameter(Line2D line) const;
+
+ /**
+ * @return Vertices, e.g. for use in a VectorObject
+ * @param substeps number of linear interpolation steps between start and end
+ */
+ std::vector<Point2D> vertices(unsigned substeps = 0);
+
+ /**
+ * @overwrite
+ */
+ std::string toString() const;
+
+private:
+ /**
+ * Start point of the line.
+ */
+ Point2D m_Start;
+
+ /**
+ * Vector from the start to the end of the line.
+ */
+ CVec2 m_Vec;
};
#endif
diff --git a/homer_nav_libs/include/homer_nav_libs/Math/Math.h b/homer_nav_libs/include/homer_nav_libs/Math/Math.h
index d1ce8e65db79a0fdef434e8e1480ac2f295f7d13..caf63a4b145985dd3dab936c34964d43bb495986 100644
--- a/homer_nav_libs/include/homer_nav_libs/Math/Math.h
+++ b/homer_nav_libs/include/homer_nav_libs/Math/Math.h
@@ -11,8 +11,8 @@
#ifndef Math_H
#define Math_H
-#include <vector>
#include <homer_nav_libs/Math/Point2D.h>
+#include <vector>
/**
* @class Math
@@ -21,109 +21,131 @@
*/
class Math
{
- public:
-
- struct WeightedValue
- {
- float value;
- float weight;
- };
-
- static const double Pi = 3.14159265358979323846;
-
- /** @return mean value */
- template<class ContainerT>
- static double mean ( const ContainerT& values );
+public:
+ struct WeightedValue
+ {
+ float value;
+ float weight;
+ };
- /** @return variance of given values */
- template<class ContainerT>
- static double variance ( const ContainerT& values );
+ static const double Pi = 3.14159265358979323846;
- /** @return mean angle of given values
- * @note there are always two possible choices for the mean angle. This function returns the one with the smallest deviation
- * @note Works for angles in [-Pi..Pi], negative angles are treated
- */
- static float meanAngle ( const std::vector<float>& angles );
+ /** @return mean value */
+ template <class ContainerT>
+ static double mean(const ContainerT& values);
- static float meanAngleWeighted ( const std::vector< WeightedValue >& weightedAngles );
+ /** @return variance of given values */
+ template <class ContainerT>
+ static double variance(const ContainerT& values);
- /** @return variance for given mean */
- static float angleVariance ( float meanAngle, const std::vector<float>& angles );
+ /** @return mean angle of given values
+ * @note there are always two possible choices for the mean angle. This
+ * function returns the one with the smallest deviation
+ * @note Works for angles in [-Pi..Pi], negative angles are treated
+ */
+ static float meanAngle(const std::vector<float>& angles);
- /** @return minimal angle needed to turn from angle 1 to angle 2 [-Pi..Pi] */
- static float minTurnAngle ( float angle1, float angle2 );
+ static float
+ meanAngleWeighted(const std::vector<WeightedValue>& weightedAngles);
- static Point2D center ( std::vector<Point2D>& points );
+ /** @return variance for given mean */
+ static float angleVariance(float meanAngle, const std::vector<float>& angles);
- static float deg2Rad ( float deg ) { return deg / 180.0*Pi; }
+ /** @return minimal angle needed to turn from angle 1 to angle 2 [-Pi..Pi] */
+ static float minTurnAngle(float angle1, float angle2);
- static float rad2Deg ( float rad ) { return rad / Pi*180.0; }
+ static Point2D center(std::vector<Point2D>& points);
- static double randomGauss ( float variance = 1.0 );
+ static float deg2Rad(float deg)
+ {
+ return deg / 180.0 * Pi;
+ }
- static double random01 ( unsigned long init = 0 );
+ static float rad2Deg(float rad)
+ {
+ return rad / Pi * 180.0;
+ }
- /** @return ratio between one dimension seen under old viewangle and dimension under new viewangle*/
- static double angleToPercent ( double newAngle, double oldAngle ) { return tan ( ( Pi / 180.0 ) * newAngle / 2 ) / tan ( ( Pi / 180.0 ) * oldAngle / 2 ); };
+ static double randomGauss(float variance = 1.0);
- /** @return angle under which the ratio between dimension seen under old viewangle and new viewangle equals percent*/
- static double percentToAngle ( double percent, double angle ) { return 2* atan ( tan ( ( Pi / 180.0 ) * angle / 2 ) * percent ) * ( 180 / Pi ); };
+ static double random01(unsigned long init = 0);
- /** @return horizontal view angle corresponding to diagonal view angle and aspect ratio (e.g. 4.0/3.0)*/
- static double horizontalViewAngle ( double diagonalAngle, double aspectRatio ) { return verticalViewAngle ( diagonalAngle, 1.0 / aspectRatio ); };
+ /** @return ratio between one dimension seen under old viewangle and dimension
+ * under new viewangle*/
+ static double angleToPercent(double newAngle, double oldAngle)
+ {
+ return tan((Pi / 180.0) * newAngle / 2) / tan((Pi / 180.0) * oldAngle / 2);
+ };
- /** @return vertical view angle corresponding to diagonal view angle and aspect ratio (e.g. 4.0/3.0)*/
- static double verticalViewAngle ( double diagonalAngle, double aspectRatio )
- {
- return percentToAngle ( 1.0 / sqrt ( pow ( aspectRatio, 2 ) + 1.0 ), diagonalAngle );
- };
+ /** @return angle under which the ratio between dimension seen under old
+ * viewangle and new viewangle equals percent*/
+ static double percentToAngle(double percent, double angle)
+ {
+ return 2 * atan(tan((Pi / 180.0) * angle / 2) * percent) * (180 / Pi);
+ };
- template<class ValueT>
- static inline ValueT min ( ValueT a, ValueT b ) { return a < b ? a : b; }
+ /** @return horizontal view angle corresponding to diagonal view angle and
+ * aspect ratio (e.g. 4.0/3.0)*/
+ static double horizontalViewAngle(double diagonalAngle, double aspectRatio)
+ {
+ return verticalViewAngle(diagonalAngle, 1.0 / aspectRatio);
+ };
- template<class ValueT>
- static inline ValueT max ( ValueT a, ValueT b ) { return a > b ? a : b; }
+ /** @return vertical view angle corresponding to diagonal view angle and
+ * aspect ratio (e.g. 4.0/3.0)*/
+ static double verticalViewAngle(double diagonalAngle, double aspectRatio)
+ {
+ return percentToAngle(1.0 / sqrt(pow(aspectRatio, 2) + 1.0), diagonalAngle);
+ };
- private:
+ template <class ValueT>
+ static inline ValueT min(ValueT a, ValueT b)
+ {
+ return a < b ? a : b;
+ }
- /** @brief The constructor */
- Math();
+ template <class ValueT>
+ static inline ValueT max(ValueT a, ValueT b)
+ {
+ return a > b ? a : b;
+ }
- /** @brief The destructor */
- ~Math();
+private:
+ /** @brief The constructor */
+ Math();
+ /** @brief The destructor */
+ ~Math();
};
-template<class ContainerT>
-double Math::mean ( const ContainerT& values )
+template <class ContainerT>
+double Math::mean(const ContainerT& values)
{
typename ContainerT::const_iterator it;
it = values.begin();
double sum = 0;
- while ( it != values.end() )
+ while (it != values.end())
{
sum += *it;
it++;
}
- return sum / double ( values.size() );
+ return sum / double(values.size());
}
-
-template<class ContainerT>
-double Math::variance ( const ContainerT& values )
+template <class ContainerT>
+double Math::variance(const ContainerT& values)
{
- double mean = mean ( values );
+ double mean = mean(values);
typename ContainerT::const_iterator it;
it = values.begin();
double sum = 0;
- while ( it != values.end() )
+ while (it != values.end())
{
double diff = *it - mean;
sum += diff * diff;
it++;
}
- return sum / double ( values.size() );
+ return sum / double(values.size());
}
-
#endif
diff --git a/homer_nav_libs/include/homer_nav_libs/Math/Pixel.h b/homer_nav_libs/include/homer_nav_libs/Math/Pixel.h
index cc58f229a0cfc4edc11eced1275f42f209a361ed..a5eb3a6c17d3320966e5ad4de1cdcc777138ff9c 100644
--- a/homer_nav_libs/include/homer_nav_libs/Math/Pixel.h
+++ b/homer_nav_libs/include/homer_nav_libs/Math/Pixel.h
@@ -17,43 +17,70 @@
* @brief Stores discrete pixel coordinates
* @author Stephan Wirth, David Gossow (RX)
*/
-class Pixel {
+class Pixel
+{
+public:
+ inline Pixel(int x = 0, int y = 0)
+ {
+ m_X = x;
+ m_Y = y;
+ };
+ inline ~Pixel(){};
- public:
+ inline int x() const
+ {
+ return m_X;
+ }
+ inline int y() const
+ {
+ return m_Y;
+ }
- inline Pixel( int x = 0, int y = 0) { m_X = x; m_Y = y; };
- inline ~Pixel() {};
+ inline void setX(int x)
+ {
+ m_X = x;
+ }
+ inline void setY(int y)
+ {
+ m_Y = y;
+ }
- inline int x() const { return m_X; }
- inline int y() const { return m_Y; }
+ inline bool operator==(Pixel& rhs)
+ {
+ return (m_X == rhs.m_X) && (m_Y == rhs.m_Y);
+ }
+ inline bool operator!=(Pixel& rhs)
+ {
+ return (m_X != rhs.m_X) || (m_Y != rhs.m_Y);
+ }
+ inline Pixel operator*(float rhs)
+ {
+ return Pixel(m_X * rhs, m_Y * rhs);
+ }
+ inline Pixel operator/(float rhs)
+ {
+ return Pixel(m_X / rhs, m_Y / rhs);
+ }
- inline void setX( int x ) { m_X = x; }
- inline void setY( int y ) { m_Y = y; }
+ inline Point2D toPoint2D()
+ {
+ return Point2D(m_X, m_Y);
+ }
- inline bool operator ==( Pixel& rhs ) { return ( m_X == rhs.m_X ) && ( m_Y == rhs.m_Y ); }
- inline bool operator !=( Pixel& rhs ) { return ( m_X != rhs.m_X ) || ( m_Y != rhs.m_Y ); }
- inline Pixel operator *( float rhs ) { return Pixel( m_X * rhs, m_Y * rhs ); }
- inline Pixel operator /( float rhs ) { return Pixel( m_X / rhs, m_Y / rhs ); }
-
- inline Point2D toPoint2D() { return Point2D( m_X, m_Y ); }
-
- inline std::vector<Point2D> vertices()
- {
- std::vector<Point2D> result(5);
- result[0]=Point2D( m_X-0.5, m_Y-0.5 );
- result[1]=Point2D( m_X+0.5, m_Y-0.5 );
- result[2]=Point2D( m_X+0.5, m_Y+0.5 );
- result[3]=Point2D( m_X-0.5, m_Y+0.5 );
- result[4]=result[0];
- return result;
- }
-
- private:
-
- int m_X;
- int m_Y;
+ inline std::vector<Point2D> vertices()
+ {
+ std::vector<Point2D> result(5);
+ result[0] = Point2D(m_X - 0.5, m_Y - 0.5);
+ result[1] = Point2D(m_X + 0.5, m_Y - 0.5);
+ result[2] = Point2D(m_X + 0.5, m_Y + 0.5);
+ result[3] = Point2D(m_X - 0.5, m_Y + 0.5);
+ result[4] = result[0];
+ return result;
+ }
+private:
+ int m_X;
+ int m_Y;
};
-
#endif
diff --git a/homer_nav_libs/include/homer_nav_libs/Math/Point2D.h b/homer_nav_libs/include/homer_nav_libs/Math/Point2D.h
index fdbf6fd0564580b686f501d678ad429fddd1722c..a5df65aa6901113ddaf203a74dcfb4294d5eef2d 100644
--- a/homer_nav_libs/include/homer_nav_libs/Math/Point2D.h
+++ b/homer_nav_libs/include/homer_nav_libs/Math/Point2D.h
@@ -10,8 +10,8 @@
#include <iostream>
#include <sstream>
-#include <homer_nav_libs/Math/vec2.h>
#include <float.h>
+#include <homer_nav_libs/Math/vec2.h>
#ifndef POINT2D_H
#define POINT2D_H
@@ -24,289 +24,293 @@
*/
class Point2D
{
-
- public:
-
- /**
- * Creates a new point in 2D with x- and y-coordinat set to zero.
- */
- inline Point2D()
- {
- m_X = 0.0;
- m_Y = 0.0;
- }
-
- /**
- * Creates a new point in 2D.
- * @param x x-coordinate of the point.
- * @param y y-coordinate of the point.
- */
- inline Point2D ( double x, double y )
- {
- m_X = x;
- m_Y = y;
- }
-
-
-
- /**
- * Copy construcor
- */
- inline Point2D (const Point2D& p){
- m_X = p.x();
- m_Y = p.y();
- }
-
- /**
- * Creates a new point in 2D.
- * @param v Vector form origin to the point.
- */
- inline Point2D ( const CVec2& v )
- {
- m_X = v[0];
- m_Y = v[1];
- }
-
- /**
- * Destructor, does nothing.
- */
- inline ~Point2D()
- {
- }
-
- /**
- * Returns the x-coordinate of the point.
- * @return the x-coordinate of the point.
- */
- inline double x() const
- {
- return m_X;
- }
-
- /**
- * Returns the y-coordinate of the point.
- * @return the y-coordinate of the point.
- */
- inline double y() const
- {
- return m_Y;
- }
-
- /**
- * Sets the x- and y-coordinate of the point to new values.
- * @param x the new value of the x coordinate.
- * @param y the new value of the x coordinate.
- */
- inline void set ( double x, double y )
- {
- m_X = x;
- m_Y = y;
- }
-
- /**
- * Sets the x-coordinate of the point to a new value.
- * @param x the new value of the x coordinate.
- */
- inline void setX ( double x )
- {
- m_X = x;
- }
-
- /**
- * Sets the y-coordinate of the point to a new value.
- * @param y the new value of the x coordinate.
- */
- inline void setY ( double y )
- {
- m_Y = y;
- }
-
- /**
- * Overloaded operators.
- */
-
- inline Point2D& operator= ( const Point2D& p) {
- m_X = p.x();
- m_Y = p.y();
- return *this;
- }
-
- inline Point2D operator+ ( const CVec2& v ) const
- {
- return Point2D ( m_X + v[0], m_Y + v[1] );
- }
-
- inline Point2D operator+ ( const Point2D& p ) const
- {
- return Point2D ( m_X + p.x(), m_Y + p.y() );
- }
-
- inline CVec2 operator- ( const Point2D& p ) const
- {
- return CVec2 ( m_X - p.x(), m_Y - p.y() );
- }
-
- inline Point2D operator- ( const CVec2& v ) const
- {
- return Point2D ( m_X - v[0], m_Y - v[1] );
- }
-
- inline Point2D operator* ( double scalar ) const
- {
- return Point2D ( m_X * scalar, m_Y * scalar );
- }
-
- inline Point2D operator/ ( double scalar ) const
- {
- return Point2D ( m_X / scalar, m_Y / scalar );
- }
-
- inline Point2D& operator+= ( const CVec2& v )
- {
- m_X += v[0];
- m_Y += v[1];
- return ( *this );
- }
-
- inline Point2D& operator-= ( const CVec2& v )
- {
- m_X -= v[0];
- m_Y -= v[1];
- return ( *this );
- }
-
- inline Point2D& operator*= ( double scalar )
- {
- m_X *= scalar;
- m_Y *= scalar;
- return ( *this );
- }
-
- inline Point2D& operator/= ( double scalar )
- {
- m_X /= scalar;
- m_Y /= scalar;
- return ( *this );
- }
-
- inline double operator [] ( unsigned int i ) const
- {
- return ( ( double* ) this ) [i];
- }
-
- inline double& operator [] ( unsigned int i )
- {
- return ( ( double* ) this ) [i];
- }
-
- inline bool operator== ( const Point2D& point ) const
- {
- return ( fabs(m_X - point.x()) < 0.001 && fabs(m_Y - point.y()) < 0.001 );
- }
-
- inline bool operator!= ( const Point2D& point ) const
+public:
+ /**
+ * Creates a new point in 2D with x- and y-coordinat set to zero.
+ */
+ inline Point2D()
+ {
+ m_X = 0.0;
+ m_Y = 0.0;
+ }
+
+ /**
+ * Creates a new point in 2D.
+ * @param x x-coordinate of the point.
+ * @param y y-coordinate of the point.
+ */
+ inline Point2D(double x, double y)
+ {
+ m_X = x;
+ m_Y = y;
+ }
+
+ /**
+ * Copy construcor
+ */
+ inline Point2D(const Point2D& p)
+ {
+ m_X = p.x();
+ m_Y = p.y();
+ }
+
+ /**
+ * Creates a new point in 2D.
+ * @param v Vector form origin to the point.
+ */
+ inline Point2D(const CVec2& v)
+ {
+ m_X = v[0];
+ m_Y = v[1];
+ }
+
+ /**
+ * Destructor, does nothing.
+ */
+ inline ~Point2D()
+ {
+ }
+
+ /**
+ * Returns the x-coordinate of the point.
+ * @return the x-coordinate of the point.
+ */
+ inline double x() const
+ {
+ return m_X;
+ }
+
+ /**
+ * Returns the y-coordinate of the point.
+ * @return the y-coordinate of the point.
+ */
+ inline double y() const
+ {
+ return m_Y;
+ }
+
+ /**
+ * Sets the x- and y-coordinate of the point to new values.
+ * @param x the new value of the x coordinate.
+ * @param y the new value of the x coordinate.
+ */
+ inline void set(double x, double y)
+ {
+ m_X = x;
+ m_Y = y;
+ }
+
+ /**
+ * Sets the x-coordinate of the point to a new value.
+ * @param x the new value of the x coordinate.
+ */
+ inline void setX(double x)
+ {
+ m_X = x;
+ }
+
+ /**
+ * Sets the y-coordinate of the point to a new value.
+ * @param y the new value of the x coordinate.
+ */
+ inline void setY(double y)
+ {
+ m_Y = y;
+ }
+
+ /**
+ * Overloaded operators.
+ */
+
+ inline Point2D& operator=(const Point2D& p)
+ {
+ m_X = p.x();
+ m_Y = p.y();
+ return *this;
+ }
+
+ inline Point2D operator+(const CVec2& v) const
+ {
+ return Point2D(m_X + v[0], m_Y + v[1]);
+ }
+
+ inline Point2D operator+(const Point2D& p) const
+ {
+ return Point2D(m_X + p.x(), m_Y + p.y());
+ }
+
+ inline CVec2 operator-(const Point2D& p) const
+ {
+ return CVec2(m_X - p.x(), m_Y - p.y());
+ }
+
+ inline Point2D operator-(const CVec2& v) const
+ {
+ return Point2D(m_X - v[0], m_Y - v[1]);
+ }
+
+ inline Point2D operator*(double scalar) const
+ {
+ return Point2D(m_X * scalar, m_Y * scalar);
+ }
+
+ inline Point2D operator/(double scalar) const
+ {
+ return Point2D(m_X / scalar, m_Y / scalar);
+ }
+
+ inline Point2D& operator+=(const CVec2& v)
+ {
+ m_X += v[0];
+ m_Y += v[1];
+ return (*this);
+ }
+
+ inline Point2D& operator-=(const CVec2& v)
+ {
+ m_X -= v[0];
+ m_Y -= v[1];
+ return (*this);
+ }
+
+ inline Point2D& operator*=(double scalar)
+ {
+ m_X *= scalar;
+ m_Y *= scalar;
+ return (*this);
+ }
+
+ inline Point2D& operator/=(double scalar)
+ {
+ m_X /= scalar;
+ m_Y /= scalar;
+ return (*this);
+ }
+
+ inline double operator[](unsigned int i) const
+ {
+ return ((double*)this)[i];
+ }
+
+ inline double& operator[](unsigned int i)
+ {
+ return ((double*)this)[i];
+ }
+
+ inline bool operator==(const Point2D& point) const
+ {
+ return (fabs(m_X - point.x()) < 0.001 && fabs(m_Y - point.y()) < 0.001);
+ }
+
+ inline bool operator!=(const Point2D& point) const
+ {
+ return !((*this) == point);
+ }
+
+ /**
+ * Returns the distance to a given point.
+ * @param point The point to calculate the distance to.
+ * @return the distance between point the two points.
+ */
+ inline double distance(const Point2D& point) const
+ {
+ return sqrt((m_X - point.x()) * (m_X - point.x()) +
+ (m_Y - point.y()) * (m_Y - point.y()));
+ }
+
+ /**
+ * Returns the distance to origin.
+ * @return the distance between point the two points.
+ */
+ inline double distance() const
+ {
+ return sqrt(m_X * m_X + m_Y * m_Y);
+ }
+
+ /**
+ * Checks whether two points are equal.
+ * @param p The point to check equality.
+ * @return true if points are equal, false otherwise.
+ */
+ inline bool equal(const Point2D& point) const
+ {
+ if ((*this - point).magnitude() < 0.0001)
{
- return !((*this)== point);
+ return true;
}
-
- /**
- * Returns the distance to a given point.
- * @param point The point to calculate the distance to.
- * @return the distance between point the two points.
- */
- inline double distance ( const Point2D& point ) const
+ else
{
- return sqrt ( ( m_X-point.x() ) * ( m_X-point.x() ) + ( m_Y-point.y() ) * ( m_Y-point.y() ) );
- }
-
- /**
- * Returns the distance to origin.
- * @return the distance between point the two points.
- */
- inline double distance ( ) const
- {
- return sqrt ( m_X * m_X + m_Y * m_Y );
- }
-
- /**
- * Checks whether two points are equal.
- * @param p The point to check equality.
- * @return true if points are equal, false otherwise.
- */
- inline bool equal ( const Point2D& point ) const
- {
- if ( ( *this - point ).magnitude() < 0.0001 )
- {
- return true;
- }
- else
- {
- return false;
- }
- }
-
- /**
- * Returns the vector which represents the point in 2D.
- * @return vector which represents the point in 2D.
- */
- inline CVec2 toVector() const
- {
- return CVec2 ( m_X, m_Y );
- }
-
- /**
- * Returns the angle of the corresponding polar coordinates.
- * @return polar angle.
- */
- float getPolarAngle () const;
-
- /**
- * Rotate by angle (in radiants) around center.
- * @param center Center of rotation
- * @param angle Angle in radiants
- */
- inline void rotate ( const Point2D& center, float angle )
- {
- double x0=m_X-center.m_X;
- double y0=m_Y-center.m_Y;
- double xRot = x0*cos ( angle ) - y0*sin ( angle );
- double yRot = x0*sin ( angle ) + y0*cos ( angle );
- m_X = xRot+center.m_X;
- m_Y = yRot+center.m_Y;
- }
-
- /**
- * Rotate by angle (in radiants) around (0,0).
- * @param angle Angle in radiants
- */
- inline void rotate ( float angle )
- {
- double xRot = m_X*cos ( angle ) - m_Y*sin ( angle );
- double yRot = m_X*sin ( angle ) + m_Y*cos ( angle );
- m_X = xRot;
- m_Y = yRot;
- }
-
- /**
- * Returns the string representation of the point.
- * @return string representation of the point.
- */
- inline std::string toString() const
- {
- std::ostringstream str;
- str << m_X << " " << m_Y;
- return str.str();
- }
-
- /** @return "invalid" Point (used as end marker in vector drawings) **/
- static Point2D invalidPoint() { return Point2D( DBL_MAX, DBL_MAX ); }
-
- bool isValid() { return ( ( m_X != DBL_MAX ) || ( m_Y != DBL_MAX ) ); }
-
- protected:
-
- double m_X;
- double m_Y;
+ return false;
+ }
+ }
+
+ /**
+ * Returns the vector which represents the point in 2D.
+ * @return vector which represents the point in 2D.
+ */
+ inline CVec2 toVector() const
+ {
+ return CVec2(m_X, m_Y);
+ }
+
+ /**
+*Returns the angle of the corresponding polar coordinates.
+*@return polar angle.
+ */
+ float getPolarAngle() const;
+
+ /**
+ * Rotate by angle (in radiants) around center.
+ * @param center Center of rotation
+ * @param angle Angle in radiants
+ */
+ inline void rotate(const Point2D& center, float angle)
+ {
+ double x0 = m_X - center.m_X;
+ double y0 = m_Y - center.m_Y;
+ double xRot = x0 * cos(angle) - y0 * sin(angle);
+ double yRot = x0 * sin(angle) + y0 * cos(angle);
+ m_X = xRot + center.m_X;
+ m_Y = yRot + center.m_Y;
+ }
+
+ /**
+ * Rotate by angle (in radiants) around (0,0).
+ * @param angle Angle in radiants
+ */
+ inline void rotate(float angle)
+ {
+ double xRot = m_X * cos(angle) - m_Y * sin(angle);
+ double yRot = m_X * sin(angle) + m_Y * cos(angle);
+ m_X = xRot;
+ m_Y = yRot;
+ }
+
+ /**
+ * Returns the string representation of the point.
+ * @return string representation of the point.
+ */
+ inline std::string toString() const
+ {
+ std::ostringstream str;
+ str << m_X << " " << m_Y;
+ return str.str();
+ }
+
+ /** @return "invalid" Point (used as end marker in vector drawings) **/
+ static Point2D invalidPoint()
+ {
+ return Point2D(DBL_MAX, DBL_MAX);
+ }
+
+ bool isValid()
+ {
+ return ((m_X != DBL_MAX) || (m_Y != DBL_MAX));
+ }
+
+protected:
+ double m_X;
+ double m_Y;
};
#endif
diff --git a/homer_nav_libs/include/homer_nav_libs/Math/Pose.h b/homer_nav_libs/include/homer_nav_libs/Math/Pose.h
index a4b47b9ad72304d903b8506556804dfe1746e9e3..7adb46489daaffbc8241469791e438c2105d5836 100644
--- a/homer_nav_libs/include/homer_nav_libs/Math/Pose.h
+++ b/homer_nav_libs/include/homer_nav_libs/Math/Pose.h
@@ -18,55 +18,54 @@ class Transformation2D;
* @class Pose
*
* @author Stephan Wirth, Susanne Maur (RX), David Gossow (RX)
- * @brief Class to describe and hold a pose of the robot (x, y)-Position + Orientation
+ * @brief Class to describe and hold a pose of the robot (x, y)-Position +
+ * Orientation
* in world-coordinates
*/
-class Pose : public Point2D {
-
+class Pose : public Point2D
+{
public:
+ /**
+ * Constructor which initializes the members with the given values.
+ * @param x x-position
+ * @param y y-position
+ * @param theta orientation in radiants
+ */
+ Pose(float x, float y, float theta);
- /**
- * Constructor which initializes the members with the given values.
- * @param x x-position
- * @param y y-position
- * @param theta orientation in radiants
- */
- Pose(float x, float y, float theta);
-
- /**
- * Default constructor, initializes members to 0.
- */
- Pose();
+ /**
+ * Default constructor, initializes members to 0.
+ */
+ Pose();
- /**
- * The destructor is empty.
- */
- ~Pose();
+ /**
+ * The destructor is empty.
+ */
+ ~Pose();
- float theta() const;
+ float theta() const;
- void setTheta(float theta);
+ void setTheta(float theta);
- Pose operator+ ( const Transformation2D& transformation ) const;
- Pose operator- ( const Transformation2D& transformation ) const;
- Transformation2D operator- ( const Pose& pose ) const;
+ Pose operator+(const Transformation2D& transformation) const;
+ Pose operator-(const Transformation2D& transformation) const;
+ Transformation2D operator-(const Pose& pose) const;
- /**
- * Interpolates between two poses and returns a pose which correlates with
- * current pose + t * (reference pose - current pose)
- * @param referencePose The second pose to interpolate between.
- * @param t The factor of interpolation.
- * @return Interpolated pose
- */
- Pose interpolate(const Pose& referencePose, float t) const;
+ /**
+ * Interpolates between two poses and returns a pose which correlates with
+ * current pose + t * (reference pose - current pose)
+ * @param referencePose The second pose to interpolate between.
+ * @param t The factor of interpolation.
+ * @return Interpolated pose
+ */
+ Pose interpolate(const Pose& referencePose, float t) const;
-// Pose( ExtendedInStream& extStrm );
+ // Pose( ExtendedInStream& extStrm );
-// void storer( ExtendedOutStream& extStrm ) const;
+ // void storer( ExtendedOutStream& extStrm ) const;
protected:
-
- float m_Theta;
+ float m_Theta;
};
#endif
diff --git a/homer_nav_libs/include/homer_nav_libs/Math/Transformation2D.h b/homer_nav_libs/include/homer_nav_libs/Math/Transformation2D.h
index a2ba0d67c093a667f36d340a3bf5fa54d6678df0..ef34922a95c2cf47ddca68924140b40f304a30a8 100644
--- a/homer_nav_libs/include/homer_nav_libs/Math/Transformation2D.h
+++ b/homer_nav_libs/include/homer_nav_libs/Math/Transformation2D.h
@@ -10,11 +10,10 @@
#ifndef TRANSFORMATION2D_H
#define TRANSFORMATION2D_H
+#include <homer_nav_libs/Math/Line2D.h>
+#include <homer_nav_libs/Math/Point2D.h>
#include <cmath>
#include <vector>
-#include <homer_nav_libs/Math/Point2D.h>
-#include <homer_nav_libs/Math/Line2D.h>
-
/**
* @class Transformation2D
@@ -26,118 +25,116 @@
*/
class Transformation2D : public CVec2
{
-
- public:
-
- /**
- * Constructor that initializes the members.
- * @param x translation in x direction in m
- * @param y translation in y direction in m
- * @param theta rotation in radiants
- */
- Transformation2D ( double x, double y, double theta );
-
- /**
- * Constructor that initializes the members.
- * @param vec a vector which represents the translation in x and y direction
- * @param theta rotation in radiants
- */
- Transformation2D ( const CVec2& vec, double theta );
-
- /**
- * Default constructor sets all members to 0.0.
- */
- Transformation2D();
-
- /**
- * Default destructor.
- */
- ~Transformation2D();
-
- /**
- * Sets the values of transformation.
- * @param x translation in x direction in mm
- * @param y translation in y direction in mm
- * @param theta rotation in radiants
- */
- void set ( double x, double y, double theta );
-
- /**
- * Returns the rotation in radiants.
- * @return rotation in radiants
- */
- double theta() const;
-
- /**
- * Adds two transformations.
- */
- Transformation2D operator+ ( Transformation2D t ) const;
- Transformation2D& operator+= ( Transformation2D t );
-
- /**
- * Subtracts two transformations.
- */
- Transformation2D operator- ( Transformation2D t ) const;
- Transformation2D& operator-= ( Transformation2D t );
-
- /**
- * Scales a transformation by a factor
- */
- Transformation2D operator* ( float factor ) const;
- Transformation2D& operator*= ( float factor );
-
- /**
- * Scales a transformation by a factor
- */
- Transformation2D operator/ ( float factor ) const;
- Transformation2D& operator/= ( float factor );
-
- /**
- * Test equality of transformations.
- */
- bool operator== ( Transformation2D t ) const;
- bool operator!= ( Transformation2D t ) const;
-
- /**
- * Compare transformations.
- * (attention: algebraic signs are taken into account, if necessary use fabs())
- */
- bool operator<= ( Transformation2D t ) const;
- bool operator>= ( Transformation2D t ) const;
- bool operator< ( Transformation2D t ) const;
- bool operator> ( Transformation2D t ) const;
-
- /**
- * Applies abs() on every attribute.
- */
- Transformation2D abs() const;
-
- /**
- * Inverts the transformation, scales every attribute with -1.
- */
- Transformation2D inverse() const;
-
- /**
- * Transformes points by first rotation, then translating.
- */
- Point2D transform ( const Point2D& point ) const;
- std::vector<Point2D> transform ( const std::vector<Point2D>& points ) const;
-
- /**
- * Transformes lines by first rotation, then translating.
- */
- Line2D transform ( const Line2D& line ) const;
- std::vector<Line2D> transform ( const std::vector<Line2D>& lines ) const;
-
- /**
- * Returns the string representation of the transformation.
- * @return string representation of the transformation.
- */
- std::string toString() const;
-
- private:
- double m_Theta;
+public:
+ /**
+ * Constructor that initializes the members.
+ * @param x translation in x direction in m
+ * @param y translation in y direction in m
+ * @param theta rotation in radiants
+ */
+ Transformation2D(double x, double y, double theta);
+
+ /**
+ * Constructor that initializes the members.
+ * @param vec a vector which represents the translation in x and y direction
+ * @param theta rotation in radiants
+ */
+ Transformation2D(const CVec2& vec, double theta);
+
+ /**
+ * Default constructor sets all members to 0.0.
+ */
+ Transformation2D();
+
+ /**
+ * Default destructor.
+ */
+ ~Transformation2D();
+
+ /**
+ * Sets the values of transformation.
+ * @param x translation in x direction in mm
+ * @param y translation in y direction in mm
+ * @param theta rotation in radiants
+ */
+ void set(double x, double y, double theta);
+
+ /**
+ * Returns the rotation in radiants.
+ * @return rotation in radiants
+ */
+ double theta() const;
+
+ /**
+ * Adds two transformations.
+ */
+ Transformation2D operator+(Transformation2D t) const;
+ Transformation2D& operator+=(Transformation2D t);
+
+ /**
+ * Subtracts two transformations.
+ */
+ Transformation2D operator-(Transformation2D t) const;
+ Transformation2D& operator-=(Transformation2D t);
+
+ /**
+ * Scales a transformation by a factor
+ */
+ Transformation2D operator*(float factor) const;
+ Transformation2D& operator*=(float factor);
+
+ /**
+ * Scales a transformation by a factor
+ */
+ Transformation2D operator/(float factor) const;
+ Transformation2D& operator/=(float factor);
+
+ /**
+ * Test equality of transformations.
+ */
+ bool operator==(Transformation2D t) const;
+ bool operator!=(Transformation2D t) const;
+
+ /**
+ * Compare transformations.
+ * (attention: algebraic signs are taken into account, if necessary use
+ * fabs())
+ */
+ bool operator<=(Transformation2D t) const;
+ bool operator>=(Transformation2D t) const;
+ bool operator<(Transformation2D t) const;
+ bool operator>(Transformation2D t) const;
+
+ /**
+ * Applies abs() on every attribute.
+ */
+ Transformation2D abs() const;
+
+ /**
+ * Inverts the transformation, scales every attribute with -1.
+ */
+ Transformation2D inverse() const;
+
+ /**
+ * Transformes points by first rotation, then translating.
+ */
+ Point2D transform(const Point2D& point) const;
+ std::vector<Point2D> transform(const std::vector<Point2D>& points) const;
+
+ /**
+ * Transformes lines by first rotation, then translating.
+ */
+ Line2D transform(const Line2D& line) const;
+ std::vector<Line2D> transform(const std::vector<Line2D>& lines) const;
+
+ /**
+ * Returns the string representation of the transformation.
+ * @return string representation of the transformation.
+ */
+ std::string toString() const;
+
+private:
+ double m_Theta;
};
#endif
-
diff --git a/homer_nav_libs/include/homer_nav_libs/Math/mat2.h b/homer_nav_libs/include/homer_nav_libs/Math/mat2.h
index 45365d81907ee2b5fb78662b6039c01798c1588a..d163d0820a8988b248fe9d4007ada4e8f344ac78 100644
--- a/homer_nav_libs/include/homer_nav_libs/Math/mat2.h
+++ b/homer_nav_libs/include/homer_nav_libs/Math/mat2.h
@@ -10,67 +10,70 @@
#ifndef MAT2_H
#define MAT2_H
-#include <math.h>
#include <homer_nav_libs/Math/Point2D.h>
#include <homer_nav_libs/Math/vec2.h>
+#include <math.h>
class CMat2
{
- public:
- CMat2();
- CMat2(float rot);
- ~CMat2();
-
- CMat2 operator *(const CMat2 &mat) const;
-
- CVec2 operator *(const CVec2& v) const;
-
- Point2D operator *(const Point2D& p) const;
-
- float& operator [] (unsigned int position) {
- return fMatrix[position];
- }
-
- CMat2 operator +(const CMat2 rhs) const {
- CMat2 newMatrix;
- for (unsigned int i = 0; i < 4; i++) {
- newMatrix[i] = valueAt(i) + rhs.valueAt(i);
- }
- return newMatrix;
- }
-
- CMat2 operator -(const CMat2 rhs) const {
- CMat2 newMatrix;
- for (unsigned int i = 0; i < 4; i++) {
- newMatrix[i] = valueAt(i) - rhs.valueAt(i);
- }
- return newMatrix;
- }
-
- float valueAt(unsigned int position) const{
- return fMatrix[position];
- }
-
- union
- {
- float fMatrix[4];
- float m[2][2];
- struct
- {
- float xx, xy;
- float yx, yy;
- };
- };
-
- void transpose();
- void loadIdentity();
-
- void makeRotation(float fA);
-
- bool invert();
-};
+public:
+ CMat2();
+ CMat2(float rot);
+ ~CMat2();
+
+ CMat2 operator*(const CMat2 &mat) const;
+
+ CVec2 operator*(const CVec2 &v) const;
+
+ Point2D operator*(const Point2D &p) const;
+
+ float &operator[](unsigned int position)
+ {
+ return fMatrix[position];
+ }
+ CMat2 operator+(const CMat2 rhs) const
+ {
+ CMat2 newMatrix;
+ for (unsigned int i = 0; i < 4; i++)
+ {
+ newMatrix[i] = valueAt(i) + rhs.valueAt(i);
+ }
+ return newMatrix;
+ }
+ CMat2 operator-(const CMat2 rhs) const
+ {
+ CMat2 newMatrix;
+ for (unsigned int i = 0; i < 4; i++)
+ {
+ newMatrix[i] = valueAt(i) - rhs.valueAt(i);
+ }
+ return newMatrix;
+ }
+
+ float valueAt(unsigned int position) const
+ {
+ return fMatrix[position];
+ }
+
+ union {
+ float fMatrix[4];
+ float m[2][2];
+ struct
+ {
+ float xx, xy;
+ float yx, yy;
+ };
+ };
+
+ void transpose();
+ void loadIdentity();
+
+ void makeRotation(float fA);
+
+ bool invert();
+};
#include "mat2_inl.h"
diff --git a/homer_nav_libs/include/homer_nav_libs/Math/mat2_inl.h b/homer_nav_libs/include/homer_nav_libs/Math/mat2_inl.h
index e3f77e161151e98c7df7f06b43db1d40fd9c4753..3db82d6debfc7d6c8b0fcc0fae9c24f75ed7d2a4 100644
--- a/homer_nav_libs/include/homer_nav_libs/Math/mat2_inl.h
+++ b/homer_nav_libs/include/homer_nav_libs/Math/mat2_inl.h
@@ -9,78 +9,83 @@
#include <assert.h>
-inline CMat2::CMat2(){
- for (unsigned int i = 0; i < 4; i++) {
- fMatrix[i] = 0;
- }
+inline CMat2::CMat2()
+{
+ for (unsigned int i = 0; i < 4; i++)
+ {
+ fMatrix[i] = 0;
+ }
}
inline CMat2::~CMat2()
-{}
+{
+}
inline CMat2::CMat2(float rot)
{
- makeRotation(rot);
+ makeRotation(rot);
}
-inline CMat2 CMat2::operator * ( const CMat2 & mat ) const
+inline CMat2 CMat2::operator*(const CMat2& mat) const
{
- CMat2 retValue;
- for (unsigned int line = 0; line < 2; line++) {
- for (unsigned int column = 0; column < 2; column++) {
- retValue[line*2 + column] = valueAt(line*2 + column) + mat.valueAt(column*2 + line);
- }
+ CMat2 retValue;
+ for (unsigned int line = 0; line < 2; line++)
+ {
+ for (unsigned int column = 0; column < 2; column++)
+ {
+ retValue[line * 2 + column] =
+ valueAt(line * 2 + column) + mat.valueAt(column * 2 + line);
}
- return retValue;
+ }
+ return retValue;
}
-inline CVec2 CMat2::operator * ( const CVec2& v ) const
+inline CVec2 CMat2::operator*(const CVec2& v) const
{
- return CVec2(xx*v[0] + xy*v[1],yx*v[0] + yy*v[1]);
+ return CVec2(xx * v[0] + xy * v[1], yx * v[0] + yy * v[1]);
}
-inline Point2D CMat2::operator * ( const Point2D& p ) const
+inline Point2D CMat2::operator*(const Point2D& p) const
{
- return Point2D(xx*p.x() + xy*p.y(), yx*p.x() + yy*p.y());
+ return Point2D(xx * p.x() + xy * p.y(), yx * p.x() + yy * p.y());
}
inline void CMat2::transpose()
{
- float t=xy;
- xy=yx;
- yx=t;
+ float t = xy;
+ xy = yx;
+ yx = t;
}
inline void CMat2::loadIdentity()
{
- xx=1.0f; xy=0.0f;
- yx=0.0f; yy=1.0f;
+ xx = 1.0f;
+ xy = 0.0f;
+ yx = 0.0f;
+ yy = 1.0f;
}
-inline void CMat2::makeRotation ( float fA )
+inline void CMat2::makeRotation(float fA)
{
- xx=yy=cosf(fA);
- yx=sinf(fA);
- xy=-yx;
+ xx = yy = cosf(fA);
+ yx = sinf(fA);
+ xy = -yx;
}
inline bool CMat2::invert()
{
- CMat2 tmp;
- float det = fMatrix[0]*fMatrix[3] - fMatrix[1]*fMatrix[2];
+ CMat2 tmp;
+ float det = fMatrix[0] * fMatrix[3] - fMatrix[1] * fMatrix[2];
- if(fabs(det) < 0.001f) return false;
+ if (fabs(det) < 0.001f)
+ return false;
- det = 1.0 / det;
- tmp.fMatrix[0] = fMatrix[3]*det;
- tmp.fMatrix[1] = -fMatrix[1]*det;
- tmp.fMatrix[2] = -fMatrix[2]*det;
- tmp.fMatrix[3] = fMatrix[0]*det;
+ det = 1.0 / det;
+ tmp.fMatrix[0] = fMatrix[3] * det;
+ tmp.fMatrix[1] = -fMatrix[1] * det;
+ tmp.fMatrix[2] = -fMatrix[2] * det;
+ tmp.fMatrix[3] = fMatrix[0] * det;
- (*this)=tmp;
- return true;
+ (*this) = tmp;
+ return true;
}
-
-
-
-
diff --git a/homer_nav_libs/include/homer_nav_libs/Math/vec2.h b/homer_nav_libs/include/homer_nav_libs/Math/vec2.h
index 39a72c8eb60c6b6bf836dc7d7be932c376e25fd0..e34dfe72d1e402017c7b74c716cfae9d18126c07 100644
--- a/homer_nav_libs/include/homer_nav_libs/Math/vec2.h
+++ b/homer_nav_libs/include/homer_nav_libs/Math/vec2.h
@@ -17,173 +17,176 @@
class CVec2
{
- public:
-
- inline CVec2()
- {
- m_X = 0; m_Y = 0;
- }
-
- inline CVec2 ( double x, double y )
- {
- m_X=x; m_Y=y;
- }
-
- inline CVec2 ( const CVec2& vec )
- {
- m_X=vec.x(); m_Y=vec.y();
- }
-
- inline CVec2 operator+ ( const CVec2& vVector ) const
- {
- return CVec2 ( vVector[0] + m_X, vVector[1] + m_Y );
- };
-
- inline CVec2 operator- ( const CVec2& vVector ) const
- {
- return CVec2 ( m_X - vVector[0], m_Y - vVector[1] );
- };
-
- inline CVec2 operator- ( ) const
- {
- return CVec2 ( - m_X, - m_Y );
- };
-
- inline CVec2 operator* ( double num ) const
- {
- return CVec2 ( m_X * num, m_Y * num );
- };
-
- inline double operator* ( const CVec2& vVector ) const
- {
- return m_X*vVector[0]+m_Y*vVector[1];
- }
-
- inline CVec2 operator/ ( double num ) const
- {
- return CVec2 ( m_X / num, m_Y / num );
- }
-
- inline void set ( double fx, double fy )
- {
- m_X=fx; m_Y=fy;
- }
-
- inline double x() const
- {
- return m_X;
- }
-
- inline double y() const
- {
- return m_Y;
- }
-
- inline double magnitude() const
- {
- double sumOfSquares = m_X*m_X + m_Y*m_Y;
- return sqrt ( sumOfSquares );
- }
-
- inline double operator [] ( unsigned int i ) const
- {
- return ( ( double* ) this ) [i];
- }
-
- inline double& operator [] ( unsigned int i )
- {
- return ( ( double* ) this ) [i];
- }
-
- inline CVec2& operator/= ( double num )
- {
- double inv=1.0f/num;
- m_X*=inv;
- m_Y*=inv;
- return ( *this );
- }
-
- inline CVec2& operator*= ( double num )
- {
- m_X*=num;
- m_Y*=num;
- return ( *this );
- }
-
- inline CVec2& normalize()
- {
- return ( *this/=magnitude() );
- }
-
- inline CVec2& makePerp()
- {
- double xn=m_X;
- m_X=-m_Y;
- m_Y=xn;
- return *this;
- }
-
- inline CVec2 getNormal() const
- {
- return CVec2 ( m_Y, -m_X ); //?
- }
-
- inline CVec2 getNormalized() const
- {
- return ( *this ) /magnitude();
- }
-
- inline double sqr() const
- {
- return ( *this ) * ( *this );
- }
-
- inline double dot ( const CVec2& vec ) const
- {
- return ( m_X*vec[0] ) + ( m_Y*vec[1] );
- }
-
- inline double getAngle ( const CVec2& vec ) const
- {
- return acos ( dot ( vec ) / ( magnitude() *vec.magnitude() ) );
- }
-
- /// @param angle Rotation angle in radiants
- inline CVec2 rotate ( float angle ) const
- {
- double xRot = m_X*cos ( angle ) - m_Y*sin ( angle );
- double yRot = m_X*sin ( angle ) + m_Y*cos ( angle );
- return CVec2 ( xRot, yRot );
- }
-
- inline bool equal ( CVec2 vec ) const
- {
- return ( m_X==vec.x() && m_Y==vec.y() );
- }
-
- /**
- * Returns the string representation of the vector.
- * @return string representation of the point.
- */
- inline std::string toString() const
- {
- std::ostringstream str;
- str << m_X << " " << m_Y;
- return str.str();
- }
-
- protected:
- double m_X, m_Y;
+public:
+ inline CVec2()
+ {
+ m_X = 0;
+ m_Y = 0;
+ }
+
+ inline CVec2(double x, double y)
+ {
+ m_X = x;
+ m_Y = y;
+ }
+
+ inline CVec2(const CVec2& vec)
+ {
+ m_X = vec.x();
+ m_Y = vec.y();
+ }
+
+ inline CVec2 operator+(const CVec2& vVector) const
+ {
+ return CVec2(vVector[0] + m_X, vVector[1] + m_Y);
+ };
+
+ inline CVec2 operator-(const CVec2& vVector) const
+ {
+ return CVec2(m_X - vVector[0], m_Y - vVector[1]);
+ };
+
+ inline CVec2 operator-() const
+ {
+ return CVec2(-m_X, -m_Y);
+ };
+
+ inline CVec2 operator*(double num) const
+ {
+ return CVec2(m_X * num, m_Y * num);
+ };
+
+ inline double operator*(const CVec2& vVector) const
+ {
+ return m_X * vVector[0] + m_Y * vVector[1];
+ }
+
+ inline CVec2 operator/(double num) const
+ {
+ return CVec2(m_X / num, m_Y / num);
+ }
+
+ inline void set(double fx, double fy)
+ {
+ m_X = fx;
+ m_Y = fy;
+ }
+
+ inline double x() const
+ {
+ return m_X;
+ }
+
+ inline double y() const
+ {
+ return m_Y;
+ }
+
+ inline double magnitude() const
+ {
+ double sumOfSquares = m_X * m_X + m_Y * m_Y;
+ return sqrt(sumOfSquares);
+ }
+
+ inline double operator[](unsigned int i) const
+ {
+ return ((double*)this)[i];
+ }
+
+ inline double& operator[](unsigned int i)
+ {
+ return ((double*)this)[i];
+ }
+
+ inline CVec2& operator/=(double num)
+ {
+ double inv = 1.0f / num;
+ m_X *= inv;
+ m_Y *= inv;
+ return (*this);
+ }
+
+ inline CVec2& operator*=(double num)
+ {
+ m_X *= num;
+ m_Y *= num;
+ return (*this);
+ }
+
+ inline CVec2& normalize()
+ {
+ return (*this /= magnitude());
+ }
+
+ inline CVec2& makePerp()
+ {
+ double xn = m_X;
+ m_X = -m_Y;
+ m_Y = xn;
+ return *this;
+ }
+
+ inline CVec2 getNormal() const
+ {
+ return CVec2(m_Y, -m_X); //?
+ }
+
+ inline CVec2 getNormalized() const
+ {
+ return (*this) / magnitude();
+ }
+
+ inline double sqr() const
+ {
+ return (*this) * (*this);
+ }
+
+ inline double dot(const CVec2& vec) const
+ {
+ return (m_X * vec[0]) + (m_Y * vec[1]);
+ }
+
+ inline double getAngle(const CVec2& vec) const
+ {
+ return acos(dot(vec) / (magnitude() * vec.magnitude()));
+ }
+
+ /// @param angle Rotation angle in radiants
+ inline CVec2 rotate(float angle) const
+ {
+ double xRot = m_X * cos(angle) - m_Y * sin(angle);
+ double yRot = m_X * sin(angle) + m_Y * cos(angle);
+ return CVec2(xRot, yRot);
+ }
+
+ inline bool equal(CVec2 vec) const
+ {
+ return (m_X == vec.x() && m_Y == vec.y());
+ }
+
+ /**
+ * Returns the string representation of the vector.
+ * @return string representation of the point.
+ */
+ inline std::string toString() const
+ {
+ std::ostringstream str;
+ str << m_X << " " << m_Y;
+ return str.str();
+ }
+
+protected:
+ double m_X, m_Y;
};
-inline CVec2 operator* ( double f, const CVec2& v )
+inline CVec2 operator*(double f, const CVec2& v)
{
- return v*f;
+ return v * f;
}
-inline CVec2 normalize ( const CVec2& v )
+inline CVec2 normalize(const CVec2& v)
{
- return v/v.magnitude();
+ return v / v.magnitude();
}
#endif
diff --git a/homer_nav_libs/include/homer_nav_libs/tools.h b/homer_nav_libs/include/homer_nav_libs/tools.h
index e5a30b006d07cc306e2650e7982593350aa836c6..93490f81c6f38e459e155c2a61d83ab1b8b3ba13 100644
--- a/homer_nav_libs/include/homer_nav_libs/tools.h
+++ b/homer_nav_libs/include/homer_nav_libs/tools.h
@@ -15,8 +15,8 @@
* Convenience functions that are often used in the mapping and navigation
* process
*/
-namespace map_tools {
-
+namespace map_tools
+{
/**
* @brief Converts a point p in world frame /map to the respective cell position
* in the map
@@ -26,11 +26,12 @@ namespace map_tools {
* @return Cell position of the point
*/
Eigen::Vector2i toMapCoords(geometry_msgs::Point p, geometry_msgs::Pose origin,
- float resolution) {
- int x_idx = (p.x - origin.position.x) / resolution + 0.51;
- int y_idx = (p.y - origin.position.y) / resolution + 0.51;
- Eigen::Vector2i ret(x_idx, y_idx);
- return ret;
+ float resolution)
+{
+ int x_idx = (p.x - origin.position.x) / resolution + 0.51;
+ int y_idx = (p.y - origin.position.y) / resolution + 0.51;
+ Eigen::Vector2i ret(x_idx, y_idx);
+ return ret;
}
/**
@@ -42,13 +43,14 @@ Eigen::Vector2i toMapCoords(geometry_msgs::Point p, geometry_msgs::Pose origin,
* @return Cell position of the point
*/
Eigen::Vector2i toMapCoords(const geometry_msgs::Point p,
- const nav_msgs::OccupancyGrid::ConstPtr& cmap) {
- int x_idx =
- (p.x - cmap->info.origin.position.x) / cmap->info.resolution + 0.51;
- int y_idx =
- (p.y - cmap->info.origin.position.y) / cmap->info.resolution + 0.51;
- Eigen::Vector2i ret(x_idx, y_idx);
- return ret;
+ const nav_msgs::OccupancyGrid::ConstPtr& cmap)
+{
+ int x_idx =
+ (p.x - cmap->info.origin.position.x) / cmap->info.resolution + 0.51;
+ int y_idx =
+ (p.y - cmap->info.origin.position.y) / cmap->info.resolution + 0.51;
+ Eigen::Vector2i ret(x_idx, y_idx);
+ return ret;
}
/**
@@ -60,13 +62,14 @@ Eigen::Vector2i toMapCoords(const geometry_msgs::Point p,
* @return Point in world frame
*/
geometry_msgs::Point fromMapCoords(
- const Eigen::Vector2i idx, const nav_msgs::OccupancyGrid::ConstPtr& cmap) {
- geometry_msgs::Point ret;
- ret.x =
- cmap->info.origin.position.x + (idx.x() - 0.5) * cmap->info.resolution;
- ret.y =
- cmap->info.origin.position.y + (idx.y() - 0.5) * cmap->info.resolution;
- return ret;
+ const Eigen::Vector2i idx, const nav_msgs::OccupancyGrid::ConstPtr& cmap)
+{
+ geometry_msgs::Point ret;
+ ret.x =
+ cmap->info.origin.position.x + (idx.x() - 0.5) * cmap->info.resolution;
+ ret.y =
+ cmap->info.origin.position.y + (idx.y() - 0.5) * cmap->info.resolution;
+ return ret;
}
/**
@@ -78,12 +81,12 @@ geometry_msgs::Point fromMapCoords(
* @return Point in world frame
*/
geometry_msgs::Point fromMapCoords(Eigen::Vector2i idx,
- geometry_msgs::Pose origin,
- float resolution) {
- geometry_msgs::Point ret;
- ret.x = origin.position.x + (idx.x() - 0.5) * resolution;
- ret.y = origin.position.y + (idx.y() - 0.5) * resolution;
- return ret;
+ geometry_msgs::Pose origin, float resolution)
+{
+ geometry_msgs::Point ret;
+ ret.x = origin.position.x + (idx.x() - 0.5) * resolution;
+ ret.y = origin.position.y + (idx.y() - 0.5) * resolution;
+ return ret;
}
/**
@@ -96,11 +99,12 @@ geometry_msgs::Point fromMapCoords(Eigen::Vector2i idx,
*/
geometry_msgs::Point qtFromMapCoords(Eigen::Vector2i idx,
geometry_msgs::Pose origin,
- float resolution) {
- geometry_msgs::Point ret;
- ret.x = -(origin.position.x + idx.y()) * resolution;
- ret.y = -(origin.position.y + idx.x()) * resolution;
- return ret;
+ float resolution)
+{
+ geometry_msgs::Point ret;
+ ret.x = -(origin.position.x + idx.y()) * resolution;
+ ret.y = -(origin.position.y + idx.x()) * resolution;
+ return ret;
}
/**
@@ -113,9 +117,10 @@ geometry_msgs::Point qtFromMapCoords(Eigen::Vector2i idx,
* @return index of point in the map
*/
int map_index(geometry_msgs::Point p, geometry_msgs::Pose origin, float width,
- float resolution) {
- return (int)(width * ((p.y - origin.position.y) / resolution + 0.51) +
- ((p.x - origin.position.x) / resolution + 0.51));
+ float resolution)
+{
+ return (int)(width * ((p.y - origin.position.y) / resolution + 0.51) +
+ ((p.x - origin.position.x) / resolution + 0.51));
}
/**
@@ -128,12 +133,13 @@ int map_index(geometry_msgs::Point p, geometry_msgs::Pose origin, float width,
* @return true or false
*/
bool point_in_map(geometry_msgs::Point p, geometry_msgs::Pose origin,
- float width, float resolution) {
- int x_idx = (p.x - origin.position.x) / resolution + 0.51;
- int y_idx = (p.y - origin.position.y) / resolution + 0.51;
- if (x_idx < 0 || y_idx < 0 || x_idx >= width || y_idx >= width)
- return false;
- return true;
+ float width, float resolution)
+{
+ int x_idx = (p.x - origin.position.x) / resolution + 0.51;
+ int y_idx = (p.y - origin.position.y) / resolution + 0.51;
+ if (x_idx < 0 || y_idx < 0 || x_idx >= width || y_idx >= width)
+ return false;
+ return true;
}
/**
@@ -148,51 +154,57 @@ geometry_msgs::Point transformPoint(geometry_msgs::Point point,
tf::TransformListener& listener,
std::string from_frame,
std::string to_frame,
- ros::Time stamp = ros::Time(0)) {
- geometry_msgs::PointStamped pin;
- geometry_msgs::PointStamped pout;
- pin.header.frame_id = from_frame;
- pin.point = point;
- try {
- listener.transformPoint(to_frame, stamp, pin, "/map", pout);
- return pout.point;
- } catch (tf::TransformException ex) {
- ROS_ERROR("%s", ex.what());
- }
+ ros::Time stamp = ros::Time(0))
+{
+ geometry_msgs::PointStamped pin;
+ geometry_msgs::PointStamped pout;
+ pin.header.frame_id = from_frame;
+ pin.point = point;
+ try
+ {
+ listener.transformPoint(to_frame, stamp, pin, "/map", pout);
+ return pout.point;
+ }
+ catch (tf::TransformException ex)
+ {
+ ROS_ERROR("%s", ex.what());
+ }
}
geometry_msgs::Point transformPoint(const geometry_msgs::Point point,
- tf::StampedTransform transform) {
- geometry_msgs::Point point_out;
- tf::Vector3 pin;
- tf::Vector3 pout;
- pin.setX(point.x);
- pin.setY(point.y);
- pin.setZ(point.z);
+ tf::StampedTransform transform)
+{
+ geometry_msgs::Point point_out;
+ tf::Vector3 pin;
+ tf::Vector3 pout;
+ pin.setX(point.x);
+ pin.setY(point.y);
+ pin.setZ(point.z);
- pout = transform * pin;
+ pout = transform * pin;
- point_out.x = pout.x();
- point_out.y = pout.y();
+ point_out.x = pout.x();
+ point_out.y = pout.y();
- return point_out;
+ return point_out;
}
geometry_msgs::Point transformPoint(const geometry_msgs::Point point,
- tf::Transform transform) {
- geometry_msgs::Point point_out;
- tf::Vector3 pin;
- tf::Vector3 pout;
- pin.setX(point.x);
- pin.setY(point.y);
- pin.setZ(point.z);
-
- pout = transform * pin;
-
- point_out.x = pout.x();
- point_out.y = pout.y();
- point_out.z = pout.z();
-
- return point_out;
+ tf::Transform transform)
+{
+ geometry_msgs::Point point_out;
+ tf::Vector3 pin;
+ tf::Vector3 pout;
+ pin.setX(point.x);
+ pin.setY(point.y);
+ pin.setZ(point.z);
+
+ pout = transform * pin;
+
+ point_out.x = pout.x();
+ point_out.y = pout.y();
+ point_out.z = pout.z();
+
+ return point_out;
}
/**
@@ -209,17 +221,21 @@ geometry_msgs::Point transformPoint(geometry_msgs::Point point,
tf::TransformListener& listener,
const ros::Time& time,
std::string from_frame,
- std::string to_frame) {
- geometry_msgs::PointStamped pin;
- geometry_msgs::PointStamped pout;
- pin.header.frame_id = from_frame;
- pin.point = point;
- try {
- listener.transformPoint(to_frame, time, pin, "/map", pout);
- return pout.point;
- } catch (tf::TransformException ex) {
- ROS_ERROR("%s", ex.what());
- }
+ std::string to_frame)
+{
+ geometry_msgs::PointStamped pin;
+ geometry_msgs::PointStamped pout;
+ pin.header.frame_id = from_frame;
+ pin.point = point;
+ try
+ {
+ listener.transformPoint(to_frame, time, pin, "/map", pout);
+ return pout.point;
+ }
+ catch (tf::TransformException ex)
+ {
+ ROS_ERROR("%s", ex.what());
+ }
}
/**
@@ -238,20 +254,24 @@ geometry_msgs::Point transformPoint(geometry_msgs::Point point,
geometry_msgs::Point laser_range_to_point(
float laser_range, int index, float start_angle, float angle_step,
tf::TransformListener& listener, std::string from_frame,
- std::string to_frame, ros::Time stamp = ros::Time(0), float time_inc = 0) {
- float alpha = start_angle + index * angle_step;
- geometry_msgs::PointStamped pin;
- geometry_msgs::PointStamped pout;
- pin.header.frame_id = from_frame;
- pin.point.x = cos(alpha) * laser_range;
- pin.point.y = sin(alpha) * laser_range;
-
- try {
- listener.transformPoint(to_frame, stamp, pin, "/map", pout);
- return pout.point;
- } catch (tf::TransformException ex) {
- // ROS_ERROR("%s",ex.what());
- }
+ std::string to_frame, ros::Time stamp = ros::Time(0), float time_inc = 0)
+{
+ float alpha = start_angle + index * angle_step;
+ geometry_msgs::PointStamped pin;
+ geometry_msgs::PointStamped pout;
+ pin.header.frame_id = from_frame;
+ pin.point.x = cos(alpha) * laser_range;
+ pin.point.y = sin(alpha) * laser_range;
+
+ try
+ {
+ listener.transformPoint(to_frame, stamp, pin, "/map", pout);
+ return pout.point;
+ }
+ catch (tf::TransformException ex)
+ {
+ // ROS_ERROR("%s",ex.what());
+ }
}
/**
@@ -270,32 +290,38 @@ std::vector<geometry_msgs::Point> laser_ranges_to_points(
const std::vector<float>& laser_data, float start_angle, float angle_step,
float range_min, float range_max, tf::TransformListener& listener,
std::string from_frame, std::string to_frame,
- ros::Time stamp = ros::Time(0), float time_inc = 0) {
- std::vector<geometry_msgs::Point> ret;
- float alpha = start_angle;
- for (int i = 0; i < laser_data.size(); i++) {
- if (laser_data[i] < range_min || laser_data[i] > range_max) {
- alpha += angle_step;
- continue;
- }
- geometry_msgs::Point point;
- point.x = cos(alpha) * laser_data.at(i);
- point.y = sin(alpha) * laser_data.at(i);
-
- geometry_msgs::PointStamped pin;
- pin.header.frame_id = from_frame;
- pin.point = point;
- geometry_msgs::PointStamped pout;
- try {
- listener.transformPoint(to_frame, stamp, pin, "/map", pout);
- ret.push_back(pout.point);
- } catch (tf::TransformException ex) {
- // ROS_ERROR("%s",ex.what());
- }
+ ros::Time stamp = ros::Time(0), float time_inc = 0)
+{
+ std::vector<geometry_msgs::Point> ret;
+ float alpha = start_angle;
+ for (int i = 0; i < laser_data.size(); i++)
+ {
+ if (laser_data[i] < range_min || laser_data[i] > range_max)
+ {
+ alpha += angle_step;
+ continue;
+ }
+ geometry_msgs::Point point;
+ point.x = cos(alpha) * laser_data.at(i);
+ point.y = sin(alpha) * laser_data.at(i);
- alpha += angle_step;
+ geometry_msgs::PointStamped pin;
+ pin.header.frame_id = from_frame;
+ pin.point = point;
+ geometry_msgs::PointStamped pout;
+ try
+ {
+ listener.transformPoint(to_frame, stamp, pin, "/map", pout);
+ ret.push_back(pout.point);
}
- return ret;
+ catch (tf::TransformException ex)
+ {
+ // ROS_ERROR("%s",ex.what());
+ }
+
+ alpha += angle_step;
+ }
+ return ret;
}
/**
@@ -307,42 +333,48 @@ std::vector<geometry_msgs::Point> laser_ranges_to_points(
* @param to_frame target frame
* @return vector containing the laser measurements in euclidean points
*/
-std::vector<geometry_msgs::Point> laser_msg_to_points(
- const sensor_msgs::LaserScan::ConstPtr& scan,
- tf::TransformListener& listener, std::string to_frame,
- ros::Time stamp = ros::Time(0)) {
- std::vector<geometry_msgs::Point> ret;
- float alpha = scan->angle_min;
- if (!listener.waitForTransform(scan->header.frame_id, to_frame, stamp,
- ros::Duration(0.3))) {
- return ret;
+std::vector<geometry_msgs::Point>
+laser_msg_to_points(const sensor_msgs::LaserScan::ConstPtr& scan,
+ tf::TransformListener& listener, std::string to_frame,
+ ros::Time stamp = ros::Time(0))
+{
+ std::vector<geometry_msgs::Point> ret;
+ float alpha = scan->angle_min;
+ if (!listener.waitForTransform(scan->header.frame_id, to_frame, stamp,
+ ros::Duration(0.3)))
+ {
+ return ret;
+ }
+ for (int i = 0; i < scan->ranges.size(); i++)
+ {
+ if (scan->ranges[i] < scan->range_min || scan->ranges[i] > scan->range_max)
+ {
+ alpha += scan->angle_increment;
+ continue;
}
- for (int i = 0; i < scan->ranges.size(); i++) {
- if (scan->ranges[i] < scan->range_min ||
- scan->ranges[i] > scan->range_max) {
- alpha += scan->angle_increment;
- continue;
- }
- geometry_msgs::Point point;
- point.x = cos(alpha) * scan->ranges.at(i);
- point.y = sin(alpha) * scan->ranges.at(i);
-
- geometry_msgs::PointStamped pin;
- pin.header.frame_id = scan->header.frame_id;
- pin.point = point;
- geometry_msgs::PointStamped pout;
- try {
- // listener.transformPoint(to_frame, (stamp + ros::Duration( i *
- // scan->time_increment)), pin, "/map" ,pout);
- listener.transformPoint(to_frame, stamp, pin, "/map", pout);
- ret.push_back(pout.point);
- } catch (tf::TransformException ex) {
- ROS_ERROR("%s", ex.what());
- }
+ geometry_msgs::Point point;
+ point.x = cos(alpha) * scan->ranges.at(i);
+ point.y = sin(alpha) * scan->ranges.at(i);
- alpha += scan->angle_increment;
+ geometry_msgs::PointStamped pin;
+ pin.header.frame_id = scan->header.frame_id;
+ pin.point = point;
+ geometry_msgs::PointStamped pout;
+ try
+ {
+ // listener.transformPoint(to_frame, (stamp + ros::Duration( i *
+ // scan->time_increment)), pin, "/map" ,pout);
+ listener.transformPoint(to_frame, stamp, pin, "/map", pout);
+ ret.push_back(pout.point);
}
- return ret;
+ catch (tf::TransformException ex)
+ {
+ ROS_ERROR("%s", ex.what());
+ }
+
+ alpha += scan->angle_increment;
+ }
+ return ret;
}
/**
@@ -352,22 +384,27 @@ std::vector<geometry_msgs::Point> laser_msg_to_points(
* @return float distance to nearest Point
*/
float get_max_move_distance(std::vector<geometry_msgs::Point> points,
- float min_x, float min_y) {
- float minDistance = 30;
- for (unsigned int i = 0; i < points.size(); i++) {
- if (std::fabs(points[i].y) < min_y && points[i].x > min_x) {
- float distance =
- sqrt((points[i].x * points[i].x) + (points[i].y * points[i].y));
- if (distance < minDistance) {
- minDistance = distance;
- }
- }
- }
- float maxMoveDist = minDistance - min_x;
- if (maxMoveDist < 0) {
- maxMoveDist = 0.0;
+ float min_x, float min_y)
+{
+ float minDistance = 30;
+ for (unsigned int i = 0; i < points.size(); i++)
+ {
+ if (std::fabs(points[i].y) < min_y && points[i].x > min_x)
+ {
+ float distance =
+ sqrt((points[i].x * points[i].x) + (points[i].y * points[i].y));
+ if (distance < minDistance)
+ {
+ minDistance = distance;
+ }
}
- return maxMoveDist;
+ }
+ float maxMoveDist = minDistance - min_x;
+ if (maxMoveDist < 0)
+ {
+ maxMoveDist = 0.0;
+ }
+ return maxMoveDist;
}
/**
@@ -377,9 +414,10 @@ float get_max_move_distance(std::vector<geometry_msgs::Point> points,
* @param b point b
* @return euclidean distance in cells
*/
-double distance(const Eigen::Vector2i& a, const Eigen::Vector2i& b) {
- return sqrt((a.x() - b.x()) * (a.x() - b.x()) +
- (a.y() - b.y()) * (a.y() - b.y()));
+double distance(const Eigen::Vector2i& a, const Eigen::Vector2i& b)
+{
+ return sqrt((a.x() - b.x()) * (a.x() - b.x()) +
+ (a.y() - b.y()) * (a.y() - b.y()));
}
/**
@@ -389,8 +427,9 @@ double distance(const Eigen::Vector2i& a, const Eigen::Vector2i& b) {
* @param b point b
* @return euclidean distance in m
*/
-double distance(const geometry_msgs::Point& a, const geometry_msgs::Point& b) {
- return sqrt((a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y));
+double distance(const geometry_msgs::Point& a, const geometry_msgs::Point& b)
+{
+ return sqrt((a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y));
}
/**
@@ -406,39 +445,47 @@ double distance(const geometry_msgs::Point& a, const geometry_msgs::Point& b) {
* (x,y)
*/
bool findValue(const std::vector<int8_t>* map, int width, int height,
- int center_x, int center_y, unsigned char value, float radius) {
- int start_x = int(center_x - radius);
- int start_y = int(center_y - radius);
- int end_x = int(center_x + radius);
- int end_y = int(center_y + radius);
-
- if (start_x < 0) {
- start_x = 0;
- }
- if (start_y < 0) {
- start_y = 0;
- }
- if (end_x >= int(width)) {
- end_x = width - 1;
- }
- if (end_y >= int(height)) {
- end_y = height - 1;
- }
-
- float sqr_radius = radius * radius;
-
- for (int y = start_y; y <= end_y; y++)
- for (int x = start_x; x <= end_x; x++) {
- if (map->at(x + width * y) > value) {
- float sqr_dist = float(x - center_x) * float(x - center_x) +
- float(y - center_y) * float(y - center_y);
- if (sqr_dist <= sqr_radius) {
- return true;
- }
- }
+ int center_x, int center_y, unsigned char value, float radius)
+{
+ int start_x = int(center_x - radius);
+ int start_y = int(center_y - radius);
+ int end_x = int(center_x + radius);
+ int end_y = int(center_y + radius);
+
+ if (start_x < 0)
+ {
+ start_x = 0;
+ }
+ if (start_y < 0)
+ {
+ start_y = 0;
+ }
+ if (end_x >= int(width))
+ {
+ end_x = width - 1;
+ }
+ if (end_y >= int(height))
+ {
+ end_y = height - 1;
+ }
+
+ float sqr_radius = radius * radius;
+
+ for (int y = start_y; y <= end_y; y++)
+ for (int x = start_x; x <= end_x; x++)
+ {
+ if (map->at(x + width * y) > value)
+ {
+ float sqr_dist = float(x - center_x) * float(x - center_x) +
+ float(y - center_y) * float(y - center_y);
+ if (sqr_dist <= sqr_radius)
+ {
+ return true;
}
+ }
+ }
- return false;
+ return false;
}
}
diff --git a/homer_nav_libs/src/Explorer/Explorer.cpp b/homer_nav_libs/src/Explorer/Explorer.cpp
index a209d748152fd4e9fdc9d576ea133a9479612809..40a6bf50691cb46d8db88f319ce4b1e009fbbf17 100644
--- a/homer_nav_libs/src/Explorer/Explorer.cpp
+++ b/homer_nav_libs/src/Explorer/Explorer.cpp
@@ -7,1072 +7,1247 @@ Explorer::Explorer(double minAllowedObstacleDistance,
double maxAllowedObstacleDistance,
double minSafeObstacleDistance,
double maxSafeObstacleDistance, double safePathWeight,
- double frontierSafenessFactor, int unknownThreshold) {
- ExplorerConstants::UNKNOWN = unknownThreshold;
-
- m_MinAllowedObstacleDistance = minAllowedObstacleDistance;
- m_MaxAllowedObstacleDistance = maxAllowedObstacleDistance;
-
- m_MinSafeObstacleDistance = minSafeObstacleDistance;
- m_MaxSafeObstacleDistance = maxSafeObstacleDistance;
-
- m_SafePathWeight = safePathWeight;
- m_FrontierSafenessFactor = frontierSafenessFactor;
-
- m_OccupancyMap = 0;
- m_ObstacleTransform = 0;
- m_CostTransform = 0;
- m_TargetMap = 0;
- m_DrivingDistanceTransform = 0;
- m_TargetDistanceTransform = 0;
- m_PathTransform = 0;
- m_ExplorationTransform = 0;
- m_DesiredDistance = 0;
+ double frontierSafenessFactor, int unknownThreshold)
+{
+ ExplorerConstants::UNKNOWN = unknownThreshold;
+
+ m_MinAllowedObstacleDistance = minAllowedObstacleDistance;
+ m_MaxAllowedObstacleDistance = maxAllowedObstacleDistance;
+
+ m_MinSafeObstacleDistance = minSafeObstacleDistance;
+ m_MaxSafeObstacleDistance = maxSafeObstacleDistance;
+
+ m_SafePathWeight = safePathWeight;
+ m_FrontierSafenessFactor = frontierSafenessFactor;
+
+ m_OccupancyMap = 0;
+ m_ObstacleTransform = 0;
+ m_CostTransform = 0;
+ m_TargetMap = 0;
+ m_DrivingDistanceTransform = 0;
+ m_TargetDistanceTransform = 0;
+ m_PathTransform = 0;
+ m_ExplorationTransform = 0;
+ m_DesiredDistance = 0;
}
-Explorer::~Explorer() {
- releaseMaps();
- releaseMap(m_OccupancyMap);
+Explorer::~Explorer()
+{
+ releaseMaps();
+ releaseMap(m_OccupancyMap);
}
-void Explorer::releaseMaps() {
- releaseMap(m_TargetMap);
- releaseMap(m_ObstacleTransform);
- releaseMap(m_CostTransform);
- releaseMap(m_DrivingDistanceTransform);
- releaseMap(m_TargetDistanceTransform);
- releaseMap(m_PathTransform);
- releaseMap(m_ExplorationTransform);
+void Explorer::releaseMaps()
+{
+ releaseMap(m_TargetMap);
+ releaseMap(m_ObstacleTransform);
+ releaseMap(m_CostTransform);
+ releaseMap(m_DrivingDistanceTransform);
+ releaseMap(m_TargetDistanceTransform);
+ releaseMap(m_PathTransform);
+ releaseMap(m_ExplorationTransform);
}
// SETTERS
// ////////////////////////////////////////////////////////////////////////////////////////////////
-void Explorer::setUnknownThreshold(int unknownTresh) {
- ExplorerConstants::UNKNOWN = unknownTresh;
+void Explorer::setUnknownThreshold(int unknownTresh)
+{
+ ExplorerConstants::UNKNOWN = unknownTresh;
}
-void Explorer::setAllowedObstacleDistance(double min, double max) {
- m_MinAllowedObstacleDistance = min;
- m_MaxAllowedObstacleDistance = max;
- releaseMaps();
+void Explorer::setAllowedObstacleDistance(double min, double max)
+{
+ m_MinAllowedObstacleDistance = min;
+ m_MaxAllowedObstacleDistance = max;
+ releaseMaps();
}
-void Explorer::setSafeObstacleDistance(double min, double max) {
- m_MinSafeObstacleDistance = min;
- m_MaxSafeObstacleDistance = max;
- releaseMaps();
+void Explorer::setSafeObstacleDistance(double min, double max)
+{
+ m_MinSafeObstacleDistance = min;
+ m_MaxSafeObstacleDistance = max;
+ releaseMaps();
}
-void Explorer::setSafePathWeight(double weight) {
- m_SafePathWeight = weight;
- releaseMaps();
+void Explorer::setSafePathWeight(double weight)
+{
+ m_SafePathWeight = weight;
+ releaseMaps();
}
-void Explorer::setFrontierSafenessFactor(double frontierSafenessFactor) {
- m_FrontierSafenessFactor = frontierSafenessFactor;
- releaseMaps();
+void Explorer::setFrontierSafenessFactor(double frontierSafenessFactor)
+{
+ m_FrontierSafenessFactor = frontierSafenessFactor;
+ releaseMaps();
}
void Explorer::setOccupancyMap(int width, int height,
- geometry_msgs::Pose origin, int8_t* data) {
- if (!data) {
- ROS_ERROR("Received 0-pointer.");
- return;
- }
- releaseMaps();
- releaseMap(m_OccupancyMap);
- // m_OccupancyMap = new GridMap<unsigned char> ( width, height, data,
- // exploredRegion );
- m_OccupancyMap = new GridMap<int8_t>(width, height, data);
- m_Origin = origin;
+ geometry_msgs::Pose origin, int8_t* data)
+{
+ if (!data)
+ {
+ ROS_ERROR("Received 0-pointer.");
+ return;
+ }
+ releaseMaps();
+ releaseMap(m_OccupancyMap);
+ // m_OccupancyMap = new GridMap<unsigned char> ( width, height, data,
+ // exploredRegion );
+ m_OccupancyMap = new GridMap<int8_t>(width, height, data);
+ m_Origin = origin;
}
-void Explorer::setOccupancyMap(const nav_msgs::OccupancyGrid::ConstPtr& cmap) {
- releaseMaps();
- releaseMap(m_OccupancyMap);
- // m_OccupancyMap = new GridMap<unsigned char> ( width, height, data,
- // exploredRegion );
- nav_msgs::OccupancyGrid temp_map = *cmap;
- m_OccupancyMap = new GridMap<int8_t>(cmap->info.width, cmap->info.height,
- &(temp_map.data)[0]);
- m_Origin = cmap->info.origin;
+void Explorer::setOccupancyMap(const nav_msgs::OccupancyGrid::ConstPtr& cmap)
+{
+ releaseMaps();
+ releaseMap(m_OccupancyMap);
+ // m_OccupancyMap = new GridMap<unsigned char> ( width, height, data,
+ // exploredRegion );
+ nav_msgs::OccupancyGrid temp_map = *cmap;
+ m_OccupancyMap = new GridMap<int8_t>(cmap->info.width, cmap->info.height,
+ &(temp_map.data)[0]);
+ m_Origin = cmap->info.origin;
}
void Explorer::updateObstacles(int width, int height,
- geometry_msgs::Pose origin, int8_t* mapData) {
- if (!m_OccupancyMap) {
- ROS_ERROR("Occupancy map is missing.");
- return;
- }
- if ((width != m_OccupancyMap->width()) ||
- (height != m_OccupancyMap->height())) {
- ROS_ERROR_STREAM("Wrong map size!");
- return;
- }
- for (unsigned i = 0; i < m_OccupancyMap->width() * m_OccupancyMap->height();
- i++) {
- int8_t* myMapData = m_OccupancyMap->getDirectAccess(0, 0);
- if (myMapData[i] != UNKNOWN) {
- myMapData[i] = mapData[i];
- }
+ geometry_msgs::Pose origin, int8_t* mapData)
+{
+ if (!m_OccupancyMap)
+ {
+ ROS_ERROR("Occupancy map is missing.");
+ return;
+ }
+ if ((width != m_OccupancyMap->width()) ||
+ (height != m_OccupancyMap->height()))
+ {
+ ROS_ERROR_STREAM("Wrong map size!");
+ return;
+ }
+ for (unsigned i = 0; i < m_OccupancyMap->width() * m_OccupancyMap->height();
+ i++)
+ {
+ int8_t* myMapData = m_OccupancyMap->getDirectAccess(0, 0);
+ if (myMapData[i] != UNKNOWN)
+ {
+ myMapData[i] = mapData[i];
}
- releaseMaps();
+ }
+ releaseMaps();
}
-void Explorer::resetExploration() { m_DesiredDistance = 0; }
+void Explorer::resetExploration()
+{
+ m_DesiredDistance = 0;
+}
-void Explorer::setStart(Eigen::Vector2i start) {
- if (!m_OccupancyMap) {
- ROS_ERROR_STREAM("Occupancy map is missing.");
- return;
- }
- if (start.x() <= 1)
- {
- start.x() = 2;
- }
- if (start.y() <= 1)
- {
- start.y() = 2;
- }
- if (start.x() >= m_OccupancyMap->width() - 1)
+void Explorer::setStart(Eigen::Vector2i start)
+{
+ if (!m_OccupancyMap)
+ {
+ ROS_ERROR_STREAM("Occupancy map is missing.");
+ return;
+ }
+ if (start.x() <= 1)
+ {
+ start.x() = 2;
+ }
+ if (start.y() <= 1)
+ {
+ start.y() = 2;
+ }
+ if (start.x() >= m_OccupancyMap->width() - 1)
+ {
+ start.x() = m_OccupancyMap->width() - 2;
+ }
+ if (start.y() >= m_OccupancyMap->height() - 1)
+ {
+ start.y() = m_OccupancyMap->height() - 2;
+ }
+ computeWalkableMaps();
+
+ if (!isWalkable(start.x(), start.y()))
+ {
+ Eigen::Vector2i correctedStart = getNearestWalkablePoint(start);
+ if (!isWalkable(correctedStart.x(), correctedStart.y()))
{
- start.x() = m_OccupancyMap->width() - 2;
+ ROS_ERROR_STREAM("No walkable position was found on the map!");
}
- if (start.y() >= m_OccupancyMap->height() - 1)
+ else
{
- start.y() = m_OccupancyMap->height() -2;
- }
- computeWalkableMaps();
-
- if (!isWalkable(start.x(), start.y())) {
- Eigen::Vector2i correctedStart = getNearestWalkablePoint(start);
- if (!isWalkable(correctedStart.x(), correctedStart.y())) {
- ROS_ERROR_STREAM("No walkable position was found on the map!");
- } else {
- ROS_INFO_STREAM("Start position " << start.x() << "," << start.y()
- << " was corrected to "
- << correctedStart.x() << ","
- << correctedStart.y());
- }
- m_Start = correctedStart;
- return;
- }
- m_Start = start;
+ ROS_INFO_STREAM("Start position "
+ << start.x() << "," << start.y() << " was corrected to "
+ << correctedStart.x() << "," << correctedStart.y());
+ }
+ m_Start = correctedStart;
+ return;
+ }
+ m_Start = start;
}
-Eigen::Vector2i Explorer::getNearestAccessibleTarget(Eigen::Vector2i target) {
- if (!m_OccupancyMap) {
- ROS_ERROR("Occupancy map is missing.");
- return target;
- }
- if (target.x() <= 1)
- {
- target.x() = 2;
- }
- if (target.y() <= 1)
- {
- target.y() = 2;
- }
- if (target.x() >= m_OccupancyMap->width() - 1)
- {
- target.x() = m_OccupancyMap->width() - 2;
- }
- if (target.y() >= m_OccupancyMap->height() - 1)
+Eigen::Vector2i Explorer::getNearestAccessibleTarget(Eigen::Vector2i target)
+{
+ if (!m_OccupancyMap)
+ {
+ ROS_ERROR("Occupancy map is missing.");
+ return target;
+ }
+ if (target.x() <= 1)
+ {
+ target.x() = 2;
+ }
+ if (target.y() <= 1)
+ {
+ target.y() = 2;
+ }
+ if (target.x() >= m_OccupancyMap->width() - 1)
+ {
+ target.x() = m_OccupancyMap->width() - 2;
+ }
+ if (target.y() >= m_OccupancyMap->height() - 1)
+ {
+ target.y() = m_OccupancyMap->height() - 2;
+ }
+ computeApproachableMaps();
+ computeWalkableMaps();
+ Eigen::Vector2i correctTarget = target;
+
+ if (!isApproachable(target.x(), target.y()))
+ {
+ ROS_INFO_STREAM(
+ "target cell in drivingdistancetransform: "
+ << m_DrivingDistanceTransform->getValue(target.x(), target.y()));
+ ROS_INFO_STREAM("target " << target << " is not approachable. Correcting "
+ "target...");
+ int minSqrDist = INT_MAX;
+ for (int x = 0; x < m_ObstacleTransform->width(); x++)
{
- target.y() = m_OccupancyMap->height() -2;
- }
- computeApproachableMaps();
- computeWalkableMaps();
- Eigen::Vector2i correctTarget = target;
-
- if (!isApproachable(target.x(), target.y())) {
- ROS_INFO_STREAM(
- "target cell in drivingdistancetransform: "
- << m_DrivingDistanceTransform->getValue(target.x(), target.y()));
- ROS_INFO_STREAM("target "
- << target
- << " is not approachable. Correcting target...");
- int minSqrDist = INT_MAX;
- for (int x = 0; x < m_ObstacleTransform->width(); x++) {
- for (int y = 0; y < m_ObstacleTransform->height(); y++) {
- bool isSafe =
- m_ObstacleTransform->getValue(x, y) >
- m_FrontierSafenessFactor * m_MinAllowedObstacleDistance;
- if (isApproachable(x, y) && isWalkable(x, y) && isSafe) {
- int xDiff = target.x() - x;
- int yDiff = target.y() - y;
- int sqrDist = xDiff * xDiff + yDiff * yDiff;
- if (sqrDist < minSqrDist) {
- correctTarget.x() = x;
- correctTarget.y() = y;
- minSqrDist = sqrDist;
- }
- }
- }
+ for (int y = 0; y < m_ObstacleTransform->height(); y++)
+ {
+ bool isSafe = m_ObstacleTransform->getValue(x, y) >
+ m_FrontierSafenessFactor * m_MinAllowedObstacleDistance;
+ if (isApproachable(x, y) && isWalkable(x, y) && isSafe)
+ {
+ int xDiff = target.x() - x;
+ int yDiff = target.y() - y;
+ int sqrDist = xDiff * xDiff + yDiff * yDiff;
+ if (sqrDist < minSqrDist)
+ {
+ correctTarget.x() = x;
+ correctTarget.y() = y;
+ minSqrDist = sqrDist;
+ }
}
+ }
}
- ROS_DEBUG_STREAM("Target position "
- << target.x() << "," << target.y() << " was corrected to "
- << correctTarget.x() << "," << correctTarget.y());
+ }
+ ROS_DEBUG_STREAM("Target position "
+ << target.x() << "," << target.y() << " was corrected to "
+ << correctTarget.x() << "," << correctTarget.y());
- return correctTarget;
+ return correctTarget;
}
-Eigen::Vector2i Explorer::getNearestWalkablePoint(Eigen::Vector2i target) {
- if (!m_OccupancyMap) {
- ROS_ERROR("Occupancy map is missing.");
- return target;
- }
- if (target.x() <= 1)
- {
- target.x() = 2;
- }
- if (target.y() <= 1)
- {
- target.y() = 2;
- }
- if (target.x() >= m_OccupancyMap->width() - 1)
- {
- target.x() = m_OccupancyMap->width() - 2;
- }
- if (target.y() >= m_OccupancyMap->height() - 1)
- {
- target.y() = m_OccupancyMap->height() -2;
- }
+Eigen::Vector2i Explorer::getNearestWalkablePoint(Eigen::Vector2i target)
+{
+ if (!m_OccupancyMap)
+ {
+ ROS_ERROR("Occupancy map is missing.");
+ return target;
+ }
+ if (target.x() <= 1)
+ {
+ target.x() = 2;
+ }
+ if (target.y() <= 1)
+ {
+ target.y() = 2;
+ }
+ if (target.x() >= m_OccupancyMap->width() - 1)
+ {
+ target.x() = m_OccupancyMap->width() - 2;
+ }
+ if (target.y() >= m_OccupancyMap->height() - 1)
+ {
+ target.y() = m_OccupancyMap->height() - 2;
+ }
- computeWalkableMaps();
- Eigen::Vector2i correctTarget = target;
-
- if (!isWalkable(target.x(), target.y())) {
- int minSqrDist = INT_MAX;
- for (int x = 0; x < m_ObstacleTransform->width(); x++) {
- for (int y = 0; y < m_ObstacleTransform->height(); y++) {
- if (isWalkable(x, y)) {
- int xDiff = target.x() - x;
- int yDiff = target.y() - y;
- int sqrDist = xDiff * xDiff + yDiff * yDiff;
- if (sqrDist < minSqrDist) {
- correctTarget.x() = x;
- correctTarget.y() = y;
- minSqrDist = sqrDist;
- }
- }
- }
+ computeWalkableMaps();
+ Eigen::Vector2i correctTarget = target;
+
+ if (!isWalkable(target.x(), target.y()))
+ {
+ int minSqrDist = INT_MAX;
+ for (int x = 0; x < m_ObstacleTransform->width(); x++)
+ {
+ for (int y = 0; y < m_ObstacleTransform->height(); y++)
+ {
+ if (isWalkable(x, y))
+ {
+ int xDiff = target.x() - x;
+ int yDiff = target.y() - y;
+ int sqrDist = xDiff * xDiff + yDiff * yDiff;
+ if (sqrDist < minSqrDist)
+ {
+ correctTarget.x() = x;
+ correctTarget.y() = y;
+ minSqrDist = sqrDist;
+ }
}
+ }
}
- ROS_DEBUG_STREAM("Position " << target.x() << "," << target.y()
- << " was corrected to " << correctTarget.x()
- << "," << correctTarget.y());
+ }
+ ROS_DEBUG_STREAM("Position " << target.x() << "," << target.y()
+ << " was corrected to " << correctTarget.x()
+ << "," << correctTarget.y());
- return correctTarget;
+ return correctTarget;
}
-void Explorer::setTarget(Eigen::Vector2i target) {
- if (!m_OccupancyMap) {
- ROS_ERROR("Occupancy map is missing.");
- return;
- }
- if (target.x() <= 1)
- {
- target.x() = 2;
- }
- if (target.y() <= 1)
- {
- target.y() = 2;
- }
- if (target.x() >= m_OccupancyMap->width() - 1)
- {
- target.x() = m_OccupancyMap->width() - 2;
- }
- if (target.y() >= m_OccupancyMap->height() - 1)
- {
- target.y() = m_OccupancyMap->height() -2;
- }
- computeApproachableMaps();
- if (!isApproachable(target.x(), target.y())) {
- ROS_WARN(
- "Target position is not approachable. Path computation will "
- "possibly fail.");
- }
- m_Target = target;
- m_DesiredDistance = 0;
+void Explorer::setTarget(Eigen::Vector2i target)
+{
+ if (!m_OccupancyMap)
+ {
+ ROS_ERROR("Occupancy map is missing.");
+ return;
+ }
+ if (target.x() <= 1)
+ {
+ target.x() = 2;
+ }
+ if (target.y() <= 1)
+ {
+ target.y() = 2;
+ }
+ if (target.x() >= m_OccupancyMap->width() - 1)
+ {
+ target.x() = m_OccupancyMap->width() - 2;
+ }
+ if (target.y() >= m_OccupancyMap->height() - 1)
+ {
+ target.y() = m_OccupancyMap->height() - 2;
+ }
+ computeApproachableMaps();
+ if (!isApproachable(target.x(), target.y()))
+ {
+ ROS_WARN("Target position is not approachable. Path computation will "
+ "possibly fail.");
+ }
+ m_Target = target;
+ m_DesiredDistance = 0;
}
-void Explorer::setTarget(Eigen::Vector2i target, int desiredDistance) {
- ROS_ERROR_STREAM("setTarget still in use!!");
- if (!m_OccupancyMap) {
- ROS_ERROR("Occupancy map is missing.");
- return;
- }
+void Explorer::setTarget(Eigen::Vector2i target, int desiredDistance)
+{
+ ROS_ERROR_STREAM("setTarget still in use!!");
+ if (!m_OccupancyMap)
+ {
+ ROS_ERROR("Occupancy map is missing.");
+ return;
+ }
- if (desiredDistance < 1) {
- setTarget(target);
- return;
- }
+ if (desiredDistance < 1)
+ {
+ setTarget(target);
+ return;
+ }
- if (target.x() + desiredDistance <= 1)
- {
- target.x() = 2;
- }
- if (target.y() + desiredDistance <= 1)
- {
- target.y() = 2;
- }
- if (target.x() - desiredDistance >= m_OccupancyMap->width() - 1)
- {
- target.x() = m_OccupancyMap->width() - 2;
- }
- if (target.y() - desiredDistance >= m_OccupancyMap->height() - 1)
- {
- target.y() = m_OccupancyMap->height() -2;
- }
- computeApproachableMaps();
- // TODO: check if region is approachable
- m_Target = target;
- m_DesiredDistance = desiredDistance;
+ if (target.x() + desiredDistance <= 1)
+ {
+ target.x() = 2;
+ }
+ if (target.y() + desiredDistance <= 1)
+ {
+ target.y() = 2;
+ }
+ if (target.x() - desiredDistance >= m_OccupancyMap->width() - 1)
+ {
+ target.x() = m_OccupancyMap->width() - 2;
+ }
+ if (target.y() - desiredDistance >= m_OccupancyMap->height() - 1)
+ {
+ target.y() = m_OccupancyMap->height() - 2;
+ }
+ computeApproachableMaps();
+ // TODO: check if region is approachable
+ m_Target = target;
+ m_DesiredDistance = desiredDistance;
}
// GETTERS
// ////////////////////////////////////////////////////////////////////////////////////////////////
-Eigen::Vector2i Explorer::getStart() const { return m_Start; }
+Eigen::Vector2i Explorer::getStart() const
+{
+ return m_Start;
+}
-Eigen::Vector2i Explorer::getTarget() const { return m_Target; }
+Eigen::Vector2i Explorer::getTarget() const
+{
+ return m_Target;
+}
-GridMap<int8_t>* Explorer::getOccupancyMap() { return m_OccupancyMap; }
+GridMap<int8_t>* Explorer::getOccupancyMap()
+{
+ return m_OccupancyMap;
+}
-GridMap<double>* Explorer::getObstacleTransform() {
- if (!m_OccupancyMap) {
- ROS_ERROR("Occupancy map is missing.");
- return 0;
- }
- computeObstacleTransform();
- return m_ObstacleTransform;
+GridMap<double>* Explorer::getObstacleTransform()
+{
+ if (!m_OccupancyMap)
+ {
+ ROS_ERROR("Occupancy map is missing.");
+ return 0;
+ }
+ computeObstacleTransform();
+ return m_ObstacleTransform;
}
-GridMap<double>* Explorer::getCostTransform() {
- if (!m_OccupancyMap) {
- ROS_ERROR("Occupancy map is missing.");
- return 0;
- }
- computeCostTransform();
- return m_CostTransform;
+GridMap<double>* Explorer::getCostTransform()
+{
+ if (!m_OccupancyMap)
+ {
+ ROS_ERROR("Occupancy map is missing.");
+ return 0;
+ }
+ computeCostTransform();
+ return m_CostTransform;
}
-GridMap<bool>* Explorer::getTargetMap() {
- if (!m_OccupancyMap) {
- ROS_ERROR("Occupancy map is missing.");
- return 0;
- }
+GridMap<bool>* Explorer::getTargetMap()
+{
+ if (!m_OccupancyMap)
+ {
+ ROS_ERROR("Occupancy map is missing.");
+ return 0;
+ }
- computeTargetMap();
- return m_TargetMap;
+ computeTargetMap();
+ return m_TargetMap;
}
-GridMap<double>* Explorer::getDrivingDistanceTransform() {
- if (!m_OccupancyMap) {
- ROS_ERROR("Occupancy map is missing.");
- return 0;
- }
- computeDrivingDistanceTransform();
- return m_DrivingDistanceTransform;
+GridMap<double>* Explorer::getDrivingDistanceTransform()
+{
+ if (!m_OccupancyMap)
+ {
+ ROS_ERROR("Occupancy map is missing.");
+ return 0;
+ }
+ computeDrivingDistanceTransform();
+ return m_DrivingDistanceTransform;
}
-GridMap<double>* Explorer::getTargetDistanceTransform() {
- if (!m_OccupancyMap) {
- ROS_ERROR("Occupancy map is missing.");
- return 0;
- }
- computeTargetDistanceTransform();
- return m_TargetDistanceTransform;
+GridMap<double>* Explorer::getTargetDistanceTransform()
+{
+ if (!m_OccupancyMap)
+ {
+ ROS_ERROR("Occupancy map is missing.");
+ return 0;
+ }
+ computeTargetDistanceTransform();
+ return m_TargetDistanceTransform;
}
-GridMap<double>* Explorer::getPathTransform() {
- if (!m_OccupancyMap) {
- ROS_ERROR("Occupancy map is missing.");
- return 0;
- }
- computePathTransform();
- return m_PathTransform;
+GridMap<double>* Explorer::getPathTransform()
+{
+ if (!m_OccupancyMap)
+ {
+ ROS_ERROR("Occupancy map is missing.");
+ return 0;
+ }
+ computePathTransform();
+ return m_PathTransform;
}
-GridMap<double>* Explorer::getExplorationTransform() {
- if (!m_OccupancyMap) {
- ROS_ERROR("Occupancy map is missing.");
- return 0;
- }
- computeExplorationTransform();
- return m_ExplorationTransform;
+GridMap<double>* Explorer::getExplorationTransform()
+{
+ if (!m_OccupancyMap)
+ {
+ ROS_ERROR("Occupancy map is missing.");
+ return 0;
+ }
+ computeExplorationTransform();
+ return m_ExplorationTransform;
}
// MAP GENERATION
// ////////////////////////////////////////////////////////////////////////////////////////////////7
-void Explorer::computeApproachableMaps() {
- if (!m_OccupancyMap) {
- ROS_ERROR("Occupancy map is missing.");
- return;
- }
- computeDrivingDistanceTransform();
+void Explorer::computeApproachableMaps()
+{
+ if (!m_OccupancyMap)
+ {
+ ROS_ERROR("Occupancy map is missing.");
+ return;
+ }
+ computeDrivingDistanceTransform();
}
-void Explorer::computeWalkableMaps() {
- if (!m_OccupancyMap) {
- ROS_ERROR("Occupancy map is missing.");
- return;
- }
- computeObstacleTransform();
+void Explorer::computeWalkableMaps()
+{
+ if (!m_OccupancyMap)
+ {
+ ROS_ERROR("Occupancy map is missing.");
+ return;
+ }
+ computeObstacleTransform();
}
-void Explorer::computeDrivingDistanceTransform() {
- if (!m_OccupancyMap) {
- ROS_ERROR("Occupancy map is missing.");
- return;
- }
+void Explorer::computeDrivingDistanceTransform()
+{
+ if (!m_OccupancyMap)
+ {
+ ROS_ERROR("Occupancy map is missing.");
+ return;
+ }
- if (m_DrivingDistanceTransform) {
- return;
- }
+ if (m_DrivingDistanceTransform)
+ {
+ return;
+ }
- ROS_DEBUG("Computing drivingDistanceTransform...");
- resetMap(m_DrivingDistanceTransform);
- distanceFloodFill(m_DrivingDistanceTransform, m_Start);
+ ROS_DEBUG("Computing drivingDistanceTransform...");
+ resetMap(m_DrivingDistanceTransform);
+ distanceFloodFill(m_DrivingDistanceTransform, m_Start);
}
-void Explorer::computeTargetDistanceTransform() {
- if (!m_OccupancyMap) {
- ROS_ERROR("Occupancy map is missing.");
- return;
- }
+void Explorer::computeTargetDistanceTransform()
+{
+ if (!m_OccupancyMap)
+ {
+ ROS_ERROR("Occupancy map is missing.");
+ return;
+ }
- if (m_TargetDistanceTransform) {
- return;
- }
+ if (m_TargetDistanceTransform)
+ {
+ return;
+ }
- ROS_DEBUG("Computing targetDistanceTransform...");
- resetMap(m_TargetDistanceTransform);
- distanceFloodFill(m_TargetDistanceTransform, m_Target);
+ ROS_DEBUG("Computing targetDistanceTransform...");
+ resetMap(m_TargetDistanceTransform);
+ distanceFloodFill(m_TargetDistanceTransform, m_Target);
}
-void Explorer::computeRegionMap() {
- if (!m_OccupancyMap) {
- ROS_ERROR("Occupancy map is missing.");
- return;
- }
+void Explorer::computeRegionMap()
+{
+ if (!m_OccupancyMap)
+ {
+ ROS_ERROR("Occupancy map is missing.");
+ return;
+ }
+
+ resetMap(m_TargetMap);
+ ROS_DEBUG("Computing target region map...");
+
+ m_TargetMap->fill(false);
+ const int desiredDistanceSquared = m_DesiredDistance * m_DesiredDistance;
+ int height = m_OccupancyMap->height();
+ int width = m_OccupancyMap->width();
+
+ // draw a circle onto the ExplorationMap
+ const int firstX = m_Target.x() - m_DesiredDistance <= 1 ?
+ 2 :
+ m_Target.x() - m_DesiredDistance;
+ const int firstY = m_Target.y() - m_DesiredDistance <= 1 ?
+ 2 :
+ m_Target.y() - m_DesiredDistance;
+ const int lastX = m_Target.x() + m_DesiredDistance >= width - 1 ?
+ width - 2 :
+ m_Target.x() + m_DesiredDistance;
+ const int lastY = m_Target.y() + m_DesiredDistance >= height - 1 ?
+ height - 2 :
+ m_Target.y() + m_DesiredDistance;
+
+ for (int y = firstY; y <= lastY; ++y)
+ {
+ for (int x = firstX; x <= lastX; ++x)
+ {
+ const int dx = x - m_Target.x();
+ const int dy = y - m_Target.y();
- resetMap(m_TargetMap);
- ROS_DEBUG("Computing target region map...");
-
- m_TargetMap->fill(false);
- const int desiredDistanceSquared = m_DesiredDistance * m_DesiredDistance;
- int height = m_OccupancyMap->height();
- int width = m_OccupancyMap->width();
-
- // draw a circle onto the ExplorationMap
- const int firstX = m_Target.x() - m_DesiredDistance <= 1
- ? 2
- : m_Target.x() - m_DesiredDistance;
- const int firstY = m_Target.y() - m_DesiredDistance <= 1
- ? 2
- : m_Target.y() - m_DesiredDistance;
- const int lastX = m_Target.x() + m_DesiredDistance >= width - 1
- ? width - 2
- : m_Target.x() + m_DesiredDistance;
- const int lastY = m_Target.y() + m_DesiredDistance >= height - 1
- ? height - 2
- : m_Target.y() + m_DesiredDistance;
-
- for (int y = firstY; y <= lastY; ++y) {
- for (int x = firstX; x <= lastX; ++x) {
- const int dx = x - m_Target.x();
- const int dy = y - m_Target.y();
-
- if (dx * dx + dy * dy <= desiredDistanceSquared) {
- m_TargetMap->setValue(x, y, true);
- }
- }
+ if (dx * dx + dy * dy <= desiredDistanceSquared)
+ {
+ m_TargetMap->setValue(x, y, true);
+ }
}
+ }
}
-void Explorer::computeFrontierMap() {
- if (!m_OccupancyMap) {
- ROS_ERROR("Occupancy map is missing.");
- return;
- }
+void Explorer::computeFrontierMap()
+{
+ if (!m_OccupancyMap)
+ {
+ ROS_ERROR("Occupancy map is missing.");
+ return;
+ }
- // if ( m_FrontierMap ) { return; }
-
- resetMap(m_TargetMap);
-
- ROS_DEBUG("Computing frontier map...");
- m_TargetMap->fill(0);
- // extract borders
- for (int y = 1; y < m_OccupancyMap->height() - 1; y++) {
- for (int x = 1; x < m_OccupancyMap->width() - 1; x++) {
- int value = m_OccupancyMap->getValue(x, y);
- int value_u = m_OccupancyMap->getValue(x, y - 1);
- int value_d = m_OccupancyMap->getValue(x, y + 1);
- int value_l = m_OccupancyMap->getValue(x - 1, y);
- int value_r = m_OccupancyMap->getValue(x + 1, y);
- bool isFree = value < UNKNOWN && value != NOT_SEEN_YET;
- bool upUnknown = (value_u == UNKNOWN || value_u == NOT_SEEN_YET);
- bool downUnknown = (value_d == UNKNOWN || value_u == NOT_SEEN_YET);
- bool leftUnknown = (value_l == UNKNOWN || value_u == NOT_SEEN_YET);
- bool rightUnknown = (value_r == UNKNOWN || value_u == NOT_SEEN_YET);
- bool hasUnknownNeighbour =
- upUnknown || downUnknown || leftUnknown || rightUnknown;
- bool isSafe =
- m_ObstacleTransform->getValue(x, y) >
- m_FrontierSafenessFactor * m_MinAllowedObstacleDistance;
- if (isFree && hasUnknownNeighbour && isSafe) {
- m_TargetMap->setValue(x, y, 1);
- } else {
- m_TargetMap->setValue(x, y, 0);
- }
- }
- }
-}
+ // if ( m_FrontierMap ) { return; }
+
+ resetMap(m_TargetMap);
-void Explorer::computeTargetMap() {
- ROS_ERROR_STREAM("target Map shouldn't be used anymore!");
- if (m_DesiredDistance < 1) {
- computeFrontierMap();
- } else {
- computeRegionMap();
+ ROS_DEBUG("Computing frontier map...");
+ m_TargetMap->fill(0);
+ // extract borders
+ for (int y = 1; y < m_OccupancyMap->height() - 1; y++)
+ {
+ for (int x = 1; x < m_OccupancyMap->width() - 1; x++)
+ {
+ int value = m_OccupancyMap->getValue(x, y);
+ int value_u = m_OccupancyMap->getValue(x, y - 1);
+ int value_d = m_OccupancyMap->getValue(x, y + 1);
+ int value_l = m_OccupancyMap->getValue(x - 1, y);
+ int value_r = m_OccupancyMap->getValue(x + 1, y);
+ bool isFree = value < UNKNOWN && value != NOT_SEEN_YET;
+ bool upUnknown = (value_u == UNKNOWN || value_u == NOT_SEEN_YET);
+ bool downUnknown = (value_d == UNKNOWN || value_u == NOT_SEEN_YET);
+ bool leftUnknown = (value_l == UNKNOWN || value_u == NOT_SEEN_YET);
+ bool rightUnknown = (value_r == UNKNOWN || value_u == NOT_SEEN_YET);
+ bool hasUnknownNeighbour =
+ upUnknown || downUnknown || leftUnknown || rightUnknown;
+ bool isSafe = m_ObstacleTransform->getValue(x, y) >
+ m_FrontierSafenessFactor * m_MinAllowedObstacleDistance;
+ if (isFree && hasUnknownNeighbour && isSafe)
+ {
+ m_TargetMap->setValue(x, y, 1);
+ }
+ else
+ {
+ m_TargetMap->setValue(x, y, 0);
+ }
}
+ }
}
-void Explorer::computeObstacleTransform() {
- if (!m_OccupancyMap) {
- ROS_ERROR("Missing occupancy map. Aborting.");
- return;
- }
+void Explorer::computeTargetMap()
+{
+ ROS_ERROR_STREAM("target Map shouldn't be used anymore!");
+ if (m_DesiredDistance < 1)
+ {
+ computeFrontierMap();
+ }
+ else
+ {
+ computeRegionMap();
+ }
+}
- if (m_ObstacleTransform) {
- return;
- }
+void Explorer::computeObstacleTransform()
+{
+ if (!m_OccupancyMap)
+ {
+ ROS_ERROR("Missing occupancy map. Aborting.");
+ return;
+ }
- resetMap(m_ObstacleTransform);
-
- ROS_DEBUG("Computing obstacle transform...");
- for (int x = 0; x < m_ObstacleTransform->width(); x++) {
- for (int y = 0; y < m_ObstacleTransform->height(); y++) {
- if (m_OccupancyMap->getValue(x, y) > UNKNOWN ||
- m_OccupancyMap->getValue(x, y) == NOT_SEEN_YET) {
- m_ObstacleTransform->setValue(x, y, 0); // Obstacle
- } else {
- m_ObstacleTransform->setValue(x, y, INT_MAX); // Free
- }
- }
- }
+ if (m_ObstacleTransform)
+ {
+ return;
+ }
- int width = m_ObstacleTransform->width();
- int height = m_ObstacleTransform->height();
- double* f = new double[width > height ? width : height];
+ resetMap(m_ObstacleTransform);
- // transform along columns
- for (int x = 0; x < width; x++) {
- for (int y = 0; y < height; y++) {
- // copy column
- f[y] = m_ObstacleTransform->getValue(x, y);
- }
- // 1-D transform of column
- double* d = distanceTransform1D(f, height);
- // copy transformed 1-D to output image
- for (int y = 0; y < height; y++) {
- m_ObstacleTransform->setValue(x, y, d[y]);
- }
- delete[] d;
+ ROS_DEBUG("Computing obstacle transform...");
+ for (int x = 0; x < m_ObstacleTransform->width(); x++)
+ {
+ for (int y = 0; y < m_ObstacleTransform->height(); y++)
+ {
+ if (m_OccupancyMap->getValue(x, y) > UNKNOWN ||
+ m_OccupancyMap->getValue(x, y) == NOT_SEEN_YET)
+ {
+ m_ObstacleTransform->setValue(x, y, 0); // Obstacle
+ }
+ else
+ {
+ m_ObstacleTransform->setValue(x, y, INT_MAX); // Free
+ }
}
+ }
- // transform along rows
- for (int y = 0; y < height; y++) {
- for (int x = 0; x < width; x++) {
- f[x] = m_ObstacleTransform->getValue(x, y);
- }
- double* d = distanceTransform1D(f, width);
- for (int x = 0; x < width; x++) {
- m_ObstacleTransform->setValue(x, y, d[x]);
- }
- delete[] d;
+ int width = m_ObstacleTransform->width();
+ int height = m_ObstacleTransform->height();
+ double* f = new double[width > height ? width : height];
+
+ // transform along columns
+ for (int x = 0; x < width; x++)
+ {
+ for (int y = 0; y < height; y++)
+ {
+ // copy column
+ f[y] = m_ObstacleTransform->getValue(x, y);
}
- delete f;
-
- // take square roots
- for (int y = 0; y < m_ObstacleTransform->height(); y++) {
- for (int x = 0; x < m_ObstacleTransform->width(); x++) {
- if (isWalkable(x, y)) {
- float value = sqrt(m_ObstacleTransform->getValue(x, y));
- m_ObstacleTransform->setValue(x, y, value);
- }
- }
+ // 1-D transform of column
+ double* d = distanceTransform1D(f, height);
+ // copy transformed 1-D to output image
+ for (int y = 0; y < height; y++)
+ {
+ m_ObstacleTransform->setValue(x, y, d[y]);
}
-}
+ delete[] d;
+ }
-void Explorer::computeCostTransform() {
- if (!m_OccupancyMap) {
- ROS_ERROR("Missing occupancy map. Aborting.");
- return;
+ // transform along rows
+ for (int y = 0; y < height; y++)
+ {
+ for (int x = 0; x < width; x++)
+ {
+ f[x] = m_ObstacleTransform->getValue(x, y);
}
-
- if (m_CostTransform) {
- return;
+ double* d = distanceTransform1D(f, width);
+ for (int x = 0; x < width; x++)
+ {
+ m_ObstacleTransform->setValue(x, y, d[x]);
}
+ delete[] d;
+ }
+ delete f;
- computeObstacleTransform();
- computeApproachableMaps();
-
- resetMap(m_CostTransform);
- m_CostTransform->fill(ExplorerConstants::MAX_COST);
-
- for (unsigned y = 0; y < m_CostTransform->height(); y++) {
- for (unsigned x = 0; x < m_CostTransform->width(); x++) {
- if (!isApproachable(x, y)) {
- continue;
- }
- double dist = m_ObstacleTransform->getValue(x, y);
- double cost = 0;
- if (dist < m_MinSafeObstacleDistance) {
- cost = m_MinSafeObstacleDistance - dist;
- }
- // if ( dist > m_MaxSafeObstacleDistance ) {
- // cost = dist - m_MaxSafeObstacleDistance;
- // }
- m_CostTransform->setValue(x, y, cost * cost);
- }
+ // take square roots
+ for (int y = 0; y < m_ObstacleTransform->height(); y++)
+ {
+ for (int x = 0; x < m_ObstacleTransform->width(); x++)
+ {
+ if (isWalkable(x, y))
+ {
+ float value = sqrt(m_ObstacleTransform->getValue(x, y));
+ m_ObstacleTransform->setValue(x, y, value);
+ }
}
+ }
}
-void Explorer::computePathTransform() {
- if (!m_OccupancyMap) {
- ROS_ERROR("Missing occupancy map. Aborting.");
- return;
- }
+void Explorer::computeCostTransform()
+{
+ if (!m_OccupancyMap)
+ {
+ ROS_ERROR("Missing occupancy map. Aborting.");
+ return;
+ }
+
+ if (m_CostTransform)
+ {
+ return;
+ }
+
+ computeObstacleTransform();
+ computeApproachableMaps();
- if (m_PathTransform) {
- return;
+ resetMap(m_CostTransform);
+ m_CostTransform->fill(ExplorerConstants::MAX_COST);
+
+ for (unsigned y = 0; y < m_CostTransform->height(); y++)
+ {
+ for (unsigned x = 0; x < m_CostTransform->width(); x++)
+ {
+ if (!isApproachable(x, y))
+ {
+ continue;
+ }
+ double dist = m_ObstacleTransform->getValue(x, y);
+ double cost = 0;
+ if (dist < m_MinSafeObstacleDistance)
+ {
+ cost = m_MinSafeObstacleDistance - dist;
+ }
+ // if ( dist > m_MaxSafeObstacleDistance ) {
+ // cost = dist - m_MaxSafeObstacleDistance;
+ // }
+ m_CostTransform->setValue(x, y, cost * cost);
}
+ }
+}
- computeObstacleTransform();
- computeCostTransform();
-
- resetMap(m_PathTransform);
-
- ROS_DEBUG("Computing path transform...");
- GridMap<double>* map = m_PathTransform;
- int width = map->width();
- int height = map->height();
- double maxDistance = MAX_DISTANCE;
- map->fill(maxDistance);
-
- int fromX = m_Target.x();
- int fromY = m_Target.y();
- map->setValue(fromX, fromY, 0);
-
- queue<int> xQueue;
- queue<int> yQueue;
- xQueue.push(fromX + 1);
- yQueue.push(fromY);
- xQueue.push(fromX - 1);
- yQueue.push(fromY);
- xQueue.push(fromX);
- yQueue.push(fromY - 1);
- xQueue.push(fromX);
- yQueue.push(fromY + 1);
- int xVal, yVal;
- while (!xQueue.empty()) {
- xVal = xQueue.front();
- yVal = yQueue.front();
- xQueue.pop();
- yQueue.pop();
- if (xVal > 0 && xVal < width - 1 && yVal > 0 && yVal < height - 1 &&
- isWalkable(xVal, yVal)) {
- float value = map->getValue(xVal, yVal);
- float value_u = map->getValue(xVal, yVal - 1) + 1;
- float value_d = map->getValue(xVal, yVal + 1) + 1;
- float value_l = map->getValue(xVal - 1, yVal) + 1;
- float value_r = map->getValue(xVal + 1, yVal) + 1;
-
- float value_ur = map->getValue(xVal + 1, yVal - 1) + 1.4142;
- float value_ul = map->getValue(xVal - 1, yVal - 1) + 1.4142;
- float value_ll = map->getValue(xVal - 1, yVal + 1) + 1.4142;
- float value_lr = map->getValue(xVal + 1, yVal + 1) + 1.4142;
-
- float min1 = value_u < value_d ? value_u : value_d;
- float min2 = value_l < value_r ? value_l : value_r;
- float min3 = value_ur < value_ul ? value_ur : value_ul;
- float min4 = value_ll < value_lr ? value_ll : value_lr;
- float min12 = min1 < min2 ? min1 : min2;
- float min34 = min3 < min4 ? min3 : min4;
- float min = min12 < min34 ? min12 : min34;
- float newVal =
- min + m_SafePathWeight * m_CostTransform->getValue(xVal, yVal);
- if (value > newVal) {
- map->setValue(xVal, yVal, newVal);
- if (map->getValue(xVal, yVal + 1) > newVal + 1) {
- xQueue.push(xVal);
- yQueue.push(yVal + 1);
- }
- if (map->getValue(xVal, yVal - 1) > newVal + 1) {
- xQueue.push(xVal);
- yQueue.push(yVal - 1);
- }
- if (map->getValue(xVal + 1, yVal) > newVal + 1) {
- xQueue.push(xVal + 1);
- yQueue.push(yVal);
- }
- if (map->getValue(xVal - 1, yVal) > newVal + 1) {
- xQueue.push(xVal - 1);
- yQueue.push(yVal);
- }
- if (map->getValue(xVal + 1, yVal - 1) > newVal + 1.4142) {
- xQueue.push(xVal + 1);
- yQueue.push(yVal - 1);
- }
- if (map->getValue(xVal - 1, yVal - 1) > newVal + 1.4142) {
- xQueue.push(xVal - 1);
- yQueue.push(yVal - 1);
- }
- if (map->getValue(xVal + 1, yVal + 1) > newVal + 1.4142) {
- xQueue.push(xVal + 1);
- yQueue.push(yVal + 1);
- }
- if (map->getValue(xVal - 1, yVal + 1) > newVal + 1.4142) {
- xQueue.push(xVal - 1);
- yQueue.push(yVal + 1);
- }
- }
+void Explorer::computePathTransform()
+{
+ if (!m_OccupancyMap)
+ {
+ ROS_ERROR("Missing occupancy map. Aborting.");
+ return;
+ }
+
+ if (m_PathTransform)
+ {
+ return;
+ }
+
+ computeObstacleTransform();
+ computeCostTransform();
+
+ resetMap(m_PathTransform);
+
+ ROS_DEBUG("Computing path transform...");
+ GridMap<double>* map = m_PathTransform;
+ int width = map->width();
+ int height = map->height();
+ double maxDistance = MAX_DISTANCE;
+ map->fill(maxDistance);
+
+ int fromX = m_Target.x();
+ int fromY = m_Target.y();
+ map->setValue(fromX, fromY, 0);
+
+ queue<int> xQueue;
+ queue<int> yQueue;
+ xQueue.push(fromX + 1);
+ yQueue.push(fromY);
+ xQueue.push(fromX - 1);
+ yQueue.push(fromY);
+ xQueue.push(fromX);
+ yQueue.push(fromY - 1);
+ xQueue.push(fromX);
+ yQueue.push(fromY + 1);
+ int xVal, yVal;
+ while (!xQueue.empty())
+ {
+ xVal = xQueue.front();
+ yVal = yQueue.front();
+ xQueue.pop();
+ yQueue.pop();
+ if (xVal > 0 && xVal < width - 1 && yVal > 0 && yVal < height - 1 &&
+ isWalkable(xVal, yVal))
+ {
+ float value = map->getValue(xVal, yVal);
+ float value_u = map->getValue(xVal, yVal - 1) + 1;
+ float value_d = map->getValue(xVal, yVal + 1) + 1;
+ float value_l = map->getValue(xVal - 1, yVal) + 1;
+ float value_r = map->getValue(xVal + 1, yVal) + 1;
+
+ float value_ur = map->getValue(xVal + 1, yVal - 1) + 1.4142;
+ float value_ul = map->getValue(xVal - 1, yVal - 1) + 1.4142;
+ float value_ll = map->getValue(xVal - 1, yVal + 1) + 1.4142;
+ float value_lr = map->getValue(xVal + 1, yVal + 1) + 1.4142;
+
+ float min1 = value_u < value_d ? value_u : value_d;
+ float min2 = value_l < value_r ? value_l : value_r;
+ float min3 = value_ur < value_ul ? value_ur : value_ul;
+ float min4 = value_ll < value_lr ? value_ll : value_lr;
+ float min12 = min1 < min2 ? min1 : min2;
+ float min34 = min3 < min4 ? min3 : min4;
+ float min = min12 < min34 ? min12 : min34;
+ float newVal =
+ min + m_SafePathWeight * m_CostTransform->getValue(xVal, yVal);
+ if (value > newVal)
+ {
+ map->setValue(xVal, yVal, newVal);
+ if (map->getValue(xVal, yVal + 1) > newVal + 1)
+ {
+ xQueue.push(xVal);
+ yQueue.push(yVal + 1);
+ }
+ if (map->getValue(xVal, yVal - 1) > newVal + 1)
+ {
+ xQueue.push(xVal);
+ yQueue.push(yVal - 1);
+ }
+ if (map->getValue(xVal + 1, yVal) > newVal + 1)
+ {
+ xQueue.push(xVal + 1);
+ yQueue.push(yVal);
+ }
+ if (map->getValue(xVal - 1, yVal) > newVal + 1)
+ {
+ xQueue.push(xVal - 1);
+ yQueue.push(yVal);
+ }
+ if (map->getValue(xVal + 1, yVal - 1) > newVal + 1.4142)
+ {
+ xQueue.push(xVal + 1);
+ yQueue.push(yVal - 1);
+ }
+ if (map->getValue(xVal - 1, yVal - 1) > newVal + 1.4142)
+ {
+ xQueue.push(xVal - 1);
+ yQueue.push(yVal - 1);
+ }
+ if (map->getValue(xVal + 1, yVal + 1) > newVal + 1.4142)
+ {
+ xQueue.push(xVal + 1);
+ yQueue.push(yVal + 1);
}
+ if (map->getValue(xVal - 1, yVal + 1) > newVal + 1.4142)
+ {
+ xQueue.push(xVal - 1);
+ yQueue.push(yVal + 1);
+ }
+ }
}
+ }
}
-void Explorer::computeExplorationTransform() {
- ROS_ERROR_STREAM("Exploration Transform shouldn't be used!");
- if (!m_OccupancyMap) {
- ROS_ERROR("Missing occupancy map. Aborting.");
- return;
- }
+void Explorer::computeExplorationTransform()
+{
+ ROS_ERROR_STREAM("Exploration Transform shouldn't be used!");
+ if (!m_OccupancyMap)
+ {
+ ROS_ERROR("Missing occupancy map. Aborting.");
+ return;
+ }
- if (m_ExplorationTransform) {
- return;
+ if (m_ExplorationTransform)
+ {
+ return;
+ }
+
+ ROS_DEBUG_STREAM("computeExplorationTransform: before obstacle transform");
+ computeObstacleTransform();
+ ROS_DEBUG_STREAM("computeExplorationTransform: before cost transform");
+ computeCostTransform();
+ ROS_DEBUG_STREAM("computeExplorationTransform: before target map");
+ computeTargetMap();
+ ROS_DEBUG_STREAM("computeExplorationTransform: before walkable maps");
+ computeWalkableMaps();
+ ROS_DEBUG_STREAM("computeExplorationTransform: before exploration transform");
+ resetMap(m_ExplorationTransform);
+
+ ROS_DEBUG("Computing exploration transform...");
+ GridMap<double>* map = m_ExplorationTransform;
+ int width = map->width();
+ int height = map->height();
+ double maxDistance = MAX_DISTANCE;
+ map->fill(maxDistance);
+ queue<int> xQueue;
+ queue<int> yQueue;
+ // fill seeds: Mark the frontiers as targets
+ ROS_DEBUG_STREAM("computeExplorationTransform: before first loop");
+ for (int y = 0; y < m_TargetMap->height(); y++)
+ {
+ for (int x = 0; x < m_TargetMap->width(); x++)
+ {
+ if (m_TargetMap->getValue(x, y) == 1)
+ {
+ map->setValue(x, y, 0);
+ xQueue.push(x + 1);
+ yQueue.push(y);
+ xQueue.push(x - 1);
+ yQueue.push(y);
+ xQueue.push(x);
+ yQueue.push(y - 1);
+ xQueue.push(x);
+ yQueue.push(y + 1);
+ }
}
-
- ROS_DEBUG_STREAM("computeExplorationTransform: before obstacle transform");
- computeObstacleTransform();
- ROS_DEBUG_STREAM("computeExplorationTransform: before cost transform");
- computeCostTransform();
- ROS_DEBUG_STREAM("computeExplorationTransform: before target map");
- computeTargetMap();
- ROS_DEBUG_STREAM("computeExplorationTransform: before walkable maps");
- computeWalkableMaps();
- ROS_DEBUG_STREAM(
- "computeExplorationTransform: before exploration transform");
- resetMap(m_ExplorationTransform);
-
- ROS_DEBUG("Computing exploration transform...");
- GridMap<double>* map = m_ExplorationTransform;
- int width = map->width();
- int height = map->height();
- double maxDistance = MAX_DISTANCE;
- map->fill(maxDistance);
- queue<int> xQueue;
- queue<int> yQueue;
- // fill seeds: Mark the frontiers as targets
- ROS_DEBUG_STREAM("computeExplorationTransform: before first loop");
- for (int y = 0; y < m_TargetMap->height(); y++) {
- for (int x = 0; x < m_TargetMap->width(); x++) {
- if (m_TargetMap->getValue(x, y) == 1) {
- map->setValue(x, y, 0);
- xQueue.push(x + 1);
- yQueue.push(y);
- xQueue.push(x - 1);
- yQueue.push(y);
- xQueue.push(x);
- yQueue.push(y - 1);
- xQueue.push(x);
- yQueue.push(y + 1);
- }
+ }
+ ROS_DEBUG_STREAM("computeExplorationTransform: After first looop");
+ // Now go through the coordinates in the queue
+ int xVal, yVal;
+ ROS_DEBUG_STREAM("computeExplorationTransform: before while loop");
+ while (!xQueue.empty())
+ {
+ xVal = xQueue.front();
+ yVal = yQueue.front();
+ xQueue.pop();
+ yQueue.pop();
+ if (xVal > 0 && xVal < width - 1 && yVal > 0 && yVal < height - 1 &&
+ isWalkable(xVal, yVal))
+ {
+ // Get own cost and the cost of the 8 neighbor cells (neighbors plus
+ // the cost to go there)
+ float value = map->getValue(xVal, yVal);
+ float value_u = map->getValue(xVal, yVal - 1) + 1;
+ float value_d = map->getValue(xVal, yVal + 1) + 1;
+ float value_l = map->getValue(xVal - 1, yVal) + 1;
+ float value_r = map->getValue(xVal + 1, yVal) + 1;
+ float value_ur = map->getValue(xVal + 1, yVal - 1) + 1.4142;
+ float value_ul = map->getValue(xVal - 1, yVal - 1) + 1.4142;
+ float value_ll = map->getValue(xVal - 1, yVal + 1) + 1.4142;
+ float value_lr = map->getValue(xVal + 1, yVal + 1) + 1.4142;
+ float min1 = value_u < value_d ? value_u : value_d;
+ float min2 = value_l < value_r ? value_l : value_r;
+ float min3 = value_ur < value_ul ? value_ur : value_ul;
+ float min4 = value_ll < value_lr ? value_ll : value_lr;
+ float min12 = min1 < min2 ? min1 : min2;
+ float min34 = min3 < min4 ? min3 : min4;
+ float min = min12 < min34 ? min12 : min34;
+ float newVal =
+ min + m_SafePathWeight * m_CostTransform->getValue(xVal, yVal);
+ if (value > newVal)
+ {
+ // Cost is lower then the currently known cost: Reduce cost here
+ map->setValue(xVal, yVal, newVal);
+ // Add the neighbours that might profit in the queue
+ if (map->getValue(xVal, yVal + 1) > newVal + 1)
+ {
+ xQueue.push(xVal);
+ yQueue.push(yVal + 1);
}
- }
- ROS_DEBUG_STREAM("computeExplorationTransform: After first looop");
- // Now go through the coordinates in the queue
- int xVal, yVal;
- ROS_DEBUG_STREAM("computeExplorationTransform: before while loop");
- while (!xQueue.empty()) {
- xVal = xQueue.front();
- yVal = yQueue.front();
- xQueue.pop();
- yQueue.pop();
- if (xVal > 0 && xVal < width - 1 && yVal > 0 && yVal < height - 1 &&
- isWalkable(xVal, yVal)) {
- // Get own cost and the cost of the 8 neighbor cells (neighbors plus
- // the cost to go there)
- float value = map->getValue(xVal, yVal);
- float value_u = map->getValue(xVal, yVal - 1) + 1;
- float value_d = map->getValue(xVal, yVal + 1) + 1;
- float value_l = map->getValue(xVal - 1, yVal) + 1;
- float value_r = map->getValue(xVal + 1, yVal) + 1;
- float value_ur = map->getValue(xVal + 1, yVal - 1) + 1.4142;
- float value_ul = map->getValue(xVal - 1, yVal - 1) + 1.4142;
- float value_ll = map->getValue(xVal - 1, yVal + 1) + 1.4142;
- float value_lr = map->getValue(xVal + 1, yVal + 1) + 1.4142;
- float min1 = value_u < value_d ? value_u : value_d;
- float min2 = value_l < value_r ? value_l : value_r;
- float min3 = value_ur < value_ul ? value_ur : value_ul;
- float min4 = value_ll < value_lr ? value_ll : value_lr;
- float min12 = min1 < min2 ? min1 : min2;
- float min34 = min3 < min4 ? min3 : min4;
- float min = min12 < min34 ? min12 : min34;
- float newVal =
- min + m_SafePathWeight * m_CostTransform->getValue(xVal, yVal);
- if (value > newVal) {
- // Cost is lower then the currently known cost: Reduce cost here
- map->setValue(xVal, yVal, newVal);
- // Add the neighbours that might profit in the queue
- if (map->getValue(xVal, yVal + 1) > newVal + 1) {
- xQueue.push(xVal);
- yQueue.push(yVal + 1);
- }
- if (map->getValue(xVal, yVal - 1) > newVal + 1) {
- xQueue.push(xVal);
- yQueue.push(yVal - 1);
- }
- if (map->getValue(xVal + 1, yVal) > newVal + 1) {
- xQueue.push(xVal + 1);
- yQueue.push(yVal);
- }
- if (map->getValue(xVal - 1, yVal) > newVal + 1) {
- xQueue.push(xVal - 1);
- yQueue.push(yVal);
- }
- if (map->getValue(xVal + 1, yVal - 1) > newVal + 1.4142) {
- xQueue.push(xVal + 1);
- yQueue.push(yVal - 1);
- }
- if (map->getValue(xVal - 1, yVal - 1) > newVal + 1.4142) {
- xQueue.push(xVal - 1);
- yQueue.push(yVal - 1);
- }
- if (map->getValue(xVal + 1, yVal + 1) > newVal + 1.4142) {
- xQueue.push(xVal + 1);
- yQueue.push(yVal + 1);
- }
- if (map->getValue(xVal - 1, yVal + 1) > newVal + 1.4142) {
- xQueue.push(xVal - 1);
- yQueue.push(yVal + 1);
- }
- }
+ if (map->getValue(xVal, yVal - 1) > newVal + 1)
+ {
+ xQueue.push(xVal);
+ yQueue.push(yVal - 1);
+ }
+ if (map->getValue(xVal + 1, yVal) > newVal + 1)
+ {
+ xQueue.push(xVal + 1);
+ yQueue.push(yVal);
+ }
+ if (map->getValue(xVal - 1, yVal) > newVal + 1)
+ {
+ xQueue.push(xVal - 1);
+ yQueue.push(yVal);
+ }
+ if (map->getValue(xVal + 1, yVal - 1) > newVal + 1.4142)
+ {
+ xQueue.push(xVal + 1);
+ yQueue.push(yVal - 1);
+ }
+ if (map->getValue(xVal - 1, yVal - 1) > newVal + 1.4142)
+ {
+ xQueue.push(xVal - 1);
+ yQueue.push(yVal - 1);
}
+ if (map->getValue(xVal + 1, yVal + 1) > newVal + 1.4142)
+ {
+ xQueue.push(xVal + 1);
+ yQueue.push(yVal + 1);
+ }
+ if (map->getValue(xVal - 1, yVal + 1) > newVal + 1.4142)
+ {
+ xQueue.push(xVal - 1);
+ yQueue.push(yVal + 1);
+ }
+ }
}
- ROS_DEBUG_STREAM(
- "computeExplorationTransform: after exploration transform");
+ }
+ ROS_DEBUG_STREAM("computeExplorationTransform: after exploration transform");
}
vector<Eigen::Vector2i> Explorer::sampleWaypointsFromPath(
- std::vector<Eigen::Vector2i> pathPoints, float threshold) {
- if (!m_OccupancyMap) {
- ROS_ERROR("Missing occupancy map. Aborting.");
- return pathPoints;
- }
- if (pathPoints.size() < 3) {
- return pathPoints;
- }
+ std::vector<Eigen::Vector2i> pathPoints, float threshold)
+{
+ if (!m_OccupancyMap)
+ {
+ ROS_ERROR("Missing occupancy map. Aborting.");
+ return pathPoints;
+ }
+ if (pathPoints.size() < 3)
+ {
+ return pathPoints;
+ }
- computeObstacleTransform();
-
- vector<Eigen::Vector2i> simplifiedPath;
- simplifiedPath.reserve(pathPoints.size());
-
- Eigen::Vector2i lastAddedPoint = pathPoints[0];
- simplifiedPath.push_back(lastAddedPoint);
-
- for (unsigned int i = 1; i < pathPoints.size() - 1; i++) {
- double distanceToNextPoint =
- map_tools::distance(lastAddedPoint, pathPoints.at(i));
- double obstacleDistanceLastAddedPoint = m_ObstacleTransform->getValue(
- lastAddedPoint.x(), lastAddedPoint.y());
- double obstacleDistancePossibleNextPoint =
- m_ObstacleTransform->getValue(pathPoints[i].x(), pathPoints[i].y());
- if ((distanceToNextPoint >=
- obstacleDistanceLastAddedPoint * threshold) ||
- (distanceToNextPoint >=
- obstacleDistancePossibleNextPoint * threshold)) {
- simplifiedPath.push_back(pathPoints[i]);
- lastAddedPoint = pathPoints[i];
- }
- }
- simplifiedPath.push_back(pathPoints[pathPoints.size() - 1]);
- return simplifiedPath;
-}
+ computeObstacleTransform();
-std::vector<Eigen::Vector2i> Explorer::getPath(bool& success) {
- success = false;
+ vector<Eigen::Vector2i> simplifiedPath;
+ simplifiedPath.reserve(pathPoints.size());
- if (!m_OccupancyMap) {
- ROS_ERROR("Missing occupancy map. Aborting.");
- return vector<Eigen::Vector2i>();
- }
+ Eigen::Vector2i lastAddedPoint = pathPoints[0];
+ simplifiedPath.push_back(lastAddedPoint);
- if (m_DesiredDistance > 0) {
- // we are actually performing an exploration since the target
- // is a region.
- ROS_ERROR_STREAM(
- "Desired Distance > 0: Executing getExplorationTransformPath");
- return getExplorationTransformPath(success);
+ for (unsigned int i = 1; i < pathPoints.size() - 1; i++)
+ {
+ double distanceToNextPoint =
+ map_tools::distance(lastAddedPoint, pathPoints.at(i));
+ double obstacleDistanceLastAddedPoint =
+ m_ObstacleTransform->getValue(lastAddedPoint.x(), lastAddedPoint.y());
+ double obstacleDistancePossibleNextPoint =
+ m_ObstacleTransform->getValue(pathPoints[i].x(), pathPoints[i].y());
+ if ((distanceToNextPoint >= obstacleDistanceLastAddedPoint * threshold) ||
+ (distanceToNextPoint >= obstacleDistancePossibleNextPoint * threshold))
+ {
+ simplifiedPath.push_back(pathPoints[i]);
+ lastAddedPoint = pathPoints[i];
}
- ROS_DEBUG_STREAM("Computing Path Transform");
- computePathTransform();
- ROS_DEBUG_STREAM("Finished Path Transform");
-
- vector<Eigen::Vector2i> path;
+ }
+ simplifiedPath.push_back(pathPoints[pathPoints.size() - 1]);
+ return simplifiedPath;
+}
- int x = m_Start.x();
- int y = m_Start.y();
+std::vector<Eigen::Vector2i> Explorer::getPath(bool& success)
+{
+ success = false;
- int width = m_OccupancyMap->width();
- int height = m_OccupancyMap->height();
+ if (!m_OccupancyMap)
+ {
+ ROS_ERROR("Missing occupancy map. Aborting.");
+ return vector<Eigen::Vector2i>();
+ }
- // special case: start and end point are equal, return single waypoint
- if (map_tools::distance(m_Start, m_Target) < 2.0) {
- success = true;
- path.push_back(Eigen::Vector2i(m_Start.x(), m_Start.y()));
- return path;
- }
+ if (m_DesiredDistance > 0)
+ {
+ // we are actually performing an exploration since the target
+ // is a region.
+ ROS_ERROR_STREAM("Desired Distance > 0: Executing "
+ "getExplorationTransformPath");
+ return getExplorationTransformPath(success);
+ }
+ ROS_DEBUG_STREAM("Computing Path Transform");
+ computePathTransform();
+ ROS_DEBUG_STREAM("Finished Path Transform");
+
+ vector<Eigen::Vector2i> path;
+
+ int x = m_Start.x();
+ int y = m_Start.y();
+
+ int width = m_OccupancyMap->width();
+ int height = m_OccupancyMap->height();
+
+ // special case: start and end point are equal, return single waypoint
+ if (map_tools::distance(m_Start, m_Target) < 2.0)
+ {
+ success = true;
+ path.push_back(Eigen::Vector2i(m_Start.x(), m_Start.y()));
+ return path;
+ }
- while (x != m_Target.x() || y != m_Target.y()) {
- path.push_back(Eigen::Vector2i(x, y));
- int minPosX = x;
- int minPosY = y;
- double min = m_PathTransform->getValue(x, y);
+ while (x != m_Target.x() || y != m_Target.y())
+ {
+ path.push_back(Eigen::Vector2i(x, y));
+ int minPosX = x;
+ int minPosY = y;
+ double min = m_PathTransform->getValue(x, y);
- if ((x <= 1) || (y <= 1) || (x >= width - 1) || (y >= height - 1)) {
- ROS_ERROR("Out of map bounds");
- return vector<Eigen::Vector2i>();
- }
+ if ((x <= 1) || (y <= 1) || (x >= width - 1) || (y >= height - 1))
+ {
+ ROS_ERROR("Out of map bounds");
+ return vector<Eigen::Vector2i>();
+ }
- for (int i = -1; i <= 1; i++) {
- for (int j = -1; j <= 1; j++) {
- double pt = m_PathTransform->getValue(x + i, y + j);
- if (pt < min) {
- min = pt;
- minPosX = x + i;
- minPosY = y + j;
- }
- }
- }
- if (minPosX == x && minPosY == y) {
- ROS_WARN("Target is unreachable!");
- return vector<Eigen::Vector2i>();
- } else {
- x = minPosX;
- y = minPosY;
+ for (int i = -1; i <= 1; i++)
+ {
+ for (int j = -1; j <= 1; j++)
+ {
+ double pt = m_PathTransform->getValue(x + i, y + j);
+ if (pt < min)
+ {
+ min = pt;
+ minPosX = x + i;
+ minPosY = y + j;
}
+ }
}
- success = true;
+ if (minPosX == x && minPosY == y)
+ {
+ ROS_WARN("Target is unreachable!");
+ return vector<Eigen::Vector2i>();
+ }
+ else
+ {
+ x = minPosX;
+ y = minPosY;
+ }
+ }
+ success = true;
- return path;
+ return path;
}
-vector<Eigen::Vector2i> Explorer::getExplorationTransformPath(bool& success) {
- success = false;
+vector<Eigen::Vector2i> Explorer::getExplorationTransformPath(bool& success)
+{
+ success = false;
- if (!m_OccupancyMap) {
- ROS_ERROR("Missing occupancy map. Aborting.");
- return vector<Eigen::Vector2i>();
- }
+ if (!m_OccupancyMap)
+ {
+ ROS_ERROR("Missing occupancy map. Aborting.");
+ return vector<Eigen::Vector2i>();
+ }
+
+ ROS_DEBUG_STREAM("Exploration Transform: Before obstacle transform");
+ computeObstacleTransform();
+ ROS_DEBUG_STREAM("Exploration Transform: Before exploration transform");
+ computeExplorationTransform();
+ ROS_DEBUG_STREAM("Exploration Transform: after obstacle transform");
+
+ // check if we are already there
+ if (m_TargetMap->getValue(m_Start.x(), m_Start.y()))
+ {
+ success = true;
+ vector<Eigen::Vector2i> path;
+ path.push_back(Eigen::Vector2i(m_Start.x(), m_Start.y()));
+ return path;
+ }
- ROS_DEBUG_STREAM("Exploration Transform: Before obstacle transform");
- computeObstacleTransform();
- ROS_DEBUG_STREAM("Exploration Transform: Before exploration transform");
- computeExplorationTransform();
- ROS_DEBUG_STREAM("Exploration Transform: after obstacle transform");
-
- // check if we are already there
- if (m_TargetMap->getValue(m_Start.x(), m_Start.y())) {
- success = true;
- vector<Eigen::Vector2i> path;
- path.push_back(Eigen::Vector2i(m_Start.x(), m_Start.y()));
- return path;
- }
+ int width = m_OccupancyMap->width();
+ int height = m_OccupancyMap->height();
- int width = m_OccupancyMap->width();
- int height = m_OccupancyMap->height();
+ vector<Eigen::Vector2i> path;
+ int x = m_Start.x();
+ int y = m_Start.y();
- vector<Eigen::Vector2i> path;
- int x = m_Start.x();
- int y = m_Start.y();
-
- if (m_ObstacleTransform->getValue(x, y) < m_MinAllowedObstacleDistance) {
- // robot got stuck!
- // find way out using ObstacleTransform...
- int maxPosX = x;
- int maxPosY = y;
-
- if ((x <= 1) || (y <= 1) || (x >= width - 1) || (y >= height - 1)) {
- ROS_ERROR("Out of map bounds");
- return vector<Eigen::Vector2i>();
- }
+ if (m_ObstacleTransform->getValue(x, y) < m_MinAllowedObstacleDistance)
+ {
+ // robot got stuck!
+ // find way out using ObstacleTransform...
+ int maxPosX = x;
+ int maxPosY = y;
- while (m_ObstacleTransform->getValue(maxPosX, maxPosY) <
- m_MinAllowedObstacleDistance) {
- double max = m_ObstacleTransform->getValue(x, y);
- for (int i = -1; i <= 1; i++) {
- for (int j = -1; j <= 1; j++) {
- double pt = m_ObstacleTransform->getValue(x + i, y + j);
- if (pt > max) {
- max = pt;
- maxPosX = x + i;
- maxPosY = y + j;
- }
- }
- }
- if (maxPosX == x && maxPosY == y) // no ascentFound
- {
- break;
- } else {
- path.push_back(Eigen::Vector2i(maxPosX, maxPosY));
- x = maxPosX;
- y = maxPosY;
- }
- }
+ if ((x <= 1) || (y <= 1) || (x >= width - 1) || (y >= height - 1))
+ {
+ ROS_ERROR("Out of map bounds");
+ return vector<Eigen::Vector2i>();
}
- // now path is "free"
- bool descentFound = true;
- while (descentFound) {
- descentFound = false;
- int minPosX = x;
- int minPosY = y;
- double min = m_ExplorationTransform->getValue(x, y);
- if ((x <= 1) || (y <= 1) || (x >= width - 1) || (y >= height - 1)) {
- ROS_ERROR("Out of map bounds");
- return vector<Eigen::Vector2i>();
- }
- for (int i = -1; i <= 1; i++) {
- for (int j = -1; j <= 1; j++) {
- double pt = m_ExplorationTransform->getValue(x + i, y + j);
- if (pt < min) {
- min = pt;
- minPosX = x + i;
- minPosY = y + j;
- }
- }
+ while (m_ObstacleTransform->getValue(maxPosX, maxPosY) <
+ m_MinAllowedObstacleDistance)
+ {
+ double max = m_ObstacleTransform->getValue(x, y);
+ for (int i = -1; i <= 1; i++)
+ {
+ for (int j = -1; j <= 1; j++)
+ {
+ double pt = m_ObstacleTransform->getValue(x + i, y + j);
+ if (pt > max)
+ {
+ max = pt;
+ maxPosX = x + i;
+ maxPosY = y + j;
+ }
}
- if (minPosX == x && minPosY == y) // no descentFound
+ }
+ if (maxPosX == x && maxPosY == y) // no ascentFound
+ {
+ break;
+ }
+ else
+ {
+ path.push_back(Eigen::Vector2i(maxPosX, maxPosY));
+ x = maxPosX;
+ y = maxPosY;
+ }
+ }
+ }
+ // now path is "free"
+ bool descentFound = true;
+ while (descentFound)
+ {
+ descentFound = false;
+ int minPosX = x;
+ int minPosY = y;
+ double min = m_ExplorationTransform->getValue(x, y);
+ if ((x <= 1) || (y <= 1) || (x >= width - 1) || (y >= height - 1))
+ {
+ ROS_ERROR("Out of map bounds");
+ return vector<Eigen::Vector2i>();
+ }
+
+ for (int i = -1; i <= 1; i++)
+ {
+ for (int j = -1; j <= 1; j++)
+ {
+ double pt = m_ExplorationTransform->getValue(x + i, y + j);
+ if (pt < min)
{
- descentFound = false;
- } else {
- descentFound = true;
- path.push_back(Eigen::Vector2i(minPosX, minPosY));
- x = minPosX;
- y = minPosY;
+ min = pt;
+ minPosX = x + i;
+ minPosY = y + j;
}
+ }
}
- success = true;
+ if (minPosX == x && minPosY == y) // no descentFound
+ {
+ descentFound = false;
+ }
+ else
+ {
+ descentFound = true;
+ path.push_back(Eigen::Vector2i(minPosX, minPosY));
+ x = minPosX;
+ y = minPosY;
+ }
+ }
+ success = true;
- ROS_INFO_STREAM("Exploration Transform: End of function");
- return path;
+ ROS_INFO_STREAM("Exploration Transform: End of function");
+ return path;
#if 0
// START P2AT HACK
@@ -1155,174 +1330,200 @@ vector<Eigen::Vector2i> Explorer::getExplorationTransformPath(bool& success) {
#endif
}
-bool Explorer::getNearestFrontier(Eigen::Vector2i& nextFrontier) {
- if (!m_OccupancyMap) {
- ROS_ERROR("Missing occupancy map. Aborting.");
- return false;
- }
-
- computeFrontierMap();
- computeDrivingDistanceTransform();
-
- bool found = false;
- int distXPos = -1;
- int distYPos = -1;
- double dist = 10000000;
- for (int y = 0; y < m_TargetMap->height(); y++) {
- for (int x = 0; x < m_TargetMap->width(); x++) {
- if (m_TargetMap->getValue(x, y) == 1 &&
- m_DrivingDistanceTransform->getValue(x, y) < 999999) {
- if (m_DrivingDistanceTransform->getValue(x, y) < dist) {
- found = true;
- dist = m_DrivingDistanceTransform->getValue(x, y);
- distXPos = x;
- distYPos = y;
- }
- }
+bool Explorer::getNearestFrontier(Eigen::Vector2i& nextFrontier)
+{
+ if (!m_OccupancyMap)
+ {
+ ROS_ERROR("Missing occupancy map. Aborting.");
+ return false;
+ }
+
+ computeFrontierMap();
+ computeDrivingDistanceTransform();
+
+ bool found = false;
+ int distXPos = -1;
+ int distYPos = -1;
+ double dist = 10000000;
+ for (int y = 0; y < m_TargetMap->height(); y++)
+ {
+ for (int x = 0; x < m_TargetMap->width(); x++)
+ {
+ if (m_TargetMap->getValue(x, y) == 1 &&
+ m_DrivingDistanceTransform->getValue(x, y) < 999999)
+ {
+ if (m_DrivingDistanceTransform->getValue(x, y) < dist)
+ {
+ found = true;
+ dist = m_DrivingDistanceTransform->getValue(x, y);
+ distXPos = x;
+ distYPos = y;
}
+ }
}
- if (found) {
- nextFrontier.x() = distXPos;
- nextFrontier.y() = distYPos;
- return true;
- } else {
- return false;
- }
+ }
+ if (found)
+ {
+ nextFrontier.x() = distXPos;
+ nextFrontier.y() = distYPos;
+ return true;
+ }
+ else
+ {
+ return false;
+ }
}
// HELPERS
// //////////////////////////////////////////////////////////////////////////////////////////////////////////
-void Explorer::distanceFloodFill(GridMap<double>* map, Eigen::Vector2i start) {
- if (!map) {
- ROS_ERROR("Received 0-pointer!");
- }
-
- computeObstacleTransform();
-
- int width = map->width();
- int height = map->height();
- map->fill(MAX_DISTANCE);
-
- int fromX = start.x();
- int fromY = start.y();
- map->setValue(fromX, fromY, 0);
-
- queue<int> xQueue;
- queue<int> yQueue;
- xQueue.push(fromX + 1);
- yQueue.push(fromY);
- xQueue.push(fromX - 1);
- yQueue.push(fromY);
- xQueue.push(fromX);
- yQueue.push(fromY - 1);
- xQueue.push(fromX);
- yQueue.push(fromY + 1);
- int xVal, yVal;
- while (!xQueue.empty()) {
- xVal = xQueue.front();
- yVal = yQueue.front();
- xQueue.pop();
- yQueue.pop();
- bool isFree = (m_OccupancyMap->getValue(xVal, yVal) < UNKNOWN ||
- m_OccupancyMap->getValue(xVal, yVal) !=
- NOT_SEEN_YET); // only fill free cells
- bool isSafe = m_ObstacleTransform->getValue(xVal, yVal) >
- m_MinAllowedObstacleDistance;
- if (xVal > 0 && xVal < width - 1 && yVal > 0 && yVal < height - 1 &&
- isFree && isSafe) {
- float value = map->getValue(xVal, yVal);
- float value_u = map->getValue(xVal, yVal - 1) + 1;
- float value_d = map->getValue(xVal, yVal + 1) + 1;
- float value_l = map->getValue(xVal - 1, yVal) + 1;
- float value_r = map->getValue(xVal + 1, yVal) + 1;
-
- float value_ur = map->getValue(xVal + 1, yVal - 1) + 1.4142;
- float value_ul = map->getValue(xVal - 1, yVal - 1) + 1.4142;
- float value_ll = map->getValue(xVal - 1, yVal + 1) + 1.4142;
- float value_lr = map->getValue(xVal + 1, yVal + 1) + 1.4142;
-
- float min1 = value_u < value_d ? value_u : value_d;
- float min2 = value_l < value_r ? value_l : value_r;
- float min3 = value_ur < value_ul ? value_ur : value_ul;
- float min4 = value_ll < value_lr ? value_ll : value_lr;
- float min12 = min1 < min2 ? min1 : min2;
- float min34 = min3 < min4 ? min3 : min4;
- float min = min12 < min34 ? min12 : min34;
- float newVal = min;
- if (value > newVal) {
- map->setValue(xVal, yVal, newVal);
- if (map->getValue(xVal, yVal + 1) > newVal + 1) {
- xQueue.push(xVal);
- yQueue.push(yVal + 1);
- }
- if (map->getValue(xVal, yVal - 1) > newVal + 1) {
- xQueue.push(xVal);
- yQueue.push(yVal - 1);
- }
- if (map->getValue(xVal + 1, yVal) > newVal + 1) {
- xQueue.push(xVal + 1);
- yQueue.push(yVal);
- }
- if (map->getValue(xVal - 1, yVal) > newVal + 1) {
- xQueue.push(xVal - 1);
- yQueue.push(yVal);
- }
- if (map->getValue(xVal + 1, yVal - 1) > newVal + 1.4142) {
- xQueue.push(xVal + 1);
- yQueue.push(yVal - 1);
- }
- if (map->getValue(xVal - 1, yVal - 1) > newVal + 1.4142) {
- xQueue.push(xVal - 1);
- yQueue.push(yVal - 1);
- }
- if (map->getValue(xVal + 1, yVal + 1) > newVal + 1.4142) {
- xQueue.push(xVal + 1);
- yQueue.push(yVal + 1);
- }
- if (map->getValue(xVal - 1, yVal + 1) > newVal + 1.4142) {
- xQueue.push(xVal - 1);
- yQueue.push(yVal + 1);
- }
- }
+void Explorer::distanceFloodFill(GridMap<double>* map, Eigen::Vector2i start)
+{
+ if (!map)
+ {
+ ROS_ERROR("Received 0-pointer!");
+ }
+
+ computeObstacleTransform();
+
+ int width = map->width();
+ int height = map->height();
+ map->fill(MAX_DISTANCE);
+
+ int fromX = start.x();
+ int fromY = start.y();
+ map->setValue(fromX, fromY, 0);
+
+ queue<int> xQueue;
+ queue<int> yQueue;
+ xQueue.push(fromX + 1);
+ yQueue.push(fromY);
+ xQueue.push(fromX - 1);
+ yQueue.push(fromY);
+ xQueue.push(fromX);
+ yQueue.push(fromY - 1);
+ xQueue.push(fromX);
+ yQueue.push(fromY + 1);
+ int xVal, yVal;
+ while (!xQueue.empty())
+ {
+ xVal = xQueue.front();
+ yVal = yQueue.front();
+ xQueue.pop();
+ yQueue.pop();
+ bool isFree = (m_OccupancyMap->getValue(xVal, yVal) < UNKNOWN ||
+ m_OccupancyMap->getValue(xVal, yVal) !=
+ NOT_SEEN_YET); // only fill free cells
+ bool isSafe = m_ObstacleTransform->getValue(xVal, yVal) >
+ m_MinAllowedObstacleDistance;
+ if (xVal > 0 && xVal < width - 1 && yVal > 0 && yVal < height - 1 &&
+ isFree && isSafe)
+ {
+ float value = map->getValue(xVal, yVal);
+ float value_u = map->getValue(xVal, yVal - 1) + 1;
+ float value_d = map->getValue(xVal, yVal + 1) + 1;
+ float value_l = map->getValue(xVal - 1, yVal) + 1;
+ float value_r = map->getValue(xVal + 1, yVal) + 1;
+
+ float value_ur = map->getValue(xVal + 1, yVal - 1) + 1.4142;
+ float value_ul = map->getValue(xVal - 1, yVal - 1) + 1.4142;
+ float value_ll = map->getValue(xVal - 1, yVal + 1) + 1.4142;
+ float value_lr = map->getValue(xVal + 1, yVal + 1) + 1.4142;
+
+ float min1 = value_u < value_d ? value_u : value_d;
+ float min2 = value_l < value_r ? value_l : value_r;
+ float min3 = value_ur < value_ul ? value_ur : value_ul;
+ float min4 = value_ll < value_lr ? value_ll : value_lr;
+ float min12 = min1 < min2 ? min1 : min2;
+ float min34 = min3 < min4 ? min3 : min4;
+ float min = min12 < min34 ? min12 : min34;
+ float newVal = min;
+ if (value > newVal)
+ {
+ map->setValue(xVal, yVal, newVal);
+ if (map->getValue(xVal, yVal + 1) > newVal + 1)
+ {
+ xQueue.push(xVal);
+ yQueue.push(yVal + 1);
}
+ if (map->getValue(xVal, yVal - 1) > newVal + 1)
+ {
+ xQueue.push(xVal);
+ yQueue.push(yVal - 1);
+ }
+ if (map->getValue(xVal + 1, yVal) > newVal + 1)
+ {
+ xQueue.push(xVal + 1);
+ yQueue.push(yVal);
+ }
+ if (map->getValue(xVal - 1, yVal) > newVal + 1)
+ {
+ xQueue.push(xVal - 1);
+ yQueue.push(yVal);
+ }
+ if (map->getValue(xVal + 1, yVal - 1) > newVal + 1.4142)
+ {
+ xQueue.push(xVal + 1);
+ yQueue.push(yVal - 1);
+ }
+ if (map->getValue(xVal - 1, yVal - 1) > newVal + 1.4142)
+ {
+ xQueue.push(xVal - 1);
+ yQueue.push(yVal - 1);
+ }
+ if (map->getValue(xVal + 1, yVal + 1) > newVal + 1.4142)
+ {
+ xQueue.push(xVal + 1);
+ yQueue.push(yVal + 1);
+ }
+ if (map->getValue(xVal - 1, yVal + 1) > newVal + 1.4142)
+ {
+ xQueue.push(xVal - 1);
+ yQueue.push(yVal + 1);
+ }
+ }
}
+ }
}
// Implementation taken from http://www.cs.cmu.edu/~cil/vnew.html
-double* Explorer::distanceTransform1D(double* f, int n) {
- // int width = m_OccupancyMap->width();
- // int height = m_OccupancyMap->height();
- // double maxDistance = height > width ? height : width;
-
- double* d = new double[n];
- int* v = new int[n];
- double* z = new double[n + 1];
- int k = 0;
- v[0] = 0;
- z[0] = -INT_MAX;
- z[1] = INT_MAX;
- for (int q = 1; q <= n - 1; q++) {
- double s =
- ((f[q] + (q * q)) - (f[v[k]] + (v[k] * v[k]))) / (2 * q - 2 * v[k]);
- while (s <= z[k]) {
- k--;
- s = ((f[q] + (q * q)) - (f[v[k]] + (v[k] * v[k]))) /
- (2 * q - 2 * v[k]);
- }
- k++;
- v[k] = q;
- z[k] = s;
- z[k + 1] = INT_MAX;
- }
-
- k = 0;
- for (int q = 0; q <= n - 1; q++) {
- while (z[k + 1] < q) k++;
- d[q] = ((q - v[k]) * (q - v[k])) + f[v[k]];
+double* Explorer::distanceTransform1D(double* f, int n)
+{
+ // int width = m_OccupancyMap->width();
+ // int height = m_OccupancyMap->height();
+ // double maxDistance = height > width ? height : width;
+
+ double* d = new double[n];
+ int* v = new int[n];
+ double* z = new double[n + 1];
+ int k = 0;
+ v[0] = 0;
+ z[0] = -INT_MAX;
+ z[1] = INT_MAX;
+ for (int q = 1; q <= n - 1; q++)
+ {
+ double s =
+ ((f[q] + (q * q)) - (f[v[k]] + (v[k] * v[k]))) / (2 * q - 2 * v[k]);
+ while (s <= z[k])
+ {
+ k--;
+ s = ((f[q] + (q * q)) - (f[v[k]] + (v[k] * v[k]))) / (2 * q - 2 * v[k]);
}
+ k++;
+ v[k] = q;
+ z[k] = s;
+ z[k + 1] = INT_MAX;
+ }
- delete[] v;
- delete[] z;
- return d;
+ k = 0;
+ for (int q = 0; q <= n - 1; q++)
+ {
+ while (z[k + 1] < q)
+ k++;
+ d[q] = ((q - v[k]) * (q - v[k])) + f[v[k]];
+ }
+
+ delete[] v;
+ delete[] z;
+ return d;
}
diff --git a/homer_nav_libs/src/Math/Line2D.cpp b/homer_nav_libs/src/Math/Line2D.cpp
index 89cd5310cbb2a9db36bec894eeac2672756cecd4..0a0212ce9d8eeb85177061e4d5f28854ebaf2ec2 100644
--- a/homer_nav_libs/src/Math/Line2D.cpp
+++ b/homer_nav_libs/src/Math/Line2D.cpp
@@ -22,67 +22,71 @@
float THIS::gradient() const
{
float gradient = 10000000.0;
- if ( m_Vec[0] != 0.0 )
+ if (m_Vec[0] != 0.0)
{
- gradient = m_Vec[1]/m_Vec[0];
+ gradient = m_Vec[1] / m_Vec[0];
}
return gradient;
}
-std::vector< Point2D > THIS::vertices ( unsigned substeps )
+std::vector<Point2D> THIS::vertices(unsigned substeps)
{
- unsigned steps = substeps+2;
- std::vector<Point2D> myVertices ( steps );
- for ( unsigned i=0; i<steps; i++ )
+ unsigned steps = substeps + 2;
+ std::vector<Point2D> myVertices(steps);
+ for (unsigned i = 0; i < steps; i++)
{
- float t= float ( i ) / float ( steps-1 );
- myVertices[i] = m_Start + t*m_Vec;
+ float t = float(i) / float(steps - 1);
+ myVertices[i] = m_Start + t * m_Vec;
}
return myVertices;
}
-Point2D THIS::getClosestPoint ( Point2D point ) const
+Point2D THIS::getClosestPoint(Point2D point) const
{
- float t = ( point-m_Start ) * m_Vec;
+ float t = (point - m_Start) * m_Vec;
t /= m_Vec * m_Vec;
- if ( t > 1.0 )
+ if (t > 1.0)
{
t = 1.0;
}
- else if ( t < 0.0 )
+ else if (t < 0.0)
{
t = 0.0;
}
- Point2D pointOnLine = m_Start + ( t * m_Vec );
+ Point2D pointOnLine = m_Start + (t * m_Vec);
return pointOnLine;
}
-Point2D THIS::getIntersectionPoint ( Line2D line ) const
+Point2D THIS::getIntersectionPoint(Line2D line) const
{
Point2D intersecPoint;
- double det1 = m_Vec.x() * ( -line.vec().y() ) - ( -line.vec().x() ) * m_Vec.y();
+ double det1 = m_Vec.x() * (-line.vec().y()) - (-line.vec().x()) * m_Vec.y();
// lines are not parallel
- if ( det1 != 0 )
+ if (det1 != 0)
{
- CVec2 startToStart = line.start() -m_Start;
+ CVec2 startToStart = line.start() - m_Start;
// calculate intersection
- double lambda = ( startToStart.x() * ( -line.vec().y() ) - ( -line.vec().x() ) * startToStart.y() ) / det1;
- intersecPoint = m_Start + lambda* m_Vec;
+ double lambda = (startToStart.x() * (-line.vec().y()) -
+ (-line.vec().x()) * startToStart.y()) /
+ det1;
+ intersecPoint = m_Start + lambda * m_Vec;
}
return intersecPoint;
}
-float THIS::getIntersectionPointParameter ( Line2D line ) const
+float THIS::getIntersectionPointParameter(Line2D line) const
{
double lambda = 0.0;
- double det1 = m_Vec.x() * ( -line.vec().y() ) - ( -line.vec().x() ) * m_Vec.y();
+ double det1 = m_Vec.x() * (-line.vec().y()) - (-line.vec().x()) * m_Vec.y();
// lines are not parallel
- if ( det1 != 0 )
+ if (det1 != 0)
{
- CVec2 startToStart = line.start() -m_Start;
+ CVec2 startToStart = line.start() - m_Start;
// calculate intersection
- lambda = ( startToStart.x() * ( -line.vec().y() ) - ( -line.vec().x() ) * startToStart.y() ) / det1;
+ lambda = (startToStart.x() * (-line.vec().y()) -
+ (-line.vec().x()) * startToStart.y()) /
+ det1;
}
return lambda;
@@ -91,9 +95,10 @@ float THIS::getIntersectionPointParameter ( Line2D line ) const
std::string THIS::toString() const
{
std::ostringstream str;
-// str << "Startpoint: " << m_Start.x() << " " << m_Start.y() << " Endpoint: " << end().x() << " " << end().y() <<
-// " Vector: " << m_Vec.x() << " " << m_Vec.y() << " ";
- str << m_Start.x() << " " << m_Start.y() << std::endl << end().x() << " " << end().y() << std::endl;
+ // str << "Startpoint: " << m_Start.x() << " " << m_Start.y() << " Endpoint:
+ // " << end().x() << " " << end().y() <<
+ // " Vector: " << m_Vec.x() << " " << m_Vec.y() << " ";
+ str << m_Start.x() << " " << m_Start.y() << std::endl
+ << end().x() << " " << end().y() << std::endl;
return str.str();
}
-
diff --git a/homer_nav_libs/src/Math/Math.cpp b/homer_nav_libs/src/Math/Math.cpp
index 5ad74fc3b39b77fd9f67276d78c4558b363485b0..ee3458e5dff8ede1b55a11719fd4a606400ba581 100644
--- a/homer_nav_libs/src/Math/Math.cpp
+++ b/homer_nav_libs/src/Math/Math.cpp
@@ -5,11 +5,11 @@
* Universitaet Koblenz-Landau
*
* Additional information:
- * $Id: $
+ * $Id: $
*******************************************************************************/
-#include <limits.h>
#include <homer_nav_libs/Math/Math.h>
+#include <limits.h>
#include <math.h>
#include <homer_nav_libs/Math/vec2.h>
@@ -24,96 +24,103 @@ THIS::~THIS()
{
}
-float THIS::meanAngle( const std::vector<float>& angles )
+float THIS::meanAngle(const std::vector<float>& angles)
{
- //calculate vectors from angles
- CVec2 vectorSum(0,0);
- for ( unsigned i=0; i<angles.size(); i++ )
+ // calculate vectors from angles
+ CVec2 vectorSum(0, 0);
+ for (unsigned i = 0; i < angles.size(); i++)
+ {
+ vectorSum = vectorSum + CVec2(cos(angles[i]), sin(angles[i]));
+ }
+ // return vectorSum.getAngle( CVec2(1,0) );
+ if (vectorSum.magnitude() == 0)
{
- vectorSum = vectorSum + CVec2( cos( angles[i] ), sin ( angles[i] ) );
+ return 0;
}
- //return vectorSum.getAngle( CVec2(1,0) );
- if ( vectorSum.magnitude() == 0 ) { return 0; }
- return atan2( vectorSum.y(), vectorSum.x() );
+ return atan2(vectorSum.y(), vectorSum.x());
}
-
-float THIS::meanAngleWeighted( const std::vector< WeightedValue >& weightedAngles )
+float THIS::meanAngleWeighted(const std::vector<WeightedValue>& weightedAngles)
{
- //calculate vectors from angles
- CVec2 vectorSum(0,0);
- for ( unsigned i=0; i<weightedAngles.size(); i++ )
+ // calculate vectors from angles
+ CVec2 vectorSum(0, 0);
+ for (unsigned i = 0; i < weightedAngles.size(); i++)
+ {
+ vectorSum = vectorSum +
+ weightedAngles[i].weight * CVec2(cos(weightedAngles[i].value),
+ sin(weightedAngles[i].value));
+ }
+ // return vectorSum.getAngle( CVec2(1,0) );
+ if (vectorSum.magnitude() == 0)
{
- vectorSum = vectorSum + weightedAngles[i].weight * CVec2( cos( weightedAngles[i].value ), sin ( weightedAngles[i].value ) );
+ return 0;
}
- //return vectorSum.getAngle( CVec2(1,0) );
- if ( vectorSum.magnitude() == 0 ) { return 0; }
- return atan2( vectorSum.y(), vectorSum.x() );
+ return atan2(vectorSum.y(), vectorSum.x());
}
-
-float THIS::angleVariance( float meanAngle, const std::vector<float>& angles )
+float THIS::angleVariance(float meanAngle, const std::vector<float>& angles)
{
- float quadSum=0;
- for( unsigned i=0; i < angles.size(); i++ )
+ float quadSum = 0;
+ for (unsigned i = 0; i < angles.size(); i++)
{
- float turnAngle=minTurnAngle( angles[i], meanAngle );
- quadSum += turnAngle*turnAngle;
+ float turnAngle = minTurnAngle(angles[i], meanAngle);
+ quadSum += turnAngle * turnAngle;
}
- return quadSum / float ( angles.size() );
+ return quadSum / float(angles.size());
}
-
-float THIS::minTurnAngle( float angle1, float angle2 )
+float THIS::minTurnAngle(float angle1, float angle2)
{
-/* CVec2 vector1( cos( angle1 ), sin ( angle1 ) );
- CVec2 vector2( cos( angle2 ), sin ( angle2 ) );
- return vector1.getAngle( vector2 );
- */
- angle1 *= 180.0/M_PI;
- angle2 *= 180.0/M_PI;
- //if(angle1 < 0) angle1 += M_PI * 2;
- //if(angle2 < 0) angle2 += M_PI * 2;
- int diff= angle2 - angle1;
+ /* CVec2 vector1( cos( angle1 ), sin ( angle1 ) );
+ CVec2 vector2( cos( angle2 ), sin ( angle2 ) );
+ return vector1.getAngle( vector2 );
+ */
+ angle1 *= 180.0 / M_PI;
+ angle2 *= 180.0 / M_PI;
+ // if(angle1 < 0) angle1 += M_PI * 2;
+ // if(angle2 < 0) angle2 += M_PI * 2;
+ int diff = angle2 - angle1;
diff = (diff + 180) % 360 - 180;
- //float sign=1;
- //if ( diff < 0 ) { sign=-1; }
- //minimal turn angle:
- //if the absolute difference is above 180°, calculate the difference in other direction
- //if ( fabs(diff) > M_PI ) {
+ // float sign=1;
+ // if ( diff < 0 ) { sign=-1; }
+ // minimal turn angle:
+ // if the absolute difference is above 180°, calculate the difference in other
+ // direction
+ // if ( fabs(diff) > M_PI ) {
// diff = 2*M_PI - fabs(diff);
// diff *= sign;
//}
- float ret = static_cast<double>(diff) * M_PI/180.0;
+ float ret = static_cast<double>(diff) * M_PI / 180.0;
return ret;
}
-Point2D THIS::center( std::vector<Point2D>& points )
+Point2D THIS::center(std::vector<Point2D>& points)
{
- double numPoints = double( points.size() );
- double sumX=0, sumY=0;
- for( unsigned i=0; i < points.size(); i++ )
+ double numPoints = double(points.size());
+ double sumX = 0, sumY = 0;
+ for (unsigned i = 0; i < points.size(); i++)
{
sumX += points[i].x();
sumY += points[i].y();
}
- return Point2D( sumX / numPoints, sumY / numPoints );
+ return Point2D(sumX / numPoints, sumY / numPoints);
}
-
double THIS::randomGauss(float variance)
{
- if (variance < 0) {
+ if (variance < 0)
+ {
variance = -variance;
}
double x1, x2, w, y1;
- do {
+ do
+ {
x1 = 2.0 * random01() - 1.0;
x2 = 2.0 * random01() - 1.0;
w = x1 * x1 + x2 * x2;
- } while ( w >= 1.0 );
+ } while (w >= 1.0);
w = sqrt((-2.0 * log(w)) / w);
y1 = x1 * w;
@@ -124,12 +131,13 @@ double THIS::randomGauss(float variance)
double THIS::random01(unsigned long init)
{
static unsigned long n;
- if (init > 0) {
+ if (init > 0)
+ {
n = init;
}
n = 1664525 * n + 1013904223;
// create double from unsigned long
- return (double)(n/2) / (double)LONG_MAX;
+ return (double)(n / 2) / (double)LONG_MAX;
}
#undef THIS
diff --git a/homer_nav_libs/src/Math/Point2D.cpp b/homer_nav_libs/src/Math/Point2D.cpp
index 13c321d79216a3b67091cc0c154d7da8bdb01e25..ad117481e1d477ed6363163c11ae0548e8f2918a 100644
--- a/homer_nav_libs/src/Math/Point2D.cpp
+++ b/homer_nav_libs/src/Math/Point2D.cpp
@@ -5,27 +5,27 @@
* Universitaet Koblenz-Landau
*
* Additional information:
- * $Id: $
+ * $Id: $
*******************************************************************************/
#include <homer_nav_libs/Math/Point2D.h>
-#define THIS Point2D
+#define THIS Point2D
-float THIS::getPolarAngle () const
+float THIS::getPolarAngle() const
{
- float angle = atan ( m_Y /m_X );
- if ( m_X < 0 )
+ float angle = atan(m_Y / m_X);
+ if (m_X < 0)
{
- angle = - ( M_PI - angle );
+ angle = -(M_PI - angle);
}
- while ( angle >= M_PI )
+ while (angle >= M_PI)
{
- angle -= 2*M_PI;
+ angle -= 2 * M_PI;
}
- while ( angle < -M_PI )
+ while (angle < -M_PI)
{
- angle += 2*M_PI;
+ angle += 2 * M_PI;
}
return angle;
}
diff --git a/homer_nav_libs/src/Math/Pose.cpp b/homer_nav_libs/src/Math/Pose.cpp
index 129d123a62e47b56c8c1044e5faf8e06a9fdc63e..2b908a8fb3ab854b9475bdf13f02ff201fb53092 100644
--- a/homer_nav_libs/src/Math/Pose.cpp
+++ b/homer_nav_libs/src/Math/Pose.cpp
@@ -14,76 +14,95 @@
using namespace std;
-
#define THIS Pose
-THIS::THIS(float x, float y, float theta) : Point2D(x, y) {
+THIS::THIS(float x, float y, float theta) : Point2D(x, y)
+{
m_Theta = theta;
}
-THIS::THIS() {
+THIS::THIS()
+{
m_Theta = 0.0;
}
-THIS::~THIS() {
+THIS::~THIS()
+{
}
-float THIS::theta() const {
+float THIS::theta() const
+{
return m_Theta;
}
-void THIS::setTheta(float theta) {
+void THIS::setTheta(float theta)
+{
m_Theta = theta;
}
-Pose THIS::operator+ ( const Transformation2D& transformation ) const {
+Pose THIS::operator+(const Transformation2D& transformation) const
+{
float x, y, theta;
x = m_X + transformation.x();
y = m_Y + transformation.y();
theta = m_Theta + transformation.theta();
- while (theta >= M_PI) theta -= 2*M_PI;
- while (theta < -M_PI) theta += 2*M_PI;
+ while (theta >= M_PI)
+ theta -= 2 * M_PI;
+ while (theta < -M_PI)
+ theta += 2 * M_PI;
- return Pose(x, y, theta);
+ return Pose(x, y, theta);
}
-Pose THIS::operator- ( const Transformation2D& transformation ) const {
+Pose THIS::operator-(const Transformation2D& transformation) const
+{
float x, y, theta;
x = m_X - transformation.x();
y = m_Y - transformation.y();
theta = m_Theta - transformation.theta();
- while (theta >= M_PI) theta -= 2*M_PI;
- while (theta < -M_PI) theta += 2*M_PI;
+ while (theta >= M_PI)
+ theta -= 2 * M_PI;
+ while (theta < -M_PI)
+ theta += 2 * M_PI;
- return Pose(x, y, theta);
+ return Pose(x, y, theta);
}
-Transformation2D THIS::operator- ( const Pose& pose ) const {
+Transformation2D THIS::operator-(const Pose& pose) const
+{
float x, y, theta;
x = m_X - pose.x();
y = m_Y - pose.y();
float s1, s2;
- if (m_Theta > pose.theta()) {
- s1 = -( 2 * M_PI - m_Theta + pose.theta());
+ if (m_Theta > pose.theta())
+ {
+ s1 = -(2 * M_PI - m_Theta + pose.theta());
s2 = m_Theta - pose.theta();
- } else {
+ }
+ else
+ {
s1 = 2 * M_PI - pose.theta() + m_Theta;
s2 = -(pose.theta() - m_Theta);
}
- if (fabs(s1) > fabs(s2)) {
+ if (fabs(s1) > fabs(s2))
+ {
theta = s2;
- } else {
+ }
+ else
+ {
theta = s1;
}
- while (theta >= M_PI) theta -= 2*M_PI;
- while (theta < -M_PI) theta += 2*M_PI;
+ while (theta >= M_PI)
+ theta -= 2 * M_PI;
+ while (theta < -M_PI)
+ theta += 2 * M_PI;
- return Transformation2D(x, y, theta);
+ return Transformation2D(x, y, theta);
}
-Pose THIS::interpolate(const Pose& referencePose, float t) const {
-
+Pose THIS::interpolate(const Pose& referencePose, float t) const
+{
float newX = m_X + t * (referencePose.x() - m_X);
float newY = m_Y + t * (referencePose.y() - m_Y);
@@ -92,12 +111,12 @@ Pose THIS::interpolate(const Pose& referencePose, float t) const {
float y1 = sinf(m_Theta);
float x2 = cosf(referencePose.theta());
float y2 = sinf(referencePose.theta());
- float newTheta = atan2 (y1*(1-t)+y2*t, x1*(1-t)+x2*t);
+ float newTheta = atan2(y1 * (1 - t) + y2 * t, x1 * (1 - t) + x2 * t);
return Pose(newX, newY, newTheta);
}
-//THIS::THIS( ExtendedInStream& extStrm )
+// THIS::THIS( ExtendedInStream& extStrm )
//{
// char version;
// extStrm >> version;
@@ -106,7 +125,7 @@ Pose THIS::interpolate(const Pose& referencePose, float t) const {
// extStrm >> m_Theta;
//}
-//void THIS::storer( ExtendedOutStream& extStrm ) const
+// void THIS::storer( ExtendedOutStream& extStrm ) const
//{
// char version=10;
// extStrm << version;
@@ -114,4 +133,3 @@ Pose THIS::interpolate(const Pose& referencePose, float t) const {
// extStrm << m_Y;
// extStrm << m_Theta;
//}
-
diff --git a/homer_nav_libs/src/Math/Transformation2D.cpp b/homer_nav_libs/src/Math/Transformation2D.cpp
index f60629cdf404785af35c273d4fadd55294696d11..9821fb4e3b83d72fa8f84cf25958b052266509f1 100644
--- a/homer_nav_libs/src/Math/Transformation2D.cpp
+++ b/homer_nav_libs/src/Math/Transformation2D.cpp
@@ -9,14 +9,14 @@
#include <homer_nav_libs/Math/Transformation2D.h>
+#include <homer_nav_libs/Math/Line2D.h>
+#include <homer_nav_libs/Math/Point2D.h>
+#include <homer_nav_libs/Math/mat2.h> // TODO das sieht nach baselib aus ggf. durch baselib ersetzen
+#include <homer_nav_libs/Math/vec2.h> // TODO das sieht nach baselib aus ggf. durch baselib ersetzen
#include <cmath>
-#include <vector>
#include <iostream>
#include <sstream>
-#include <homer_nav_libs/Math/vec2.h> // TODO das sieht nach baselib aus ggf. durch baselib ersetzen
-#include <homer_nav_libs/Math/mat2.h> // TODO das sieht nach baselib aus ggf. durch baselib ersetzen
-#include <homer_nav_libs/Math/Point2D.h>
-#include <homer_nav_libs/Math/Line2D.h>
+#include <vector>
#define THIS Transformation2D
#define BASE CVec2
@@ -26,12 +26,12 @@ THIS::Transformation2D() : BASE()
m_Theta = 0.0;
}
-THIS::Transformation2D ( double x, double y, double theta ) : BASE ( x,y )
+THIS::Transformation2D(double x, double y, double theta) : BASE(x, y)
{
m_Theta = theta;
}
-THIS::Transformation2D ( const CVec2& vec, double theta ) : BASE ( vec )
+THIS::Transformation2D(const CVec2& vec, double theta) : BASE(vec)
{
m_Theta = theta;
}
@@ -40,7 +40,7 @@ THIS::~Transformation2D()
{
}
-void THIS::set ( double x, double y, double theta )
+void THIS::set(double x, double y, double theta)
{
m_X = x;
m_Y = y;
@@ -52,40 +52,40 @@ double THIS::theta() const
return m_Theta;
}
-Transformation2D THIS::operator+ ( Transformation2D t ) const
+Transformation2D THIS::operator+(Transformation2D t) const
{
double theta = m_Theta + t.theta();
// TODO comment only for scan matching test
-// while ( theta >= M_PI ) theta -= 2*M_PI;
-// while ( theta < -M_PI ) theta += 2*M_PI;
- return Transformation2D ( m_X + t.x(), m_Y + t.y(), theta );
+ // while ( theta >= M_PI ) theta -= 2*M_PI;
+ // while ( theta < -M_PI ) theta += 2*M_PI;
+ return Transformation2D(m_X + t.x(), m_Y + t.y(), theta);
}
-Transformation2D& THIS::operator+= ( Transformation2D t )
+Transformation2D& THIS::operator+=(Transformation2D t)
{
m_X += t.x();
m_Y += t.y();
m_Theta += t.theta();
// TODO comment only for scan matching test
-// while ( m_Theta >= M_PI ) m_Theta -= 2*M_PI;
-// while ( m_Theta < -M_PI ) m_Theta += 2*M_PI;
- return ( *this );
+ // while ( m_Theta >= M_PI ) m_Theta -= 2*M_PI;
+ // while ( m_Theta < -M_PI ) m_Theta += 2*M_PI;
+ return (*this);
}
-Transformation2D THIS::operator- ( Transformation2D t ) const
+Transformation2D THIS::operator-(Transformation2D t) const
{
float s1, s2, theta;
- if ( m_Theta > t.theta() )
+ if (m_Theta > t.theta())
{
- s1 = - ( 2 * M_PI - m_Theta + t.theta() );
+ s1 = -(2 * M_PI - m_Theta + t.theta());
s2 = m_Theta - t.theta();
}
else
{
s1 = 2 * M_PI - t.theta() + m_Theta;
- s2 = - ( t.theta() - m_Theta );
+ s2 = -(t.theta() - m_Theta);
}
- if ( fabs ( s1 ) > fabs ( s2 ) )
+ if (fabs(s1) > fabs(s2))
{
theta = s2;
}
@@ -93,31 +93,33 @@ Transformation2D THIS::operator- ( Transformation2D t ) const
{
theta = s1;
}
- while ( theta >= M_PI ) theta -= 2*M_PI;
- while ( theta < -M_PI ) theta += 2*M_PI;
-// double theta = m_Theta - t.theta();
-// while ( theta >= M_PI ) theta -= 2*M_PI;
-// while ( theta < -M_PI ) theta += 2*M_PI;
- return Transformation2D ( m_X - t.x(), m_Y - t.y(), theta );
+ while (theta >= M_PI)
+ theta -= 2 * M_PI;
+ while (theta < -M_PI)
+ theta += 2 * M_PI;
+ // double theta = m_Theta - t.theta();
+ // while ( theta >= M_PI ) theta -= 2*M_PI;
+ // while ( theta < -M_PI ) theta += 2*M_PI;
+ return Transformation2D(m_X - t.x(), m_Y - t.y(), theta);
}
-Transformation2D& THIS::operator-= ( Transformation2D t )
+Transformation2D& THIS::operator-=(Transformation2D t)
{
m_X -= t.x();
m_Y -= t.y();
float s1, s2, theta;
- if ( m_Theta > t.theta() )
+ if (m_Theta > t.theta())
{
- s1 = - ( 2 * M_PI - m_Theta + t.theta() );
+ s1 = -(2 * M_PI - m_Theta + t.theta());
s2 = m_Theta - t.theta();
}
else
{
s1 = 2 * M_PI - t.theta() + m_Theta;
- s2 = - ( t.theta() - m_Theta );
+ s2 = -(t.theta() - m_Theta);
}
- if ( fabs ( s1 ) > fabs ( s2 ) )
+ if (fabs(s1) > fabs(s2))
{
theta = s2;
}
@@ -125,57 +127,61 @@ Transformation2D& THIS::operator-= ( Transformation2D t )
{
theta = s1;
}
- while ( theta >= M_PI ) theta -= 2*M_PI;
- while ( theta < -M_PI ) theta += 2*M_PI;
+ while (theta >= M_PI)
+ theta -= 2 * M_PI;
+ while (theta < -M_PI)
+ theta += 2 * M_PI;
m_Theta = theta;
- return ( *this );
+ return (*this);
-// m_X -= t.x();
-// m_Y -= t.y();
-// m_Theta -= t.theta();
-// while ( m_Theta >= M_PI ) m_Theta -= 2*M_PI;
-// while ( m_Theta < -M_PI ) m_Theta += 2*M_PI;
-// return ( *this );
+ // m_X -= t.x();
+ // m_Y -= t.y();
+ // m_Theta -= t.theta();
+ // while ( m_Theta >= M_PI ) m_Theta -= 2*M_PI;
+ // while ( m_Theta < -M_PI ) m_Theta += 2*M_PI;
+ // return ( *this );
}
-Transformation2D THIS::operator* ( float factor ) const
+Transformation2D THIS::operator*(float factor) const
{
-
double theta = m_Theta * factor;
- while ( theta >= M_PI ) theta -= 2*M_PI;
- while ( theta < -M_PI ) theta += 2*M_PI;
- return Transformation2D ( m_X * factor, m_Y * factor, theta );
+ while (theta >= M_PI)
+ theta -= 2 * M_PI;
+ while (theta < -M_PI)
+ theta += 2 * M_PI;
+ return Transformation2D(m_X * factor, m_Y * factor, theta);
}
-Transformation2D& THIS::operator*= ( float factor )
+Transformation2D& THIS::operator*=(float factor)
{
m_X *= factor;
m_Y *= factor;
m_Theta *= factor;
- while ( m_Theta >= M_PI ) m_Theta -= 2*M_PI;
- while ( m_Theta < -M_PI ) m_Theta += 2*M_PI;
- return ( *this );
+ while (m_Theta >= M_PI)
+ m_Theta -= 2 * M_PI;
+ while (m_Theta < -M_PI)
+ m_Theta += 2 * M_PI;
+ return (*this);
}
-
-Transformation2D THIS::operator/ ( float factor ) const
+Transformation2D THIS::operator/(float factor) const
{
double theta = m_Theta / factor;
- return Transformation2D ( m_X / factor, m_Y / factor, theta );
+ return Transformation2D(m_X / factor, m_Y / factor, theta);
}
-Transformation2D& THIS::operator/= ( float factor )
+Transformation2D& THIS::operator/=(float factor)
{
m_X /= factor;
m_Y /= factor;
m_Theta /= factor;
- return ( *this );
+ return (*this);
}
-bool THIS::operator== ( Transformation2D t ) const
+bool THIS::operator==(Transformation2D t) const
{
- if ( t.x() == m_X && t.y() == m_Y && t.theta() == m_Theta )
+ if (t.x() == m_X && t.y() == m_Y && t.theta() == m_Theta)
{
return true;
}
@@ -185,61 +191,63 @@ bool THIS::operator== ( Transformation2D t ) const
}
}
-bool THIS::operator!= ( Transformation2D t ) const
+bool THIS::operator!=(Transformation2D t) const
{
- return ! ( ( *this ) ==t );
+ return !((*this) == t);
}
-bool THIS::operator<= ( Transformation2D t ) const
+bool THIS::operator<=(Transformation2D t) const
{
- return ( this->magnitude() <= t.magnitude() ) && ( m_Theta <= t.theta() );
+ return (this->magnitude() <= t.magnitude()) && (m_Theta <= t.theta());
}
-bool THIS::operator>= ( Transformation2D t ) const
+bool THIS::operator>=(Transformation2D t) const
{
- return ( this->magnitude() >= t.magnitude() ) && ( m_Theta >= t.theta() );
+ return (this->magnitude() >= t.magnitude()) && (m_Theta >= t.theta());
}
-bool THIS::operator< ( Transformation2D t ) const
+bool THIS::operator<(Transformation2D t) const
{
- return ( m_X < t.x() ) || ( m_Y < t.y() ) || ( ( m_Theta < t.theta() ) && ( *this <= t ) );
+ return (m_X < t.x()) || (m_Y < t.y()) ||
+ ((m_Theta < t.theta()) && (*this <= t));
}
-bool THIS::operator> ( Transformation2D t ) const
+bool THIS::operator>(Transformation2D t) const
{
- return ( m_X > t.x() ) || ( m_Y > t.y() ) || ( ( m_Theta > t.theta() ) && ( *this >= t ) );
+ return (m_X > t.x()) || (m_Y > t.y()) ||
+ ((m_Theta > t.theta()) && (*this >= t));
}
Transformation2D THIS::abs() const
{
- return Transformation2D ( std::abs ( m_X ), std::abs ( m_Y ), std::abs ( m_Theta ) );
+ return Transformation2D(std::abs(m_X), std::abs(m_Y), std::abs(m_Theta));
}
Transformation2D THIS::inverse() const
{
- return ( *this ) * ( -1.0 );
+ return (*this) * (-1.0);
}
-Point2D THIS::transform ( const Point2D& point ) const
+Point2D THIS::transform(const Point2D& point) const
{
- CMat2 rotMat = CMat2 ( m_Theta );
- CVec2 transVec = CVec2 ( m_X, m_Y );
- Point2D transformedPoint = rotMat * ( point );
+ CMat2 rotMat = CMat2(m_Theta);
+ CVec2 transVec = CVec2(m_X, m_Y);
+ Point2D transformedPoint = rotMat * (point);
transformedPoint += transVec;
return transformedPoint;
}
-std::vector<Point2D> THIS::transform ( const std::vector<Point2D>& points ) const
+std::vector<Point2D> THIS::transform(const std::vector<Point2D>& points) const
{
- CMat2 rotMat = CMat2 ( m_Theta );
- CVec2 transVec = CVec2 ( m_X, m_Y );
+ CMat2 rotMat = CMat2(m_Theta);
+ CVec2 transVec = CVec2(m_X, m_Y);
std::vector<Point2D> transformedPoints;
std::vector<Point2D>::const_iterator iter = points.begin();
- while ( iter != points.end() )
+ while (iter != points.end())
{
- Point2D currPoint = rotMat * ( *iter );
+ Point2D currPoint = rotMat * (*iter);
currPoint += transVec;
- transformedPoints.push_back ( currPoint );
+ transformedPoints.push_back(currPoint);
iter++;
}
return transformedPoints;
@@ -272,23 +280,24 @@ std::vector<Point2D> THIS::transform ( const std::vector<Point2D>& points ) cons
// return transformedPoints;
// }
-Line2D THIS::transform ( const Line2D& line ) const
+Line2D THIS::transform(const Line2D& line) const
{
- CMat2 rotMat = CMat2 ( m_Theta );
- CVec2 transVec = CVec2 ( m_X, m_Y );
- Line2D transformedLine = Line2D ( rotMat * line.start() + transVec, rotMat * line.end() + transVec );
+ CMat2 rotMat = CMat2(m_Theta);
+ CVec2 transVec = CVec2(m_X, m_Y);
+ Line2D transformedLine =
+ Line2D(rotMat * line.start() + transVec, rotMat * line.end() + transVec);
return transformedLine;
}
-std::vector<Line2D> THIS::transform ( const std::vector<Line2D>& lines ) const
+std::vector<Line2D> THIS::transform(const std::vector<Line2D>& lines) const
{
- //CMat2 rotMat = CMat2 ( m_Theta );
- //CVec2 transVec = CVec2 ( m_X, m_Y );
+ // CMat2 rotMat = CMat2 ( m_Theta );
+ // CVec2 transVec = CVec2 ( m_X, m_Y );
std::vector<Line2D> transformedLines;
std::vector<Line2D>::const_iterator iter = lines.begin();
- while ( iter != lines.end() )
+ while (iter != lines.end())
{
- transformedLines.push_back ( transform(*iter) );
+ transformedLines.push_back(transform(*iter));
iter++;
}
return transformedLines;
@@ -297,12 +306,10 @@ std::vector<Line2D> THIS::transform ( const std::vector<Line2D>& lines ) const
std::string THIS::toString() const
{
std::ostringstream str;
- str << "deltaX: " << m_X << ", deltaY: " << m_Y << ", deltaTheta: " << m_Theta;
+ str << "deltaX: " << m_X << ", deltaY: " << m_Y
+ << ", deltaTheta: " << m_Theta;
return str.str();
}
-
-
#undef THIS
#undef BASE
-
diff --git a/homer_nav_libs/src/SpeedControl/SpeedControl.cpp b/homer_nav_libs/src/SpeedControl/SpeedControl.cpp
index c939197e0c3cf63ad6142b350b711ee7210e8e68..82287f21397eb82d1555f0ca4c9f886bc2cf9a28 100644
--- a/homer_nav_libs/src/SpeedControl/SpeedControl.cpp
+++ b/homer_nav_libs/src/SpeedControl/SpeedControl.cpp
@@ -8,8 +8,8 @@
using namespace std;
// Robot dimensions in m
-//
-//
+//
+//
// /-------------\ <-- MAX_X
// | x |
// | | |
@@ -24,99 +24,113 @@ using namespace std;
// MAX_Y MIN_Y
//
float ROBOT_MIN_X = -0.30;
-float ROBOT_MAX_X = 0.30;
+float ROBOT_MAX_X = 0.30;
float ROBOT_MIN_Y = -0.27;
-float ROBOT_MAX_Y = 0.27;
+float ROBOT_MAX_Y = 0.27;
+namespace
+{
+Eigen::AlignedBox2f InnerDangerZone, OuterDangerZone;
+float InnerDangerZoneFactor, OuterDangerZoneFactor;
-namespace {
- Eigen::AlignedBox2f InnerDangerZone,
- OuterDangerZone;
- float InnerDangerZoneFactor,
- OuterDangerZoneFactor;
-
- inline Eigen::AlignedBox2f loadRect(const string& path)
- {
- pair<float, float> pX, pY;
- ros::param::get(path + "/x_min", pX.first);
- ros::param::get(path + "/x_max", pX.second);
- ros::param::get(path + "/y_min", pY.first);
- ros::param::get(path + "/y_max", pY.second);
-
- Eigen::Vector2f first(pX.first, pY.first), second(pX.second, pY.second);
- return Eigen::AlignedBox2f(first, second);
- }
+inline Eigen::AlignedBox2f loadRect(const string& path)
+{
+ pair<float, float> pX, pY;
+ ros::param::get(path + "/x_min", pX.first);
+ ros::param::get(path + "/x_max", pX.second);
+ ros::param::get(path + "/y_min", pY.first);
+ ros::param::get(path + "/y_max", pY.second);
+
+ Eigen::Vector2f first(pX.first, pY.first), second(pX.second, pY.second);
+ return Eigen::AlignedBox2f(first, second);
+}
}
void SpeedControl::loadDimensions()
{
- InnerDangerZone = loadRect("/homer_navigation/speed_control/inner_danger_zone");
+ InnerDangerZone =
+ loadRect("/homer_navigation/speed_control/inner_danger_zone");
InnerDangerZoneFactor;
- ros::param::get("/homer_navigation/speed_control/inner_danger_zone/speed_factor", InnerDangerZoneFactor);
- OuterDangerZone = loadRect("/homer_navigation/speed_control/inner_danger_zone");
+ ros::param::get("/homer_navigation/speed_control/inner_danger_zone/"
+ "speed_factor",
+ InnerDangerZoneFactor);
+ OuterDangerZone =
+ loadRect("/homer_navigation/speed_control/inner_danger_zone");
OuterDangerZoneFactor;
- ros::param::get("/homer_navigation/speed_control/outer_danger_zone/speed_factor", OuterDangerZoneFactor);
- if(!OuterDangerZone.contains(InnerDangerZone))
+ ros::param::get("/homer_navigation/speed_control/outer_danger_zone/"
+ "speed_factor",
+ OuterDangerZoneFactor);
+ if (!OuterDangerZone.contains(InnerDangerZone))
ROS_WARN_STREAM("InnerDangerZone is not contained in OuterDangerZone");
}
-float SpeedControl::getSpeedFactor(const vector<geometry_msgs::Point>& points, float minVal, float maxVal )
+float SpeedControl::getSpeedFactor(const vector<geometry_msgs::Point>& points,
+ float minVal, float maxVal)
{
float minFactor = 1.0;
for (unsigned i = 0; i < points.size(); i++)
{
Eigen::Vector2f point(points[i].x, points[i].y);
- if(InnerDangerZone.contains(point))
+ if (InnerDangerZone.contains(point))
{
minFactor = InnerDangerZoneFactor;
break;
}
- if(OuterDangerZone.contains(point))
+ if (OuterDangerZone.contains(point))
minFactor = OuterDangerZoneFactor;
}
minFactor = sqrt(minFactor);
float range = maxVal - minVal;
- minFactor = minVal + range*minFactor;
+ minFactor = minVal + range * minFactor;
return minFactor;
}
float SpeedControl::getMaxMoveDistance(vector<geometry_msgs::Point> points)
{
- float minDistance = 4; // distance in m to nearest obstacle in front
+ float minDistance = 4; // distance in m to nearest obstacle in front
for (unsigned int i = 0; i < points.size(); i++)
{
- if(points[i].y > ROBOT_MIN_Y && points[i].y < ROBOT_MAX_Y && points[i].x > ROBOT_MAX_X)
+ if (points[i].y > ROBOT_MIN_Y && points[i].y < ROBOT_MAX_Y &&
+ points[i].x > ROBOT_MAX_X)
+ {
+ float distance =
+ sqrt((points[i].x * points[i].x) + (points[i].y * points[i].y));
+ if (distance < minDistance)
{
- float distance = sqrt((points[i].x * points[i].x) + (points[i].y * points[i].y));
- if (distance < minDistance)
- {
- minDistance = distance;
- }
+ minDistance = distance;
}
+ }
}
float maxMoveDist = minDistance - ROBOT_MAX_X;
- if (maxMoveDist < 0) {
+ if (maxMoveDist < 0)
+ {
maxMoveDist = 0.0;
}
return maxMoveDist;
}
-float SpeedControl::getMaxMoveDistance(std::vector< Eigen::Vector3d >* kinectData, float minObstacleHeight, float minObstacleFromRobotDistance, float maxObstacleFromRobotDistance)
+float SpeedControl::getMaxMoveDistance(std::vector<Eigen::Vector3d>* kinectData,
+ float minObstacleHeight,
+ float minObstacleFromRobotDistance,
+ float maxObstacleFromRobotDistance)
{
// Check for obstacles in Kinect image: Look for closest point
- float minDistance = 4; // distance to nearest obstacle in front
+ float minDistance = 4; // distance to nearest obstacle in front
- for(int i=0;i<kinectData->size();++i)
+ for (int i = 0; i < kinectData->size(); ++i)
{
- Eigen::Vector2d p = Eigen::Vector2d(kinectData->at(i).x(), kinectData->at(i).y());
- if(!std::isnan(p.x()))
+ Eigen::Vector2d p =
+ Eigen::Vector2d(kinectData->at(i).x(), kinectData->at(i).y());
+ if (!std::isnan(p.x()))
{
// Filter point cloud
- if(p.x() > minObstacleFromRobotDistance && p.x() < maxObstacleFromRobotDistance && kinectData->at(i).z() > minObstacleHeight)
+ if (p.x() > minObstacleFromRobotDistance &&
+ p.x() < maxObstacleFromRobotDistance &&
+ kinectData->at(i).z() > minObstacleHeight)
{
// Check for collisions outside of robot
- if(p.y() > ROBOT_MIN_Y && p.y() < ROBOT_MAX_Y && p.x() > ROBOT_MAX_X)
+ if (p.y() > ROBOT_MIN_Y && p.y() < ROBOT_MAX_Y && p.x() > ROBOT_MAX_X)
{
float distance = sqrt((p.x() * p.x()) + (p.y() * p.y()));
if (distance < minDistance)
@@ -129,49 +143,56 @@ float SpeedControl::getMaxMoveDistance(std::vector< Eigen::Vector3d >* kinectDat
}
float maxMoveDist = minDistance - ROBOT_MAX_X;
- if (maxMoveDist < 0) {
+ if (maxMoveDist < 0)
+ {
maxMoveDist = 0.0;
}
return maxMoveDist;
}
-float SpeedControl::getTurnSpeedFactor( float speedFactor, float turnAngle, float minVal, float maxVal )
+float SpeedControl::getTurnSpeedFactor(float speedFactor, float turnAngle,
+ float minVal, float maxVal)
{
- //turn faster for larger angles
- float angleDependentFactor = sqrt( fabs(turnAngle) / M_PI );
- angleDependentFactor = minVal + angleDependentFactor*(maxVal-minVal);
- return sqrt( speedFactor * angleDependentFactor );
+ // turn faster for larger angles
+ float angleDependentFactor = sqrt(fabs(turnAngle) / M_PI);
+ angleDependentFactor = minVal + angleDependentFactor * (maxVal - minVal);
+ return sqrt(speedFactor * angleDependentFactor);
}
-float SpeedControl::getMinTurnAngle(std::vector<geometry_msgs::Point> laserData, float minAngle, float maxAngle, float minDistance, float maxDistance)
+float SpeedControl::getMinTurnAngle(std::vector<geometry_msgs::Point> laserData,
+ float minAngle, float maxAngle,
+ float minDistance, float maxDistance)
{
- float turn_factor = 1.0;
- for (unsigned int i = 0; i < laserData.size(); i++)
+ float turn_factor = 1.0;
+ for (unsigned int i = 0; i < laserData.size(); i++)
+ {
+ if (laserData[i].y > ROBOT_MIN_Y && laserData[i].y < ROBOT_MAX_Y &&
+ laserData[i].x > ROBOT_MAX_X)
{
- if(laserData[i].y > ROBOT_MIN_Y && laserData[i].y < ROBOT_MAX_Y && laserData[i].x > ROBOT_MAX_X)
- {
- float distance = sqrt((laserData[i].x * laserData[i].x) + (laserData[i].y * laserData[i].y));
- if (distance < minDistance + ROBOT_MAX_X)
- {
- turn_factor = 0.0;
- }
- else if(distance > maxDistance + ROBOT_MAX_X)
- {
- turn_factor = 1.0;
- }
- else
- {
- turn_factor = (distance - minDistance)/maxDistance;
- }
- }
+ float distance = sqrt((laserData[i].x * laserData[i].x) +
+ (laserData[i].y * laserData[i].y));
+ if (distance < minDistance + ROBOT_MAX_X)
+ {
+ turn_factor = 0.0;
+ }
+ else if (distance > maxDistance + ROBOT_MAX_X)
+ {
+ turn_factor = 1.0;
+ }
+ else
+ {
+ turn_factor = (distance - minDistance) / maxDistance;
+ }
}
- float range = maxAngle - minAngle;
- return minAngle + turn_factor * range;
+ }
+ float range = maxAngle - minAngle;
+ return minAngle + turn_factor * range;
}
-SpeedControl::SpeedControl() {
+SpeedControl::SpeedControl()
+{
}
-SpeedControl::~SpeedControl() {
+SpeedControl::~SpeedControl()
+{
}
-
diff --git a/homer_nav_libs/src/SpeedControl/SpeedControl.h b/homer_nav_libs/src/SpeedControl/SpeedControl.h
index 7bef0e9a16eb1f433c7ba23e7bd1c1b671253d7b..3b11daf0799fdb3e76bf9df3399a61612da8db4b 100644
--- a/homer_nav_libs/src/SpeedControl/SpeedControl.h
+++ b/homer_nav_libs/src/SpeedControl/SpeedControl.h
@@ -1,71 +1,82 @@
#ifndef SPEEDCONTROL_H
-#define SPEEDCONTROL_H
+#define SPEEDCONTROL_H
-#include <vector>
-#include <Eigen/Geometry>
#include <geometry_msgs/Point.h>
+#include <Eigen/Geometry>
+#include <vector>
-/**
+/**
* @class SpeedControl
* @author Malte Knauf, Stephan Wirth
- * @brief Class for computing a speed factor with respect to a given laser measurement.
+ * @brief Class for computing a speed factor with respect to a given laser
+ * measurement.
*/
-class SpeedControl {
-
- public:
+class SpeedControl
+{
+public:
+ /**
+ * @brief Loads robot and safety zone dimensions config values
+ */
+ static void loadDimensions();
- /**
- * @brief Loads robot and safety zone dimensions config values
- */
- static void loadDimensions();
+ /**
+ * Calculates the speed factor for the robot. If a measured obstacle lies in
+ * the "danger zone"
+ * that is defined in SpeedControl.cpp, the speed factor will be below maxVal.
+ * The nearer the obstacle,
+ * the smaller the speed factor.
+ * @param laserData Laser measurement
+ * @param minVal,maxVal range of return values
+ * @return Speed factor, value between minVal and maxVal. The higher the speed
+ * factor, the safer is it to drive fast.
+ */
+ static float getSpeedFactor(const std::vector<geometry_msgs::Point>& points,
+ float minVal = 0.2, float maxVal = 1.0);
- /**
- * Calculates the speed factor for the robot. If a measured obstacle lies in the "danger zone"
- * that is defined in SpeedControl.cpp, the speed factor will be below maxVal. The nearer the obstacle,
- * the smaller the speed factor.
- * @param laserData Laser measurement
- * @param minVal,maxVal range of return values
- * @return Speed factor, value between minVal and maxVal. The higher the speed factor, the safer is it to drive fast.
- */
- static float getSpeedFactor(const std::vector<geometry_msgs::Point>& points, float minVal=0.2, float maxVal=1.0);
+ /**
+ * Calculates the maximum distance the robot can move without touching an
+ * obstacle.
+ * @param laserPoints Current laser measurement transformed to (valid) points
+ * in map frame
+ * @param laserConf The configuration of the LRF that took the measurement
+ * @return maximum distance (m) the robot can move based on the given
+ * laserscan.
+ */
+ static float getMaxMoveDistance(std::vector<geometry_msgs::Point> laserData);
- /**
- * Calculates the maximum distance the robot can move without touching an obstacle.
- * @param laserPoints Current laser measurement transformed to (valid) points in map frame
- * @param laserConf The configuration of the LRF that took the measurement
- * @return maximum distance (m) the robot can move based on the given laserscan.
- */
- static float getMaxMoveDistance(std::vector<geometry_msgs::Point> laserData);
+ static float getMaxMoveDistance(std::vector<Eigen::Vector3d>* kinectData,
+ float minObstacleHeight,
+ float minObstacleFromRobotDistance,
+ float maxObstacleFromRobotDistance);
- static float getMaxMoveDistance(std::vector< Eigen::Vector3d >* kinectData, float minObstacleHeight, float minObstacleFromRobotDistance, float maxObstacleFromRobotDistance);
+ /// @return if the angle is larger, the turn speed factor will be higher
+ static float getTurnSpeedFactor(float speedFactor, float turnAngle,
+ float minVal, float maxVal);
- /// @return if the angle is larger, the turn speed factor will be higher
- static float getTurnSpeedFactor( float speedFactor, float turnAngle, float minVal, float maxVal );
+ /**
+ * Calculates the minimum angle between the robot's orientation and the next
+ * waypoint which is necessary
+ * to trigger a rotation instead of a straight line
+ * @brief getMinTurnAngle
+ * @param laserData
+ * @param minAngle
+ * @param maxAngle
+ * @return
+ */
+ static float getMinTurnAngle(std::vector<geometry_msgs::Point> laserData,
+ float minAngle, float maxAngle,
+ float minDistance, float maxDistance);
- /**
- * Calculates the minimum angle between the robot's orientation and the next waypoint which is necessary
- * to trigger a rotation instead of a straight line
- * @brief getMinTurnAngle
- * @param laserData
- * @param minAngle
- * @param maxAngle
- * @return
- */
- static float getMinTurnAngle(std::vector<geometry_msgs::Point> laserData, float minAngle, float maxAngle,
- float minDistance, float maxDistance);
-
- private:
-
- /**
- * Constructor is empty and private because this class will never be instanciated.
- */
- SpeedControl();
-
- /**
- * Destructor is empty.
- */
- ~SpeedControl();
+private:
+ /**
+ * Constructor is empty and private because this class will never be
+ * instanciated.
+ */
+ SpeedControl();
+ /**
+ * Destructor is empty.
+ */
+ ~SpeedControl();
};
#endif
-
diff --git a/homer_navigation/include/homer_navigation/homer_navigation_node.h b/homer_navigation/include/homer_navigation/homer_navigation_node.h
index 5765fae5f6d3415c4b29078493c363d9e63b698c..32646f7fd95c24c44302b20a537c70a018e2aa7e 100644
--- a/homer_navigation/include/homer_navigation/homer_navigation_node.h
+++ b/homer_navigation/include/homer_navigation/homer_navigation_node.h
@@ -40,307 +40,320 @@ class Explorer;
* @author Malte Knauf, Stephan Wirth, David Gossow (RX), Florian Polster
* @brief Performs autonomous navigation
*/
-class HomerNavigationNode {
- public:
- /**
- * @brief States of the state machines
- */
- enum ProcessState { IDLE, FOLLOWING_PATH, AVOIDING_COLLISION, FINAL_TURN };
-
- /**
- * The constructor
- */
- HomerNavigationNode();
-
- /**
- * The destructor
- */
- virtual ~HomerNavigationNode();
-
- /** @brief Is called in constant intervals. */
- void idleProcess();
-
- protected:
- /** @brief Handles incoming messages. */
- void mapCallback(const nav_msgs::OccupancyGrid::ConstPtr& msg);
- void ignoreLaserCallback(const std_msgs::String::ConstPtr& msg);
- void poseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg);
- void laserDataCallback(const sensor_msgs::LaserScan::ConstPtr& msg);
- void startNavigationCallback(
- const homer_mapnav_msgs::StartNavigation::ConstPtr& msg);
- void moveBaseSimpleGoalCallback(
- const geometry_msgs::PoseStamped::ConstPtr& msg);
- void stopNavigationCallback(const std_msgs::Empty::ConstPtr& msg);
-
- void navigateToPOICallback(
- const homer_mapnav_msgs::NavigateToPOI::ConstPtr& msg);
- void unknownThresholdCallback(const std_msgs::Int8::ConstPtr& msg);
- void maxDepthMoveDistanceCallback(const std_msgs::Float32::ConstPtr& msg);
-
- /** @brief initializes and refreshs parameters */
- void loadParameters();
-
- /** @brief Is called when all modules are loaded and thread has started. */
- virtual void init();
-
- void initNewTarget();
-
- private:
- /** @brief Start navigation to m_Target on last_map_data_ */
- void startNavigation();
-
- geometry_msgs::Point calculateMeanPoint(
- const std::vector<geometry_msgs::Point>& points);
- /** @brief Check if obstacles are blocking the way in last_map_data_ */
- bool obstacleOnPath();
-
- /** @brief calculate path from current robot position to target
- * approximation
- */
- void calculatePath();
-
- void setExplorerMap();
-
- /** @brief Send message containing current navigation path */
- void sendPathData();
-
- /** @brief Sends target reached and stops the robot. */
- void sendTargetReachedMsg();
-
- /**
- * @brief Sends a target unreachable with given reason and stops the robot.
- * @param reason reason for unreachable target (see
- * homer_mapnav_msgs::TargetUnreachable for possible reasons)
- */
- void sendTargetUnreachableMsg(int8_t reason);
-
- /** @brief reloads all params from the parameterserver */
- void refreshParamsCallback(const std_msgs::Empty::ConstPtr& msg);
-
- /** @brief Navigate robot to next waypoint */
- void performNextMove();
-
- /** @brief Finishes navigation or starts turning to target direction if the
- * target position has been reached */
- bool isTargetPositionReached();
- bool m_new_target;
-
- /** @return Angle from robot_pose_ to point in degrees */
- int angleToPointDeg(geometry_msgs::Point point);
-
- /** @brief Calculates current maximal backwards distance on map Data */
- bool backwardObstacle();
-
- /** @brief stops the Robot */
- void stopRobot();
-
- bool isInIgnoreList(std::string frame_id);
-
- /**
- * @brief Sets each cell of the map to -1 outside the bounding box
- * containing the robot pose and the current target
- */
- void maskMap(nav_msgs::OccupancyGrid& cmap);
-
- /**
- * @brief Current path was finished (either successful or not),
- * sets state machine to path planning to check if the robot is
- * already
- * at the goal
- */
- void currentPathFinished();
+class HomerNavigationNode
+{
+public:
+ /**
+ * @brief States of the state machines
+ */
+ enum ProcessState
+ {
+ IDLE,
+ FOLLOWING_PATH,
+ AVOIDING_COLLISION,
+ FINAL_TURN
+ };
+
+ /**
+ * The constructor
+ */
+ HomerNavigationNode();
+
+ /**
+ * The destructor
+ */
+ virtual ~HomerNavigationNode();
+
+ /** @brief Is called in constant intervals. */
+ void idleProcess();
+
+protected:
+ /** @brief Handles incoming messages. */
+ void mapCallback(const nav_msgs::OccupancyGrid::ConstPtr& msg);
+ void ignoreLaserCallback(const std_msgs::String::ConstPtr& msg);
+ void poseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg);
+ void laserDataCallback(const sensor_msgs::LaserScan::ConstPtr& msg);
+ void startNavigationCallback(
+ const homer_mapnav_msgs::StartNavigation::ConstPtr& msg);
+ void
+ moveBaseSimpleGoalCallback(const geometry_msgs::PoseStamped::ConstPtr& msg);
+ void stopNavigationCallback(const std_msgs::Empty::ConstPtr& msg);
+
+ void
+ navigateToPOICallback(const homer_mapnav_msgs::NavigateToPOI::ConstPtr& msg);
+ void unknownThresholdCallback(const std_msgs::Int8::ConstPtr& msg);
+ void maxDepthMoveDistanceCallback(const std_msgs::Float32::ConstPtr& msg);
+
+ /** @brief initializes and refreshs parameters */
+ void loadParameters();
+
+ /** @brief Is called when all modules are loaded and thread has started. */
+ virtual void init();
+
+ void initNewTarget();
+
+private:
+ /** @brief Start navigation to m_Target on last_map_data_ */
+ void startNavigation();
+
+ geometry_msgs::Point
+ calculateMeanPoint(const std::vector<geometry_msgs::Point>& points);
+ /** @brief Check if obstacles are blocking the way in last_map_data_ */
+ bool obstacleOnPath();
+
+ /** @brief calculate path from current robot position to target
+ * approximation
+ */
+ void calculatePath();
+
+ void setExplorerMap();
+
+ /** @brief Send message containing current navigation path */
+ void sendPathData();
+
+ /** @brief Sends target reached and stops the robot. */
+ void sendTargetReachedMsg();
+
+ /**
+ * @brief Sends a target unreachable with given reason and stops the robot.
+ * @param reason reason for unreachable target (see
+ * homer_mapnav_msgs::TargetUnreachable for possible reasons)
+ */
+ void sendTargetUnreachableMsg(int8_t reason);
+
+ /** @brief reloads all params from the parameterserver */
+ void refreshParamsCallback(const std_msgs::Empty::ConstPtr& msg);
+
+ /** @brief Navigate robot to next waypoint */
+ void performNextMove();
+
+ /** @brief Finishes navigation or starts turning to target direction if the
+ * target position has been reached */
+ bool isTargetPositionReached();
+ bool m_new_target;
+
+ /** @return Angle from robot_pose_ to point in degrees */
+ int angleToPointDeg(geometry_msgs::Point point);
+
+ /** @brief Calculates current maximal backwards distance on map Data */
+ bool backwardObstacle();
+
+ /** @brief stops the Robot */
+ void stopRobot();
+
+ bool isInIgnoreList(std::string frame_id);
+
+ /**
+ * @brief Sets each cell of the map to -1 outside the bounding box
+ * containing the robot pose and the current target
+ */
+ void maskMap(nav_msgs::OccupancyGrid& cmap);
+
+ /**
+ * @brief Current path was finished (either successful or not),
+ * sets state machine to path planning to check if the robot is
+ * already
+ * at the goal
+ */
+ void currentPathFinished();
+
+ // convenience math functions
+ /**
+ * Computes minimum turn angle from angle 1 to angle 2
+ * @param angle1 from angle
+ * @param angle2 to angle
+ * @return minimal angle needed to turn from angle 1 to angle 2 [-Pi..Pi]
+ */
+ static float minTurnAngle(float angle1, float angle2);
- // convenience math functions
- /**
- * Computes minimum turn angle from angle 1 to angle 2
- * @param angle1 from angle
- * @param angle2 to angle
- * @return minimal angle needed to turn from angle 1 to angle 2 [-Pi..Pi]
- */
- static float minTurnAngle(float angle1, float angle2);
+ /**
+ * converts value from degree to radiant
+ * @param deg Value in degree
+ * @return value in radiants
+ */
+ static float deg2Rad(float deg)
+ {
+ return deg / 180.0 * M_PI;
+ }
- /**
- * converts value from degree to radiant
- * @param deg Value in degree
- * @return value in radiants
- */
- static float deg2Rad(float deg) { return deg / 180.0 * M_PI; }
+ /**
+ * converts value from radiants to degrees
+ * @param rad Value in radiants
+ * @return value in degrees
+ */
+ static float rad2Deg(float rad)
+ {
+ return rad / M_PI * 180.0;
+ }
- /**
- * converts value from radiants to degrees
- * @param rad Value in radiants
- * @return value in degrees
- */
- static float rad2Deg(float rad) { return rad / M_PI * 180.0; }
+ bool drawPolygon(std::vector<geometry_msgs::Point> vertices);
+ void drawLine(std::vector<int>& data, int startX, int startY, int endX,
+ int endY, int value);
+ bool fillPolygon(std::vector<int>& data, int x, int y, int value);
- bool drawPolygon(std::vector<geometry_msgs::Point> vertices);
- void drawLine(std::vector<int>& data, int startX, int startY, int endX,
- int endY, int value);
- bool fillPolygon(std::vector<int>& data, int x, int y, int value);
+ /** @brief calcs the maximal move distance from Laser and DepthData */
+ void calcMaxMoveDist();
- /** @brief calcs the maximal move distance from Laser and DepthData */
- void calcMaxMoveDist();
+ /// @brief Worker instances
+ Explorer* m_explorer;
- /// @brief Worker instances
- Explorer* m_explorer;
+ /// @brief State machine
+ StateMachine<ProcessState> m_MainMachine;
- /// @brief State machine
- StateMachine<ProcessState> m_MainMachine;
+ /// @brief Navigation options & data
- /// @brief Navigation options & data
+ /** list of waypoints subsampled from m_PixelPath */
+ std::vector<geometry_msgs::PoseStamped> m_waypoints;
- /** list of waypoints subsampled from m_PixelPath */
- std::vector<geometry_msgs::PoseStamped> m_waypoints;
+ /** Path planned by Explorer, pixel accuracy */
+ std::vector<Eigen::Vector2i> m_pixel_path;
- /** Path planned by Explorer, pixel accuracy */
- std::vector<Eigen::Vector2i> m_pixel_path;
+ /** target point */
+ geometry_msgs::Point m_target_point;
- /** target point */
- geometry_msgs::Point m_target_point;
+ /** target name if called via Navigate_to_POI */
+ std::string m_target_name;
- /** target name if called via Navigate_to_POI */
- std::string m_target_name;
+ /** orientation the robot should have at the target point */
+ double m_target_orientation;
- /** orientation the robot should have at the target point */
- double m_target_orientation;
+ /** allowed distance to target */
+ float m_desired_distance;
- /** allowed distance to target */
- float m_desired_distance;
+ /** check if the final turn should be skipped */
+ bool m_skip_final_turn;
- /** check if the final turn should be skipped */
- bool m_skip_final_turn;
+ /**
+ * check if navigation should perform fast planning. In this mode a path is
+ * only planned within
+ * a bounding box containing the robot pose and the target point
+ */
+ bool m_fast_path_planning;
- /**
- * check if navigation should perform fast planning. In this mode a path is
- * only planned within
- * a bounding box containing the robot pose and the target point
- */
- bool m_fast_path_planning;
+ /** current pose of the robot */
+ geometry_msgs::Pose m_robot_pose;
- /** current pose of the robot */
- geometry_msgs::Pose m_robot_pose;
+ /** last pose of the robot */
+ geometry_msgs::Pose m_robot_last_pose;
+
+ std::map<std::string, sensor_msgs::LaserScan::ConstPtr> m_scan_map;
+ std::map<std::string, float> m_max_move_distances;
+
+ std::string m_ignore_scan;
+
+ /** time stamp of the last incoming laser scan */
+ ros::Time m_last_laser_time;
+ /** time stamp of the last incoming pose */
+ ros::Time m_last_pose_time;
+
+ /** Distance factor of a frontier cell considered save for exploration */
+ float m_FrontierSafenessFactor;
+
+ double m_SafePathWeight;
+
+ /// @brief Configuration parameters
+
+ /** Allowed distances of obstacles to robot. Robot must move within these
+ * bounds */
+ std::pair<float, float> m_AllowedObstacleDistance;
+
+ /** Safe distances of obstacles to robot. If possible, robot should move
+ * within these bounds */
+ std::pair<float, float> m_SafeObstacleDistance;
+
+ /** threshold to sample down waypoints */
+ float m_waypoint_sampling_threshold;
+
+ float m_max_move_distance;
+
+ /** if distance to nearest obstacle is below collision distance trigger
+ * collision avoidance */
+ float m_collision_distance;
+
+ /** if distance to nearest obstacle is below collision distance don't drive
+ * backwards */
+ float m_backward_collision_distance;
+ /** do not drive back in collision avoidance when this near target */
+ float m_collision_distance_near_target;
- /** last pose of the robot */
- geometry_msgs::Pose m_robot_last_pose;
+ /** if true, obstacles in path will be detected and path will be replanned
+ */
+ bool m_check_path;
- std::map<std::string, sensor_msgs::LaserScan::ConstPtr> m_scan_map;
- std::map<std::string, float> m_max_move_distances;
+ bool m_obstacle_on_path;
+ geometry_msgs::Point m_obstacle_position;
- std::string m_ignore_scan;
+ /** waypoints will only be checked for obstacles if they are closer than
+ * check_path_max_distance to robot */
+ float m_check_path_max_distance;
+ ros::Time m_last_check_path_time;
- /** time stamp of the last incoming laser scan */
- ros::Time m_last_laser_time;
- /** time stamp of the last incoming pose */
- ros::Time m_last_pose_time;
-
- /** Distance factor of a frontier cell considered save for exploration */
- float m_FrontierSafenessFactor;
-
- double m_SafePathWeight;
-
- /// @brief Configuration parameters
-
- /** Allowed distances of obstacles to robot. Robot must move within these
- * bounds */
- std::pair<float, float> m_AllowedObstacleDistance;
-
- /** Safe distances of obstacles to robot. If possible, robot should move
- * within these bounds */
- std::pair<float, float> m_SafeObstacleDistance;
-
- /** threshold to sample down waypoints */
- float m_waypoint_sampling_threshold;
-
- float m_max_move_distance;
-
- /** if distance to nearest obstacle is below collision distance trigger
- * collision avoidance */
- float m_collision_distance;
+ /** do not replan if lisa avoids an obstacle, instead send target
+ * unreachable*/
+ bool m_stop_before_obstacle;
- /** if distance to nearest obstacle is below collision distance don't drive
- * backwards */
- float m_backward_collision_distance;
- /** do not drive back in collision avoidance when this near target */
- float m_collision_distance_near_target;
+ bool m_avoided_collision;
- /** if true, obstacles in path will be detected and path will be replanned
- */
- bool m_check_path;
+ float m_min_turn_angle;
+ float m_max_turn_speed;
+ float m_min_turn_speed;
+ float m_max_move_speed;
+ float m_max_drive_angle;
+ float m_waypoint_radius_factor;
- bool m_obstacle_on_path;
- geometry_msgs::Point m_obstacle_position;
+ float m_distance_to_target;
+ float m_act_speed;
+ float m_angular_avoidance;
- /** waypoints will only be checked for obstacles if they are closer than
- * check_path_max_distance to robot */
- float m_check_path_max_distance;
- ros::Time m_last_check_path_time;
+ float m_map_speed_factor;
+ float m_waypoint_speed_factor;
+ float m_obstacle_speed_factor;
+ float m_target_distance_speed_factor;
- /** do not replan if lisa avoids an obstacle, instead send target
- * unreachable*/
- bool m_stop_before_obstacle;
+ float m_min_y;
+ float m_min_x;
- bool m_avoided_collision;
-
- float m_min_turn_angle;
- float m_max_turn_speed;
- float m_min_turn_speed;
- float m_max_move_speed;
- float m_max_drive_angle;
- float m_waypoint_radius_factor;
-
- float m_distance_to_target;
- float m_act_speed;
- float m_angular_avoidance;
-
- float m_map_speed_factor;
- float m_waypoint_speed_factor;
- float m_obstacle_speed_factor;
- float m_target_distance_speed_factor;
-
- float m_min_y;
- float m_min_x;
-
- float m_callback_error_duration;
-
- bool m_seen_obstacle_before;
- bool m_use_ptu;
-
- bool m_path_reaches_target;
- bool m_initial_path_reaches_target;
- int m_unknown_threshold;
-
- /** last map data */
- nav_msgs::OccupancyGrid::ConstPtr m_map;
-
- // ros specific members
- tf::TransformListener m_transform_listener;
-
- // subscribers
- ros::Subscriber m_map_sub;
- ros::Subscriber m_pose_sub;
- ros::Subscriber m_laser_data_sub;
- ros::Subscriber m_down_laser_data_sub;
- ros::Subscriber m_laser_back_data_sub;
- ros::Subscriber m_start_navigation_sub;
- ros::Subscriber m_stop_navigation_sub;
- ros::Subscriber m_navigate_to_poi_sub;
- ros::Subscriber m_unknown_threshold_sub;
- ros::Subscriber m_refresh_param_sub;
- ros::Subscriber m_max_move_depth_sub;
- ros::Subscriber m_move_base_simple_goal_sub;
- ros::Subscriber m_ignore_laser_sub;
-
- // publishers
- ros::Publisher m_cmd_vel_pub;
- ros::Publisher m_target_reached_string_pub;
- // ros::Publisher m_target_reached_empty_pub;
- ros::Publisher m_target_unreachable_pub;
- ros::Publisher m_path_pub;
- ros::Publisher m_ptu_center_world_point_pub;
- ros::Publisher m_set_pan_tilt_pub;
- ros::Publisher m_debug_pub;
-
- // service clients
- ros::ServiceClient m_get_POIs_client;
+ float m_callback_error_duration;
+
+ bool m_seen_obstacle_before;
+ bool m_use_ptu;
+
+ bool m_path_reaches_target;
+ bool m_initial_path_reaches_target;
+ int m_unknown_threshold;
+
+ /** last map data */
+ nav_msgs::OccupancyGrid::ConstPtr m_map;
+
+ // ros specific members
+ tf::TransformListener m_transform_listener;
+
+ // subscribers
+ ros::Subscriber m_map_sub;
+ ros::Subscriber m_pose_sub;
+ ros::Subscriber m_laser_data_sub;
+ ros::Subscriber m_down_laser_data_sub;
+ ros::Subscriber m_laser_back_data_sub;
+ ros::Subscriber m_start_navigation_sub;
+ ros::Subscriber m_stop_navigation_sub;
+ ros::Subscriber m_navigate_to_poi_sub;
+ ros::Subscriber m_unknown_threshold_sub;
+ ros::Subscriber m_refresh_param_sub;
+ ros::Subscriber m_max_move_depth_sub;
+ ros::Subscriber m_move_base_simple_goal_sub;
+ ros::Subscriber m_ignore_laser_sub;
+
+ // publishers
+ ros::Publisher m_cmd_vel_pub;
+ ros::Publisher m_target_reached_string_pub;
+ // ros::Publisher m_target_reached_empty_pub;
+ ros::Publisher m_target_unreachable_pub;
+ ros::Publisher m_path_pub;
+ ros::Publisher m_ptu_center_world_point_pub;
+ ros::Publisher m_set_pan_tilt_pub;
+ ros::Publisher m_debug_pub;
+
+ // service clients
+ ros::ServiceClient m_get_POIs_client;
};
#endif
diff --git a/homer_navigation/src/homer_navigation_node.cpp b/homer_navigation/src/homer_navigation_node.cpp
index d355ef413ca784eb800a07ae9da2141a6621e9a6..ee445ad4ea8549790bd1d5a7f2ba3a4c23898cb1 100644
--- a/homer_navigation/src/homer_navigation_node.cpp
+++ b/homer_navigation/src/homer_navigation_node.cpp
@@ -1,1256 +1,1442 @@
#include "homer_navigation/homer_navigation_node.h"
-HomerNavigationNode::HomerNavigationNode() {
- ros::NodeHandle nh;
-
- // subscribers
- m_map_sub = nh.subscribe<nav_msgs::OccupancyGrid>(
- "/map", 1, &HomerNavigationNode::mapCallback, this);
- m_pose_sub = nh.subscribe<geometry_msgs::PoseStamped>(
- "/pose", 1, &HomerNavigationNode::poseCallback, this);
- m_laser_data_sub = nh.subscribe<sensor_msgs::LaserScan>(
- "/scan", 1, &HomerNavigationNode::laserDataCallback, this);
- m_down_laser_data_sub = nh.subscribe<sensor_msgs::LaserScan>(
- "/front_scan", 1, &HomerNavigationNode::laserDataCallback, this);
- m_start_navigation_sub = nh.subscribe<homer_mapnav_msgs::StartNavigation>(
- "/homer_navigation/start_navigation", 1,
- &HomerNavigationNode::startNavigationCallback, this);
- m_move_base_simple_goal_sub = nh.subscribe<geometry_msgs::PoseStamped>(
- "/move_base_simple/goal", 1,
- &HomerNavigationNode::moveBaseSimpleGoalCallback,
- this); // for RVIZ usage
- m_stop_navigation_sub = nh.subscribe<std_msgs::Empty>(
- "/homer_navigation/stop_navigation", 1,
- &HomerNavigationNode::stopNavigationCallback, this);
- m_navigate_to_poi_sub = nh.subscribe<homer_mapnav_msgs::NavigateToPOI>(
- "/homer_navigation/navigate_to_POI", 1,
- &HomerNavigationNode::navigateToPOICallback, this);
- m_unknown_threshold_sub = nh.subscribe<std_msgs::Int8>(
- "/homer_navigation/unknown_threshold", 1,
- &HomerNavigationNode::unknownThresholdCallback, this);
- m_refresh_param_sub = nh.subscribe<std_msgs::Empty>(
- "/homer_navigation/refresh_params", 1,
- &HomerNavigationNode::refreshParamsCallback, this);
- m_max_move_depth_sub = nh.subscribe<std_msgs::Float32>(
- "/homer_navigation/max_depth_move_distance", 1,
- &HomerNavigationNode::maxDepthMoveDistanceCallback, this);
- m_ignore_laser_sub = nh.subscribe<std_msgs::String>(
- "/homer_navigation/ignore_laser", 1,
- &HomerNavigationNode::ignoreLaserCallback, this);
-
- m_cmd_vel_pub =
- nh.advertise<geometry_msgs::Twist>("/robot_platform/cmd_vel", 3);
- m_target_reached_string_pub =
- nh.advertise<std_msgs::String>("/homer_navigation/target_reached", 1);
- // m_target_reached_empty_pub =
- // nh.advertise<std_msgs::Empty>("/homer_navigation/target_reached", 1);
- m_target_unreachable_pub =
- nh.advertise<homer_mapnav_msgs::TargetUnreachable>(
- "/homer_navigation/target_unreachable", 1);
- m_path_pub = nh.advertise<nav_msgs::Path>("/homer_navigation/path", 1);
- m_ptu_center_world_point_pub =
- nh.advertise<homer_ptu_msgs::CenterWorldPoint>(
- "/ptu/center_world_point", 1);
- m_set_pan_tilt_pub =
- nh.advertise<homer_ptu_msgs::SetPanTilt>("/ptu/set_pan_tilt", 1);
- m_debug_pub = nh.advertise<std_msgs::String>("/homer_navigation/debug", 1);
-
- m_get_POIs_client =
- nh.serviceClient<homer_mapnav_msgs::GetPointsOfInterest>(
- "/map_manager/get_pois");
-
- m_MainMachine.setName("HomerNavigation Main");
- ADD_MACHINE_STATE(m_MainMachine, IDLE);
- ADD_MACHINE_STATE(m_MainMachine, FOLLOWING_PATH);
- ADD_MACHINE_STATE(m_MainMachine, AVOIDING_COLLISION);
- ADD_MACHINE_STATE(m_MainMachine, FINAL_TURN);
-
- init();
+HomerNavigationNode::HomerNavigationNode()
+{
+ ros::NodeHandle nh;
+
+ // subscribers
+ m_map_sub = nh.subscribe<nav_msgs::OccupancyGrid>(
+ "/map", 1, &HomerNavigationNode::mapCallback, this);
+ m_pose_sub = nh.subscribe<geometry_msgs::PoseStamped>(
+ "/pose", 1, &HomerNavigationNode::poseCallback, this);
+ m_laser_data_sub = nh.subscribe<sensor_msgs::LaserScan>(
+ "/scan", 1, &HomerNavigationNode::laserDataCallback, this);
+ m_down_laser_data_sub = nh.subscribe<sensor_msgs::LaserScan>(
+ "/front_scan", 1, &HomerNavigationNode::laserDataCallback, this);
+ m_start_navigation_sub = nh.subscribe<homer_mapnav_msgs::StartNavigation>(
+ "/homer_navigation/start_navigation", 1,
+ &HomerNavigationNode::startNavigationCallback, this);
+ m_move_base_simple_goal_sub = nh.subscribe<geometry_msgs::PoseStamped>(
+ "/move_base_simple/goal", 1,
+ &HomerNavigationNode::moveBaseSimpleGoalCallback,
+ this); // for RVIZ usage
+ m_stop_navigation_sub = nh.subscribe<std_msgs::Empty>(
+ "/homer_navigation/stop_navigation", 1,
+ &HomerNavigationNode::stopNavigationCallback, this);
+ m_navigate_to_poi_sub = nh.subscribe<homer_mapnav_msgs::NavigateToPOI>(
+ "/homer_navigation/navigate_to_POI", 1,
+ &HomerNavigationNode::navigateToPOICallback, this);
+ m_unknown_threshold_sub = nh.subscribe<std_msgs::Int8>(
+ "/homer_navigation/unknown_threshold", 1,
+ &HomerNavigationNode::unknownThresholdCallback, this);
+ m_refresh_param_sub = nh.subscribe<std_msgs::Empty>(
+ "/homer_navigation/refresh_params", 1,
+ &HomerNavigationNode::refreshParamsCallback, this);
+ m_max_move_depth_sub = nh.subscribe<std_msgs::Float32>(
+ "/homer_navigation/max_depth_move_distance", 1,
+ &HomerNavigationNode::maxDepthMoveDistanceCallback, this);
+ m_ignore_laser_sub = nh.subscribe<std_msgs::String>(
+ "/homer_navigation/ignore_laser", 1,
+ &HomerNavigationNode::ignoreLaserCallback, this);
+
+ m_cmd_vel_pub =
+ nh.advertise<geometry_msgs::Twist>("/robot_platform/cmd_vel", 3);
+ m_target_reached_string_pub =
+ nh.advertise<std_msgs::String>("/homer_navigation/target_reached", 1);
+ // m_target_reached_empty_pub =
+ // nh.advertise<std_msgs::Empty>("/homer_navigation/target_reached", 1);
+ m_target_unreachable_pub = nh.advertise<homer_mapnav_msgs::TargetUnreachable>(
+ "/homer_navigation/target_unreachable", 1);
+ m_path_pub = nh.advertise<nav_msgs::Path>("/homer_navigation/path", 1);
+ m_ptu_center_world_point_pub = nh.advertise<homer_ptu_msgs::CenterWorldPoint>(
+ "/ptu/center_world_point", 1);
+ m_set_pan_tilt_pub =
+ nh.advertise<homer_ptu_msgs::SetPanTilt>("/ptu/set_pan_tilt", 1);
+ m_debug_pub = nh.advertise<std_msgs::String>("/homer_navigation/debug", 1);
+
+ m_get_POIs_client = nh.serviceClient<homer_mapnav_msgs::GetPointsOfInterest>(
+ "/map_manager/get_pois");
+
+ m_MainMachine.setName("HomerNavigation Main");
+ ADD_MACHINE_STATE(m_MainMachine, IDLE);
+ ADD_MACHINE_STATE(m_MainMachine, FOLLOWING_PATH);
+ ADD_MACHINE_STATE(m_MainMachine, AVOIDING_COLLISION);
+ ADD_MACHINE_STATE(m_MainMachine, FINAL_TURN);
+
+ init();
}
-void HomerNavigationNode::loadParameters() {
- // Explorer constructor
- ros::param::param("/homer_navigation/allowed_obstacle_distance/min",
- m_AllowedObstacleDistance.first, (float)0.3);
- ros::param::param("/homer_navigation/allowed_obstacle_distance/max",
- m_AllowedObstacleDistance.second, (float)5.0);
- ros::param::param("/homer_navigation/safe_obstacle_distance/min",
- m_SafeObstacleDistance.first, (float)0.7);
- ros::param::param("/homer_navigation/safe_obstacle_distance/max",
- m_SafeObstacleDistance.second, (float)1.5);
- ros::param::param("/homer_navigation/frontier_safeness_factor",
- m_FrontierSafenessFactor, (float)1.4);
- ros::param::param("/homer_navigation/safe_path_weight", m_SafePathWeight,
- (double)1.2);
- ros::param::param("/homer_navigation/waypoint_sampling_threshold",
- m_waypoint_sampling_threshold, (float)1.5);
-
- // check path
- ros::param::param("/homer_navigation/check_path", m_check_path, (bool)true);
- ros::param::param("/homer_navigation/check_path_max_distance",
- m_check_path_max_distance, (float)2.0);
-
- // collision
- ros::param::param("/homer_navigation/collision_distance",
- m_collision_distance, (float)0.3);
- ros::param::param("/homer_navigation/collision_distance_near_target",
- m_collision_distance_near_target, (float)0.2);
- ros::param::param("/homer_navigation/backward_collision_distance",
- m_backward_collision_distance, (float)0.5);
-
- // cmd_vel config values
- ros::param::param("/homer_navigation/min_turn_angle", m_min_turn_angle,
- (float)0.15);
- ros::param::param("/homer_navigation/max_turn_speed", m_max_turn_speed,
- (float)0.6);
- ros::param::param("/homer_navigation/min_turn_speed", m_min_turn_speed,
- (float)0.3);
- ros::param::param("/homer_navigation/max_move_speed", m_max_move_speed,
- (float)0.4);
- ros::param::param("/homer_navigation/max_drive_angle", m_max_drive_angle,
- (float)0.6);
-
- // caution factors
- ros::param::param("/homer_navigation/map_speed_factor", m_map_speed_factor,
- (float)1.0);
- ros::param::param("/homer_navigation/waypoint_speed_factor",
- m_waypoint_speed_factor, (float)1.0);
- ros::param::param("/homer_navigation/obstacle_speed_factor",
- m_obstacle_speed_factor, (float)1.0);
- ros::param::param("/homer_navigation/target_distance_speed_factor",
- m_target_distance_speed_factor, (float)0.4);
-
- // robot dimensions
- ros::param::param("/homer_navigation/min_x", m_min_x, (float)0.3);
- ros::param::param("/homer_navigation/min_y", m_min_y, (float)0.27);
-
- // error durations
- ros::param::param("/homer_navigation/callback_error_duration",
- m_callback_error_duration, (float)0.3);
-
- ros::param::param("/homer_navigation/use_ptu", m_use_ptu, (bool)false);
-
- ros::param::param("/homer_navigation/unknown_threshold",
- m_unknown_threshold, (int)50);
- ros::param::param("/homer_navigation/waypoint_radius_factor",
- m_waypoint_radius_factor, (float)0.25);
- if (m_explorer) {
- delete m_explorer;
- }
+void HomerNavigationNode::loadParameters()
+{
+ // Explorer constructor
+ ros::param::param("/homer_navigation/allowed_obstacle_distance/min",
+ m_AllowedObstacleDistance.first, (float)0.3);
+ ros::param::param("/homer_navigation/allowed_obstacle_distance/max",
+ m_AllowedObstacleDistance.second, (float)5.0);
+ ros::param::param("/homer_navigation/safe_obstacle_distance/min",
+ m_SafeObstacleDistance.first, (float)0.7);
+ ros::param::param("/homer_navigation/safe_obstacle_distance/max",
+ m_SafeObstacleDistance.second, (float)1.5);
+ ros::param::param("/homer_navigation/frontier_safeness_factor",
+ m_FrontierSafenessFactor, (float)1.4);
+ ros::param::param("/homer_navigation/safe_path_weight", m_SafePathWeight,
+ (double)1.2);
+ ros::param::param("/homer_navigation/waypoint_sampling_threshold",
+ m_waypoint_sampling_threshold, (float)1.5);
+
+ // check path
+ ros::param::param("/homer_navigation/check_path", m_check_path, (bool)true);
+ ros::param::param("/homer_navigation/check_path_max_distance",
+ m_check_path_max_distance, (float)2.0);
+
+ // collision
+ ros::param::param("/homer_navigation/collision_distance",
+ m_collision_distance, (float)0.3);
+ ros::param::param("/homer_navigation/collision_distance_near_target",
+ m_collision_distance_near_target, (float)0.2);
+ ros::param::param("/homer_navigation/backward_collision_distance",
+ m_backward_collision_distance, (float)0.5);
+
+ // cmd_vel config values
+ ros::param::param("/homer_navigation/min_turn_angle", m_min_turn_angle,
+ (float)0.15);
+ ros::param::param("/homer_navigation/max_turn_speed", m_max_turn_speed,
+ (float)0.6);
+ ros::param::param("/homer_navigation/min_turn_speed", m_min_turn_speed,
+ (float)0.3);
+ ros::param::param("/homer_navigation/max_move_speed", m_max_move_speed,
+ (float)0.4);
+ ros::param::param("/homer_navigation/max_drive_angle", m_max_drive_angle,
+ (float)0.6);
+
+ // caution factors
+ ros::param::param("/homer_navigation/map_speed_factor", m_map_speed_factor,
+ (float)1.0);
+ ros::param::param("/homer_navigation/waypoint_speed_factor",
+ m_waypoint_speed_factor, (float)1.0);
+ ros::param::param("/homer_navigation/obstacle_speed_factor",
+ m_obstacle_speed_factor, (float)1.0);
+ ros::param::param("/homer_navigation/target_distance_speed_factor",
+ m_target_distance_speed_factor, (float)0.4);
+
+ // robot dimensions
+ ros::param::param("/homer_navigation/min_x", m_min_x, (float)0.3);
+ ros::param::param("/homer_navigation/min_y", m_min_y, (float)0.27);
+
+ // error durations
+ ros::param::param("/homer_navigation/callback_error_duration",
+ m_callback_error_duration, (float)0.3);
+
+ ros::param::param("/homer_navigation/use_ptu", m_use_ptu, (bool)false);
+
+ ros::param::param("/homer_navigation/unknown_threshold", m_unknown_threshold,
+ (int)50);
+ ros::param::param("/homer_navigation/waypoint_radius_factor",
+ m_waypoint_radius_factor, (float)0.25);
+ if (m_explorer)
+ {
+ delete m_explorer;
+ }
}
-void HomerNavigationNode::init() {
- m_explorer = 0;
- m_robot_pose.position.x = 0.0;
- m_robot_pose.position.y = 0.0;
- m_robot_pose.position.z = 0.0;
- m_robot_pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
- m_robot_last_pose = m_robot_pose;
- m_avoided_collision = false;
- m_act_speed = 0;
- m_angular_avoidance = 0;
- m_last_check_path_time = ros::Time::now();
- m_ignore_scan = "";
-
- m_obstacle_on_path = false;
- m_obstacle_position.x = 0;
- m_obstacle_position.y = 0;
- m_obstacle_position.z = 0;
-
- nav_msgs::OccupancyGrid tmp_map;
- tmp_map.header.frame_id = "/map";
- tmp_map.info.resolution = 0;
- tmp_map.info.width = 0;
- tmp_map.info.height = 0;
- tmp_map.info.origin.position.x = 0;
- tmp_map.info.origin.position.y = 0;
- tmp_map.info.origin.position.z = 0;
- m_map = boost::make_shared<nav_msgs::OccupancyGrid>(tmp_map);
-
- loadParameters();
-
- m_MainMachine.setState(IDLE);
+void HomerNavigationNode::init()
+{
+ m_explorer = 0;
+ m_robot_pose.position.x = 0.0;
+ m_robot_pose.position.y = 0.0;
+ m_robot_pose.position.z = 0.0;
+ m_robot_pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
+ m_robot_last_pose = m_robot_pose;
+ m_avoided_collision = false;
+ m_act_speed = 0;
+ m_angular_avoidance = 0;
+ m_last_check_path_time = ros::Time::now();
+ m_ignore_scan = "";
+
+ m_obstacle_on_path = false;
+ m_obstacle_position.x = 0;
+ m_obstacle_position.y = 0;
+ m_obstacle_position.z = 0;
+
+ nav_msgs::OccupancyGrid tmp_map;
+ tmp_map.header.frame_id = "/map";
+ tmp_map.info.resolution = 0;
+ tmp_map.info.width = 0;
+ tmp_map.info.height = 0;
+ tmp_map.info.origin.position.x = 0;
+ tmp_map.info.origin.position.y = 0;
+ tmp_map.info.origin.position.z = 0;
+ m_map = boost::make_shared<nav_msgs::OccupancyGrid>(tmp_map);
+
+ loadParameters();
+
+ m_MainMachine.setState(IDLE);
}
-HomerNavigationNode::~HomerNavigationNode() {
- if (m_explorer) {
- delete m_explorer;
- }
+HomerNavigationNode::~HomerNavigationNode()
+{
+ if (m_explorer)
+ {
+ delete m_explorer;
+ }
}
-void HomerNavigationNode::stopRobot() {
- m_act_speed = 0;
- geometry_msgs::Twist cmd_vel_msg;
- cmd_vel_msg.linear.x = 0;
- cmd_vel_msg.linear.y = 0;
- cmd_vel_msg.angular.z = 0;
- m_cmd_vel_pub.publish(cmd_vel_msg);
- // ros::Duration(0.1).sleep();
- m_cmd_vel_pub.publish(cmd_vel_msg);
- // ros::Duration(0.1).sleep();
- m_cmd_vel_pub.publish(cmd_vel_msg);
+void HomerNavigationNode::stopRobot()
+{
+ m_act_speed = 0;
+ geometry_msgs::Twist cmd_vel_msg;
+ cmd_vel_msg.linear.x = 0;
+ cmd_vel_msg.linear.y = 0;
+ cmd_vel_msg.angular.z = 0;
+ m_cmd_vel_pub.publish(cmd_vel_msg);
+ // ros::Duration(0.1).sleep();
+ m_cmd_vel_pub.publish(cmd_vel_msg);
+ // ros::Duration(0.1).sleep();
+ m_cmd_vel_pub.publish(cmd_vel_msg);
}
-void HomerNavigationNode::idleProcess() {
- if (m_MainMachine.state() != IDLE) {
- if ((ros::Time::now() - m_last_laser_time) >
- ros::Duration(m_callback_error_duration)) {
- ROS_ERROR_STREAM("Laser data timeout!\n");
- stopRobot();
- }
- if ((ros::Time::now() - m_last_pose_time) >
- ros::Duration(m_callback_error_duration)) {
- ROS_ERROR_STREAM("Pose timeout!\n");
- stopRobot();
- }
+void HomerNavigationNode::idleProcess()
+{
+ if (m_MainMachine.state() != IDLE)
+ {
+ if ((ros::Time::now() - m_last_laser_time) >
+ ros::Duration(m_callback_error_duration))
+ {
+ ROS_ERROR_STREAM("Laser data timeout!\n");
+ stopRobot();
+ }
+ if ((ros::Time::now() - m_last_pose_time) >
+ ros::Duration(m_callback_error_duration))
+ {
+ ROS_ERROR_STREAM("Pose timeout!\n");
+ stopRobot();
}
+ }
}
-bool HomerNavigationNode::isInIgnoreList(std::string frame_id) {
- std::regex reg("(^|\\s)" + frame_id + "(\\s|$)");
- return regex_match(m_ignore_scan, reg);
+bool HomerNavigationNode::isInIgnoreList(std::string frame_id)
+{
+ std::regex reg("(^|\\s)" + frame_id + "(\\s|$)");
+ return regex_match(m_ignore_scan, reg);
}
-void HomerNavigationNode::setExplorerMap() {
- // adding lasers to map
- nav_msgs::OccupancyGrid temp_map = *m_map;
- for (const auto& scan : m_scan_map) {
- if (!isInIgnoreList(scan.second->header.frame_id)) {
- std::vector<geometry_msgs::Point> scan_points;
- scan_points = map_tools::laser_msg_to_points(
- scan.second, m_transform_listener, "/map");
- for (const auto& point : scan_points) {
- Eigen::Vector2i map_point =
- map_tools::toMapCoords(point, m_map);
- int index = map_point.y() * m_map->info.width + map_point.x();
- if (index > 0 &&
- index < m_map->info.width * m_map->info.height) {
- temp_map.data[index] = (int8_t)100;
- }
- }
+void HomerNavigationNode::setExplorerMap()
+{
+ // adding lasers to map
+ nav_msgs::OccupancyGrid temp_map = *m_map;
+ for (const auto& scan : m_scan_map)
+ {
+ if (!isInIgnoreList(scan.second->header.frame_id))
+ {
+ std::vector<geometry_msgs::Point> scan_points;
+ scan_points = map_tools::laser_msg_to_points(
+ scan.second, m_transform_listener, "/map");
+ for (const auto& point : scan_points)
+ {
+ Eigen::Vector2i map_point = map_tools::toMapCoords(point, m_map);
+ int index = map_point.y() * m_map->info.width + map_point.x();
+ if (index > 0 && index < m_map->info.width * m_map->info.height)
+ {
+ temp_map.data[index] = (int8_t)100;
}
+ }
}
- if (m_fast_path_planning) {
- //TODO check why not functional
- //maskMap(temp_map);
- }
- m_explorer->setOccupancyMap(
- boost::make_shared<nav_msgs::OccupancyGrid>(temp_map));
+ }
+ if (m_fast_path_planning)
+ {
+ // TODO check why not functional
+ // maskMap(temp_map);
+ }
+ m_explorer->setOccupancyMap(
+ boost::make_shared<nav_msgs::OccupancyGrid>(temp_map));
}
-void HomerNavigationNode::calculatePath() {
- if (!m_explorer) {
- return;
+void HomerNavigationNode::calculatePath()
+{
+ if (!m_explorer)
+ {
+ return;
+ }
+ if (isTargetPositionReached())
+ {
+ return;
+ }
+
+ // setExplorerMap();
+ m_explorer->setStart(map_tools::toMapCoords(m_robot_pose.position, m_map));
+
+ bool success;
+ m_pixel_path = m_explorer->getPath(success);
+ if (!success)
+ {
+ ROS_WARN_STREAM("no path to target possible - drive to obstacle");
+ m_obstacle_on_path = true;
+ }
+ else
+ {
+ m_obstacle_on_path = false;
+ std::vector<Eigen::Vector2i> waypoint_pixels =
+ m_explorer->sampleWaypointsFromPath(m_pixel_path,
+ m_waypoint_sampling_threshold);
+ m_waypoints.clear();
+ ROS_INFO_STREAM("homer_navigation::calculatePath - Path Size: "
+ << waypoint_pixels.size());
+ if (waypoint_pixels.size() > 0)
+ {
+ for (std::vector<Eigen::Vector2i>::iterator it = waypoint_pixels.begin();
+ it != waypoint_pixels.end(); ++it)
+ {
+ geometry_msgs::PoseStamped poseStamped;
+ poseStamped.header.frame_id = "/map";
+ poseStamped.pose.position = map_tools::fromMapCoords(*it, m_map);
+ poseStamped.pose.orientation.x = 0.0;
+ poseStamped.pose.orientation.y = 0.0;
+ poseStamped.pose.orientation.z = 0.0;
+ poseStamped.pose.orientation.w = 1.0;
+ m_waypoints.push_back(poseStamped);
+ }
+ if (m_path_reaches_target)
+ {
+ geometry_msgs::PoseStamped poseStamped;
+ poseStamped.header.frame_id = "/map";
+ poseStamped.pose.position = m_target_point;
+ poseStamped.pose.orientation.x = 0;
+ poseStamped.pose.orientation.y = 0;
+ poseStamped.pose.orientation.z = 0;
+ poseStamped.pose.orientation.w = 1;
+ m_waypoints.push_back(poseStamped);
+ }
+ sendPathData();
}
- if (isTargetPositionReached()) {
- return;
+ else
+ {
+ sendTargetUnreachableMsg(
+ homer_mapnav_msgs::TargetUnreachable::NO_PATH_FOUND);
}
- // setExplorerMap();
+ m_last_laser_time = ros::Time::now();
+ m_last_pose_time = ros::Time::now();
+ }
+}
+
+void HomerNavigationNode::startNavigation()
+{
+ if (!m_explorer)
+ {
+ return;
+ }
+ if (isTargetPositionReached())
+ {
+ return;
+ }
+
+ ROS_INFO_STREAM("Distance to target still too large ("
+ << m_distance_to_target
+ << "m; requested: " << m_desired_distance << "m)");
+ setExplorerMap();
+
+ // check if there still exists a path to the original target
+ if (m_avoided_collision && m_initial_path_reaches_target &&
+ m_stop_before_obstacle)
+ {
m_explorer->setStart(map_tools::toMapCoords(m_robot_pose.position, m_map));
bool success;
m_pixel_path = m_explorer->getPath(success);
- if (!success) {
- ROS_WARN_STREAM("no path to target possible - drive to obstacle");
- m_obstacle_on_path = true;
- } else {
- m_obstacle_on_path = false;
- std::vector<Eigen::Vector2i> waypoint_pixels =
- m_explorer->sampleWaypointsFromPath(m_pixel_path,
- m_waypoint_sampling_threshold);
- m_waypoints.clear();
- ROS_INFO_STREAM("homer_navigation::calculatePath - Path Size: "
- << waypoint_pixels.size());
- if (waypoint_pixels.size() > 0) {
- for (std::vector<Eigen::Vector2i>::iterator it =
- waypoint_pixels.begin();
- it != waypoint_pixels.end(); ++it) {
- geometry_msgs::PoseStamped poseStamped;
- poseStamped.header.frame_id = "/map";
- poseStamped.pose.position =
- map_tools::fromMapCoords(*it, m_map);
- poseStamped.pose.orientation.x = 0.0;
- poseStamped.pose.orientation.y = 0.0;
- poseStamped.pose.orientation.z = 0.0;
- poseStamped.pose.orientation.w = 1.0;
- m_waypoints.push_back(poseStamped);
- }
- if (m_path_reaches_target) {
- geometry_msgs::PoseStamped poseStamped;
- poseStamped.header.frame_id = "/map";
- poseStamped.pose.position = m_target_point;
- poseStamped.pose.orientation.x = 0;
- poseStamped.pose.orientation.y = 0;
- poseStamped.pose.orientation.z = 0;
- poseStamped.pose.orientation.w = 1;
- m_waypoints.push_back(poseStamped);
- }
- sendPathData();
- } else {
- sendTargetUnreachableMsg(
- homer_mapnav_msgs::TargetUnreachable::NO_PATH_FOUND);
- }
-
- m_last_laser_time = ros::Time::now();
- m_last_pose_time = ros::Time::now();
+ if (!success)
+ {
+ ROS_INFO_STREAM(
+ "Initial path would have reached target, new path does not. "
+ << "Sending target unreachable.");
+ sendTargetUnreachableMsg(
+ homer_mapnav_msgs::TargetUnreachable::LASER_OBSTACLE);
+ return;
}
+ }
+
+ m_explorer->setStart(map_tools::toMapCoords(m_robot_pose.position, m_map));
+ Eigen::Vector2i new_target_approx = m_explorer->getNearestAccessibleTarget(
+ map_tools::toMapCoords(m_target_point, m_map));
+
+ geometry_msgs::Point new_target_approx_world =
+ map_tools::fromMapCoords(new_target_approx, m_map);
+ ROS_INFO_STREAM(
+ "start Navigation: Approx target: " << new_target_approx_world);
+
+ m_path_reaches_target =
+ (map_tools::distance(new_target_approx_world, m_target_point) <
+ m_desired_distance);
+ m_initial_path_reaches_target = m_path_reaches_target;
+
+ if (map_tools::distance(m_robot_pose.position, new_target_approx_world) <
+ m_map->info.resolution * 1.5)
+ {
+ ROS_WARN_STREAM("close to aprox target - final turn");
+ m_MainMachine.setState(FINAL_TURN);
+ }
+
+ bool new_approx_is_better =
+ (map_tools::distance(m_robot_pose.position, m_target_point) -
+ map_tools::distance(new_target_approx_world, m_target_point)) >
+ 2 * m_desired_distance;
+ if (!new_approx_is_better && !m_path_reaches_target)
+ {
+ ROS_WARN_STREAM("No better way to target found, turning and then reporting "
+ "as "
+ "unreachable."
+ << std::endl
+ << "Distance to target: " << m_distance_to_target
+ << "m; requested: " << m_desired_distance << "m");
+ m_MainMachine.setState(FINAL_TURN);
+ }
+ else
+ {
+ m_explorer->setTarget(new_target_approx);
+ m_MainMachine.setState(FOLLOWING_PATH);
+ calculatePath();
+ }
}
-void HomerNavigationNode::startNavigation() {
- if (!m_explorer) {
- return;
- }
- if (isTargetPositionReached()) {
- return;
- }
-
- ROS_INFO_STREAM("Distance to target still too large ("
- << m_distance_to_target
- << "m; requested: " << m_desired_distance << "m)");
- setExplorerMap();
-
- // check if there still exists a path to the original target
- if (m_avoided_collision && m_initial_path_reaches_target &&
- m_stop_before_obstacle) {
- m_explorer->setStart(
- map_tools::toMapCoords(m_robot_pose.position, m_map));
-
- bool success;
- m_pixel_path = m_explorer->getPath(success);
- if (!success) {
- ROS_INFO_STREAM(
- "Initial path would have reached target, new path does not. "
- << "Sending target unreachable.");
- sendTargetUnreachableMsg(
- homer_mapnav_msgs::TargetUnreachable::LASER_OBSTACLE);
- return;
- }
- }
-
- m_explorer->setStart(map_tools::toMapCoords(m_robot_pose.position, m_map));
- Eigen::Vector2i new_target_approx = m_explorer->getNearestAccessibleTarget(
- map_tools::toMapCoords(m_target_point, m_map));
-
- geometry_msgs::Point new_target_approx_world =
- map_tools::fromMapCoords(new_target_approx, m_map);
- ROS_INFO_STREAM(
- "start Navigation: Approx target: " << new_target_approx_world);
-
- m_path_reaches_target =
- (map_tools::distance(new_target_approx_world, m_target_point) <
- m_desired_distance);
- m_initial_path_reaches_target = m_path_reaches_target;
-
- if (map_tools::distance(m_robot_pose.position, new_target_approx_world) <
- m_map->info.resolution * 1.5) {
- ROS_WARN_STREAM("close to aprox target - final turn");
- m_MainMachine.setState(FINAL_TURN);
- }
-
- bool new_approx_is_better =
- (map_tools::distance(m_robot_pose.position, m_target_point) -
- map_tools::distance(new_target_approx_world, m_target_point)) >
- 2 * m_desired_distance;
- if (!new_approx_is_better && !m_path_reaches_target) {
- ROS_WARN_STREAM(
- "No better way to target found, turning and then reporting as "
- "unreachable."
- << std::endl
- << "Distance to target: " << m_distance_to_target
- << "m; requested: " << m_desired_distance << "m");
- m_MainMachine.setState(FINAL_TURN);
- } else {
- m_explorer->setTarget(new_target_approx);
- m_MainMachine.setState(FOLLOWING_PATH);
- calculatePath();
- }
+void HomerNavigationNode::sendPathData()
+{
+ nav_msgs::Path msg;
+ msg.header.frame_id = "/map";
+ msg.header.stamp = ros::Time::now();
+ if (m_waypoints.size() > 0)
+ {
+ msg.poses = m_waypoints;
+ geometry_msgs::PoseStamped pose_stamped;
+ pose_stamped.pose = m_robot_pose;
+ pose_stamped.header.frame_id = "/map";
+ pose_stamped.header.stamp = ros::Time::now();
+ msg.poses.insert(msg.poses.begin(), pose_stamped);
+ }
+ m_path_pub.publish(msg);
}
-void HomerNavigationNode::sendPathData() {
- nav_msgs::Path msg;
- msg.header.frame_id = "/map";
- msg.header.stamp = ros::Time::now();
- if (m_waypoints.size() > 0) {
- msg.poses = m_waypoints;
- geometry_msgs::PoseStamped pose_stamped;
- pose_stamped.pose = m_robot_pose;
- pose_stamped.header.frame_id = "/map";
- pose_stamped.header.stamp = ros::Time::now();
- msg.poses.insert(msg.poses.begin(), pose_stamped);
- }
- m_path_pub.publish(msg);
+void HomerNavigationNode::sendTargetReachedMsg()
+{
+ stopRobot();
+ m_MainMachine.setState(IDLE);
+ std_msgs::String reached_string_msg;
+ reached_string_msg.data = m_target_name;
+ m_target_reached_string_pub.publish(reached_string_msg);
+ m_waypoints.clear();
+ ROS_INFO_STREAM("=== Reached Target " << m_target_name << " ===");
}
-void HomerNavigationNode::sendTargetReachedMsg() {
- stopRobot();
- m_MainMachine.setState(IDLE);
- std_msgs::String reached_string_msg;
- reached_string_msg.data = m_target_name;
- m_target_reached_string_pub.publish(reached_string_msg);
- m_waypoints.clear();
- ROS_INFO_STREAM("=== Reached Target " << m_target_name << " ===");
+void HomerNavigationNode::sendTargetUnreachableMsg(int8_t reason)
+{
+ stopRobot();
+ m_MainMachine.setState(IDLE);
+ homer_mapnav_msgs::TargetUnreachable unreachable_msg;
+ unreachable_msg.reason = reason;
+ unreachable_msg.point = geometry_msgs::PointStamped();
+ unreachable_msg.point.header.frame_id = "/map";
+ unreachable_msg.point.point = m_obstacle_position;
+ m_target_unreachable_pub.publish(unreachable_msg);
+ m_waypoints.clear();
+ ROS_INFO_STREAM("=== TargetUnreachableMsg ===");
}
-void HomerNavigationNode::sendTargetUnreachableMsg(int8_t reason) {
+bool HomerNavigationNode::isTargetPositionReached()
+{
+ if (m_distance_to_target < m_desired_distance && !m_new_target)
+ {
+ ROS_INFO_STREAM("Target position reached. Distance to target: "
+ << m_distance_to_target
+ << "m. Desired distance:" << m_desired_distance << "m");
stopRobot();
- m_MainMachine.setState(IDLE);
- homer_mapnav_msgs::TargetUnreachable unreachable_msg;
- unreachable_msg.reason = reason;
- unreachable_msg.point = geometry_msgs::PointStamped();
- unreachable_msg.point.header.frame_id = "/map";
- unreachable_msg.point.point = m_obstacle_position;
- m_target_unreachable_pub.publish(unreachable_msg);
m_waypoints.clear();
- ROS_INFO_STREAM("=== TargetUnreachableMsg ===");
-}
-
-bool HomerNavigationNode::isTargetPositionReached() {
- if (m_distance_to_target < m_desired_distance && !m_new_target) {
- ROS_INFO_STREAM("Target position reached. Distance to target: "
- << m_distance_to_target
- << "m. Desired distance:" << m_desired_distance << "m");
- stopRobot();
- m_waypoints.clear();
- m_MainMachine.setState(FINAL_TURN);
- ROS_INFO_STREAM("Turning to look-at point");
- return true;
- } else {
- return false;
- }
+ m_MainMachine.setState(FINAL_TURN);
+ ROS_INFO_STREAM("Turning to look-at point");
+ return true;
+ }
+ else
+ {
+ return false;
+ }
}
geometry_msgs::Point HomerNavigationNode::calculateMeanPoint(
- const std::vector<geometry_msgs::Point>& points) {
- geometry_msgs::Point mean_point = geometry_msgs::Point();
- for (auto const& point : points) {
- mean_point.x += point.x;
- mean_point.y += point.y;
- }
- if (points.size() > 0) {
- mean_point.x /= points.size();
- mean_point.y /= points.size();
- }
- return mean_point;
+ const std::vector<geometry_msgs::Point>& points)
+{
+ geometry_msgs::Point mean_point = geometry_msgs::Point();
+ for (auto const& point : points)
+ {
+ mean_point.x += point.x;
+ mean_point.y += point.y;
+ }
+ if (points.size() > 0)
+ {
+ mean_point.x /= points.size();
+ mean_point.y /= points.size();
+ }
+ return mean_point;
}
-bool HomerNavigationNode::obstacleOnPath() {
- m_last_check_path_time = ros::Time::now();
- if (m_pixel_path.size() == 0) {
- ROS_DEBUG_STREAM("no path found for finding an obstacle");
- return false;
- }
- std::vector<geometry_msgs::Point> obstacle_vec;
-
- for (auto const& scan : m_scan_map) {
- if (!isInIgnoreList(scan.second->header.frame_id)) {
- std::vector<geometry_msgs::Point> scan_points;
- scan_points = map_tools::laser_msg_to_points(
- scan.second, m_transform_listener, "/map");
-
- for (unsigned i = 1; i < m_pixel_path.size() - 1; i++) {
- geometry_msgs::Point lp =
- map_tools::fromMapCoords(m_pixel_path.at(i - 1), m_map);
- geometry_msgs::Point p =
- map_tools::fromMapCoords(m_pixel_path.at(i), m_map);
- if (map_tools::distance(m_robot_pose.position, p) >
- m_check_path_max_distance * 2) {
- if (obstacle_vec.size() > 0) {
- m_obstacle_position = calculateMeanPoint(obstacle_vec);
- ROS_DEBUG_STREAM(
- "found obstacle at: " << m_obstacle_position);
- return true;
- } else {
- return false;
- }
- }
- for (int k = 0; k < 4; k++) {
- geometry_msgs::Point t;
- t.x = lp.x + (p.x - lp.x) * k / 4.0;
- t.y = lp.y + (p.y - lp.y) * k / 4.0;
- for (const auto& sp : scan_points) {
- if (map_tools::distance(sp, t) <
- m_AllowedObstacleDistance.first -
- m_map->info.resolution) {
- if (map_tools::distance(m_robot_pose.position, sp) <
- m_check_path_max_distance) {
- obstacle_vec.push_back(sp);
- }
- }
- }
- }
+bool HomerNavigationNode::obstacleOnPath()
+{
+ m_last_check_path_time = ros::Time::now();
+ if (m_pixel_path.size() == 0)
+ {
+ ROS_DEBUG_STREAM("no path found for finding an obstacle");
+ return false;
+ }
+ std::vector<geometry_msgs::Point> obstacle_vec;
+
+ for (auto const& scan : m_scan_map)
+ {
+ if (!isInIgnoreList(scan.second->header.frame_id))
+ {
+ std::vector<geometry_msgs::Point> scan_points;
+ scan_points = map_tools::laser_msg_to_points(
+ scan.second, m_transform_listener, "/map");
+
+ for (unsigned i = 1; i < m_pixel_path.size() - 1; i++)
+ {
+ geometry_msgs::Point lp =
+ map_tools::fromMapCoords(m_pixel_path.at(i - 1), m_map);
+ geometry_msgs::Point p =
+ map_tools::fromMapCoords(m_pixel_path.at(i), m_map);
+ if (map_tools::distance(m_robot_pose.position, p) >
+ m_check_path_max_distance * 2)
+ {
+ if (obstacle_vec.size() > 0)
+ {
+ m_obstacle_position = calculateMeanPoint(obstacle_vec);
+ ROS_DEBUG_STREAM("found obstacle at: " << m_obstacle_position);
+ return true;
+ }
+ else
+ {
+ return false;
+ }
+ }
+ for (int k = 0; k < 4; k++)
+ {
+ geometry_msgs::Point t;
+ t.x = lp.x + (p.x - lp.x) * k / 4.0;
+ t.y = lp.y + (p.y - lp.y) * k / 4.0;
+ for (const auto& sp : scan_points)
+ {
+ if (map_tools::distance(sp, t) <
+ m_AllowedObstacleDistance.first - m_map->info.resolution)
+ {
+ if (map_tools::distance(m_robot_pose.position, sp) <
+ m_check_path_max_distance)
+ {
+ obstacle_vec.push_back(sp);
+ }
}
+ }
}
+ }
}
- if (obstacle_vec.size() > 0) {
- m_obstacle_position = calculateMeanPoint(obstacle_vec);
- ROS_DEBUG_STREAM("found obstacle at: " << m_obstacle_position);
- return true;
- } else {
- return false;
- }
+ }
+ if (obstacle_vec.size() > 0)
+ {
+ m_obstacle_position = calculateMeanPoint(obstacle_vec);
+ ROS_DEBUG_STREAM("found obstacle at: " << m_obstacle_position);
+ return true;
+ }
+ else
+ {
+ return false;
+ }
}
-void HomerNavigationNode::performNextMove() {
- if (m_MainMachine.state() == FOLLOWING_PATH) {
- if (isTargetPositionReached()) {
- return;
- }
- // check if an obstacle is on the current path
- if (m_check_path &&
- (ros::Time::now() - m_last_check_path_time) > ros::Duration(0.2)) {
- if (obstacleOnPath()) {
- if (m_seen_obstacle_before) {
- if (!m_obstacle_on_path) {
- stopRobot();
- calculatePath();
- return;
- } else {
- calculatePath();
- }
- }
- m_seen_obstacle_before = true;
- } else {
- m_seen_obstacle_before = false;
- m_obstacle_on_path = false;
- }
- }
-
- if (m_waypoints.size() == 0) {
- ROS_WARN_STREAM(
- "No waypoints but trying to perform next move! Skipping.");
+void HomerNavigationNode::performNextMove()
+{
+ if (m_MainMachine.state() == FOLLOWING_PATH)
+ {
+ if (isTargetPositionReached())
+ {
+ return;
+ }
+ // check if an obstacle is on the current path
+ if (m_check_path &&
+ (ros::Time::now() - m_last_check_path_time) > ros::Duration(0.2))
+ {
+ if (obstacleOnPath())
+ {
+ if (m_seen_obstacle_before)
+ {
+ if (!m_obstacle_on_path)
+ {
+ stopRobot();
+ calculatePath();
return;
+ }
+ else
+ {
+ calculatePath();
+ }
}
- // if we have accidentaly skipped waypoints, recalculate path
- float minDistance = FLT_MAX;
- float distance;
- unsigned nearestWaypoint = 0;
- for (unsigned i = 0; i < m_waypoints.size(); i++) {
- distance = map_tools::distance(m_robot_pose.position,
- m_waypoints[i].pose.position);
- if (distance < minDistance) {
- nearestWaypoint = i;
- minDistance = distance;
- }
- }
- if (nearestWaypoint != 0) {
- // if the target is nearer than the waypoint don't recalculate
- if (m_waypoints.size() != 2) {
- ROS_WARN_STREAM("Waypoints skipped. Recalculating path!");
- calculatePath();
- if (m_MainMachine.state() != FOLLOWING_PATH) {
- return;
- }
- } else {
- m_waypoints.erase(m_waypoints.begin());
- }
- }
-
- Eigen::Vector2i waypointPixel =
- map_tools::toMapCoords(m_waypoints[0].pose.position, m_map);
- float obstacleDistanceMap =
- m_explorer->getObstacleTransform()->getValue(waypointPixel.x(),
- waypointPixel.y()) *
- m_map->info.resolution;
- float waypointRadius = m_waypoint_radius_factor * obstacleDistanceMap;
-
- if ((waypointRadius < m_map->info.resolution) ||
- (m_waypoints.size() == 1)) {
- waypointRadius = m_map->info.resolution;
- }
-
- if (m_waypoints.size() != 0) {
- // calculate distance between last_pose->current_pose and waypoint
- Eigen::Vector2f line; // direction of the line
- line[0] = m_robot_pose.position.x - m_robot_last_pose.position.x;
- line[1] = m_robot_pose.position.y - m_robot_last_pose.position.y;
- Eigen::Vector2f point_to_start; // vector from the point to the
- // start of the line
- point_to_start[0] =
- m_robot_last_pose.position.x - m_waypoints[0].pose.position.x;
- point_to_start[1] =
- m_robot_last_pose.position.y - m_waypoints[0].pose.position.y;
- float dot = point_to_start[0] * line[0] +
- point_to_start[1] *
- line[1]; // dot product of point_to_start * line
- Eigen::Vector2f
- distance; // shortest distance vector from point to line
- distance[0] = point_to_start[0] - dot * line[0];
- distance[1] = point_to_start[1] - dot * line[1];
- float distance_to_waypoint = sqrt((
- double)(distance[0] * distance[0] + distance[1] * distance[1]));
-
- // check if current waypoint has been reached
- if (distance_to_waypoint < waypointRadius &&
- m_waypoints.size() >= 1) {
- m_waypoints.erase(m_waypoints.begin());
- }
- }
+ m_seen_obstacle_before = true;
+ }
+ else
+ {
+ m_seen_obstacle_before = false;
+ m_obstacle_on_path = false;
+ }
+ }
- if (m_waypoints.size() == 0) {
- ROS_INFO_STREAM("Last waypoint reached");
- currentPathFinished();
- return;
+ if (m_waypoints.size() == 0)
+ {
+ ROS_WARN_STREAM("No waypoints but trying to perform next move! "
+ "Skipping.");
+ return;
+ }
+ // if we have accidentaly skipped waypoints, recalculate path
+ float minDistance = FLT_MAX;
+ float distance;
+ unsigned nearestWaypoint = 0;
+ for (unsigned i = 0; i < m_waypoints.size(); i++)
+ {
+ distance = map_tools::distance(m_robot_pose.position,
+ m_waypoints[i].pose.position);
+ if (distance < minDistance)
+ {
+ nearestWaypoint = i;
+ minDistance = distance;
+ }
+ }
+ if (nearestWaypoint != 0)
+ {
+ // if the target is nearer than the waypoint don't recalculate
+ if (m_waypoints.size() != 2)
+ {
+ ROS_WARN_STREAM("Waypoints skipped. Recalculating path!");
+ calculatePath();
+ if (m_MainMachine.state() != FOLLOWING_PATH)
+ {
+ return;
}
+ }
+ else
+ {
+ m_waypoints.erase(m_waypoints.begin());
+ }
+ }
- sendPathData();
-
- if (m_use_ptu) {
- ROS_INFO_STREAM("Moving PTU to center next Waypoint");
- homer_ptu_msgs::CenterWorldPoint ptu_msg;
- ptu_msg.point.x = m_waypoints[0].pose.position.x;
- ptu_msg.point.y = m_waypoints[0].pose.position.y;
- ptu_msg.point.z = 0;
- ptu_msg.permanent = true;
- m_ptu_center_world_point_pub.publish(ptu_msg);
- }
+ Eigen::Vector2i waypointPixel =
+ map_tools::toMapCoords(m_waypoints[0].pose.position, m_map);
+ float obstacleDistanceMap = m_explorer->getObstacleTransform()->getValue(
+ waypointPixel.x(), waypointPixel.y()) *
+ m_map->info.resolution;
+ float waypointRadius = m_waypoint_radius_factor * obstacleDistanceMap;
- float distanceToWaypoint = map_tools::distance(
- m_robot_pose.position, m_waypoints[0].pose.position);
- float angleToWaypoint = angleToPointDeg(m_waypoints[0].pose.position);
- if (angleToWaypoint < -180) {
- angleToWaypoint += 360;
- }
- float angle = deg2Rad(angleToWaypoint);
+ if ((waypointRadius < m_map->info.resolution) || (m_waypoints.size() == 1))
+ {
+ waypointRadius = m_map->info.resolution;
+ }
- // linear speed calculation
- if (m_avoided_collision) {
- if (std::fabs(angleToWaypoint) < 10) {
- m_avoided_collision = false;
- }
- } else if (fabs(angle) > m_max_drive_angle) {
- m_act_speed = 0.0;
- } else if (m_obstacle_on_path &&
- map_tools::distance(m_robot_pose.position,
- m_obstacle_position) < 1.0) {
- m_act_speed = 0.0;
- float angleToWaypoint2 = angleToPointDeg(m_obstacle_position);
- if (angleToWaypoint2 < -180) {
- angleToWaypoint2 += 360;
- }
- angle = deg2Rad(angleToWaypoint2);
-
- if (std::fabs(angle) < m_min_turn_angle) {
- ROS_INFO_STREAM("angle = " << angle);
- m_avoided_collision = true;
- m_initial_path_reaches_target = true;
- m_stop_before_obstacle = true;
- startNavigation();
- return;
- }
- } else {
- float obstacleMapDistance = 1;
- for (int wpi = -1; wpi < std::min((int)m_waypoints.size(), (int)2);
- wpi++) {
- Eigen::Vector2i robotPixel;
- if (wpi == -1) {
- robotPixel =
- map_tools::toMapCoords(m_robot_pose.position, m_map);
- } else {
- robotPixel = map_tools::toMapCoords(
- m_waypoints[wpi].pose.position, m_map);
- }
- obstacleMapDistance = std::min(
- (float)obstacleMapDistance,
- (float)(m_explorer->getObstacleTransform()->getValue(
- robotPixel.x(), robotPixel.y()) *
- m_map->info.resolution));
- if (obstacleMapDistance <= 0.00001) {
- ROS_ERROR_STREAM(
- "obstacleMapDistance is below threshold to 0 setting "
- "to 1");
- obstacleMapDistance = 1;
- }
- }
+ if (m_waypoints.size() != 0)
+ {
+ // calculate distance between last_pose->current_pose and waypoint
+ Eigen::Vector2f line; // direction of the line
+ line[0] = m_robot_pose.position.x - m_robot_last_pose.position.x;
+ line[1] = m_robot_pose.position.y - m_robot_last_pose.position.y;
+ Eigen::Vector2f point_to_start; // vector from the point to the
+ // start of the line
+ point_to_start[0] =
+ m_robot_last_pose.position.x - m_waypoints[0].pose.position.x;
+ point_to_start[1] =
+ m_robot_last_pose.position.y - m_waypoints[0].pose.position.y;
+ float dot =
+ point_to_start[0] * line[0] +
+ point_to_start[1] * line[1]; // dot product of point_to_start * line
+ Eigen::Vector2f distance; // shortest distance vector from point to line
+ distance[0] = point_to_start[0] - dot * line[0];
+ distance[1] = point_to_start[1] - dot * line[1];
+ float distance_to_waypoint =
+ sqrt((double)(distance[0] * distance[0] + distance[1] * distance[1]));
+
+ // check if current waypoint has been reached
+ if (distance_to_waypoint < waypointRadius && m_waypoints.size() >= 1)
+ {
+ m_waypoints.erase(m_waypoints.begin());
+ }
+ }
- float max_move_distance_speed = m_max_move_speed *
- m_max_move_distance *
- m_obstacle_speed_factor;
- float max_map_distance_speed =
- m_max_move_speed * obstacleMapDistance * m_map_speed_factor;
-
- float max_waypoint_speed = 1;
- if (m_waypoints.size() > 1) {
- float angleToNextWaypoint =
- angleToPointDeg(m_waypoints[1].pose.position);
- max_waypoint_speed = (1 - (angleToNextWaypoint / 180.0)) *
- distanceToWaypoint *
- m_waypoint_speed_factor;
- }
+ if (m_waypoints.size() == 0)
+ {
+ ROS_INFO_STREAM("Last waypoint reached");
+ currentPathFinished();
+ return;
+ }
- m_act_speed = std::min(
- {std::max((float)0.1, m_distance_to_target *
- m_target_distance_speed_factor),
- m_max_move_speed, max_move_distance_speed,
- max_map_distance_speed, max_waypoint_speed});
- std_msgs::String tmp;
- std::stringstream str;
- str << "m_obstacle_speed " << max_move_distance_speed
- << " max_map_distance_speed " << max_map_distance_speed;
- tmp.data = str.str();
- m_debug_pub.publish(tmp);
- }
+ sendPathData();
+
+ if (m_use_ptu)
+ {
+ ROS_INFO_STREAM("Moving PTU to center next Waypoint");
+ homer_ptu_msgs::CenterWorldPoint ptu_msg;
+ ptu_msg.point.x = m_waypoints[0].pose.position.x;
+ ptu_msg.point.y = m_waypoints[0].pose.position.y;
+ ptu_msg.point.z = 0;
+ ptu_msg.permanent = true;
+ m_ptu_center_world_point_pub.publish(ptu_msg);
+ }
- // angular speed calculation
- if (angle < 0) {
- angle = std::max(angle * (float)0.8, -m_max_turn_speed);
- m_act_speed = m_act_speed + angle / 2.0;
- if (m_act_speed < 0) {
- m_act_speed = 0;
- }
- } else {
- angle = std::min(angle * (float)0.8, m_max_turn_speed);
- m_act_speed = m_act_speed - angle / 2.0;
- if (m_act_speed < 0) {
- m_act_speed = 0;
- }
+ float distanceToWaypoint = map_tools::distance(
+ m_robot_pose.position, m_waypoints[0].pose.position);
+ float angleToWaypoint = angleToPointDeg(m_waypoints[0].pose.position);
+ if (angleToWaypoint < -180)
+ {
+ angleToWaypoint += 360;
+ }
+ float angle = deg2Rad(angleToWaypoint);
+
+ // linear speed calculation
+ if (m_avoided_collision)
+ {
+ if (std::fabs(angleToWaypoint) < 10)
+ {
+ m_avoided_collision = false;
+ }
+ }
+ else if (fabs(angle) > m_max_drive_angle)
+ {
+ m_act_speed = 0.0;
+ }
+ else if (m_obstacle_on_path &&
+ map_tools::distance(m_robot_pose.position, m_obstacle_position) <
+ 1.0)
+ {
+ m_act_speed = 0.0;
+ float angleToWaypoint2 = angleToPointDeg(m_obstacle_position);
+ if (angleToWaypoint2 < -180)
+ {
+ angleToWaypoint2 += 360;
+ }
+ angle = deg2Rad(angleToWaypoint2);
+
+ if (std::fabs(angle) < m_min_turn_angle)
+ {
+ ROS_INFO_STREAM("angle = " << angle);
+ m_avoided_collision = true;
+ m_initial_path_reaches_target = true;
+ m_stop_before_obstacle = true;
+ startNavigation();
+ return;
+ }
+ }
+ else
+ {
+ float obstacleMapDistance = 1;
+ for (int wpi = -1; wpi < std::min((int)m_waypoints.size(), (int)2); wpi++)
+ {
+ Eigen::Vector2i robotPixel;
+ if (wpi == -1)
+ {
+ robotPixel = map_tools::toMapCoords(m_robot_pose.position, m_map);
}
- geometry_msgs::Twist cmd_vel_msg;
- cmd_vel_msg.linear.x = m_act_speed;
- cmd_vel_msg.angular.z = angle;
- m_cmd_vel_pub.publish(cmd_vel_msg);
-
- ROS_DEBUG_STREAM(
- "Driving & turning"
- << std::endl
- << "linear: " << m_act_speed << " angular: " << angle << std::endl
- << "distanceToWaypoint:" << distanceToWaypoint
- << "angleToWaypoint: " << angleToWaypoint << std::endl);
- } else if (m_MainMachine.state() == AVOIDING_COLLISION) {
- if (isTargetPositionReached()) {
- return;
- } else if ((m_max_move_distance <= m_collision_distance &&
- m_waypoints.size() > 1) ||
- m_max_move_distance <= m_collision_distance_near_target) {
- ROS_WARN_STREAM("Maximum driving distance too short ("
- << m_max_move_distance << "m)! Moving back.");
- geometry_msgs::Twist cmd_vel_msg;
- if (!HomerNavigationNode::backwardObstacle()) {
- cmd_vel_msg.linear.x = -0.2;
- } else {
- if (m_angular_avoidance == 0) {
- float angleToWaypoint =
- angleToPointDeg(m_waypoints[0].pose.position);
- if (angleToWaypoint < -180) {
- angleToWaypoint += 360;
- }
- if (angleToWaypoint < 0) {
- m_angular_avoidance = -0.4;
- } else {
- m_angular_avoidance = 0.4;
- }
- }
- cmd_vel_msg.angular.z = m_angular_avoidance;
- }
- m_cmd_vel_pub.publish(cmd_vel_msg);
- } else {
- m_angular_avoidance = 0;
- m_avoided_collision = true;
- ROS_WARN_STREAM("Collision avoided. Updating path.");
- currentPathFinished();
+ else
+ {
+ robotPixel =
+ map_tools::toMapCoords(m_waypoints[wpi].pose.position, m_map);
}
- } else if (m_MainMachine.state() == FINAL_TURN) {
- if (m_use_ptu) {
- // reset PTU
- homer_ptu_msgs::SetPanTilt msg;
- msg.absolute = true;
- msg.panAngle = 0;
- msg.tiltAngle = 0;
- m_set_pan_tilt_pub.publish(msg);
+ obstacleMapDistance =
+ std::min((float)obstacleMapDistance,
+ (float)(m_explorer->getObstacleTransform()->getValue(
+ robotPixel.x(), robotPixel.y()) *
+ m_map->info.resolution));
+ if (obstacleMapDistance <= 0.00001)
+ {
+ ROS_ERROR_STREAM(
+ "obstacleMapDistance is below threshold to 0 setting "
+ "to 1");
+ obstacleMapDistance = 1;
}
+ }
+
+ float max_move_distance_speed =
+ m_max_move_speed * m_max_move_distance * m_obstacle_speed_factor;
+ float max_map_distance_speed =
+ m_max_move_speed * obstacleMapDistance * m_map_speed_factor;
+
+ float max_waypoint_speed = 1;
+ if (m_waypoints.size() > 1)
+ {
+ float angleToNextWaypoint =
+ angleToPointDeg(m_waypoints[1].pose.position);
+ max_waypoint_speed = (1 - (angleToNextWaypoint / 180.0)) *
+ distanceToWaypoint * m_waypoint_speed_factor;
+ }
+
+ m_act_speed =
+ std::min({ std::max((float)0.1, m_distance_to_target *
+ m_target_distance_speed_factor),
+ m_max_move_speed, max_move_distance_speed,
+ max_map_distance_speed, max_waypoint_speed });
+ std_msgs::String tmp;
+ std::stringstream str;
+ str << "m_obstacle_speed " << max_move_distance_speed
+ << " max_map_distance_speed " << max_map_distance_speed;
+ tmp.data = str.str();
+ m_debug_pub.publish(tmp);
+ }
- if (m_skip_final_turn) {
- ROS_INFO_STREAM("Final turn skipped. Target reached.");
- if (m_path_reaches_target) {
- sendTargetReachedMsg();
- } else {
- sendTargetUnreachableMsg(
- homer_mapnav_msgs::TargetUnreachable::NO_PATH_FOUND);
- }
- return;
- }
+ // angular speed calculation
+ if (angle < 0)
+ {
+ angle = std::max(angle * (float)0.8, -m_max_turn_speed);
+ m_act_speed = m_act_speed + angle / 2.0;
+ if (m_act_speed < 0)
+ {
+ m_act_speed = 0;
+ }
+ }
+ else
+ {
+ angle = std::min(angle * (float)0.8, m_max_turn_speed);
+ m_act_speed = m_act_speed - angle / 2.0;
+ if (m_act_speed < 0)
+ {
+ m_act_speed = 0;
+ }
+ }
+ geometry_msgs::Twist cmd_vel_msg;
+ cmd_vel_msg.linear.x = m_act_speed;
+ cmd_vel_msg.angular.z = angle;
+ m_cmd_vel_pub.publish(cmd_vel_msg);
- float turnAngle = minTurnAngle(tf::getYaw(m_robot_pose.orientation),
- m_target_orientation);
- ROS_DEBUG_STREAM(
- "homer_navigation::PerformNextMove:: Final Turn. Robot "
- "orientation: "
- << rad2Deg(tf::getYaw(m_robot_pose.orientation))
- << ". Target orientation: " << rad2Deg(m_target_orientation)
- << "homer_navigation::PerformNextMove:: turnAngle: "
- << rad2Deg(turnAngle));
-
- if (std::fabs(turnAngle) < m_min_turn_angle) {
- ROS_INFO_STREAM(
- ":::::::NEAREST WALKABLE TARGET REACHED BECAUSE lower "
- << m_min_turn_angle);
- ROS_INFO_STREAM("target angle = " << m_target_orientation);
- ROS_INFO_STREAM(
- "is angle = " << tf::getYaw(m_robot_pose.orientation));
- ROS_INFO_STREAM("m_distance_to_target = " << m_distance_to_target);
- ROS_INFO_STREAM("m_desired_distance = " << m_desired_distance);
- if (m_path_reaches_target) {
- sendTargetReachedMsg();
- } else {
- sendTargetUnreachableMsg(
- homer_mapnav_msgs::TargetUnreachable::NO_PATH_FOUND);
- }
- return;
- } else {
- if (turnAngle < 0) {
- turnAngle = std::max(std::min(turnAngle, -m_min_turn_speed),
- -m_max_turn_speed);
- } else {
- turnAngle = std::min(std::max(turnAngle, m_min_turn_speed),
- m_max_turn_speed);
- }
- geometry_msgs::Twist cmd_vel_msg;
- cmd_vel_msg.angular.z = turnAngle;
- m_cmd_vel_pub.publish(cmd_vel_msg);
+ ROS_DEBUG_STREAM("Driving & turning"
+ << std::endl
+ << "linear: " << m_act_speed << " angular: " << angle
+ << std::endl
+ << "distanceToWaypoint:" << distanceToWaypoint
+ << "angleToWaypoint: " << angleToWaypoint << std::endl);
+ }
+ else if (m_MainMachine.state() == AVOIDING_COLLISION)
+ {
+ if (isTargetPositionReached())
+ {
+ return;
+ }
+ else if ((m_max_move_distance <= m_collision_distance &&
+ m_waypoints.size() > 1) ||
+ m_max_move_distance <= m_collision_distance_near_target)
+ {
+ ROS_WARN_STREAM("Maximum driving distance too short ("
+ << m_max_move_distance << "m)! Moving back.");
+ geometry_msgs::Twist cmd_vel_msg;
+ if (!HomerNavigationNode::backwardObstacle())
+ {
+ cmd_vel_msg.linear.x = -0.2;
+ }
+ else
+ {
+ if (m_angular_avoidance == 0)
+ {
+ float angleToWaypoint = angleToPointDeg(m_waypoints[0].pose.position);
+ if (angleToWaypoint < -180)
+ {
+ angleToWaypoint += 360;
+ }
+ if (angleToWaypoint < 0)
+ {
+ m_angular_avoidance = -0.4;
+ }
+ else
+ {
+ m_angular_avoidance = 0.4;
+ }
}
+ cmd_vel_msg.angular.z = m_angular_avoidance;
+ }
+ m_cmd_vel_pub.publish(cmd_vel_msg);
+ }
+ else
+ {
+ m_angular_avoidance = 0;
+ m_avoided_collision = true;
+ ROS_WARN_STREAM("Collision avoided. Updating path.");
+ currentPathFinished();
+ }
+ }
+ else if (m_MainMachine.state() == FINAL_TURN)
+ {
+ if (m_use_ptu)
+ {
+ // reset PTU
+ homer_ptu_msgs::SetPanTilt msg;
+ msg.absolute = true;
+ msg.panAngle = 0;
+ msg.tiltAngle = 0;
+ m_set_pan_tilt_pub.publish(msg);
+ }
+
+ if (m_skip_final_turn)
+ {
+ ROS_INFO_STREAM("Final turn skipped. Target reached.");
+ if (m_path_reaches_target)
+ {
+ sendTargetReachedMsg();
+ }
+ else
+ {
+ sendTargetUnreachableMsg(
+ homer_mapnav_msgs::TargetUnreachable::NO_PATH_FOUND);
+ }
+ return;
+ }
+
+ float turnAngle = minTurnAngle(tf::getYaw(m_robot_pose.orientation),
+ m_target_orientation);
+ ROS_DEBUG_STREAM("homer_navigation::PerformNextMove:: Final Turn. Robot "
+ "orientation: "
+ << rad2Deg(tf::getYaw(m_robot_pose.orientation))
+ << ". Target orientation: "
+ << rad2Deg(m_target_orientation)
+ << "homer_navigation::PerformNextMove:: turnAngle: "
+ << rad2Deg(turnAngle));
+
+ if (std::fabs(turnAngle) < m_min_turn_angle)
+ {
+ ROS_INFO_STREAM(":::::::NEAREST WALKABLE TARGET REACHED BECAUSE lower "
+ << m_min_turn_angle);
+ ROS_INFO_STREAM("target angle = " << m_target_orientation);
+ ROS_INFO_STREAM("is angle = " << tf::getYaw(m_robot_pose.orientation));
+ ROS_INFO_STREAM("m_distance_to_target = " << m_distance_to_target);
+ ROS_INFO_STREAM("m_desired_distance = " << m_desired_distance);
+ if (m_path_reaches_target)
+ {
+ sendTargetReachedMsg();
+ }
+ else
+ {
+ sendTargetUnreachableMsg(
+ homer_mapnav_msgs::TargetUnreachable::NO_PATH_FOUND);
+ }
+ return;
+ }
+ else
+ {
+ if (turnAngle < 0)
+ {
+ turnAngle =
+ std::max(std::min(turnAngle, -m_min_turn_speed), -m_max_turn_speed);
+ }
+ else
+ {
+ turnAngle =
+ std::min(std::max(turnAngle, m_min_turn_speed), m_max_turn_speed);
+ }
+ geometry_msgs::Twist cmd_vel_msg;
+ cmd_vel_msg.angular.z = turnAngle;
+ m_cmd_vel_pub.publish(cmd_vel_msg);
}
+ }
}
-void HomerNavigationNode::currentPathFinished() {
- ROS_INFO_STREAM("Current path was finished, initiating recalculation.");
- m_waypoints.clear();
- stopRobot();
- startNavigation();
+void HomerNavigationNode::currentPathFinished()
+{
+ ROS_INFO_STREAM("Current path was finished, initiating recalculation.");
+ m_waypoints.clear();
+ stopRobot();
+ startNavigation();
}
-int HomerNavigationNode::angleToPointDeg(geometry_msgs::Point target) {
- double cx = m_robot_pose.position.x;
- double cy = m_robot_pose.position.y;
- int targetAngle = rad2Deg(atan2(target.y - cy, target.x - cx));
- int currentAngle = rad2Deg(tf::getYaw(m_robot_pose.orientation));
+int HomerNavigationNode::angleToPointDeg(geometry_msgs::Point target)
+{
+ double cx = m_robot_pose.position.x;
+ double cy = m_robot_pose.position.y;
+ int targetAngle = rad2Deg(atan2(target.y - cy, target.x - cx));
+ int currentAngle = rad2Deg(tf::getYaw(m_robot_pose.orientation));
- int angleDiff = targetAngle - currentAngle;
- angleDiff = (angleDiff + 180) % 360 - 180;
- return angleDiff;
+ int angleDiff = targetAngle - currentAngle;
+ angleDiff = (angleDiff + 180) % 360 - 180;
+ return angleDiff;
}
bool HomerNavigationNode::drawPolygon(
- std::vector<geometry_msgs::Point> vertices) {
- if (vertices.size() == 0) {
- ROS_INFO_STREAM("No vertices given!");
- return false;
- }
- // make temp. map
- std::vector<int> data(m_map->info.width * m_map->info.height);
- for (int i = 0; i < data.size(); i++) {
- data[i] = 0;
- }
-
- // draw the lines surrounding the polygon
- for (unsigned int i = 0; i < vertices.size(); i++) {
- int i2 = (i + 1) % vertices.size();
- drawLine(data, vertices[i].x, vertices[i].y, vertices[i2].x,
- vertices[i2].y, 30);
- }
- // calculate a point in the middle of the polygon
- float midX = 0;
- float midY = 0;
- for (unsigned int i = 0; i < vertices.size(); i++) {
- midX += vertices[i].x;
- midY += vertices[i].y;
- }
- midX /= vertices.size();
- midY /= vertices.size();
- // fill polygon
- return fillPolygon(data, (int)midX, (int)midY, 30);
+ std::vector<geometry_msgs::Point> vertices)
+{
+ if (vertices.size() == 0)
+ {
+ ROS_INFO_STREAM("No vertices given!");
+ return false;
+ }
+ // make temp. map
+ std::vector<int> data(m_map->info.width * m_map->info.height);
+ for (int i = 0; i < data.size(); i++)
+ {
+ data[i] = 0;
+ }
+
+ // draw the lines surrounding the polygon
+ for (unsigned int i = 0; i < vertices.size(); i++)
+ {
+ int i2 = (i + 1) % vertices.size();
+ drawLine(data, vertices[i].x, vertices[i].y, vertices[i2].x, vertices[i2].y,
+ 30);
+ }
+ // calculate a point in the middle of the polygon
+ float midX = 0;
+ float midY = 0;
+ for (unsigned int i = 0; i < vertices.size(); i++)
+ {
+ midX += vertices[i].x;
+ midY += vertices[i].y;
+ }
+ midX /= vertices.size();
+ midY /= vertices.size();
+ // fill polygon
+ return fillPolygon(data, (int)midX, (int)midY, 30);
}
void HomerNavigationNode::drawLine(std::vector<int>& data, int startX,
- int startY, int endX, int endY, int value) {
- // bresenham algorithm
- int x, y, t, dist, xerr, yerr, dx, dy, incx, incy;
- // compute distances
- dx = endX - startX;
- dy = endY - startY;
-
- // compute increment
- if (dx < 0) {
- incx = -1;
- dx = -dx;
- } else {
- incx = dx ? 1 : 0;
+ int startY, int endX, int endY, int value)
+{
+ // bresenham algorithm
+ int x, y, t, dist, xerr, yerr, dx, dy, incx, incy;
+ // compute distances
+ dx = endX - startX;
+ dy = endY - startY;
+
+ // compute increment
+ if (dx < 0)
+ {
+ incx = -1;
+ dx = -dx;
+ }
+ else
+ {
+ incx = dx ? 1 : 0;
+ }
+
+ if (dy < 0)
+ {
+ incy = -1;
+ dy = -dy;
+ }
+ else
+ {
+ incy = dy ? 1 : 0;
+ }
+
+ // which distance is greater?
+ dist = (dx > dy) ? dx : dy;
+ // initializing
+ x = startX;
+ y = startY;
+ xerr = dx;
+ yerr = dy;
+
+ // compute cells
+ for (t = 0; t < dist; t++)
+ {
+ int index = x + m_map->info.width * y;
+ if (index < 0 || index > data.size())
+ {
+ continue;
}
-
- if (dy < 0) {
- incy = -1;
- dy = -dy;
- } else {
- incy = dy ? 1 : 0;
+ data[index] = value;
+
+ xerr += dx;
+ yerr += dy;
+ if (xerr > dist)
+ {
+ xerr -= dist;
+ x += incx;
}
-
- // which distance is greater?
- dist = (dx > dy) ? dx : dy;
- // initializing
- x = startX;
- y = startY;
- xerr = dx;
- yerr = dy;
-
- // compute cells
- for (t = 0; t < dist; t++) {
- int index = x + m_map->info.width * y;
- if (index < 0 || index > data.size()) {
- continue;
- }
- data[index] = value;
-
- xerr += dx;
- yerr += dy;
- if (xerr > dist) {
- xerr -= dist;
- x += incx;
- }
- if (yerr > dist) {
- yerr -= dist;
- y += incy;
- }
+ if (yerr > dist)
+ {
+ yerr -= dist;
+ y += incy;
}
+ }
}
bool HomerNavigationNode::fillPolygon(std::vector<int>& data, int x, int y,
- int value) {
- int index = x + m_map->info.width * y;
- bool tmp = false;
-
- if ((int)m_map->data.at(index) > 90) {
- tmp = true;
+ int value)
+{
+ int index = x + m_map->info.width * y;
+ bool tmp = false;
+
+ if ((int)m_map->data.at(index) > 90)
+ {
+ tmp = true;
+ }
+ if (data[index] != value)
+ {
+ data[index] = value;
+ if (fillPolygon(data, x + 1, y, value))
+ {
+ tmp = true;
}
- if (data[index] != value) {
- data[index] = value;
- if (fillPolygon(data, x + 1, y, value)) {
- tmp = true;
- }
- if (fillPolygon(data, x - 1, y, value)) {
- tmp = true;
- }
- if (fillPolygon(data, x, y + 1, value)) {
- tmp = true;
- }
- if (fillPolygon(data, x, y - 1, value)) {
- tmp = true;
- }
+ if (fillPolygon(data, x - 1, y, value))
+ {
+ tmp = true;
}
- return tmp;
-}
-
-bool HomerNavigationNode::backwardObstacle() {
- std::vector<geometry_msgs::Point> vertices;
- geometry_msgs::Point base_link_point;
- geometry_msgs::Point map_point;
- Eigen::Vector2i map_coord;
-
- std::vector<float> x;
- std::vector<float> y;
-
- x.push_back(-m_min_x - m_backward_collision_distance);
- y.push_back(m_min_y);
-
- x.push_back(-m_min_x - m_backward_collision_distance);
- y.push_back(-m_min_y);
-
- x.push_back(-0.1);
- y.push_back(-m_min_y);
-
- x.push_back(-0.1);
- y.push_back(m_min_y);
-
- for (int i = 0; i < x.size(); i++) {
- base_link_point.x = x[i];
- base_link_point.y = y[i];
- map_coord = map_tools::toMapCoords(
- map_tools::transformPoint(base_link_point, m_transform_listener,
- "/base_link", "/map"),
- m_map);
- map_point.x = map_coord.x();
- map_point.y = map_coord.y();
- vertices.push_back(map_point);
+ if (fillPolygon(data, x, y + 1, value))
+ {
+ tmp = true;
+ }
+ if (fillPolygon(data, x, y - 1, value))
+ {
+ tmp = true;
}
+ }
+ return tmp;
+}
- return drawPolygon(vertices);
+bool HomerNavigationNode::backwardObstacle()
+{
+ std::vector<geometry_msgs::Point> vertices;
+ geometry_msgs::Point base_link_point;
+ geometry_msgs::Point map_point;
+ Eigen::Vector2i map_coord;
+
+ std::vector<float> x;
+ std::vector<float> y;
+
+ x.push_back(-m_min_x - m_backward_collision_distance);
+ y.push_back(m_min_y);
+
+ x.push_back(-m_min_x - m_backward_collision_distance);
+ y.push_back(-m_min_y);
+
+ x.push_back(-0.1);
+ y.push_back(-m_min_y);
+
+ x.push_back(-0.1);
+ y.push_back(m_min_y);
+
+ for (int i = 0; i < x.size(); i++)
+ {
+ base_link_point.x = x[i];
+ base_link_point.y = y[i];
+ map_coord = map_tools::toMapCoords(
+ map_tools::transformPoint(base_link_point, m_transform_listener,
+ "/base_link", "/map"),
+ m_map);
+ map_point.x = map_coord.x();
+ map_point.y = map_coord.y();
+ vertices.push_back(map_point);
+ }
+
+ return drawPolygon(vertices);
}
-void HomerNavigationNode::maskMap(nav_msgs::OccupancyGrid& cmap) {
- // generate bounding box
- ROS_INFO_STREAM("Calculating Bounding box for fast planning");
- Eigen::Vector2i pose_pixel =
- map_tools::toMapCoords(m_robot_pose.position, m_map);
- Eigen::Vector2i target_pixel =
- map_tools::toMapCoords(m_target_point, m_map);
- Eigen::Vector2i safe_pixel_distance(m_AllowedObstacleDistance.first * 4,
- m_AllowedObstacleDistance.first * 4);
- Eigen::AlignedBox2i planning_box;
- planning_box.extend(pose_pixel);
- planning_box.extend(target_pixel);
- ROS_INFO_STREAM("Bounding Box: (" << planning_box.min() << " "
- << planning_box.max());
- Eigen::AlignedBox2i safe_planning_box(
- planning_box.min() - safe_pixel_distance,
- planning_box.max() + safe_pixel_distance);
- ROS_INFO_STREAM("safe Bounding Box: (" << safe_planning_box.min() << " "
- << safe_planning_box.max());
- ROS_INFO_STREAM("min in m: " << map_tools::fromMapCoords(
- safe_planning_box.min(), m_map));
- ROS_INFO_STREAM("max in m: " << map_tools::fromMapCoords(
- safe_planning_box.max(), m_map));
- for (size_t x = 0; x < m_map->info.width; x++) {
- for (size_t y = 0; y < m_map->info.height; y++) {
- if (!safe_planning_box.contains(Eigen::Vector2i(x, y))) {
- cmap.data.at(y * m_map->info.width + x) = -1;
- }
- }
+void HomerNavigationNode::maskMap(nav_msgs::OccupancyGrid& cmap)
+{
+ // generate bounding box
+ ROS_INFO_STREAM("Calculating Bounding box for fast planning");
+ Eigen::Vector2i pose_pixel =
+ map_tools::toMapCoords(m_robot_pose.position, m_map);
+ Eigen::Vector2i target_pixel = map_tools::toMapCoords(m_target_point, m_map);
+ Eigen::Vector2i safe_pixel_distance(m_AllowedObstacleDistance.first * 4,
+ m_AllowedObstacleDistance.first * 4);
+ Eigen::AlignedBox2i planning_box;
+ planning_box.extend(pose_pixel);
+ planning_box.extend(target_pixel);
+ ROS_INFO_STREAM("Bounding Box: (" << planning_box.min() << " "
+ << planning_box.max());
+ Eigen::AlignedBox2i safe_planning_box(
+ planning_box.min() - safe_pixel_distance,
+ planning_box.max() + safe_pixel_distance);
+ ROS_INFO_STREAM("safe Bounding Box: (" << safe_planning_box.min() << " "
+ << safe_planning_box.max());
+ ROS_INFO_STREAM(
+ "min in m: " << map_tools::fromMapCoords(safe_planning_box.min(), m_map));
+ ROS_INFO_STREAM(
+ "max in m: " << map_tools::fromMapCoords(safe_planning_box.max(), m_map));
+ for (size_t x = 0; x < m_map->info.width; x++)
+ {
+ for (size_t y = 0; y < m_map->info.height; y++)
+ {
+ if (!safe_planning_box.contains(Eigen::Vector2i(x, y)))
+ {
+ cmap.data.at(y * m_map->info.width + x) = -1;
+ }
}
+ }
}
// convenience math functions
-float HomerNavigationNode::minTurnAngle(float angle1, float angle2) {
- angle1 *= 180.0 / M_PI;
- angle2 *= 180.0 / M_PI;
-
- int diff = angle2 - angle1;
- diff = (diff + 180) % 360 - 180;
- if (diff < -180) {
- diff += 360;
- }
-
- float ret = static_cast<double>(diff) * M_PI / 180.0;
- return ret;
+float HomerNavigationNode::minTurnAngle(float angle1, float angle2)
+{
+ angle1 *= 180.0 / M_PI;
+ angle2 *= 180.0 / M_PI;
+
+ int diff = angle2 - angle1;
+ diff = (diff + 180) % 360 - 180;
+ if (diff < -180)
+ {
+ diff += 360;
+ }
+
+ float ret = static_cast<double>(diff) * M_PI / 180.0;
+ return ret;
}
void HomerNavigationNode::refreshParamsCallback(
- const std_msgs::Empty::ConstPtr& msg) {
- (void) msg;
- ROS_INFO_STREAM("Refreshing Parameters");
- loadParameters();
+ const std_msgs::Empty::ConstPtr& msg)
+{
+ (void)msg;
+ ROS_INFO_STREAM("Refreshing Parameters");
+ loadParameters();
}
void HomerNavigationNode::mapCallback(
- const nav_msgs::OccupancyGrid::ConstPtr& msg) {
- if (!m_explorer) {
- float resolution = msg->info.resolution;
- m_explorer = new Explorer(m_AllowedObstacleDistance.first / resolution,
- m_AllowedObstacleDistance.second / resolution,
- m_SafeObstacleDistance.first / resolution,
- m_SafeObstacleDistance.second / resolution,
- m_SafePathWeight, m_FrontierSafenessFactor,
- m_unknown_threshold);
- }
- m_map = msg;
- if (m_MainMachine.state() != IDLE) {
- setExplorerMap();
- }
+ const nav_msgs::OccupancyGrid::ConstPtr& msg)
+{
+ if (!m_explorer)
+ {
+ float resolution = msg->info.resolution;
+ m_explorer = new Explorer(m_AllowedObstacleDistance.first / resolution,
+ m_AllowedObstacleDistance.second / resolution,
+ m_SafeObstacleDistance.first / resolution,
+ m_SafeObstacleDistance.second / resolution,
+ m_SafePathWeight, m_FrontierSafenessFactor,
+ m_unknown_threshold);
+ }
+ m_map = msg;
+ if (m_MainMachine.state() != IDLE)
+ {
+ setExplorerMap();
+ }
}
void HomerNavigationNode::poseCallback(
- const geometry_msgs::PoseStamped::ConstPtr& msg) {
- m_robot_last_pose = m_robot_pose;
- m_robot_pose = msg->pose;
- m_last_pose_time = ros::Time::now();
- m_new_target = false;
- m_distance_to_target =
- map_tools::distance(m_robot_pose.position, m_target_point);
- performNextMove();
+ const geometry_msgs::PoseStamped::ConstPtr& msg)
+{
+ m_robot_last_pose = m_robot_pose;
+ m_robot_pose = msg->pose;
+ m_last_pose_time = ros::Time::now();
+ m_new_target = false;
+ m_distance_to_target =
+ map_tools::distance(m_robot_pose.position, m_target_point);
+ performNextMove();
}
-void HomerNavigationNode::calcMaxMoveDist() {
- m_max_move_distance = 99;
- for (auto d : m_max_move_distances) {
- m_max_move_distance = std::min(m_max_move_distance, d.second);
- }
- if ((m_max_move_distance <= m_collision_distance &&
- std::fabs(m_act_speed) > 0.1 && m_waypoints.size() > 1) ||
- (m_max_move_distance <= m_collision_distance_near_target &&
- std::fabs(m_act_speed) > 0.1 && m_waypoints.size() == 1) ||
- m_max_move_distance <= 0.1) {
- if (m_MainMachine.state() == FOLLOWING_PATH) {
- stopRobot();
- m_MainMachine.setState(AVOIDING_COLLISION);
- ROS_WARN_STREAM("Collision detected while following path!");
- }
+void HomerNavigationNode::calcMaxMoveDist()
+{
+ m_max_move_distance = 99;
+ for (auto d : m_max_move_distances)
+ {
+ m_max_move_distance = std::min(m_max_move_distance, d.second);
+ }
+ if ((m_max_move_distance <= m_collision_distance &&
+ std::fabs(m_act_speed) > 0.1 && m_waypoints.size() > 1) ||
+ (m_max_move_distance <= m_collision_distance_near_target &&
+ std::fabs(m_act_speed) > 0.1 && m_waypoints.size() == 1) ||
+ m_max_move_distance <= 0.1)
+ {
+ if (m_MainMachine.state() == FOLLOWING_PATH)
+ {
+ stopRobot();
+ m_MainMachine.setState(AVOIDING_COLLISION);
+ ROS_WARN_STREAM("Collision detected while following path!");
}
+ }
}
void HomerNavigationNode::ignoreLaserCallback(
- const std_msgs::String::ConstPtr& msg) {
- ROS_INFO_STREAM("changed ignore laser to: " << msg->data);
- m_ignore_scan = msg->data;
+ const std_msgs::String::ConstPtr& msg)
+{
+ ROS_INFO_STREAM("changed ignore laser to: " << msg->data);
+ m_ignore_scan = msg->data;
}
void HomerNavigationNode::maxDepthMoveDistanceCallback(
- const std_msgs::Float32::ConstPtr& msg) {
- m_max_move_distances["depth"] = msg->data;
- calcMaxMoveDist();
+ const std_msgs::Float32::ConstPtr& msg)
+{
+ m_max_move_distances["depth"] = msg->data;
+ calcMaxMoveDist();
}
void HomerNavigationNode::laserDataCallback(
- const sensor_msgs::LaserScan::ConstPtr& msg) {
- m_scan_map[msg->header.frame_id] = msg;
- m_last_laser_time = ros::Time::now();
- if (m_MainMachine.state() != IDLE) {
- if (!isInIgnoreList(msg->header.frame_id)) {
- m_max_move_distances[msg->header.frame_id] =
- map_tools::get_max_move_distance(
- map_tools::laser_msg_to_points(msg, m_transform_listener,
- "/base_link"),
- m_min_x, m_min_y);
- } else {
- m_max_move_distances[msg->header.frame_id] = 99;
- }
- calcMaxMoveDist();
+ const sensor_msgs::LaserScan::ConstPtr& msg)
+{
+ m_scan_map[msg->header.frame_id] = msg;
+ m_last_laser_time = ros::Time::now();
+ if (m_MainMachine.state() != IDLE)
+ {
+ if (!isInIgnoreList(msg->header.frame_id))
+ {
+ m_max_move_distances[msg->header.frame_id] =
+ map_tools::get_max_move_distance(
+ map_tools::laser_msg_to_points(msg, m_transform_listener, "/base_"
+ "link"),
+ m_min_x, m_min_y);
+ }
+ else
+ {
+ m_max_move_distances[msg->header.frame_id] = 99;
}
+ calcMaxMoveDist();
+ }
}
-void HomerNavigationNode::initNewTarget() {
- m_initial_path_reaches_target = false;
- m_avoided_collision = false;
- m_new_target = true;
+void HomerNavigationNode::initNewTarget()
+{
+ m_initial_path_reaches_target = false;
+ m_avoided_collision = false;
+ m_new_target = true;
}
void HomerNavigationNode::startNavigationCallback(
- const homer_mapnav_msgs::StartNavigation::ConstPtr& msg) {
- m_target_point = msg->goal.position;
- m_target_orientation = tf::getYaw(msg->goal.orientation);
- m_desired_distance =
- msg->distance_to_target < 0.1 ? 0.1 : msg->distance_to_target;
- m_skip_final_turn = msg->skip_final_turn;
- m_fast_path_planning = msg->fast_planning;
- m_target_name = "";
-
- ROS_INFO_STREAM("Navigating to target "
- << m_target_point.x << ", " << m_target_point.y
- << "\nTarget orientation: " << m_target_orientation
- << "Desired distance to target: " << m_desired_distance);
- initNewTarget();
- startNavigation();
+ const homer_mapnav_msgs::StartNavigation::ConstPtr& msg)
+{
+ m_target_point = msg->goal.position;
+ m_target_orientation = tf::getYaw(msg->goal.orientation);
+ m_desired_distance =
+ msg->distance_to_target < 0.1 ? 0.1 : msg->distance_to_target;
+ m_skip_final_turn = msg->skip_final_turn;
+ m_fast_path_planning = msg->fast_planning;
+ m_target_name = "";
+
+ ROS_INFO_STREAM("Navigating to target "
+ << m_target_point.x << ", " << m_target_point.y
+ << "\nTarget orientation: " << m_target_orientation
+ << "Desired distance to target: " << m_desired_distance);
+ initNewTarget();
+ startNavigation();
}
void HomerNavigationNode::moveBaseSimpleGoalCallback(
- const geometry_msgs::PoseStamped::ConstPtr& msg) {
- if (msg->header.frame_id != "map") {
- tf::StampedTransform transform;
- if (m_transform_listener.waitForTransform("map", msg->header.frame_id,
- msg->header.stamp,
- ros::Duration(1.0))) {
- try {
- m_transform_listener.lookupTransform(
- "map", msg->header.frame_id, msg->header.stamp, transform);
- tf::Vector3 targetPos(msg->pose.position.x,
- msg->pose.position.y,
- msg->pose.position.z);
- targetPos = transform * targetPos;
- m_target_point.x = targetPos.getX();
- m_target_point.y = targetPos.getY();
- m_target_orientation = tf::getYaw(
- transform * tf::Quaternion(msg->pose.orientation.x,
- msg->pose.orientation.y,
- msg->pose.orientation.z,
- msg->pose.orientation.w));
- } catch (tf::TransformException ex) {
- ROS_ERROR_STREAM(ex.what());
- return;
- }
- } else {
- try {
- m_transform_listener.lookupTransform(
- "map", msg->header.frame_id, ros::Time(0), transform);
- tf::Vector3 targetPos(msg->pose.position.x,
- msg->pose.position.y,
- msg->pose.position.z);
- targetPos = transform * targetPos;
- m_target_point.x = targetPos.getX();
- m_target_point.y = targetPos.getY();
- m_target_orientation = tf::getYaw(
- transform * tf::Quaternion(msg->pose.orientation.x,
- msg->pose.orientation.y,
- msg->pose.orientation.z,
- msg->pose.orientation.w));
- } catch (tf::TransformException ex) {
- ROS_ERROR_STREAM(ex.what());
- return;
- }
- }
- } else {
- m_target_point = msg->pose.position;
- m_target_orientation = tf::getYaw(msg->pose.orientation);
+ const geometry_msgs::PoseStamped::ConstPtr& msg)
+{
+ if (msg->header.frame_id != "map")
+ {
+ tf::StampedTransform transform;
+ if (m_transform_listener.waitForTransform(
+ "map", msg->header.frame_id, msg->header.stamp, ros::Duration(1.0)))
+ {
+ try
+ {
+ m_transform_listener.lookupTransform("map", msg->header.frame_id,
+ msg->header.stamp, transform);
+ tf::Vector3 targetPos(msg->pose.position.x, msg->pose.position.y,
+ msg->pose.position.z);
+ targetPos = transform * targetPos;
+ m_target_point.x = targetPos.getX();
+ m_target_point.y = targetPos.getY();
+ m_target_orientation = tf::getYaw(
+ transform *
+ tf::Quaternion(msg->pose.orientation.x, msg->pose.orientation.y,
+ msg->pose.orientation.z, msg->pose.orientation.w));
+ }
+ catch (tf::TransformException ex)
+ {
+ ROS_ERROR_STREAM(ex.what());
+ return;
+ }
+ }
+ else
+ {
+ try
+ {
+ m_transform_listener.lookupTransform("map", msg->header.frame_id,
+ ros::Time(0), transform);
+ tf::Vector3 targetPos(msg->pose.position.x, msg->pose.position.y,
+ msg->pose.position.z);
+ targetPos = transform * targetPos;
+ m_target_point.x = targetPos.getX();
+ m_target_point.y = targetPos.getY();
+ m_target_orientation = tf::getYaw(
+ transform *
+ tf::Quaternion(msg->pose.orientation.x, msg->pose.orientation.y,
+ msg->pose.orientation.z, msg->pose.orientation.w));
+ }
+ catch (tf::TransformException ex)
+ {
+ ROS_ERROR_STREAM(ex.what());
+ return;
+ }
}
- m_desired_distance = 0.1;
- m_skip_final_turn = false;
- m_fast_path_planning = false;
-
- ROS_INFO_STREAM("Navigating to target via Move Base Simple x: "
- << m_target_point.x << ", y: " << m_target_point.y
- << "\nTarget orientation: " << m_target_orientation
- << " Desired distance to target: " << m_desired_distance
- << "\nframe_id: " << msg->header.frame_id);
- initNewTarget();
- startNavigation();
+ }
+ else
+ {
+ m_target_point = msg->pose.position;
+ m_target_orientation = tf::getYaw(msg->pose.orientation);
+ }
+ m_desired_distance = 0.1;
+ m_skip_final_turn = false;
+ m_fast_path_planning = false;
+
+ ROS_INFO_STREAM("Navigating to target via Move Base Simple x: "
+ << m_target_point.x << ", y: " << m_target_point.y
+ << "\nTarget orientation: " << m_target_orientation
+ << " Desired distance to target: " << m_desired_distance
+ << "\nframe_id: " << msg->header.frame_id);
+ initNewTarget();
+ startNavigation();
}
void HomerNavigationNode::navigateToPOICallback(
- const homer_mapnav_msgs::NavigateToPOI::ConstPtr& msg) {
- homer_mapnav_msgs::GetPointsOfInterest srv;
- m_get_POIs_client.call(srv);
- std::vector<homer_mapnav_msgs::PointOfInterest>::iterator it;
- for (it = srv.response.poi_list.pois.begin();
- it != srv.response.poi_list.pois.end(); ++it) {
- if (it->name == msg->poi_name) {
- m_target_point = it->pose.position;
- m_target_orientation = tf::getYaw(it->pose.orientation);
- m_desired_distance =
- msg->distance_to_target < 0.1 ? 0.1 : msg->distance_to_target;
- m_skip_final_turn = msg->skip_final_turn;
- m_fast_path_planning = false;
- m_target_name = msg->poi_name;
- m_stop_before_obstacle = msg->stop_before_obstacle;
-
- ROS_INFO_STREAM("Navigating to target "
- << m_target_point.x << ", " << m_target_point.y
- << "\nTarget orientation: " << m_target_orientation
- << "Desired distance to target: "
- << m_desired_distance);
- initNewTarget();
- startNavigation();
- return;
- }
+ const homer_mapnav_msgs::NavigateToPOI::ConstPtr& msg)
+{
+ homer_mapnav_msgs::GetPointsOfInterest srv;
+ m_get_POIs_client.call(srv);
+ std::vector<homer_mapnav_msgs::PointOfInterest>::iterator it;
+ for (it = srv.response.poi_list.pois.begin();
+ it != srv.response.poi_list.pois.end(); ++it)
+ {
+ if (it->name == msg->poi_name)
+ {
+ m_target_point = it->pose.position;
+ m_target_orientation = tf::getYaw(it->pose.orientation);
+ m_desired_distance =
+ msg->distance_to_target < 0.1 ? 0.1 : msg->distance_to_target;
+ m_skip_final_turn = msg->skip_final_turn;
+ m_fast_path_planning = false;
+ m_target_name = msg->poi_name;
+ m_stop_before_obstacle = msg->stop_before_obstacle;
+
+ ROS_INFO_STREAM("Navigating to target "
+ << m_target_point.x << ", " << m_target_point.y
+ << "\nTarget orientation: " << m_target_orientation
+ << "Desired distance to target: " << m_desired_distance);
+ initNewTarget();
+ startNavigation();
+ return;
}
+ }
- ROS_ERROR_STREAM("No point of interest with name '"
- << msg->poi_name << "' found in current poi list");
- sendTargetUnreachableMsg(homer_mapnav_msgs::TargetUnreachable::UNKNOWN);
+ ROS_ERROR_STREAM("No point of interest with name '"
+ << msg->poi_name << "' found in current poi list");
+ sendTargetUnreachableMsg(homer_mapnav_msgs::TargetUnreachable::UNKNOWN);
}
void HomerNavigationNode::stopNavigationCallback(
- const std_msgs::Empty::ConstPtr& msg) {
- (void) msg;
- ROS_INFO_STREAM("Stopping navigation.");
- m_MainMachine.setState(IDLE);
- stopRobot();
-
- m_waypoints.clear();
+ const std_msgs::Empty::ConstPtr& msg)
+{
+ (void)msg;
+ ROS_INFO_STREAM("Stopping navigation.");
+ m_MainMachine.setState(IDLE);
+ stopRobot();
+
+ m_waypoints.clear();
}
void HomerNavigationNode::unknownThresholdCallback(
- const std_msgs::Int8::ConstPtr& msg) {
- if (m_explorer) {
- m_explorer->setUnknownThreshold(static_cast<int>(msg->data));
- }
+ const std_msgs::Int8::ConstPtr& msg)
+{
+ if (m_explorer)
+ {
+ m_explorer->setUnknownThreshold(static_cast<int>(msg->data));
+ }
}
-int main(int argc, char** argv) {
- ros::init(argc, argv, "homer_navigation");
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "homer_navigation");
- HomerNavigationNode node;
+ HomerNavigationNode node;
- ros::Rate rate(50);
+ ros::Rate rate(50);
- while (ros::ok()) {
- ros::spinOnce();
- node.idleProcess();
- rate.sleep();
- }
+ while (ros::ok())
+ {
+ ros::spinOnce();
+ node.idleProcess();
+ rate.sleep();
+ }
- return 0;
+ return 0;
}