Commit 3a801d77 authored by Projekt Robbie's avatar Projekt Robbie
Browse files

Imported upstream version '0.1.19' of 'upstream'

parent 9ebb71a5
## homer_mapping (kinetic) - 0.1.18-0
The packages in the `homer_mapping` repository were released into the `kinetic` distro by running `/usr/bin/bloom-release --rosdistro kinetic --track kinetic homer_mapping` on `Tue, 28 Feb 2017 15:53:51 -0000`
These packages were released:
- `homer_map_manager`
- `homer_mapnav_msgs`
- `homer_mapping`
- `homer_nav_libs`
- `homer_navigation`
Version of package(s) in repository `homer_mapping`:
- upstream repository: git@gitlab.uni-koblenz.de:robbie/homer_mapping.git
- release repository: git@gitlab.uni-koblenz.de:robbie/homer_mapping.git
- rosdistro version: `0.1.17-0`
- old version: `0.1.17-1`
- new version: `0.1.18-0`
Versions of tools used:
- bloom version: `0.5.24`
- catkin_pkg version: `0.3.1`
- rosdep version: `0.11.5`
- rosdistro version: `0.6.1`
- vcstools version: `0.1.39`
## homer_mapping (kinetic) - 0.1.17-1
The packages in the `homer_mapping` repository were released into the `kinetic` distro by running `/usr/bin/bloom-release --rosdistro kinetic --track kinetic homer_mapping` on `Sun, 26 Feb 2017 23:11:15 -0000`
......
......@@ -2,6 +2,9 @@
Changelog for package homer_map_manager
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.1.19 (2017-03-02)
-------------------
0.1.18 (2017-02-28)
-------------------
......
<package>
<name>homer_map_manager</name>
<version>0.1.18</version>
<version>0.1.19</version>
<description>
map_manager
</description>
......
......@@ -2,6 +2,9 @@
Changelog for package homer_mapnav_msgs
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.1.19 (2017-03-02)
-------------------
0.1.18 (2017-02-28)
-------------------
* reworked ignore laser and obstacle position feedback
......
<package>
<name>homer_mapnav_msgs</name>
<version>0.1.18</version>
<version>0.1.19</version>
<description> homer_mapnav_msgs contains the messages used for mapping and navigation
</description>
<maintainer email="fpolster@uni-koblenz.de">Florian Polster</maintainer>
......
......@@ -2,6 +2,13 @@
Changelog for package homer_mapping
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.1.19 (2017-03-02)
-------------------
* enable smaller moves to be able to use higher odometry rates
* no ros.spin spinOnce instead
* use the loop_rate
* Contributors: Florian Polster, Lisa
0.1.18 (2017-02-28)
-------------------
......
<package>
<name>homer_mapping</name>
<version>0.1.18</version>
<version>0.1.19</version>
<description>
homer_mapping
......
This diff is collapsed.
......@@ -2,6 +2,9 @@
Changelog for package homer_nav_libs
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.1.19 (2017-03-02)
-------------------
0.1.18 (2017-02-28)
-------------------
......
<?xml version="1.0"?>
<package>
<name>homer_nav_libs</name>
<version>0.1.18</version>
<version>0.1.19</version>
<description>The nav_libs package</description>
<maintainer email="fpolster@uni-koblenz.de">Florian Polster</maintainer>
......
......@@ -2,6 +2,13 @@
Changelog for package homer_navigation
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.1.19 (2017-03-02)
-------------------
* maskMap disabled because not functional at the moment
* no seperate laserCallback
* give position information of obstacle in target unreachable message
* Contributors: Florian Polster, Lisa
0.1.18 (2017-02-28)
-------------------
* reworked ignore laser and obstacle position feedback
......
......@@ -2,10 +2,10 @@
#define FastNavigationModule_H
#include <cmath>
#include <regex>
#include <sstream>
#include <string>
#include <vector>
#include <regex>
#include <ros/package.h>
#include <ros/ros.h>
......@@ -66,7 +66,6 @@ class HomerNavigationNode {
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 downlaserDataCallback(const sensor_msgs::LaserScan::ConstPtr& msg);
void startNavigationCallback(
const homer_mapnav_msgs::StartNavigation::ConstPtr& msg);
void moveBaseSimpleGoalCallback(
......@@ -85,12 +84,13 @@ class HomerNavigationNode {
virtual void init();
void initNewTarget();
void processLaserScan(const sensor_msgs::LaserScan::ConstPtr& msg);
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();
......
<?xml version="1.0"?>
<package>
<name>homer_navigation</name>
<version>0.1.18</version>
<version>0.1.19</version>
<description>The homer_navigation package</description>
<maintainer email="fpolster@uni-koblenz.de">Florian Polster</maintainer>
......
......@@ -11,7 +11,7 @@ HomerNavigationNode::HomerNavigationNode() {
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::downlaserDataCallback, this);
"/front_scan", 1, &HomerNavigationNode::laserDataCallback, this);
m_start_navigation_sub = nh.subscribe<homer_mapnav_msgs::StartNavigation>(
"/homer_navigation/start_navigation", 1,
&HomerNavigationNode::startNavigationCallback, this);
......@@ -34,8 +34,9 @@ HomerNavigationNode::HomerNavigationNode() {
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_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);
......@@ -156,9 +157,9 @@ void HomerNavigationNode::init() {
m_ignore_scan = "";
m_obstacle_on_path = false;
m_obstacle_position.x = 0;
m_obstacle_position.y = 0;
m_obstacle_position.z = 0;
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";
......@@ -209,8 +210,8 @@ void HomerNavigationNode::idleProcess() {
}
}
bool HomerNavigationNode::isInIgnoreList(std::string frame_id){
std::regex reg("(^|\\s)"+frame_id+"(\\s|$)");
bool HomerNavigationNode::isInIgnoreList(std::string frame_id) {
std::regex reg("(^|\\s)" + frame_id + "(\\s|$)");
return regex_match(m_ignore_scan, reg);
}
......@@ -218,21 +219,24 @@ 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)){
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");
scan.second, m_transform_listener, "/map");
for (const auto& point : scan_points) {
Eigen::Vector2i map_point = map_tools::toMapCoords(point, m_map);
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) {
if (index > 0 &&
index < m_map->info.width * m_map->info.height) {
temp_map.data[index] = (int8_t)100;
}
}
}
}
if (m_fast_path_planning) {
maskMap(temp_map);
//TODO check why not functional
//maskMap(temp_map);
}
m_explorer->setOccupancyMap(
boost::make_shared<nav_msgs::OccupancyGrid>(temp_map));
......@@ -420,39 +424,61 @@ bool HomerNavigationNode::isTargetPositionReached() {
}
}
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;
}
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(!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 (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)
{
m_obstacle_position = t;
ROS_INFO_STREAM("Found obstacle");
return true;
}
}
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);
}
}
}
......@@ -460,7 +486,13 @@ bool HomerNavigationNode::obstacleOnPath() {
}
}
}
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() {
......@@ -473,14 +505,11 @@ void HomerNavigationNode::performNextMove() {
(ros::Time::now() - m_last_check_path_time) > ros::Duration(0.2)) {
if (obstacleOnPath()) {
if (m_seen_obstacle_before) {
if(!m_obstacle_on_path)
{
if (!m_obstacle_on_path) {
stopRobot();
calculatePath();
return;
}
else
{
} else {
calculatePath();
}
}
......@@ -594,18 +623,18 @@ void HomerNavigationNode::performNextMove() {
}
} 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 )
{
} 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);
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;
......@@ -644,10 +673,12 @@ void HomerNavigationNode::performNextMove() {
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() > 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(
......@@ -1028,10 +1059,8 @@ void HomerNavigationNode::poseCallback(
}
void HomerNavigationNode::calcMaxMoveDist() {
m_max_move_distance = 99;
for(auto d: m_max_move_distances)
{
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 &&
......@@ -1047,8 +1076,9 @@ void HomerNavigationNode::calcMaxMoveDist() {
}
}
void HomerNavigationNode::ignoreLaserCallback(const std_msgs::String::ConstPtr& msg){
ROS_INFO_STREAM("changed ignore laser to: "<<msg->data);
void HomerNavigationNode::ignoreLaserCallback(
const std_msgs::String::ConstPtr& msg) {
ROS_INFO_STREAM("changed ignore laser to: " << msg->data);
m_ignore_scan = msg->data;
}
......@@ -1058,35 +1088,22 @@ void HomerNavigationNode::maxDepthMoveDistanceCallback(
calcMaxMoveDist();
}
void HomerNavigationNode::processLaserScan(const sensor_msgs::LaserScan::ConstPtr& msg)
{
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"),
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
{
} else {
m_max_move_distances[msg->header.frame_id] = 99;
}
calcMaxMoveDist();
}
}
void HomerNavigationNode::laserDataCallback(
const sensor_msgs::LaserScan::ConstPtr& msg) {
processLaserScan(msg);
}
void HomerNavigationNode::downlaserDataCallback(
const sensor_msgs::LaserScan::ConstPtr& msg) {
processLaserScan(msg);
}
void HomerNavigationNode::initNewTarget() {
......
......@@ -13,10 +13,10 @@ tracks:
- git-bloom-generate -y rosrpm --prefix release/:{ros_distro} :{ros_distro} -i
:{release_inc}
devel_branch: master
last_version: 0.1.17
last_version: 0.1.18
name: upstream
patches: null
release_inc: '1'
release_inc: '0'
release_repo_url: git@gitlab.uni-koblenz.de:robbie/homer_mapping.git
release_tag: :{version}
ros_distro: kinetic
......
Supports Markdown
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