Skip to content
Snippets Groups Projects
Commit 33e162db authored by Florian Polster's avatar Florian Polster
Browse files

latching enabled

parent ea7a0eec
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
homer_map_manager/src/Managers/MapManager.cpp
+ 184
− 177
View file @ 33e162db
#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");
}
homer_map_manager/src/Managers/PoiManager.cpp
+ 78
− 86
View file @ 33e162db
......@@ -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());
}
homer_map_manager/src/Managers/RoiManager.cpp
+ 178
− 203
View file @ 33e162db
#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);
}
}
}
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment