From 5eae66d210e571a1bc9474896754f2c2bc446adb Mon Sep 17 00:00:00 2001
From: Lisa <robbie@uni-koblenz.de>
Date: Tue, 28 Feb 2017 16:51:27 +0100
Subject: [PATCH] reworked ignore laser and obstacle position feedback
---
homer_mapnav_msgs/msg/TargetUnreachable.msg | 2 ++
.../homer_navigation/homer_navigation_node.h | 3 +++
homer_navigation/src/homer_navigation_node.cpp | 14 +++++++++++---
3 files changed, 16 insertions(+), 3 deletions(-)
diff --git a/homer_mapnav_msgs/msg/TargetUnreachable.msg b/homer_mapnav_msgs/msg/TargetUnreachable.msg
index f69ddf67..ce02d885 100644
--- a/homer_mapnav_msgs/msg/TargetUnreachable.msg
+++ b/homer_mapnav_msgs/msg/TargetUnreachable.msg
@@ -7,3 +7,5 @@ int8 LASER_TIMEOUT=35
int8 NO_PATH_FOUND=40
int8 reason
+
+geometry_msgs/PointStamped point
diff --git a/homer_navigation/include/homer_navigation/homer_navigation_node.h b/homer_navigation/include/homer_navigation/homer_navigation_node.h
index 21ad99f9..5c594333 100644
--- a/homer_navigation/include/homer_navigation/homer_navigation_node.h
+++ b/homer_navigation/include/homer_navigation/homer_navigation_node.h
@@ -5,6 +5,7 @@
#include <sstream>
#include <string>
#include <vector>
+#include <regex>
#include <ros/package.h>
#include <ros/ros.h>
@@ -133,6 +134,8 @@ class HomerNavigationNode {
/** @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
diff --git a/homer_navigation/src/homer_navigation_node.cpp b/homer_navigation/src/homer_navigation_node.cpp
index 2b0296eb..66a8372c 100644
--- a/homer_navigation/src/homer_navigation_node.cpp
+++ b/homer_navigation/src/homer_navigation_node.cpp
@@ -209,11 +209,16 @@ void HomerNavigationNode::idleProcess() {
}
}
+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(m_ignore_scan.find(scan.second->header.frame_id) == std::string::npos){
+ 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");
@@ -392,6 +397,9 @@ void HomerNavigationNode::sendTargetUnreachableMsg(int8_t reason) {
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 ===");
@@ -416,7 +424,7 @@ bool HomerNavigationNode::obstacleOnPath() {
m_last_check_path_time = ros::Time::now();
if (m_pixel_path.size() != 0) {
for (auto const& scan : m_scan_map) {
- if(m_ignore_scan.find(scan.second->header.frame_id) == std::string::npos){
+ 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");
@@ -1056,7 +1064,7 @@ void HomerNavigationNode::processLaserScan(const sensor_msgs::LaserScan::ConstPt
m_last_laser_time = ros::Time::now();
if(m_MainMachine.state() != IDLE)
{
- if(m_ignore_scan.find(msg->header.frame_id)== std::string::npos)
+ 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"),
--
GitLab