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/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_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 e04970da5bed78abd754dcb44dbde96fb74dbc3c..a9ab121b73136fba405304be68987e321f125e40 100644
--- a/homer_mapping/include/homer_mapping/ParticleFilter/ParticleFilter.h
+++ b/homer_mapping/include/homer_mapping/ParticleFilter/ParticleFilter.h
@@ -1,11 +1,11 @@
#ifndef PARTICLEFILTER_H
#define PARTICLEFILTER_H
+#include <homer_nav_libs/Math/Transformation2D.h>
#include <limits.h>
#include <omp.h>
-#include <cmath>
-#include <homer_nav_libs/Math/Transformation2D.h>
#include <sensor_msgs/LaserScan.h>
+#include <cmath>
#include <iostream>
#include <ros/ros.h>
@@ -32,297 +32,328 @@ const float MIN_EFFECTIVE_PARTICLE_WEIGHT = 0.2;
* @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(Transformation2D odoTrans) = 0;
-
- /**
- * This method has to be implemented in sub-classes. It is used to determine
- * the weight of each particle.
- */
- virtual void measure(sensor_msgs::LaserScanPtr laserData) = 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(Transformation2D odoTrans) = 0;
+
+ /**
+ * This method has to be implemented in sub-classes. It is used to determine
+ * the weight of each particle.
+ */
+ virtual void measure(sensor_msgs::LaserScanPtr laserData) = 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) {
- // initialize particle lists
- m_CurrentList = new ParticleType*[particleNum];
- m_LastList = new ParticleType*[particleNum];
+ParticleFilter<ParticleType>::ParticleFilter(int particleNum)
+{
+ // initialize particle lists
+ m_CurrentList = new ParticleType*[particleNum];
+ m_LastList = new ParticleType*[particleNum];
- // initialize random number generator
- random01(time(0));
+ // initialize random number generator
+ random01(time(0));
- m_ParticleNum = particleNum;
+ m_ParticleNum = particleNum;
}
template <class ParticleType>
-ParticleFilter<ParticleType>::~ParticleFilter() {
- if (m_CurrentList) {
- delete[] m_CurrentList;
- m_CurrentList = 0;
- }
- if (m_LastList) {
- delete[] m_LastList;
- m_LastList = 0;
- }
+ParticleFilter<ParticleType>::~ParticleFilter()
+{
+ if (m_CurrentList)
+ {
+ delete[] m_CurrentList;
+ m_CurrentList = 0;
+ }
+ if (m_LastList)
+ {
+ delete[] m_LastList;
+ m_LastList = 0;
+ }
}
template <class ParticleType>
-int ParticleFilter<ParticleType>::getParticleNum() {
- return m_ParticleNum;
+int ParticleFilter<ParticleType>::getParticleNum()
+{
+ return m_ParticleNum;
}
template <class ParticleType>
-double ParticleFilter<ParticleType>::random01(unsigned long init) const {
- static unsigned long n;
- if (init > 0) {
- n = init;
- }
- n = 1664525 * n + 1013904223;
- // create double from unsigned long
- return (double)(n / 2) / (double)LONG_MAX;
+double ParticleFilter<ParticleType>::random01(unsigned long init) const
+{
+ static unsigned long n;
+ if (init > 0)
+ {
+ n = init;
+ }
+ n = 1664525 * n + 1013904223;
+ // create double from unsigned long
+ return (double)(n / 2) / (double)LONG_MAX;
}
template <class ParticleType>
-void ParticleFilter<ParticleType>::sort(int indexLeft, int indexRight) {
- // SOMETHING LEFT TO SORT?
- if (indexLeft >= indexRight) {
- // ready!
- return;
+void ParticleFilter<ParticleType>::sort(int indexLeft, int indexRight)
+{
+ // SOMETHING LEFT TO SORT?
+ if (indexLeft >= indexRight)
+ {
+ // ready!
+ return;
+ }
+
+ // CREATE PARTITION
+ int le = indexLeft;
+ int ri = indexRight;
+ int first = le;
+ int pivot = ri--;
+ while (le <= ri)
+ {
+ // skip from left
+ while (m_CurrentList[le]->getWeight() > m_CurrentList[pivot]->getWeight())
+ {
+ le++;
}
-
- // CREATE PARTITION
- int le = indexLeft;
- int ri = indexRight;
- int first = le;
- int pivot = ri--;
- while (le <= ri) {
- // skip from left
- while (m_CurrentList[le]->getWeight() >
- m_CurrentList[pivot]->getWeight()) {
- le++;
- }
- // skip from right
- while ((ri >= first) && (m_CurrentList[ri]->getWeight() <=
- m_CurrentList[pivot]->getWeight())) {
- ri--;
- }
- // now we have two elements to swap
- if (le < ri) {
- // swap
- ParticleType* temp = m_CurrentList[le];
- m_CurrentList[le] = m_CurrentList[ri];
- m_CurrentList[ri] = temp;
- le++;
- }
+ // skip from right
+ while ((ri >= first) && (m_CurrentList[ri]->getWeight() <=
+ m_CurrentList[pivot]->getWeight()))
+ {
+ ri--;
}
-
- if (le != pivot) {
- // swap
- ParticleType* temp = m_CurrentList[le];
- m_CurrentList[le] = m_CurrentList[pivot];
- m_CurrentList[pivot] = temp;
+ // now we have two elements to swap
+ if (le < ri)
+ {
+ // swap
+ ParticleType* temp = m_CurrentList[le];
+ m_CurrentList[le] = m_CurrentList[ri];
+ m_CurrentList[ri] = temp;
+ le++;
}
-
- // sort left side
- sort(indexLeft, le - 1);
- // sort right side
- sort(le + 1, indexRight);
+ }
+
+ if (le != pivot)
+ {
+ // swap
+ ParticleType* temp = m_CurrentList[le];
+ m_CurrentList[le] = m_CurrentList[pivot];
+ m_CurrentList[pivot] = temp;
+ }
+
+ // sort left side
+ sort(indexLeft, le - 1);
+ // sort right side
+ sort(le + 1, indexRight);
}
template <class ParticleType>
-void ParticleFilter<ParticleType>::normalize() {
- float weightSum = 0.0;
- 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) {
- // calculating parallel on 8 threats
- omp_set_num_threads(8);
- int i = 0;
- // #pragma omp parallel for
- for (i = 0; i < m_ParticleNum; i++) {
- float newWeight = m_CurrentList[i]->getWeight() / weightSum;
- m_CurrentList[i]->setWeight(newWeight);
- }
- } else {
- ROS_WARN_STREAM("Particle weights VERY small: " << weightSum << ". Got "
- << m_ParticleNum
- << " particles.");
+void ParticleFilter<ParticleType>::normalize()
+{
+ float weightSum = 0.0;
+ 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)
+ {
+ // calculating parallel on 8 threats
+ omp_set_num_threads(8);
+ int i = 0;
+ // #pragma omp parallel for
+ for (i = 0; i < m_ParticleNum; i++)
+ {
+ float newWeight = m_CurrentList[i]->getWeight() / weightSum;
+ m_CurrentList[i]->setWeight(newWeight);
}
+ }
+ else
+ {
+ ROS_WARN_STREAM("Particle weights VERY small: "
+ << weightSum << ". Got " << m_ParticleNum << " particles.");
+ }
}
template <class ParticleType>
-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();
- }
- *m_CurrentList[i] = *m_LastList[drawIndex];
+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();
+ }
+ *m_CurrentList[i] = *m_LastList[drawIndex];
+ }
}
template <class ParticleType>
-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++) {
- float weight = m_CurrentList[i]->getWeight();
- squareSum += weight * weight;
- }
- return static_cast<int>(1.0f / squareSum);
+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++)
+ {
+ 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 effectiveParticleNum = 0;
- for (int i = 0; i < m_ParticleNum; i++) {
- if (m_CurrentList[i]->getWeight() > MIN_EFFECTIVE_PARTICLE_WEIGHT) {
- effectiveParticleNum++;
- }
+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++;
}
- return effectiveParticleNum;
+ }
+ return effectiveParticleNum;
}
template <class ParticleType>
-ParticleType* ParticleFilter<ParticleType>::getBestParticle() const {
- return m_CurrentList[0];
+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 2f2d2939ef6b82af14c938a7851c712e48f8d396..7f6dd0fc16f4b5bdb870cf7089d9c9c4556b312c 100644
--- a/homer_mapping/include/homer_mapping/ParticleFilter/SlamFilter.h
+++ b/homer_mapping/include/homer_mapping/ParticleFilter/SlamFilter.h
@@ -40,266 +40,266 @@ class OccupancyMap;
* @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.
- */
- void filter(Transformation2D trans,
- sensor_msgs::LaserScanConstPtr laserData);
-
- /**
- * @return The Pose of the most important particle (particle with highest
- * weight).
- */
- Pose getLikeliestPose();
-
- /**
- * 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;
-
- 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 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.
- * @param[out] poseVarianceT The variance of particle poses in T rotation.
- */
- void getPoseVariances(int particleNum, float& poseVarianceX,
- float& poseVarianceY, float& poseVarianceT);
-
- /**
- * 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(Transformation2D odoTrans);
-
- /**
- * This method weightens each particle according to the given laser
- * measurement in m_LaserData.
- */
- void measure(sensor_msgs::LaserScanPtr laserData);
-
- /**
- * For weightening the particles, the filter needs a map.
- * This variable holds a pointer to a map.
- * @see OccupancyMap
- */
- OccupancyMap* m_OccupancyMap;
-
-
- /**
- * 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;
-
- /**
- * 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;
-
- /**
- * Time stamp of the last particle filter step
- */
-
- 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.
+ */
+ void filter(Transformation2D trans, sensor_msgs::LaserScanConstPtr laserData);
+
+ /**
+ * @return The Pose of the most important particle (particle with highest
+ * weight).
+ */
+ Pose getLikeliestPose();
+
+ /**
+ * 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;
+
+ 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 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.
+ * @param[out] poseVarianceT The variance of particle poses in T rotation.
+ */
+ void getPoseVariances(int particleNum, float& poseVarianceX,
+ float& poseVarianceY, float& poseVarianceT);
+
+ /**
+ * 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(Transformation2D odoTrans);
+
+ /**
+ * This method weightens each particle according to the given laser
+ * measurement in m_LaserData.
+ */
+ void measure(sensor_msgs::LaserScanPtr laserData);
+
+ /**
+ * For weightening the particles, the filter needs a map.
+ * This variable holds a pointer to a map.
+ * @see OccupancyMap
+ */
+ OccupancyMap* m_OccupancyMap;
+
+ /**
+ * 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;
+
+ /**
+ * 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;
+
+ /**
+ * Time stamp of the last particle filter step
+ */
+
+ 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 8cc3c11c9e275ce8d23c27d5debf11dbc6ec1872..63d50746bb39f7619fd69b2ecf23944a364f56d7 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>
#include <homer_nav_libs/Math/Pose.h>
@@ -14,7 +14,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
@@ -22,54 +23,52 @@
*/
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 );
- void setRobotPose (Pose pose);
+ /**
+ * 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 );
- Pose getRobotPose ();
+ /**
+ * 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);
+ void setRobotPose(Pose pose);
+ /**
+ * 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);
+ Pose getRobotPose();
- 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/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/SlamParticle.cpp b/homer_mapping/src/ParticleFilter/SlamParticle.cpp
index 3d564cde000959b43ecf3bf6b53454a604ac95a4..d88fa0dfb8dd50ffcac377f3d908e26bb68601b7 100644
--- a/homer_mapping/src/ParticleFilter/SlamParticle.cpp
+++ b/homer_mapping/src/ParticleFilter/SlamParticle.cpp
@@ -1,41 +1,47 @@
#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::setRobotPose(Pose pose){
- m_RobotPositionX = pose.x();
- m_RobotPositionY = pose.y();
- m_RobotOrientation = pose.theta();
+void SlamParticle::setRobotPose(Pose pose)
+{
+ m_RobotPositionX = pose.x();
+ m_RobotPositionY = pose.y();
+ m_RobotOrientation = pose.theta();
}
-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;
}
-Pose SlamParticle::getRobotPose(){
- return Pose(m_RobotPositionX, m_RobotPositionY, m_RobotOrientation);
+Pose SlamParticle::getRobotPose()
+{
+ return Pose(m_RobotPositionX, m_RobotPositionY, m_RobotOrientation);
}
-
-
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;
}