Skip to content
Snippets Groups Projects
Commit 3e337dcf authored by Projekt Robbie's avatar Projekt Robbie
Browse files

Clang Format

parent fafa2d33
No related branches found
No related tags found
1 merge request!4Recent changes
Hide whitespace changes
Inline Side-by-side
homer_map_manager/src/Managers/MapManager.cpp
+ 204
− 200
View file @ 3e337dcf
#include <homer_map_manager/Managers/MapManager.h> #include <homer_map_manager/Managers/MapManager.h>
#include <omp.h> #include <omp.h>
MapManager::MapManager(ros::NodeHandle *nh) { MapManager::MapManager(ros::NodeHandle *nh)
m_MapPublisher = nh->advertise<nav_msgs::OccupancyGrid>("/map", 1, true); {
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::SLAM_LAYER);
m_map_layers.push_back(homer_mapnav_msgs::MapLayers::RAPID_MAP); m_map_layers.push_back(homer_mapnav_msgs::MapLayers::KINECT_LAYER);
m_map_layers.push_back(homer_mapnav_msgs::MapLayers::MASKING_LAYER); m_map_layers.push_back(homer_mapnav_msgs::MapLayers::RAPID_MAP);
m_laser_layers.push_back(homer_mapnav_msgs::MapLayers::HOKUYO_FRONT); m_map_layers.push_back(homer_mapnav_msgs::MapLayers::MASKING_LAYER);
m_laser_layers.push_back(homer_mapnav_msgs::MapLayers::SICK_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
// 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_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++)
{
//try { m_MapVisibility[m_laser_layers[i]] = true;
//m_TransformListener.waitForTransform("/base_link", "/laser", }
//ros::Time(0), ros::Duration(3));
//m_TransformListener.lookupTransform("/base_link", "/laser", geometry_msgs::PoseStamped pose;
//ros::Time(0), m_sick_transform); pose.pose.position.x = 0;
//m_TransformListener.lookupTransform("/base_link", "/hokuyo_front", pose.pose.position.y = 0;
//ros::Time(0), m_hokuyo_transform); pose.pose.position.z = 0;
//m_got_transform = true; pose.pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
//} catch (tf::TransformException ex) {
//ROS_ERROR("%s", ex.what()); m_pose = boost::make_shared<geometry_msgs::PoseStamped>(pose);
//m_got_transform = false;
//}
geometry_msgs::PoseStamped pose;
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.position.z = 0;
pose.pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
m_pose = boost::make_shared<geometry_msgs::PoseStamped>(pose);
} }
MapManager::~MapManager() {} MapManager::~MapManager()
{
}
void MapManager::updateMapLayer(int type, void MapManager::updateMapLayer(int type,
nav_msgs::OccupancyGrid::ConstPtr layer) { nav_msgs::OccupancyGrid::ConstPtr layer)
m_MapLayers[type] = layer; {
if (type == homer_mapnav_msgs::MapLayers::SLAM_LAYER) { m_MapLayers[type] = layer;
sendMergedMap(); 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; ROS_INFO_STREAM("MapManager: " << type << ": " << state);
m_MapVisibility[type] = state;
} }
void MapManager::updateLaser(int layer, void MapManager::updateLaser(int layer,
const sensor_msgs::LaserScan::ConstPtr &msg) { const sensor_msgs::LaserScan::ConstPtr &msg)
m_laserLayers[layer] = msg; {
m_laserLayers[layer] = msg;
} }
nav_msgs::OccupancyGrid::ConstPtr MapManager::getMapLayer(int type) { nav_msgs::OccupancyGrid::ConstPtr MapManager::getMapLayer(int type)
if (m_MapLayers.find(type) == m_MapLayers.end()) {
return nav_msgs::OccupancyGrid::ConstPtr(); if (m_MapLayers.find(type) == m_MapLayers.end())
return m_MapLayers[type]; return nav_msgs::OccupancyGrid::ConstPtr();
return m_MapLayers[type];
} }
/** /**
...@@ -75,152 +73,158 @@ nav_msgs::OccupancyGrid::ConstPtr MapManager::getMapLayer(int type) { ...@@ -75,152 +73,158 @@ nav_msgs::OccupancyGrid::ConstPtr MapManager::getMapLayer(int type) {
* merged map layers to the gui node * merged map layers to the gui node
* *
*/ */
void MapManager::sendMergedMap() { void MapManager::sendMergedMap()
if (m_MapLayers.find(homer_mapnav_msgs::MapLayers::SLAM_LAYER) == {
m_MapLayers.end()) { if (m_MapLayers.find(homer_mapnav_msgs::MapLayers::SLAM_LAYER) ==
ROS_DEBUG_STREAM("SLAM map is missing!"); m_MapLayers.end())
return; {
} ROS_DEBUG_STREAM("SLAM map is missing!");
int k; return;
nav_msgs::OccupancyGrid mergedMap( }
*(m_MapLayers[homer_mapnav_msgs::MapLayers::SLAM_LAYER])); int k;
// bake maps nav_msgs::OccupancyGrid mergedMap(
for (int j = 1; j < m_map_layers.size(); j++) { *(m_MapLayers[homer_mapnav_msgs::MapLayers::SLAM_LAYER]));
if (m_MapLayers.find(m_map_layers[j]) != m_MapLayers.end() && // bake maps
m_MapVisibility[m_map_layers[j]]) { for (int j = 1; j < m_map_layers.size(); j++)
// calculating parallel on 8 threads {
omp_set_num_threads(8); if (m_MapLayers.find(m_map_layers[j]) != m_MapLayers.end() &&
size_t mapsize = m_MapLayers[m_map_layers[j]]->info.height * m_MapVisibility[m_map_layers[j]])
m_MapLayers[m_map_layers[j]]->info.width; {
const std::vector<signed char> *tempdata = // calculating parallel on 8 threads
&m_MapLayers[m_map_layers[j]]->data; omp_set_num_threads(8);
const int frei = homer_mapnav_msgs::ModifyMap::FREE; size_t mapsize = m_MapLayers[m_map_layers[j]]->info.height *
signed char currentvalue = 0; m_MapLayers[m_map_layers[j]]->info.width;
#pragma omp parallel for private(currentvalue) shared(mapsize, tempdata, \ 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) mergedMap)
for (int i = 0; i < mapsize; i++) { for (int i = 0; i < mapsize; i++)
currentvalue = tempdata->at(i); {
if (currentvalue > 50 || currentvalue == frei) { currentvalue = tempdata->at(i);
mergedMap.data[i] = currentvalue; if (currentvalue > 50 || currentvalue == frei)
} {
} mergedMap.data[i] = currentvalue;
} }
}
} }
// bake Lasers }
// bake Lasers
//try {
//int data; // try {
//if (m_got_transform) { // int data;
//for (int j = 0; j < m_laser_layers.size(); j++) { // if (m_got_transform) {
//if (m_laserLayers.find(m_laser_layers[j]) != // for (int j = 0; j < m_laser_layers.size(); j++) {
//m_laserLayers.end() && // if (m_laserLayers.find(m_laser_layers[j]) !=
//m_MapVisibility[m_laser_layers[j]]) { // m_laserLayers.end() &&
//if (m_laser_layers[j] == // m_MapVisibility[m_laser_layers[j]]) {
//homer_mapnav_msgs::MapLayers::SICK_LAYER) { // if (m_laser_layers[j] ==
//data = homer_mapnav_msgs::ModifyMap::BLOCKED; // homer_mapnav_msgs::MapLayers::SICK_LAYER) {
//} else if (m_laser_layers[j] == // data = homer_mapnav_msgs::ModifyMap::BLOCKED;
//homer_mapnav_msgs::MapLayers::HOKUYO_BACK) { //} else if (m_laser_layers[j] ==
//data = homer_mapnav_msgs::ModifyMap::HOKUYO; // homer_mapnav_msgs::MapLayers::HOKUYO_BACK) {
//} else if (m_laser_layers[j] == // data = homer_mapnav_msgs::ModifyMap::HOKUYO;
//homer_mapnav_msgs::MapLayers::HOKUYO_FRONT) { //} else if (m_laser_layers[j] ==
//data = homer_mapnav_msgs::ModifyMap::HOKUYO; // homer_mapnav_msgs::MapLayers::HOKUYO_FRONT) {
//} // data = homer_mapnav_msgs::ModifyMap::HOKUYO;
//tf::StampedTransform pose_transform; //}
//m_TransformListener.waitForTransform( // tf::StampedTransform pose_transform;
//"/map", "/base_link", // m_TransformListener.waitForTransform(
//m_laserLayers[m_laser_layers[j]]->header.stamp, //"/map", "/base_link",
//ros::Duration(0.09)); // m_laserLayers[m_laser_layers[j]]->header.stamp,
//m_TransformListener.lookupTransform( // ros::Duration(0.09));
//"/map", "/base_link", // m_TransformListener.lookupTransform(
//m_laserLayers[m_laser_layers[j]]->header.stamp, //"/map", "/base_link",
//pose_transform); // m_laserLayers[m_laser_layers[j]]->header.stamp,
// pose_transform);
//// pose_transform.setTransform(Transform);
//// pose_transform.setTransform(Transform);
//for (int i = 0;
//i < m_laserLayers[m_laser_layers[j]]->ranges.size(); // for (int i = 0;
//i++) { // i < m_laserLayers[m_laser_layers[j]]->ranges.size();
//if (m_laserLayers[m_laser_layers[j]]->ranges[i] < // i++) {
//m_laserLayers[m_laser_layers[j]]->range_max && // if (m_laserLayers[m_laser_layers[j]]->ranges[i] <
//m_laserLayers[m_laser_layers[j]]->ranges[i] > // m_laserLayers[m_laser_layers[j]]->range_max &&
//m_laserLayers[m_laser_layers[j]]->range_min) { // m_laserLayers[m_laser_layers[j]]->ranges[i] >
//float alpha = // m_laserLayers[m_laser_layers[j]]->range_min) {
//m_laserLayers[m_laser_layers[j]]->angle_min + // float alpha =
//i * // m_laserLayers[m_laser_layers[j]]->angle_min +
//m_laserLayers[m_laser_layers[j]] // i *
//->angle_increment; // m_laserLayers[m_laser_layers[j]]
//geometry_msgs::Point point; //->angle_increment;
//tf::Vector3 pin; // geometry_msgs::Point point;
//tf::Vector3 pout; // tf::Vector3 pin;
//pin.setX( // tf::Vector3 pout;
//cos(alpha) * // pin.setX(
//m_laserLayers[m_laser_layers[j]]->ranges[i]); // cos(alpha) *
//pin.setY( // m_laserLayers[m_laser_layers[j]]->ranges[i]);
//sin(alpha) * // pin.setY(
//m_laserLayers[m_laser_layers[j]]->ranges[i]); // sin(alpha) *
//pin.setZ(0); // m_laserLayers[m_laser_layers[j]]->ranges[i]);
// pin.setZ(0);
//if (m_laser_layers[j] ==
//homer_mapnav_msgs::MapLayers::SICK_LAYER) { // if (m_laser_layers[j] ==
//pout = pose_transform * m_sick_transform * pin; // homer_mapnav_msgs::MapLayers::SICK_LAYER) {
//} else if (m_laser_layers[j] == // pout = pose_transform * m_sick_transform * pin;
//homer_mapnav_msgs::MapLayers:: //} else if (m_laser_layers[j] ==
//HOKUYO_FRONT) { // homer_mapnav_msgs::MapLayers::
//pout = // HOKUYO_FRONT) {
//pose_transform * m_hokuyo_transform * pin; // pout =
//} // pose_transform * m_hokuyo_transform * pin;
//point.x = pout.x(); //}
//point.y = pout.y(); // point.x = pout.x();
//point.z = 0; // point.y = pout.y();
// point.z = 0;
//Eigen::Vector2i map_point = map_tools::toMapCoords(
//point, // Eigen::Vector2i map_point = map_tools::toMapCoords(
//m_MapLayers // point,
//[homer_mapnav_msgs::MapLayers::SLAM_LAYER] // m_MapLayers
//->info.origin, //[homer_mapnav_msgs::MapLayers::SLAM_LAYER]
//m_MapLayers //->info.origin,
//[homer_mapnav_msgs::MapLayers::SLAM_LAYER] // m_MapLayers
//->info.resolution); //[homer_mapnav_msgs::MapLayers::SLAM_LAYER]
//k = map_point.y() * //->info.resolution);
//m_MapLayers[homer_mapnav_msgs::MapLayers:: // k = map_point.y() *
//SLAM_LAYER] // m_MapLayers[homer_mapnav_msgs::MapLayers::
//->info.width + // SLAM_LAYER]
//map_point.x(); //->info.width +
//if (k < 0 || // map_point.x();
//k > m_MapLayers[homer_mapnav_msgs::MapLayers:: // if (k < 0 ||
//SLAM_LAYER] // k > m_MapLayers[homer_mapnav_msgs::MapLayers::
//->data.size()) { // SLAM_LAYER]
//continue; //->data.size()) {
//} else { // continue;
//mergedMap.data[k] = data; //} else {
//} // mergedMap.data[k] = data;
//} //}
//} //}
//} //}
//} //}
//} else { //}
//try { //} else {
//if (m_TransformListener.waitForTransform("/base_link", "/laser", // try {
//ros::Time(0), // if (m_TransformListener.waitForTransform("/base_link", "/laser",
//ros::Duration(0.1))) { // ros::Time(0),
//m_TransformListener.lookupTransform( // ros::Duration(0.1))) {
//"/base_link", "/laser", ros::Time(0), m_sick_transform); // m_TransformListener.lookupTransform(
//m_TransformListener.lookupTransform( //"/base_link", "/laser", ros::Time(0), m_sick_transform);
//"/base_link", "/hokuyo_front", ros::Time(0), // m_TransformListener.lookupTransform(
//m_hokuyo_transform); //"/base_link", "/hokuyo_front", ros::Time(0),
//m_got_transform = true; // m_hokuyo_transform);
//} // m_got_transform = true;
//} catch (tf::TransformException ex) { //}
//ROS_ERROR("%s", ex.what()); //} catch (tf::TransformException ex) {
//m_got_transform = false; // ROS_ERROR("%s", ex.what());
//} // m_got_transform = false;
//} //}
//} catch (tf::TransformException ex) { //}
//ROS_ERROR_STREAM(ex.what()); //} catch (tf::TransformException ex) {
//} // ROS_ERROR_STREAM(ex.what());
mergedMap.header.stamp = ros::Time::now(); //}
mergedMap.header.frame_id = "map"; mergedMap.header.stamp = ros::Time::now();
mergedMap.header.frame_id = "map";
m_MapPublisher.publish(mergedMap);
ROS_DEBUG_STREAM("Publishing map"); m_MapPublisher.publish(mergedMap);
ROS_DEBUG_STREAM("Publishing map");
} }
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