From 33e162db58de2018c09f5cacfed69e54d2652e9f Mon Sep 17 00:00:00 2001
From: Florian Polster <fpolster@uni-koblenz.de>
Date: Thu, 9 Feb 2017 22:36:35 +0100
Subject: [PATCH] latching enabled
---
homer_map_manager/src/Managers/MapManager.cpp | 361 +++++++++--------
homer_map_manager/src/Managers/PoiManager.cpp | 164 ++++----
homer_map_manager/src/Managers/RoiManager.cpp | 381 ++++++++----------
3 files changed, 440 insertions(+), 466 deletions(-)
diff --git a/homer_map_manager/src/Managers/MapManager.cpp b/homer_map_manager/src/Managers/MapManager.cpp
index 6368ff30..bbf2301d 100644
--- a/homer_map_manager/src/Managers/MapManager.cpp
+++ b/homer_map_manager/src/Managers/MapManager.cpp
@@ -1,219 +1,226 @@
#include <homer_map_manager/Managers/MapManager.h>
#include <omp.h>
-MapManager::MapManager(ros::NodeHandle* nh)
-{
- m_MapPublisher = nh->advertise<nav_msgs::OccupancyGrid>("/map", 1);
-
- m_map_layers.push_back( homer_mapnav_msgs::MapLayers::SLAM_LAYER);
- m_map_layers.push_back( homer_mapnav_msgs::MapLayers::KINECT_LAYER);
- m_map_layers.push_back( homer_mapnav_msgs::MapLayers::RAPID_MAP);
- m_map_layers.push_back( homer_mapnav_msgs::MapLayers::MASKING_LAYER);
- m_laser_layers.push_back( homer_mapnav_msgs::MapLayers::HOKUYO_FRONT);
- m_laser_layers.push_back( homer_mapnav_msgs::MapLayers::SICK_LAYER);
-
- //enable all map layers
-
- for(int i = 0; i< m_map_layers.size(); i++)
- {
- m_MapVisibility[m_map_layers[i]] = true;
+MapManager::MapManager(ros::NodeHandle *nh) {
+ m_MapPublisher = nh->advertise<nav_msgs::OccupancyGrid>("/map", 1, true);
+
+ m_map_layers.push_back(homer_mapnav_msgs::MapLayers::SLAM_LAYER);
+ m_map_layers.push_back(homer_mapnav_msgs::MapLayers::KINECT_LAYER);
+ m_map_layers.push_back(homer_mapnav_msgs::MapLayers::RAPID_MAP);
+ m_map_layers.push_back(homer_mapnav_msgs::MapLayers::MASKING_LAYER);
+ m_laser_layers.push_back(homer_mapnav_msgs::MapLayers::HOKUYO_FRONT);
+ m_laser_layers.push_back(homer_mapnav_msgs::MapLayers::SICK_LAYER);
+
+ // enable all map layers
+
+ for (int i = 0; i < m_map_layers.size(); i++) {
+ m_MapVisibility[m_map_layers[i]] = true;
}
-
- for(int i = 0; i< m_laser_layers.size(); i++)
- {
- m_MapVisibility[m_laser_layers[i]] = true;
+
+ for (int i = 0; i < m_laser_layers.size(); i++) {
+ m_MapVisibility[m_laser_layers[i]] = true;
}
-
- try
- {
- m_TransformListener.waitForTransform("/base_link", "/laser", ros::Time(0), ros::Duration(3));
- m_TransformListener.lookupTransform ("/base_link", "/laser", ros::Time(0), m_sick_transform);
- m_TransformListener.lookupTransform ("/base_link", "/hokuyo_front", ros::Time(0), m_hokuyo_transform);
+
+ try {
+ m_TransformListener.waitForTransform("/base_link", "/laser",
+ ros::Time(0), ros::Duration(3));
+ m_TransformListener.lookupTransform("/base_link", "/laser",
+ ros::Time(0), m_sick_transform);
+ m_TransformListener.lookupTransform("/base_link", "/hokuyo_front",
+ ros::Time(0), m_hokuyo_transform);
m_got_transform = true;
+ } catch (tf::TransformException ex) {
+ ROS_ERROR("%s", ex.what());
+ m_got_transform = false;
}
- catch (tf::TransformException ex)
- {
- ROS_ERROR("%s",ex.what());
- m_got_transform = false;
- }
- geometry_msgs::PoseStamped pose;
+ geometry_msgs::PoseStamped pose;
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.position.z = 0;
- pose.pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
-
- m_pose = boost::make_shared<geometry_msgs::PoseStamped>(pose) ;
-
-}
+ pose.pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
-MapManager::~MapManager()
-{
+ m_pose = boost::make_shared<geometry_msgs::PoseStamped>(pose);
}
-void MapManager::updateMapLayer(int type, nav_msgs::OccupancyGrid::ConstPtr layer)
-{
- m_MapLayers[type] = layer;
- if(type == homer_mapnav_msgs::MapLayers::SLAM_LAYER)
- {
- sendMergedMap();
+MapManager::~MapManager() {}
+
+void MapManager::updateMapLayer(int type,
+ nav_msgs::OccupancyGrid::ConstPtr layer) {
+ m_MapLayers[type] = layer;
+ if (type == homer_mapnav_msgs::MapLayers::SLAM_LAYER) {
+ sendMergedMap();
}
}
-void MapManager::clearMapLayers()
-{
- m_MapLayers.clear();
-}
+void MapManager::clearMapLayers() { m_MapLayers.clear(); }
-void MapManager::toggleMapVisibility(int type, bool state)
-{
+void MapManager::toggleMapVisibility(int type, bool state) {
ROS_INFO_STREAM("MapManager: " << type << ": " << state);
m_MapVisibility[type] = state;
}
-
-void MapManager::updateLaser(int layer, const sensor_msgs::LaserScan::ConstPtr &msg)
-{
- m_laserLayers[layer] = msg;
+
+void MapManager::updateLaser(int layer,
+ const sensor_msgs::LaserScan::ConstPtr &msg) {
+ m_laserLayers[layer] = msg;
}
-nav_msgs::OccupancyGrid::ConstPtr MapManager::getMapLayer(int type)
-{
- if(m_MapLayers.find(type) == m_MapLayers.end())
+nav_msgs::OccupancyGrid::ConstPtr MapManager::getMapLayer(int type) {
+ if (m_MapLayers.find(type) == m_MapLayers.end())
return nav_msgs::OccupancyGrid::ConstPtr();
return m_MapLayers[type];
}
/**
- * Sends the SLAM map (OccupancyGrid) and (if available and enabled) other merged map layers to the gui node
+ * Sends the SLAM map (OccupancyGrid) and (if available and enabled) other
+ * merged map layers to the gui node
*
*/
-void MapManager::sendMergedMap()
-{
-
- if ( m_MapLayers.find(homer_mapnav_msgs::MapLayers::SLAM_LAYER) == m_MapLayers.end() )
- {
- ROS_DEBUG_STREAM( "SLAM map is missing!" );
- return;
+void MapManager::sendMergedMap() {
+ if (m_MapLayers.find(homer_mapnav_msgs::MapLayers::SLAM_LAYER) ==
+ m_MapLayers.end()) {
+ ROS_DEBUG_STREAM("SLAM map is missing!");
+ return;
}
int k;
- nav_msgs::OccupancyGrid mergedMap( *(m_MapLayers[homer_mapnav_msgs::MapLayers::SLAM_LAYER]));
- //bake maps
- for(int j = 1; j < m_map_layers.size(); j++)
- {
- if ( m_MapLayers.find(m_map_layers[j]) != m_MapLayers.end()
- && m_MapVisibility[m_map_layers[j]])
- {
- //calculating parallel on 8 threads
+ nav_msgs::OccupancyGrid mergedMap(
+ *(m_MapLayers[homer_mapnav_msgs::MapLayers::SLAM_LAYER]));
+ // bake maps
+ for (int j = 1; j < m_map_layers.size(); j++) {
+ if (m_MapLayers.find(m_map_layers[j]) != m_MapLayers.end() &&
+ m_MapVisibility[m_map_layers[j]]) {
+ // calculating parallel on 8 threads
omp_set_num_threads(8);
- size_t mapsize = m_MapLayers[m_map_layers[j]]->info.height * m_MapLayers[m_map_layers[j]]->info.width;
- const std::vector<signed char> *tempdata = &m_MapLayers[m_map_layers[j]]->data;
+ size_t mapsize = m_MapLayers[m_map_layers[j]]->info.height *
+ m_MapLayers[m_map_layers[j]]->info.width;
+ const std::vector<signed char> *tempdata =
+ &m_MapLayers[m_map_layers[j]]->data;
const int frei = homer_mapnav_msgs::ModifyMap::FREE;
signed char currentvalue = 0;
- #pragma omp parallel for private(currentvalue) shared(mapsize, tempdata, mergedMap)
- for (int i = 0; i < mapsize; i++ )
- {
+#pragma omp parallel for private(currentvalue) shared(mapsize, tempdata, \
+ mergedMap)
+ for (int i = 0; i < mapsize; i++) {
currentvalue = tempdata->at(i);
- if(currentvalue > 50 || currentvalue == frei)
- {
+ if (currentvalue > 50 || currentvalue == frei) {
mergedMap.data[i] = currentvalue;
- }
- }
- }
+ }
+ }
+ }
+ }
+ // bake Lasers
+
+ try {
+ int data;
+ if (m_got_transform) {
+ for (int j = 0; j < m_laser_layers.size(); j++) {
+ if (m_laserLayers.find(m_laser_layers[j]) !=
+ m_laserLayers.end() &&
+ m_MapVisibility[m_laser_layers[j]]) {
+ if (m_laser_layers[j] ==
+ homer_mapnav_msgs::MapLayers::SICK_LAYER) {
+ data = homer_mapnav_msgs::ModifyMap::BLOCKED;
+ } else if (m_laser_layers[j] ==
+ homer_mapnav_msgs::MapLayers::HOKUYO_BACK) {
+ data = homer_mapnav_msgs::ModifyMap::HOKUYO;
+ } else if (m_laser_layers[j] ==
+ homer_mapnav_msgs::MapLayers::HOKUYO_FRONT) {
+ data = homer_mapnav_msgs::ModifyMap::HOKUYO;
+ }
+ tf::StampedTransform pose_transform;
+ m_TransformListener.waitForTransform(
+ "/map", "/base_link",
+ m_laserLayers[m_laser_layers[j]]->header.stamp,
+ ros::Duration(0.09));
+ m_TransformListener.lookupTransform(
+ "/map", "/base_link",
+ m_laserLayers[m_laser_layers[j]]->header.stamp,
+ pose_transform);
+
+ // pose_transform.setTransform(Transform);
+
+ for (int i = 0;
+ i < m_laserLayers[m_laser_layers[j]]->ranges.size();
+ i++) {
+ if (m_laserLayers[m_laser_layers[j]]->ranges[i] <
+ m_laserLayers[m_laser_layers[j]]->range_max &&
+ m_laserLayers[m_laser_layers[j]]->ranges[i] >
+ m_laserLayers[m_laser_layers[j]]->range_min) {
+ float alpha =
+ m_laserLayers[m_laser_layers[j]]->angle_min +
+ i *
+ m_laserLayers[m_laser_layers[j]]
+ ->angle_increment;
+ geometry_msgs::Point point;
+ tf::Vector3 pin;
+ tf::Vector3 pout;
+ pin.setX(
+ cos(alpha) *
+ m_laserLayers[m_laser_layers[j]]->ranges[i]);
+ pin.setY(
+ sin(alpha) *
+ m_laserLayers[m_laser_layers[j]]->ranges[i]);
+ pin.setZ(0);
+
+ if (m_laser_layers[j] ==
+ homer_mapnav_msgs::MapLayers::SICK_LAYER) {
+ pout = pose_transform * m_sick_transform * pin;
+ } else if (m_laser_layers[j] ==
+ homer_mapnav_msgs::MapLayers::
+ HOKUYO_FRONT) {
+ pout =
+ pose_transform * m_hokuyo_transform * pin;
+ }
+ point.x = pout.x();
+ point.y = pout.y();
+ point.z = 0;
+
+ Eigen::Vector2i map_point = map_tools::toMapCoords(
+ point,
+ m_MapLayers
+ [homer_mapnav_msgs::MapLayers::SLAM_LAYER]
+ ->info.origin,
+ m_MapLayers
+ [homer_mapnav_msgs::MapLayers::SLAM_LAYER]
+ ->info.resolution);
+ k = map_point.y() *
+ m_MapLayers[homer_mapnav_msgs::MapLayers::
+ SLAM_LAYER]
+ ->info.width +
+ map_point.x();
+ if (k < 0 ||
+ k > m_MapLayers[homer_mapnav_msgs::MapLayers::
+ SLAM_LAYER]
+ ->data.size()) {
+ continue;
+ } else {
+ mergedMap.data[k] = data;
+ }
+ }
+ }
+ }
+ }
+ } else {
+ try {
+ if (m_TransformListener.waitForTransform("/base_link", "/laser",
+ ros::Time(0),
+ ros::Duration(0.1))) {
+ m_TransformListener.lookupTransform(
+ "/base_link", "/laser", ros::Time(0), m_sick_transform);
+ m_TransformListener.lookupTransform(
+ "/base_link", "/hokuyo_front", ros::Time(0),
+ m_hokuyo_transform);
+ m_got_transform = true;
+ }
+ } catch (tf::TransformException ex) {
+ ROS_ERROR("%s", ex.what());
+ m_got_transform = false;
+ }
+ }
+ } catch (tf::TransformException ex) {
+ ROS_ERROR_STREAM(ex.what());
}
- //bake Lasers
-
- try
- {
- int data ;
- if(m_got_transform)
- {
- for(int j = 0; j < m_laser_layers.size(); j++)
- {
- if ( m_laserLayers.find(m_laser_layers[j]) != m_laserLayers.end()
- && m_MapVisibility[m_laser_layers[j]])
- {
- if(m_laser_layers[j] == homer_mapnav_msgs::MapLayers::SICK_LAYER)
- {
- data = homer_mapnav_msgs::ModifyMap::BLOCKED;
- }
- else if(m_laser_layers[j] == homer_mapnav_msgs::MapLayers::HOKUYO_BACK)
- {
- data = homer_mapnav_msgs::ModifyMap::HOKUYO;
- }
- else if(m_laser_layers[j] == homer_mapnav_msgs::MapLayers::HOKUYO_FRONT)
- {
- data = homer_mapnav_msgs::ModifyMap::HOKUYO;
- }
- tf::StampedTransform pose_transform;
- m_TransformListener.waitForTransform("/map", "/base_link", m_laserLayers[m_laser_layers[j]]->header.stamp, ros::Duration(0.09));
- m_TransformListener.lookupTransform("/map", "/base_link", m_laserLayers[m_laser_layers[j]]->header.stamp , pose_transform);
-
- // pose_transform.setTransform(Transform);
-
- for ( int i = 0; i < m_laserLayers[m_laser_layers[j]]->ranges.size(); i++ )
- {
- if(m_laserLayers[m_laser_layers[j]]->ranges[i] < m_laserLayers[m_laser_layers[j]]->range_max &&
- m_laserLayers[m_laser_layers[j]]->ranges[i] > m_laserLayers[m_laser_layers[j]]->range_min)
- {
- float alpha = m_laserLayers[m_laser_layers[j]]->angle_min + i * m_laserLayers[m_laser_layers[j]]->angle_increment;
- geometry_msgs::Point point;
- tf::Vector3 pin;
- tf::Vector3 pout;
- pin.setX( cos(alpha) * m_laserLayers[m_laser_layers[j]]->ranges[i]);
- pin.setY( sin(alpha) * m_laserLayers[m_laser_layers[j]]->ranges[i]);
- pin.setZ(0);
-
- if(m_laser_layers[j] == homer_mapnav_msgs::MapLayers::SICK_LAYER)
- {
- pout = pose_transform * m_sick_transform * pin;
- }
- else if(m_laser_layers[j] == homer_mapnav_msgs::MapLayers::HOKUYO_FRONT)
- {
- pout = pose_transform * m_hokuyo_transform * pin;
- }
- point.x = pout.x();
- point.y = pout.y();
- point.z = 0;
-
- Eigen::Vector2i map_point = map_tools::toMapCoords(point, m_MapLayers[homer_mapnav_msgs::MapLayers::SLAM_LAYER]->info.origin, m_MapLayers[homer_mapnav_msgs::MapLayers::SLAM_LAYER]->info.resolution);
- k = map_point.y() * m_MapLayers[homer_mapnav_msgs::MapLayers::SLAM_LAYER]->info.width + map_point.x();
- if(k < 0 || k > m_MapLayers[homer_mapnav_msgs::MapLayers::SLAM_LAYER]->data.size())
- {
- continue;
- }
- else
- {
- mergedMap.data[k] = data;
- }
- }
- }
- }
- }
- }
- else
- {
- try
- {
- if(m_TransformListener.waitForTransform("/base_link", "/laser", ros::Time(0), ros::Duration(0.1)))
- {
- m_TransformListener.lookupTransform ("/base_link", "/laser", ros::Time(0), m_sick_transform);
- m_TransformListener.lookupTransform ("/base_link", "/hokuyo_front", ros::Time(0), m_hokuyo_transform);
- m_got_transform = true;
- }
- }
- catch (tf::TransformException ex)
- {
- ROS_ERROR("%s",ex.what());
- m_got_transform = false;
- }
- }
- }
- catch(tf::TransformException ex)
- {
- ROS_ERROR_STREAM(ex.what());
- }
mergedMap.header.stamp = ros::Time::now();
mergedMap.header.frame_id = "map";
-
+
m_MapPublisher.publish(mergedMap);
ROS_DEBUG_STREAM("Publishing map");
}
diff --git a/homer_map_manager/src/Managers/PoiManager.cpp b/homer_map_manager/src/Managers/PoiManager.cpp
index de35c152..c6674322 100644
--- a/homer_map_manager/src/Managers/PoiManager.cpp
+++ b/homer_map_manager/src/Managers/PoiManager.cpp
@@ -8,131 +8,123 @@
using namespace std;
-
-PoiManager::PoiManager(ros::NodeHandle *nh)
-{
- m_POIsPublisher = nh->advertise<homer_mapnav_msgs::PointsOfInterest>("/map_manager/poi_list", 1);
+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
+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;
+ 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;
}
-bool PoiManager::modifyPointOfInterest (const homer_mapnav_msgs::ModifyPOI::ConstPtr &poi )
-{
- std::string name = poi->old_name;
+bool PoiManager::modifyPointOfInterest(
+ 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)
-{
+void PoiManager::replacePOIList(
+ 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.
- {
- if ( it->name == name )
+ for (it = m_Pois.begin(); it != m_Pois.end();) // it++ ) Iterator darf nur
+ // dann erhöht werden, wenn
+ // kein POI gelöscht wird.
{
- 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++;
+ 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_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() );
+ 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 eb45cb74..f30810c5 100644
--- a/homer_map_manager/src/Managers/RoiManager.cpp
+++ b/homer_map_manager/src/Managers/RoiManager.cpp
@@ -1,258 +1,233 @@
#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);
- m_highest_id = 0; // start with 0 so first ROI gets the 1
+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 "";
+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 "";
}
- 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));
+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);
- }
- }
- return rois;
+ 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);
+ }
+ }
+ 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);
+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 << ".");
+ ROS_INFO_STREAM("Adding ROI '" << new_roi.name << "' with ID "
+ << new_roi.id << ".");
- //insert into list
- m_Rois.push_back ( new_roi );
+ // insert into list
+ m_Rois.push_back(new_roi);
- setHighestId();
- broadcastRoiList();
- return true;
+ setHighestId();
+ broadcastRoiList();
+ return true;
}
}
-bool RoiManager::modifyRegionOfInterest (const homer_mapnav_msgs::RegionOfInterest::ConstPtr &roi )
-{
- std::vector<homer_mapnav_msgs::RegionOfInterest>::iterator it;
+bool RoiManager::modifyRegionOfInterest(
+ 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)
-{
+void RoiManager::replaceROIList(
+ 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;
- }
+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;
+ ROS_INFO_STREAM("Return false");
+ return false;
}
-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;
- }
+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;
+ }
+ }
}
- }
- 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);
- std::vector< homer_mapnav_msgs::RegionOfInterest >::iterator it;
+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++;
+ 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++;
+ }
}
- }
- if(modified)
- {
- ROS_INFO_STREAM("modified");
- broadcastRoiList();
- return true;
- }
+ if (modified) {
+ ROS_INFO_STREAM("modified");
+ broadcastRoiList();
+ return true;
+ }
- ROS_ERROR_STREAM ("ROI " << name << " does not exist.");
+ 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);
+ }
}
- }
}
--
GitLab