From 01f9cfa06965c1bdfce191cb636ba8d41e5d05f1 Mon Sep 17 00:00:00 2001
From: Niklas Yann Wettengel <niyawe@uni-koblenz.de>
Date: Thu, 17 Nov 2016 21:34:09 +0100
Subject: [PATCH] Rebase from 'upstream'
---
README.md | 28 +
homer_map_manager/CHANGELOG.rst | 3 +
homer_map_manager/package.xml | 2 +-
homer_mapnav_msgs/CHANGELOG.rst | 5 +
homer_mapnav_msgs/msg/NavigateToPOI.msg | 1 +
homer_mapnav_msgs/package.xml | 2 +-
homer_mapping/CHANGELOG.rst | 3 +
homer_mapping/package.xml | 2 +-
homer_nav_libs/CHANGELOG.rst | 3 +
homer_nav_libs/package.xml | 2 +-
homer_navigation/CHANGELOG.rst | 11 +
.../homer_navigation/homer_navigation_node.h | 584 ++---
homer_navigation/package.xml | 2 +-
.../src/homer_navigation_node.cpp | 2036 +++++++++--------
tracks.yaml | 2 +-
15 files changed, 1396 insertions(+), 1290 deletions(-)
diff --git a/README.md b/README.md
index c24a19ce..6bc6551c 100644
--- a/README.md
+++ b/README.md
@@ -1,3 +1,31 @@
+## homer_mapping (kinetic) - 0.1.6-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 `Fri, 04 Nov 2016 11:00:50 -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.5-0`
+- old version: `0.1.5-0`
+- new version: `0.1.6-0`
+
+Versions of tools used:
+
+- bloom version: `0.5.23`
+- catkin_pkg version: `0.2.10`
+- rosdep version: `0.11.5`
+- rosdistro version: `0.5.0`
+- vcstools version: `0.1.39`
+
+
## homer_mapping (kinetic) - 0.1.5-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 `Thu, 03 Nov 2016 23:31:28 -0000`
diff --git a/homer_map_manager/CHANGELOG.rst b/homer_map_manager/CHANGELOG.rst
index 27f85cdb..4f884ed2 100644
--- a/homer_map_manager/CHANGELOG.rst
+++ b/homer_map_manager/CHANGELOG.rst
@@ -2,6 +2,9 @@
Changelog for package homer_map_manager
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+0.1.7 (2016-11-17)
+------------------
+
0.1.6 (2016-11-04)
------------------
diff --git a/homer_map_manager/package.xml b/homer_map_manager/package.xml
index c31e4799..18dd54f8 100644
--- a/homer_map_manager/package.xml
+++ b/homer_map_manager/package.xml
@@ -1,6 +1,6 @@
<package>
<name>homer_map_manager</name>
- <version>0.1.6</version>
+ <version>0.1.7</version>
<description>
map_manager
</description>
diff --git a/homer_mapnav_msgs/CHANGELOG.rst b/homer_mapnav_msgs/CHANGELOG.rst
index b19dfb04..274c2535 100644
--- a/homer_mapnav_msgs/CHANGELOG.rst
+++ b/homer_mapnav_msgs/CHANGELOG.rst
@@ -2,6 +2,11 @@
Changelog for package homer_mapnav_msgs
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+0.1.7 (2016-11-17)
+------------------
+* Add a parameter to NavigateToPOI msgs to controll new behaviour (remove config parameter)
+* Contributors: Malte Roosen
+
0.1.6 (2016-11-04)
------------------
diff --git a/homer_mapnav_msgs/msg/NavigateToPOI.msg b/homer_mapnav_msgs/msg/NavigateToPOI.msg
index 342f1d1f..485fdc84 100644
--- a/homer_mapnav_msgs/msg/NavigateToPOI.msg
+++ b/homer_mapnav_msgs/msg/NavigateToPOI.msg
@@ -1,3 +1,4 @@
string poi_name
float32 distance_to_target
bool skip_final_turn
+bool stop_before_obstacle
diff --git a/homer_mapnav_msgs/package.xml b/homer_mapnav_msgs/package.xml
index 6d4e4cf6..b74d6f82 100644
--- a/homer_mapnav_msgs/package.xml
+++ b/homer_mapnav_msgs/package.xml
@@ -1,6 +1,6 @@
<package>
<name>homer_mapnav_msgs</name>
- <version>0.1.6</version>
+ <version>0.1.7</version>
<description> homer_mapnav_msgs contains the messages used for mapping and navigation
</description>
<maintainer email="vseib@uni-koblenz.de">Viktor Seib</maintainer>
diff --git a/homer_mapping/CHANGELOG.rst b/homer_mapping/CHANGELOG.rst
index d7a5b48b..97441c0c 100644
--- a/homer_mapping/CHANGELOG.rst
+++ b/homer_mapping/CHANGELOG.rst
@@ -2,6 +2,9 @@
Changelog for package homer_mapping
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+0.1.7 (2016-11-17)
+------------------
+
0.1.6 (2016-11-04)
------------------
diff --git a/homer_mapping/package.xml b/homer_mapping/package.xml
index 94819c0c..67525d3a 100644
--- a/homer_mapping/package.xml
+++ b/homer_mapping/package.xml
@@ -1,6 +1,6 @@
<package>
<name>homer_mapping</name>
- <version>0.1.6</version>
+ <version>0.1.7</version>
<description>
homer_mapping
diff --git a/homer_nav_libs/CHANGELOG.rst b/homer_nav_libs/CHANGELOG.rst
index 0baf6d03..1c8be570 100644
--- a/homer_nav_libs/CHANGELOG.rst
+++ b/homer_nav_libs/CHANGELOG.rst
@@ -2,6 +2,9 @@
Changelog for package homer_nav_libs
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+0.1.7 (2016-11-17)
+------------------
+
0.1.6 (2016-11-04)
------------------
diff --git a/homer_nav_libs/package.xml b/homer_nav_libs/package.xml
index b3efc07e..fdbb2f8a 100644
--- a/homer_nav_libs/package.xml
+++ b/homer_nav_libs/package.xml
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package>
<name>homer_nav_libs</name>
- <version>0.1.6</version>
+ <version>0.1.7</version>
<description>The nav_libs package</description>
<maintainer email="vseib@uni-koblenz.de">Viktor Seib</maintainer>
diff --git a/homer_navigation/CHANGELOG.rst b/homer_navigation/CHANGELOG.rst
index 43e43ef9..d54dcf0b 100644
--- a/homer_navigation/CHANGELOG.rst
+++ b/homer_navigation/CHANGELOG.rst
@@ -2,6 +2,17 @@
Changelog for package homer_navigation
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+0.1.7 (2016-11-17)
+------------------
+* Add a parameter to NavigateToPOI msgs to controll new behaviour (remove config parameter)
+* Introduce parameter to change behaviour after collision avoidance
+* Autoformat homer_navigation_node
+* Contributors: Malte Roosen
+
+* Introduce parameter to change behaviour after collision avoidance
+* Autoformat homer_navigation_node
+* Contributors: Malte Roosen
+
0.1.6 (2016-11-04)
------------------
diff --git a/homer_navigation/include/homer_navigation/homer_navigation_node.h b/homer_navigation/include/homer_navigation/homer_navigation_node.h
index a273994b..53447adf 100644
--- a/homer_navigation/include/homer_navigation/homer_navigation_node.h
+++ b/homer_navigation/include/homer_navigation/homer_navigation_node.h
@@ -1,37 +1,37 @@
#ifndef FastNavigationModule_H
#define FastNavigationModule_H
-#include <vector>
-#include <string>
-#include <sstream>
#include <cmath>
+#include <sstream>
+#include <string>
+#include <vector>
-#include <ros/ros.h>
#include <ros/package.h>
+#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <homer_robbie_architecture/Architecture/StateMachine/StateMachine.h>
-#include <nav_msgs/OccupancyGrid.h>
-#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
-#include <sensor_msgs/LaserScan.h>
-#include <homer_mapnav_msgs/StartNavigation.h>
+#include <homer_mapnav_msgs/GetPointsOfInterest.h>
#include <homer_mapnav_msgs/NavigateToPOI.h>
+#include <homer_mapnav_msgs/StartNavigation.h>
#include <homer_mapnav_msgs/TargetUnreachable.h>
-#include <homer_mapnav_msgs/GetPointsOfInterest.h>
-#include <std_msgs/Int8.h>
-#include <std_msgs/Float32.h>
-#include <std_msgs/Empty.h>
-#include <std_msgs/String.h>
-#include <std_msgs/Float32.h>
#include <homer_ptu_msgs/CenterWorldPoint.h>
#include <homer_ptu_msgs/SetPanTilt.h>
+#include <nav_msgs/OccupancyGrid.h>
+#include <nav_msgs/Path.h>
+#include <sensor_msgs/LaserScan.h>
+#include <std_msgs/Empty.h>
+#include <std_msgs/Float32.h>
+#include <std_msgs/Float32.h>
+#include <std_msgs/Int8.h>
+#include <std_msgs/String.h>
-#include <homer_nav_libs/tools.h>
#include <homer_nav_libs/Explorer/Explorer.h>
+#include <homer_nav_libs/tools.h>
class Explorer;
/**
@@ -40,290 +40,304 @@ class Explorer;
* @brief Performs autonomous navigation
*/
class HomerNavigationNode {
+ public:
+ /**
+ * @brief States of the state machines
+ */
+ enum ProcessState {
+ IDLE,
+ AWAITING_PATHPLANNING_MAP,
+ FOLLOWING_PATH,
+ AVOIDING_COLLISION,
+ FINAL_TURN
+ };
+
+ /**
+ * The constructor
+ */
+ HomerNavigationNode();
+
+ /**
+ * The destructor
+ */
+ virtual ~HomerNavigationNode();
+
+ /** @brief Is called in constant intervals. */
+ void idleProcess();
+
+ protected:
+ /** @brief Handles incoming messages. */
+ void mapCallback(const nav_msgs::OccupancyGrid::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(
+ const geometry_msgs::PoseStamped::ConstPtr& msg);
+ void stopNavigationCallback(const std_msgs::Empty::ConstPtr& msg);
+
+ void navigateToPOICallback(
+ const homer_mapnav_msgs::NavigateToPOI::ConstPtr& msg);
+ void unknownThresholdCallback(const std_msgs::Int8::ConstPtr& msg);
+ void maxDepthMoveDistanceCallback(const std_msgs::Float32::ConstPtr& msg);
+
+ /** @brief initializes and refreshs parameters */
+ void loadParameters();
+
+ /** @brief Is called when all modules are loaded and thread has started. */
+ virtual void init();
+
+ /** @brief Detect & handle possible collision */
+ void handleCollision();
+
+ private:
+ /** @brief Start navigation to m_Target on last_map_data_ */
+ void startNavigation();
+
+ /** @brief Check if obstacles are blocking the way in last_map_data_ */
+ bool checkPath();
+
+ /** @brief calculate path from current robot position to target approximation
+ */
+ void calculatePath();
+
+ /** @brief Send message containing current navigation path */
+ void sendPathData();
+
+ /** @brief Sends target reached and stops the robot. */
+ void sendTargetReachedMsg();
+
+ /**
+ * @brief Sends a target unreachable with given reason and stops the robot.
+ * @param reason reason for unreachable target (see
+ * homer_mapnav_msgs::TargetUnreachable for possible reasons)
+ */
+ void sendTargetUnreachableMsg(int8_t reason);
+
+ /** @brief reloads all params from the parameterserver */
+ void refreshParamsCallback(const std_msgs::Empty::ConstPtr& msg);
+
+ /** @brief Navigate robot to next waypoint */
+ void performNextMove();
+
+ /** @brief Finishes navigation or starts turning to target direction if the
+ * target position has been reached */
+ void targetPositionReached();
+
+ /** @return Angle from robot_pose_ to point in degrees */
+ int angleToPointDeg(geometry_msgs::Point point);
+
+ /** @brief Calculates current maximal backwards distance on map Data */
+ bool backwardObstacle();
+
+ /** @brief stops the Robot */
+ void stopRobot();
+
+ /**
+ * @brief Sets each cell of the map to -1 outside the bounding box
+ * containing the robot pose and the current target
+ */
+ void maskMap();
+
+ /**
+ * @brief Current path was finished (either successful or not),
+ * sets state machine to path planning to check if the robot is already
+ * at the goal
+ */
+ void currentPathFinished();
+
+ // convenience math functions
+ /**
+ * Computes minimum turn angle from angle 1 to angle 2
+ * @param angle1 from angle
+ * @param angle2 to angle
+ * @return minimal angle needed to turn from angle 1 to angle 2 [-Pi..Pi]
+ */
+ static float minTurnAngle(float angle1, float angle2);
+
+ /**
+ * converts value from degree to radiant
+ * @param deg Value in degree
+ * @return value in radiants
+ */
+ static float deg2Rad(float deg) { return deg / 180.0 * M_PI; }
+
+ /**
+ * converts value from radiants to degrees
+ * @param rad Value in radiants
+ * @return value in degrees
+ */
+ static float rad2Deg(float rad) { return rad / M_PI * 180.0; }
+
+ bool drawPolygon(std::vector<geometry_msgs::Point> vertices);
+ void drawLine(std::vector<int>& data, int startX, int startY, int endX,
+ int endY, int value);
+ bool fillPolygon(std::vector<int>& data, int x, int y, int value);
- public:
-
- /**
- * @brief States of the state machines
- */
- enum ProcessState
- {
- IDLE,
- AWAITING_PATHPLANNING_MAP,
- FOLLOWING_PATH,
- AVOIDING_COLLISION,
- FINAL_TURN
- };
-
- /**
- * The constructor
- */
- HomerNavigationNode();
-
- /**
- * The destructor
- */
- virtual ~HomerNavigationNode();
-
- /** @brief Is called in constant intervals. */
- void idleProcess();
-
-
- protected:
- /** @brief Handles incoming messages. */
- void mapCallback(const nav_msgs::OccupancyGrid::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(const geometry_msgs::PoseStamped::ConstPtr& msg);
- void stopNavigationCallback(const std_msgs::Empty::ConstPtr& msg);
-
- void navigateToPOICallback(const homer_mapnav_msgs::NavigateToPOI::ConstPtr& msg);
- void unknownThresholdCallback(const std_msgs::Int8::ConstPtr& msg);
- void maxDepthMoveDistanceCallback(const std_msgs::Float32::ConstPtr& msg);
-
-
- /** @brief initializes and refreshs parameters */
- void loadParameters();
-
- /** @brief Is called when all modules are loaded and thread has started. */
- virtual void init();
-
- /** @brief Detect & handle possible collision */
- void handleCollision();
-
- private:
- /** @brief Start navigation to m_Target on last_map_data_ */
- void startNavigation();
-
- /** @brief Check if obstacles are blocking the way in last_map_data_ */
- bool checkPath();
-
- /** @brief calculate path from current robot position to target approximation */
- void calculatePath();
-
- /** @brief Send message containing current navigation path */
- void sendPathData();
-
- /** @brief Sends target reached and stops the robot. */
- void sendTargetReachedMsg();
-
- /**
- * @brief Sends a target unreachable with given reason and stops the robot.
- * @param reason reason for unreachable target (see homer_mapnav_msgs::TargetUnreachable for possible reasons)
- */
- void sendTargetUnreachableMsg( int8_t reason );
-
- /** @brief reloads all params from the parameterserver */
- void refreshParamsCallback(const std_msgs::Empty::ConstPtr& msg);
-
- /** @brief Navigate robot to next waypoint */
- void performNextMove();
-
- /** @brief Finishes navigation or starts turning to target direction if the target position has been reached */
- void targetPositionReached();
-
- /** @return Angle from robot_pose_ to point in degrees */
- int angleToPointDeg(geometry_msgs::Point point);
-
- /** @brief Calculates current maximal backwards distance on map Data */
- bool backwardObstacle();
-
- /** @brief stops the Robot */
- void stopRobot();
-
- /**
- * @brief Sets each cell of the map to -1 outside the bounding box
- * containing the robot pose and the current target
- */
- void maskMap();
-
- /**
- * @brief Current path was finished (either successful or not),
- * sets state machine to path planning to check if the robot is already at the goal
- */
- void currentPathFinished();
-
- //convenience math functions
- /**
- * Computes minimum turn angle from angle 1 to angle 2
- * @param angle1 from angle
- * @param angle2 to angle
- * @return minimal angle needed to turn from angle 1 to angle 2 [-Pi..Pi]
- */
- static float minTurnAngle ( float angle1, float angle2 );
-
- /**
- * converts value from degree to radiant
- * @param deg Value in degree
- * @return value in radiants
- */
- static float deg2Rad ( float deg ) { return deg / 180.0*M_PI; }
-
- /**
- * converts value from radiants to degrees
- * @param rad Value in radiants
- * @return value in degrees
- */
- static float rad2Deg ( float rad ) { return rad / M_PI*180.0; }
+ /** @brief calcs the maximal move distance from Laser and DepthData */
+ void calcMaxMoveDist();
- bool drawPolygon ( std::vector< geometry_msgs::Point > vertices);
- void drawLine ( std::vector<int> &data, int startX, int startY, int endX, int endY, int value );
- bool fillPolygon ( std::vector<int> &data, int x, int y, int value );
+ /// @brief Worker instances
+ Explorer* m_explorer;
- /** @brief calcs the maximal move distance from Laser and DepthData */
- void calcMaxMoveDist();
+ /// @brief State machine
+ StateMachine<ProcessState> m_MainMachine;
- /// @brief Worker instances
- Explorer* m_explorer;
+ /// @brief Navigation options & data
- /// @brief State machine
- StateMachine<ProcessState> m_MainMachine;
+ /** list of waypoints subsampled from m_PixelPath */
+ std::vector<geometry_msgs::PoseStamped> m_waypoints;
- /// @brief Navigation options & data
+ /** Path planned by Explorer, pixel accuracy */
+ std::vector<Eigen::Vector2i> m_pixel_path;
+
+ /** target point */
+ geometry_msgs::Point m_target_point;
- /** list of waypoints subsampled from m_PixelPath */
- std::vector<geometry_msgs::PoseStamped> m_waypoints;
+ /** target name if called via Navigate_to_POI */
+ std::string m_target_name;
- /** Path planned by Explorer, pixel accuracy */
- std::vector<Eigen::Vector2i> m_pixel_path;
+ /** orientation the robot should have at the target point */
+ double m_target_orientation;
- /** target point */
- geometry_msgs::Point m_target_point;
-
- /** target name if called via Navigate_to_POI */
- std::string m_target_name;
+ /** allowed distance to target */
+ float m_desired_distance;
- /** orientation the robot should have at the target point */
- double m_target_orientation;
+ /** check if the final turn should be skipped */
+ bool m_skip_final_turn;
- /** allowed distance to target */
- float m_desired_distance;
+ /**
+ * check if navigation should perform fast planning. In this mode a path is
+ * only planned within
+ * a bounding box containing the robot pose and the target point
+ */
+ bool m_fast_path_planning;
- /** check if the final turn should be skipped */
- bool m_skip_final_turn;
+ /** current pose of the robot */
+ geometry_msgs::Pose m_robot_pose;
- /**
- * check if navigation should perform fast planning. In this mode a path is only planned within
- * a bounding box containing the robot pose and the target point
- */
- bool m_fast_path_planning;
-
- /** current pose of the robot */
- geometry_msgs::Pose m_robot_pose;
-
- /** last pose of the robot */
- geometry_msgs::Pose m_robot_last_pose;
-
- /** time stamp of the last incoming laser scan */
- ros::Time m_last_laser_time;
- /** time stamp of the last incoming pose */
- ros::Time m_last_pose_time;
-
- /** Distance factor of a frontier cell considered save for exploration */
- float m_FrontierSafenessFactor;
-
- double m_SafePathWeight;
-
- ///map parameters
- double m_resolution;
- double m_width;
- double m_height;
- geometry_msgs::Pose m_origin;
-
- /// @brief Configuration parameters
-
- /** Allowed distances of obstacles to robot. Robot must move within these bounds */
- std::pair<float,float> m_AllowedObstacleDistance;
-
- /** Safe distances of obstacles to robot. If possible, robot should move within these bounds */
- std::pair<float,float> m_SafeObstacleDistance;
-
- /** threshold to sample down waypoints */
- float m_waypoint_sampling_threshold;
-
- float m_max_move_distance;
- float m_max_move_sick;
- float m_max_move_down;
- float m_max_move_depth;
-
- /** if distance to nearest obstacle is below collision distance trigger collision avoidance */
- float m_collision_distance;
-
- /** if distance to nearest obstacle is below collision distance don't drive backwards */
- float m_backward_collision_distance;
- /** do not drive back in collision avoidance when this near target */
- float m_collision_distance_near_target;
-
- /** if true, obstacles in path will be detected and path will be replanned */
- bool m_check_path;
-
- /** waypoints will only be checked for obstacles if they are closer than check_path_max_distance to robot */
- float m_check_path_max_distance;
-
- bool m_avoided_collision;
-
- float m_min_turn_angle;
- float m_max_turn_speed;
- float m_min_turn_speed;
- float m_max_move_speed;
- float m_max_drive_angle;
- float m_waypoint_radius_factor;
+ /** last pose of the robot */
+ geometry_msgs::Pose m_robot_last_pose;
+
+ /** time stamp of the last incoming laser scan */
+ ros::Time m_last_laser_time;
+ /** time stamp of the last incoming pose */
+ ros::Time m_last_pose_time;
+
+ /** Distance factor of a frontier cell considered save for exploration */
+ float m_FrontierSafenessFactor;
+
+ double m_SafePathWeight;
+
+ /// map parameters
+ double m_resolution;
+ double m_width;
+ double m_height;
+ geometry_msgs::Pose m_origin;
+
+ /// @brief Configuration parameters
+
+ /** Allowed distances of obstacles to robot. Robot must move within these
+ * bounds */
+ std::pair<float, float> m_AllowedObstacleDistance;
+
+ /** Safe distances of obstacles to robot. If possible, robot should move
+ * within these bounds */
+ std::pair<float, float> m_SafeObstacleDistance;
+
+ /** threshold to sample down waypoints */
+ float m_waypoint_sampling_threshold;
+
+ float m_max_move_distance;
+ float m_max_move_sick;
+ float m_max_move_down;
+ float m_max_move_depth;
+
+ /** if distance to nearest obstacle is below collision distance trigger
+ * collision avoidance */
+ float m_collision_distance;
+
+ /** if distance to nearest obstacle is below collision distance don't drive
+ * backwards */
+ float m_backward_collision_distance;
+ /** do not drive back in collision avoidance when this near target */
+ float m_collision_distance_near_target;
+
+ /** if true, obstacles in path will be detected and path will be replanned */
+ bool m_check_path;
+
+ /** waypoints will only be checked for obstacles if they are closer than
+ * check_path_max_distance to robot */
+ float m_check_path_max_distance;
+
+ /** do not replan if lisa avoids an obstacle, instead send target
+ * unreachable*/
+ bool m_stop_before_obstacle;
- float m_distance_to_target;
- float m_act_speed;
- float m_angular_avoidance;
+ bool m_avoided_collision;
- float m_map_speed_factor;
- float m_waypoint_speed_factor;
- float m_obstacle_speed_factor;
- float m_target_distance_speed_factor;
-
- float m_min_y;
- float m_min_x;
-
- float m_callback_error_duration;
-
- bool m_last_check_path_res;
- bool m_use_ptu;
- bool m_new_target;
-
- bool m_path_reaches_target;
- int m_last_calculations_failed;
- int m_unknown_threshold;
-
- /** last map data */
- std::vector<int8_t> * m_last_map_data;
-
- //ros specific members
- tf::TransformListener m_transform_listener;
-
- //subscribers
- ros::Subscriber m_map_sub;
- ros::Subscriber m_pose_sub;
- ros::Subscriber m_laser_data_sub;
- ros::Subscriber m_down_laser_data_sub;
- ros::Subscriber m_laser_back_data_sub;
- ros::Subscriber m_start_navigation_sub;
- ros::Subscriber m_stop_navigation_sub;
- ros::Subscriber m_navigate_to_poi_sub;
- ros::Subscriber m_unknown_threshold_sub;
- ros::Subscriber m_refresh_param_sub;
- ros::Subscriber m_max_move_depth_sub;
- ros::Subscriber m_move_base_simple_goal_sub;
-
- //publishers
- ros::Publisher m_cmd_vel_pub;
- ros::Publisher m_target_reached_string_pub;
- //ros::Publisher m_target_reached_empty_pub;
- ros::Publisher m_target_unreachable_pub;
- ros::Publisher m_path_pub;
- ros::Publisher m_ptu_center_world_point_pub;
- ros::Publisher m_set_pan_tilt_pub;
- ros::Publisher m_debug_pub;
-
- //service clients
- ros::ServiceClient m_get_POIs_client;
+ float m_min_turn_angle;
+ float m_max_turn_speed;
+ float m_min_turn_speed;
+ float m_max_move_speed;
+ float m_max_drive_angle;
+ float m_waypoint_radius_factor;
+
+ float m_distance_to_target;
+ float m_act_speed;
+ float m_angular_avoidance;
+
+ float m_map_speed_factor;
+ float m_waypoint_speed_factor;
+ float m_obstacle_speed_factor;
+ float m_target_distance_speed_factor;
+
+ float m_min_y;
+ float m_min_x;
+
+ float m_callback_error_duration;
+
+ bool m_last_check_path_res;
+ bool m_use_ptu;
+ bool m_new_target;
+
+ bool m_path_reaches_target;
+ bool m_initial_path_reaches_target;
+ int m_last_calculations_failed;
+ int m_unknown_threshold;
+
+ /** last map data */
+ std::vector<int8_t>* m_last_map_data;
+
+ // ros specific members
+ tf::TransformListener m_transform_listener;
+
+ // subscribers
+ ros::Subscriber m_map_sub;
+ ros::Subscriber m_pose_sub;
+ ros::Subscriber m_laser_data_sub;
+ ros::Subscriber m_down_laser_data_sub;
+ ros::Subscriber m_laser_back_data_sub;
+ ros::Subscriber m_start_navigation_sub;
+ ros::Subscriber m_stop_navigation_sub;
+ ros::Subscriber m_navigate_to_poi_sub;
+ ros::Subscriber m_unknown_threshold_sub;
+ ros::Subscriber m_refresh_param_sub;
+ ros::Subscriber m_max_move_depth_sub;
+ ros::Subscriber m_move_base_simple_goal_sub;
+
+ // publishers
+ ros::Publisher m_cmd_vel_pub;
+ ros::Publisher m_target_reached_string_pub;
+ // ros::Publisher m_target_reached_empty_pub;
+ ros::Publisher m_target_unreachable_pub;
+ ros::Publisher m_path_pub;
+ ros::Publisher m_ptu_center_world_point_pub;
+ ros::Publisher m_set_pan_tilt_pub;
+ ros::Publisher m_debug_pub;
+
+ // service clients
+ ros::ServiceClient m_get_POIs_client;
};
#endif
diff --git a/homer_navigation/package.xml b/homer_navigation/package.xml
index df91397f..5a8209a2 100644
--- a/homer_navigation/package.xml
+++ b/homer_navigation/package.xml
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package>
<name>homer_navigation</name>
- <version>0.1.6</version>
+ <version>0.1.7</version>
<description>The homer_navigation package</description>
<maintainer email="vseib@uni-koblenz.de">Viktor Seib</maintainer>
diff --git a/homer_navigation/src/homer_navigation_node.cpp b/homer_navigation/src/homer_navigation_node.cpp
index 9bad6a76..d99b8ee1 100644
--- a/homer_navigation/src/homer_navigation_node.cpp
+++ b/homer_navigation/src/homer_navigation_node.cpp
@@ -1,1084 +1,1122 @@
#include "homer_navigation/homer_navigation_node.h"
-HomerNavigationNode::HomerNavigationNode()
-{
- ros::NodeHandle nh;
-
- //subscribers
- m_map_sub = nh.subscribe<nav_msgs::OccupancyGrid>("/map", 1, &HomerNavigationNode::mapCallback, this);
- m_pose_sub = nh.subscribe<geometry_msgs::PoseStamped>("/pose", 1, &HomerNavigationNode::poseCallback, this);
- 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);
- m_start_navigation_sub = nh.subscribe<homer_mapnav_msgs::StartNavigation>("/homer_navigation/start_navigation", 1, &HomerNavigationNode::startNavigationCallback, this);
- m_move_base_simple_goal_sub = nh.subscribe<geometry_msgs::PoseStamped>("/move_base_simple/goal", 1, &HomerNavigationNode::moveBaseSimpleGoalCallback, this); // for RVIZ usage
- m_stop_navigation_sub = nh.subscribe<std_msgs::Empty>("/homer_navigation/stop_navigation", 1, &HomerNavigationNode::stopNavigationCallback, this);
- m_navigate_to_poi_sub = nh.subscribe<homer_mapnav_msgs::NavigateToPOI>("/homer_navigation/navigate_to_POI", 1, &HomerNavigationNode::navigateToPOICallback, this);
- m_unknown_threshold_sub = nh.subscribe<std_msgs::Int8>("/homer_navigation/unknown_threshold", 1, &HomerNavigationNode::unknownThresholdCallback, this);
- m_refresh_param_sub = nh.subscribe<std_msgs::Empty>("/homer_navigation/refresh_params", 1, &HomerNavigationNode::refreshParamsCallback, this);
- m_max_move_depth_sub = nh.subscribe<std_msgs::Float32>("/homer_navigation/max_depth_move_distance", 1, &HomerNavigationNode::maxDepthMoveDistanceCallback, this);
-
- m_cmd_vel_pub = nh.advertise<geometry_msgs::Twist>("/robot_platform/cmd_vel", 1);
- m_target_reached_string_pub = nh.advertise<std_msgs::String>("/homer_navigation/target_reached", 1);
- //m_target_reached_empty_pub = nh.advertise<std_msgs::Empty>("/homer_navigation/target_reached", 1);
- m_target_unreachable_pub = nh.advertise<homer_mapnav_msgs::TargetUnreachable>("/homer_navigation/target_unreachable", 1);
- m_path_pub = nh.advertise<nav_msgs::Path>("/homer_navigation/path", 1);
- m_ptu_center_world_point_pub= nh.advertise<homer_ptu_msgs::CenterWorldPoint>( "/ptu/center_world_point", 1);
- m_set_pan_tilt_pub = nh.advertise<homer_ptu_msgs::SetPanTilt>("/ptu/set_pan_tilt", 1);
- m_debug_pub = nh.advertise<std_msgs::String>("/homer_navigation/debug",1);
-
- m_get_POIs_client = nh.serviceClient<homer_mapnav_msgs::GetPointsOfInterest>("/map_manager/get_pois");
-
- m_MainMachine.setName( "HomerNavigation Main" );
- ADD_MACHINE_STATE( m_MainMachine, IDLE );
- ADD_MACHINE_STATE( m_MainMachine, AWAITING_PATHPLANNING_MAP );
- ADD_MACHINE_STATE( m_MainMachine, FOLLOWING_PATH );
- ADD_MACHINE_STATE( m_MainMachine, AVOIDING_COLLISION );
- ADD_MACHINE_STATE( m_MainMachine, FINAL_TURN );
-
- init();
+HomerNavigationNode::HomerNavigationNode() {
+ ros::NodeHandle nh;
+
+ // subscribers
+ m_map_sub = nh.subscribe<nav_msgs::OccupancyGrid>(
+ "/map", 1, &HomerNavigationNode::mapCallback, this);
+ m_pose_sub = nh.subscribe<geometry_msgs::PoseStamped>(
+ "/pose", 1, &HomerNavigationNode::poseCallback, this);
+ 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);
+ m_start_navigation_sub = nh.subscribe<homer_mapnav_msgs::StartNavigation>(
+ "/homer_navigation/start_navigation", 1,
+ &HomerNavigationNode::startNavigationCallback, this);
+ m_move_base_simple_goal_sub = nh.subscribe<geometry_msgs::PoseStamped>(
+ "/move_base_simple/goal", 1,
+ &HomerNavigationNode::moveBaseSimpleGoalCallback,
+ this); // for RVIZ usage
+ m_stop_navigation_sub = nh.subscribe<std_msgs::Empty>(
+ "/homer_navigation/stop_navigation", 1,
+ &HomerNavigationNode::stopNavigationCallback, this);
+ m_navigate_to_poi_sub = nh.subscribe<homer_mapnav_msgs::NavigateToPOI>(
+ "/homer_navigation/navigate_to_POI", 1,
+ &HomerNavigationNode::navigateToPOICallback, this);
+ m_unknown_threshold_sub = nh.subscribe<std_msgs::Int8>(
+ "/homer_navigation/unknown_threshold", 1,
+ &HomerNavigationNode::unknownThresholdCallback, this);
+ m_refresh_param_sub = nh.subscribe<std_msgs::Empty>(
+ "/homer_navigation/refresh_params", 1,
+ &HomerNavigationNode::refreshParamsCallback, this);
+ m_max_move_depth_sub = nh.subscribe<std_msgs::Float32>(
+ "/homer_navigation/max_depth_move_distance", 1,
+ &HomerNavigationNode::maxDepthMoveDistanceCallback, this);
+
+ m_cmd_vel_pub =
+ nh.advertise<geometry_msgs::Twist>("/robot_platform/cmd_vel", 1);
+ m_target_reached_string_pub =
+ nh.advertise<std_msgs::String>("/homer_navigation/target_reached", 1);
+ // m_target_reached_empty_pub =
+ // nh.advertise<std_msgs::Empty>("/homer_navigation/target_reached", 1);
+ m_target_unreachable_pub = nh.advertise<homer_mapnav_msgs::TargetUnreachable>(
+ "/homer_navigation/target_unreachable", 1);
+ m_path_pub = nh.advertise<nav_msgs::Path>("/homer_navigation/path", 1);
+ m_ptu_center_world_point_pub = nh.advertise<homer_ptu_msgs::CenterWorldPoint>(
+ "/ptu/center_world_point", 1);
+ m_set_pan_tilt_pub =
+ nh.advertise<homer_ptu_msgs::SetPanTilt>("/ptu/set_pan_tilt", 1);
+ m_debug_pub = nh.advertise<std_msgs::String>("/homer_navigation/debug", 1);
+
+ m_get_POIs_client = nh.serviceClient<homer_mapnav_msgs::GetPointsOfInterest>(
+ "/map_manager/get_pois");
+
+ m_MainMachine.setName("HomerNavigation Main");
+ ADD_MACHINE_STATE(m_MainMachine, IDLE);
+ ADD_MACHINE_STATE(m_MainMachine, AWAITING_PATHPLANNING_MAP);
+ ADD_MACHINE_STATE(m_MainMachine, FOLLOWING_PATH);
+ ADD_MACHINE_STATE(m_MainMachine, AVOIDING_COLLISION);
+ ADD_MACHINE_STATE(m_MainMachine, FINAL_TURN);
+
+ init();
}
-void HomerNavigationNode::loadParameters()
-{
- //Explorer constructor
- ros::param::param("/homer_mapping/resolution", m_resolution, (double) 0.05);
- ros::param::param("/homer_navigation/allowed_obstacle_distance/min", m_AllowedObstacleDistance.first, (float) 0.3);
- ros::param::param("/homer_navigation/allowed_obstacle_distance/max", m_AllowedObstacleDistance.second, (float) 5.0);
- ros::param::param("/homer_navigation/safe_obstacle_distance/min", m_SafeObstacleDistance.first, (float) 0.7);
- ros::param::param("/homer_navigation/safe_obstacle_distance/max", m_SafeObstacleDistance.second, (float) 1.5);
- ros::param::param("/homer_navigation/frontier_safeness_factor", m_FrontierSafenessFactor, (float) 1.4);
- ros::param::param("/homer_navigation/safe_path_weight", m_SafePathWeight, (double) 1.2);
- ros::param::param("/homer_navigation/waypoint_sampling_threshold", m_waypoint_sampling_threshold, (float) 1.5);
- m_AllowedObstacleDistance.first /= m_resolution;
- m_AllowedObstacleDistance.second /= m_resolution;
- m_SafeObstacleDistance.first /= m_resolution;
- m_SafeObstacleDistance.second /= m_resolution;
-
- //check path
- ros::param::param("/homer_navigation/check_path", m_check_path, (bool) true);
- ros::param::param("/homer_navigation/check_path_max_distance", m_check_path_max_distance, (float) 2.0);
-
- //collision
- ros::param::param("/homer_navigation/collision_distance", m_collision_distance, (float) 0.3);
- ros::param::param("/homer_navigation/collision_distance_near_target", m_collision_distance_near_target, (float) 0.2);
- ros::param::param("/homer_navigation/backward_collision_distance", m_backward_collision_distance, (float) 0.5);
-
- //cmd_vel config values
- ros::param::param("/homer_navigation/min_turn_angle", m_min_turn_angle, (float) 0.15);
- ros::param::param("/homer_navigation/max_turn_speed", m_max_turn_speed, (float) 0.6);
- ros::param::param("/homer_navigation/min_turn_speed", m_min_turn_speed, (float) 0.3);
- ros::param::param("/homer_navigation/max_move_speed", m_max_move_speed, (float) 0.4);
- ros::param::param("/homer_navigation/max_drive_angle", m_max_drive_angle, (float) 0.6);
-
- //caution factors
- ros::param::param("/homer_navigation/map_speed_factor", m_map_speed_factor, (float) 1.0);
- ros::param::param("/homer_navigation/waypoint_speed_factor", m_waypoint_speed_factor, (float) 1.0);
- ros::param::param("/homer_navigation/obstacle_speed_factor", m_obstacle_speed_factor, (float) 1.0);
- ros::param::param("/homer_navigation/target_distance_speed_factor", m_target_distance_speed_factor, (float) 0.4);
-
- //robot dimensions
- ros::param::param("/homer_navigation/min_x", m_min_x, (float) 0.3);
- ros::param::param("/homer_navigation/min_y", m_min_y, (float) 0.27);
-
- //error durations
- ros::param::param("/homer_navigation/callback_error_duration", m_callback_error_duration, (float) 0.3);
-
- ros::param::param("/homer_navigation/use_ptu", m_use_ptu, (bool) false);
-
- ros::param::param("/homer_navigation/unknown_threshold", m_unknown_threshold, (int) 50);
- ros::param::param("/homer_navigation/waypoint_radius_factor", m_waypoint_radius_factor, (float) 0.25);
+void HomerNavigationNode::loadParameters() {
+ // Explorer constructor
+ ros::param::param("/homer_mapping/resolution", m_resolution, (double)0.05);
+ ros::param::param("/homer_navigation/allowed_obstacle_distance/min",
+ m_AllowedObstacleDistance.first, (float)0.3);
+ ros::param::param("/homer_navigation/allowed_obstacle_distance/max",
+ m_AllowedObstacleDistance.second, (float)5.0);
+ ros::param::param("/homer_navigation/safe_obstacle_distance/min",
+ m_SafeObstacleDistance.first, (float)0.7);
+ ros::param::param("/homer_navigation/safe_obstacle_distance/max",
+ m_SafeObstacleDistance.second, (float)1.5);
+ ros::param::param("/homer_navigation/frontier_safeness_factor",
+ m_FrontierSafenessFactor, (float)1.4);
+ ros::param::param("/homer_navigation/safe_path_weight", m_SafePathWeight,
+ (double)1.2);
+ ros::param::param("/homer_navigation/waypoint_sampling_threshold",
+ m_waypoint_sampling_threshold, (float)1.5);
+ m_AllowedObstacleDistance.first /= m_resolution;
+ m_AllowedObstacleDistance.second /= m_resolution;
+ m_SafeObstacleDistance.first /= m_resolution;
+ m_SafeObstacleDistance.second /= m_resolution;
+
+ // check path
+ ros::param::param("/homer_navigation/check_path", m_check_path, (bool)true);
+ ros::param::param("/homer_navigation/check_path_max_distance",
+ m_check_path_max_distance, (float)2.0);
+
+ // collision
+ ros::param::param("/homer_navigation/collision_distance",
+ m_collision_distance, (float)0.3);
+ ros::param::param("/homer_navigation/collision_distance_near_target",
+ m_collision_distance_near_target, (float)0.2);
+ ros::param::param("/homer_navigation/backward_collision_distance",
+ m_backward_collision_distance, (float)0.5);
+
+ // cmd_vel config values
+ ros::param::param("/homer_navigation/min_turn_angle", m_min_turn_angle,
+ (float)0.15);
+ ros::param::param("/homer_navigation/max_turn_speed", m_max_turn_speed,
+ (float)0.6);
+ ros::param::param("/homer_navigation/min_turn_speed", m_min_turn_speed,
+ (float)0.3);
+ ros::param::param("/homer_navigation/max_move_speed", m_max_move_speed,
+ (float)0.4);
+ ros::param::param("/homer_navigation/max_drive_angle", m_max_drive_angle,
+ (float)0.6);
+
+ // caution factors
+ ros::param::param("/homer_navigation/map_speed_factor", m_map_speed_factor,
+ (float)1.0);
+ ros::param::param("/homer_navigation/waypoint_speed_factor",
+ m_waypoint_speed_factor, (float)1.0);
+ ros::param::param("/homer_navigation/obstacle_speed_factor",
+ m_obstacle_speed_factor, (float)1.0);
+ ros::param::param("/homer_navigation/target_distance_speed_factor",
+ m_target_distance_speed_factor, (float)0.4);
+
+ // robot dimensions
+ ros::param::param("/homer_navigation/min_x", m_min_x, (float)0.3);
+ ros::param::param("/homer_navigation/min_y", m_min_y, (float)0.27);
+
+ // error durations
+ ros::param::param("/homer_navigation/callback_error_duration",
+ m_callback_error_duration, (float)0.3);
+
+ ros::param::param("/homer_navigation/use_ptu", m_use_ptu, (bool)false);
+
+ ros::param::param("/homer_navigation/unknown_threshold", m_unknown_threshold,
+ (int)50);
+ ros::param::param("/homer_navigation/waypoint_radius_factor",
+ m_waypoint_radius_factor, (float)0.25);
}
-void HomerNavigationNode::init()
-{
- m_max_move_sick = 40.0;
- m_max_move_down = 40.0;
- m_max_move_depth = 40.0;
- m_robot_pose.position.x = 0.0;
- m_robot_pose.position.y = 0.0;
- m_robot_pose.position.z = 0.0;
- m_robot_pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
- m_robot_last_pose = m_robot_pose;
- m_avoided_collision = false;
- m_act_speed = 0;
- m_angular_avoidance = 0;
- m_last_calculations_failed = 0;
- m_last_check_path_res = false;
- m_new_target = true;
-
- loadParameters();
-
- m_explorer = new Explorer ( m_AllowedObstacleDistance.first, m_AllowedObstacleDistance.second,
- m_SafeObstacleDistance.first, m_SafeObstacleDistance.second,
- m_SafePathWeight, m_FrontierSafenessFactor, m_unknown_threshold );
- m_last_map_data = new std::vector<int8_t>(0);
-
- m_MainMachine.setState ( IDLE );
+void HomerNavigationNode::init() {
+ m_max_move_sick = 40.0;
+ m_max_move_down = 40.0;
+ m_max_move_depth = 40.0;
+ m_robot_pose.position.x = 0.0;
+ m_robot_pose.position.y = 0.0;
+ m_robot_pose.position.z = 0.0;
+ m_robot_pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
+ m_robot_last_pose = m_robot_pose;
+ m_avoided_collision = false;
+ m_act_speed = 0;
+ m_angular_avoidance = 0;
+ m_last_calculations_failed = 0;
+ m_last_check_path_res = false;
+ m_new_target = true;
+
+ loadParameters();
+
+ m_explorer = new Explorer(
+ m_AllowedObstacleDistance.first, m_AllowedObstacleDistance.second,
+ m_SafeObstacleDistance.first, m_SafeObstacleDistance.second,
+ m_SafePathWeight, m_FrontierSafenessFactor, m_unknown_threshold);
+ m_last_map_data = new std::vector<int8_t>(0);
+
+ m_MainMachine.setState(IDLE);
}
-HomerNavigationNode::~HomerNavigationNode()
-{
- if(m_explorer)
- {
- delete m_explorer;
- }
- if(m_last_map_data)
- {
- delete m_last_map_data;
- }
+HomerNavigationNode::~HomerNavigationNode() {
+ if (m_explorer) {
+ delete m_explorer;
+ }
+ if (m_last_map_data) {
+ delete m_last_map_data;
+ }
}
-void HomerNavigationNode::stopRobot()
-{
- m_act_speed = 0;
- geometry_msgs::Twist cmd_vel_msg;
- cmd_vel_msg.linear.x = 0;
- cmd_vel_msg.linear.y = 0;
- cmd_vel_msg.linear.z = 0;
- cmd_vel_msg.angular.x = 0;
- cmd_vel_msg.angular.y = 0;
- cmd_vel_msg.angular.z = 0;
- m_cmd_vel_pub.publish(cmd_vel_msg);
- ros::Duration(0.1).sleep();
- m_cmd_vel_pub.publish(cmd_vel_msg);
- ros::Duration(0.1).sleep();
- m_cmd_vel_pub.publish(cmd_vel_msg);
+void HomerNavigationNode::stopRobot() {
+ m_act_speed = 0;
+ geometry_msgs::Twist cmd_vel_msg;
+ cmd_vel_msg.linear.x = 0;
+ cmd_vel_msg.linear.y = 0;
+ cmd_vel_msg.linear.z = 0;
+ cmd_vel_msg.angular.x = 0;
+ cmd_vel_msg.angular.y = 0;
+ cmd_vel_msg.angular.z = 0;
+ m_cmd_vel_pub.publish(cmd_vel_msg);
+ ros::Duration(0.1).sleep();
+ m_cmd_vel_pub.publish(cmd_vel_msg);
+ ros::Duration(0.1).sleep();
+ m_cmd_vel_pub.publish(cmd_vel_msg);
}
-void HomerNavigationNode::idleProcess()
-{
- if ( m_MainMachine.state() == FOLLOWING_PATH )
- {
- if ( (ros::Time::now() - m_last_laser_time) > ros::Duration( m_callback_error_duration ) )
- {
- ROS_ERROR_STREAM("Laser data timeout!\n");
- stopRobot();
- }
- if ( (ros::Time::now() - m_last_pose_time) > ros::Duration( m_callback_error_duration ) )
- {
- ROS_ERROR_STREAM("Pose timeout!\n");
- stopRobot();
- }
- }
+void HomerNavigationNode::idleProcess() {
+ if (m_MainMachine.state() == FOLLOWING_PATH) {
+ if ((ros::Time::now() - m_last_laser_time) >
+ ros::Duration(m_callback_error_duration)) {
+ ROS_ERROR_STREAM("Laser data timeout!\n");
+ stopRobot();
+ }
+ if ((ros::Time::now() - m_last_pose_time) >
+ ros::Duration(m_callback_error_duration)) {
+ ROS_ERROR_STREAM("Pose timeout!\n");
+ stopRobot();
+ }
+ }
}
-void HomerNavigationNode::calculatePath()
-{
- if ( m_distance_to_target < m_desired_distance && !m_new_target)
- {
- m_path_reaches_target = true;
- return;
- }
- m_explorer->setOccupancyMap(m_width, m_width, m_origin, &(*m_last_map_data)[0]);
- m_explorer->setStart( map_tools::toMapCoords( m_robot_pose.position, m_origin, m_resolution) );
-
- bool success;
- m_pixel_path = m_explorer->getPath( success );
- if ( !success)
- {
- ROS_WARN_STREAM("No path found for navigation" );
- m_last_calculations_failed++;
- ROS_INFO_STREAM("last_calculation_failed: "<<m_last_calculations_failed);
- if(m_last_calculations_failed > 8)
- {
- sendTargetUnreachableMsg(homer_mapnav_msgs::TargetUnreachable::NO_PATH_FOUND);
- }
- }
- else
- {
- m_last_calculations_failed = 0;
- std::vector<Eigen::Vector2i> waypoint_pixels = m_explorer->sampleWaypointsFromPath ( m_pixel_path, m_waypoint_sampling_threshold );
- m_waypoints.clear();
- ROS_INFO_STREAM("homer_navigation::calculatePath - Path Size: " << waypoint_pixels.size());
- if(waypoint_pixels.size() > 0)
- {
- for(std::vector<Eigen::Vector2i>::iterator it = waypoint_pixels.begin(); it != waypoint_pixels.end(); ++it)
- {
- geometry_msgs::PoseStamped poseStamped;
- poseStamped.header.frame_id = "/map";
- poseStamped.pose.position = map_tools::fromMapCoords(*it, m_origin, m_resolution);
- poseStamped.pose.orientation.x = 0.0;
- poseStamped.pose.orientation.y = 0.0;
- poseStamped.pose.orientation.z = 0.0;
- poseStamped.pose.orientation.w = 1.0;
- m_waypoints.push_back(poseStamped);
- }
- sendPathData();
- }
- else
- {
- sendTargetUnreachableMsg(homer_mapnav_msgs::TargetUnreachable::NO_PATH_FOUND);
- }
-
- m_last_laser_time = ros::Time::now();
- m_last_pose_time = ros::Time::now();
- }
+void HomerNavigationNode::calculatePath() {
+ if (m_distance_to_target < m_desired_distance && !m_new_target) {
+ m_path_reaches_target = true;
+ return;
+ }
+ m_explorer->setOccupancyMap(m_width, m_width, m_origin,
+ &(*m_last_map_data)[0]);
+ m_explorer->setStart(
+ map_tools::toMapCoords(m_robot_pose.position, m_origin, m_resolution));
+
+ bool success;
+ m_pixel_path = m_explorer->getPath(success);
+ if (!success) {
+ ROS_WARN_STREAM("No path found for navigation");
+ m_last_calculations_failed++;
+ ROS_INFO_STREAM("last_calculation_failed: " << m_last_calculations_failed);
+ if (m_last_calculations_failed > 8) {
+ sendTargetUnreachableMsg(
+ homer_mapnav_msgs::TargetUnreachable::NO_PATH_FOUND);
+ }
+ } else {
+ m_last_calculations_failed = 0;
+ std::vector<Eigen::Vector2i> waypoint_pixels =
+ m_explorer->sampleWaypointsFromPath(m_pixel_path,
+ m_waypoint_sampling_threshold);
+ m_waypoints.clear();
+ ROS_INFO_STREAM("homer_navigation::calculatePath - Path Size: "
+ << waypoint_pixels.size());
+ if (waypoint_pixels.size() > 0) {
+ for (std::vector<Eigen::Vector2i>::iterator it = waypoint_pixels.begin();
+ it != waypoint_pixels.end(); ++it) {
+ geometry_msgs::PoseStamped poseStamped;
+ poseStamped.header.frame_id = "/map";
+ poseStamped.pose.position =
+ map_tools::fromMapCoords(*it, m_origin, m_resolution);
+ poseStamped.pose.orientation.x = 0.0;
+ poseStamped.pose.orientation.y = 0.0;
+ poseStamped.pose.orientation.z = 0.0;
+ poseStamped.pose.orientation.w = 1.0;
+ m_waypoints.push_back(poseStamped);
+ }
+ sendPathData();
+ } else {
+ sendTargetUnreachableMsg(
+ homer_mapnav_msgs::TargetUnreachable::NO_PATH_FOUND);
+ }
+
+ m_last_laser_time = ros::Time::now();
+ m_last_pose_time = ros::Time::now();
+ }
}
-void HomerNavigationNode::startNavigation()
-{
- if ( m_distance_to_target < m_desired_distance && !m_new_target)
- {
- ROS_INFO_STREAM( "Will not (re-)plan path: Target position already reached." );
- m_path_reaches_target = true;
- targetPositionReached();
- return;
- }
- ROS_INFO_STREAM( "Distance to target still too large (" << m_distance_to_target << "m; requested: " << m_desired_distance << "m)" );
-
- if(m_fast_path_planning)
- {
- maskMap();
- }
-
- m_explorer->setOccupancyMap(m_width, m_width, m_origin, &(*m_last_map_data)[0]);
- m_explorer->setStart(map_tools::toMapCoords(m_robot_pose.position, m_origin, m_resolution));
- Eigen::Vector2i new_target_approx = m_explorer->getNearestAccessibleTarget(map_tools::toMapCoords(m_target_point, m_origin, m_resolution));
-
- geometry_msgs::Point new_target_approx_world = map_tools::fromMapCoords(new_target_approx, m_origin, m_resolution);
- ROS_INFO_STREAM("start Navigation: Approx target: " << new_target_approx_world);
-
- m_path_reaches_target = ( map_tools::distance(new_target_approx_world, m_target_point ) < m_desired_distance );
-
- bool new_approx_is_better = ( map_tools::distance( m_robot_pose.position, m_target_point ) - map_tools::distance( new_target_approx_world, m_target_point ) ) > 2 * m_desired_distance;
- if ( !new_approx_is_better && !m_path_reaches_target )
- {
- ROS_WARN_STREAM( "No better way to target found, turning and then reporting as unreachable."
- << std::endl << "Distance to target: " << m_distance_to_target << "m; requested: " << m_desired_distance << "m" );
- m_MainMachine.setState( FINAL_TURN );
- }
- else
- {
- m_explorer->setTarget ( new_target_approx);
- m_MainMachine.setState ( FOLLOWING_PATH );
- calculatePath();
- }
+void HomerNavigationNode::startNavigation() {
+ if (m_distance_to_target < m_desired_distance && !m_new_target) {
+ ROS_INFO_STREAM(
+ "Will not (re-)plan path: Target position already reached.");
+ m_path_reaches_target = true;
+ targetPositionReached();
+ return;
+ }
+ ROS_INFO_STREAM("Distance to target still too large ("
+ << m_distance_to_target
+ << "m; requested: " << m_desired_distance << "m)");
+
+ if (m_fast_path_planning) {
+ maskMap();
+ }
+
+ m_explorer->setOccupancyMap(m_width, m_width, m_origin,
+ &(*m_last_map_data)[0]);
+
+ // check if there still exists a path to the original target
+ if (m_avoided_collision && m_initial_path_reaches_target &&
+ m_stop_before_obstacle) {
+ m_explorer->setStart(
+ map_tools::toMapCoords(m_robot_pose.position, m_origin, m_resolution));
+
+ bool success;
+ m_pixel_path = m_explorer->getPath(success);
+ if (!success) {
+ ROS_INFO_STREAM(
+ "Initial path would have reached target, new path does not. "
+ << "Sending target unreachable.");
+ sendTargetUnreachableMsg(
+ homer_mapnav_msgs::TargetUnreachable::LASER_OBSTACLE);
+ return;
+ }
+ }
+
+ m_explorer->setStart(
+ map_tools::toMapCoords(m_robot_pose.position, m_origin, m_resolution));
+ Eigen::Vector2i new_target_approx = m_explorer->getNearestAccessibleTarget(
+ map_tools::toMapCoords(m_target_point, m_origin, m_resolution));
+
+ geometry_msgs::Point new_target_approx_world =
+ map_tools::fromMapCoords(new_target_approx, m_origin, m_resolution);
+ ROS_INFO_STREAM(
+ "start Navigation: Approx target: " << new_target_approx_world);
+
+ m_path_reaches_target =
+ (map_tools::distance(new_target_approx_world, m_target_point) <
+ m_desired_distance);
+ m_initial_path_reaches_target = m_path_reaches_target;
+
+ bool new_approx_is_better =
+ (map_tools::distance(m_robot_pose.position, m_target_point) -
+ map_tools::distance(new_target_approx_world, m_target_point)) >
+ 2 * m_desired_distance;
+ if (!new_approx_is_better && !m_path_reaches_target) {
+ ROS_WARN_STREAM(
+ "No better way to target found, turning and then reporting as "
+ "unreachable."
+ << std::endl
+ << "Distance to target: " << m_distance_to_target
+ << "m; requested: " << m_desired_distance << "m");
+ m_MainMachine.setState(FINAL_TURN);
+ } else {
+ m_explorer->setTarget(new_target_approx);
+ m_MainMachine.setState(FOLLOWING_PATH);
+ calculatePath();
+ }
}
-void HomerNavigationNode::sendPathData()
-{
- nav_msgs::Path msg;
- msg.header.frame_id = "/map";
- msg.header.stamp = ros::Time::now();
- if(m_waypoints.size() > 0)
- {
- msg.poses = m_waypoints;
- geometry_msgs::PoseStamped pose_stamped;
- pose_stamped.pose = m_robot_pose;
- pose_stamped.header.frame_id = "/map";
- pose_stamped.header.stamp = ros::Time::now();
- msg.poses.insert(msg.poses.begin(), pose_stamped);
- }
- m_path_pub.publish(msg);
+void HomerNavigationNode::sendPathData() {
+ nav_msgs::Path msg;
+ msg.header.frame_id = "/map";
+ msg.header.stamp = ros::Time::now();
+ if (m_waypoints.size() > 0) {
+ msg.poses = m_waypoints;
+ geometry_msgs::PoseStamped pose_stamped;
+ pose_stamped.pose = m_robot_pose;
+ pose_stamped.header.frame_id = "/map";
+ pose_stamped.header.stamp = ros::Time::now();
+ msg.poses.insert(msg.poses.begin(), pose_stamped);
+ }
+ m_path_pub.publish(msg);
}
-void HomerNavigationNode::sendTargetReachedMsg()
-{
- stopRobot();
- m_MainMachine.setState( IDLE );
- std_msgs::String reached_string_msg;
- reached_string_msg.data = m_target_name;
- m_target_reached_string_pub.publish(reached_string_msg);
- m_waypoints.clear();
- nav_msgs::Path empty_path_msg;
- empty_path_msg.poses = m_waypoints;
- m_path_pub.publish(empty_path_msg);
- ROS_INFO_STREAM("=== Reached Target " << m_target_name << " ===");
+void HomerNavigationNode::sendTargetReachedMsg() {
+ stopRobot();
+ m_MainMachine.setState(IDLE);
+ std_msgs::String reached_string_msg;
+ reached_string_msg.data = m_target_name;
+ m_target_reached_string_pub.publish(reached_string_msg);
+ m_waypoints.clear();
+ nav_msgs::Path empty_path_msg;
+ empty_path_msg.poses = m_waypoints;
+ m_path_pub.publish(empty_path_msg);
+ ROS_INFO_STREAM("=== Reached Target " << m_target_name << " ===");
}
-void HomerNavigationNode::sendTargetUnreachableMsg( int8_t reason )
-{
- stopRobot();
- m_MainMachine.setState( IDLE );
- homer_mapnav_msgs::TargetUnreachable unreachable_msg;
- unreachable_msg.reason = reason;
- m_target_unreachable_pub.publish(unreachable_msg);
- m_waypoints.clear();
- nav_msgs::Path empty_path_msg;
- empty_path_msg.poses = m_waypoints;
- m_path_pub.publish(empty_path_msg);
- ROS_INFO_STREAM("=== TargetUnreachableMsg ===");
+void HomerNavigationNode::sendTargetUnreachableMsg(int8_t reason) {
+ stopRobot();
+ m_MainMachine.setState(IDLE);
+ homer_mapnav_msgs::TargetUnreachable unreachable_msg;
+ unreachable_msg.reason = reason;
+ m_target_unreachable_pub.publish(unreachable_msg);
+ m_waypoints.clear();
+ nav_msgs::Path empty_path_msg;
+ empty_path_msg.poses = m_waypoints;
+ m_path_pub.publish(empty_path_msg);
+ ROS_INFO_STREAM("=== TargetUnreachableMsg ===");
}
-void HomerNavigationNode::targetPositionReached()
-{
- ROS_INFO_STREAM( "Target position reached. Distance to target: " << m_distance_to_target << "m. Desired distance:" << m_desired_distance << "m" );
- stopRobot();
- m_waypoints.clear();
- sendPathData();
- m_MainMachine.setState( FINAL_TURN );
- ROS_INFO_STREAM("Turning to look-at point");
+void HomerNavigationNode::targetPositionReached() {
+ ROS_INFO_STREAM("Target position reached. Distance to target: "
+ << m_distance_to_target
+ << "m. Desired distance:" << m_desired_distance << "m");
+ stopRobot();
+ m_waypoints.clear();
+ sendPathData();
+ m_MainMachine.setState(FINAL_TURN);
+ ROS_INFO_STREAM("Turning to look-at point");
}
-bool HomerNavigationNode::checkPath()
-{
- if(m_pixel_path.size() != 0)
- {
- for ( unsigned i = 0; i < m_pixel_path.size()-1; i++ )
- {
- geometry_msgs::Point p = map_tools::fromMapCoords(m_pixel_path.at(i), m_origin, m_resolution);
- if ( map_tools::distance( m_robot_pose.position, p) > m_check_path_max_distance )
- {
- return true;
- }
- for(int a = 0; a < 5; a++)
- {
- if (map_tools::findValue( m_last_map_data, m_width, m_height,
- m_pixel_path[i].x() + (m_pixel_path[i+1].x() - m_pixel_path[i].x()) * a/4,
- m_pixel_path[i].y() + (m_pixel_path[i+1].y() - m_pixel_path[i].y()) * a/4,
- 90, 2 ))
- {
- ROS_WARN_STREAM("Obstacle detected in current path recalculating" );
- return false;
- }
- }
- }
- }
+bool HomerNavigationNode::checkPath() {
+ if (m_pixel_path.size() != 0) {
+ for (unsigned i = 0; i < m_pixel_path.size() - 1; i++) {
+ geometry_msgs::Point p =
+ map_tools::fromMapCoords(m_pixel_path.at(i), m_origin, m_resolution);
+ if (map_tools::distance(m_robot_pose.position, p) >
+ m_check_path_max_distance) {
+ return true;
+ }
+ for (int a = 0; a < 5; a++) {
+ if (map_tools::findValue(
+ m_last_map_data, m_width, m_height,
+ m_pixel_path[i].x() +
+ (m_pixel_path[i + 1].x() - m_pixel_path[i].x()) * a / 4,
+ m_pixel_path[i].y() +
+ (m_pixel_path[i + 1].y() - m_pixel_path[i].y()) * a / 4,
+ 90, 2)) {
+ ROS_WARN_STREAM("Obstacle detected in current path recalculating");
+ return false;
+ }
+ }
+ }
+ }
}
-void HomerNavigationNode::handleCollision ()
-{
- if ( m_MainMachine.state() == FOLLOWING_PATH )
- {
- stopRobot();
- m_MainMachine.setState( AVOIDING_COLLISION );
- ROS_WARN_STREAM( "Collision detected while following path!" );
- }
+void HomerNavigationNode::handleCollision() {
+ if (m_MainMachine.state() == FOLLOWING_PATH) {
+ stopRobot();
+ m_MainMachine.setState(AVOIDING_COLLISION);
+ ROS_WARN_STREAM("Collision detected while following path!");
+ }
}
-void HomerNavigationNode::performNextMove()
-{
- if(m_MainMachine.state() == FOLLOWING_PATH )
- {
- if ( m_distance_to_target < m_desired_distance && !m_new_target)
- {
- ROS_INFO_STREAM( "Desired distance to target was reached." );
- targetPositionReached();
- return;
- }
- //if we have accidentaly skipped waypoints, recalculate path
- float minDistance = FLT_MAX;
- float distance;
- unsigned nearestWaypoint=0;
- for ( unsigned i=0; i<m_waypoints.size(); i++ )
- {
- distance = map_tools::distance( m_robot_pose.position, m_waypoints[i].pose.position );
- if ( distance < minDistance ) {
- nearestWaypoint = i;
- minDistance = distance;
- }
- }
- if ( nearestWaypoint != 0 )
- {
- //if the target is nearer than the waypoint don't recalculate
- if(m_waypoints.size() != 2)
- {
- ROS_WARN_STREAM("Waypoints skipped. Recalculating path!");
- calculatePath();
- if ( m_MainMachine.state() != FOLLOWING_PATH )
- {
- return;
- }
- }
- else
- {
- m_waypoints.erase(m_waypoints.begin());
- }
- }
-
- Eigen::Vector2i waypointPixel = map_tools::toMapCoords(m_waypoints[0].pose.position, m_origin, m_resolution);
- float obstacleDistanceMap = m_explorer->getObstacleTransform()->getValue( waypointPixel.x(), waypointPixel.y() ) * m_resolution;
- float waypointRadius = m_waypoint_radius_factor * obstacleDistanceMap;
-
- if ( ( waypointRadius < m_resolution ) || ( m_waypoints.size() == 1 ) )
- {
- waypointRadius = m_resolution;
- }
-
- if(m_waypoints.size() != 0)
- {
- //calculate distance between last_pose->current_pose and waypoint
- Eigen::Vector2f line; //direction of the line
- line[0] = m_robot_pose.position.x - m_robot_last_pose.position.x;
- line[1] = m_robot_pose.position.y - m_robot_last_pose.position.y;
- Eigen::Vector2f point_to_start; //vector from the point to the start of the line
- point_to_start[0] = m_robot_last_pose.position.x - m_waypoints[0].pose.position.x;
- point_to_start[1] = m_robot_last_pose.position.y - m_waypoints[0].pose.position.y;
- float dot = point_to_start[0] * line[0] + point_to_start[1] * line[1]; //dot product of point_to_start * line
- Eigen::Vector2f distance; //shortest distance vector from point to line
- distance[0] = point_to_start[0] - dot * line[0];
- distance[1] = point_to_start[1] - dot * line[1];
- float distance_to_waypoint = sqrt((double)(distance[0] * distance[0] + distance[1] * distance[1]));
-
- //check if current waypoint has been reached
- if((distance_to_waypoint < waypointRadius && m_waypoints.size() > 1) || (m_distance_to_target < waypointRadius))
- {
- m_waypoints.erase ( m_waypoints.begin() );
- }
-
- }
-
- sendPathData();
- //last wayoint reached
- if ( m_waypoints.size() == 0 )
- {
- ROS_INFO_STREAM("Last waypoint reached");
- currentPathFinished();
- return;
- }
-
- if(m_use_ptu)
- {
- ROS_INFO_STREAM("Moving PTU to center next Waypoint");
- homer_ptu_msgs::CenterWorldPoint ptu_msg;
- ptu_msg.point.x = m_waypoints[0].pose.position.x;
- ptu_msg.point.y = m_waypoints[0].pose.position.y;
- ptu_msg.point.z = 0;
- ptu_msg.permanent = true;
- m_ptu_center_world_point_pub.publish(ptu_msg);
- }
-
- float distanceToWaypoint = map_tools::distance( m_robot_pose.position, m_waypoints[0].pose.position );
- float angleToWaypoint = angleToPointDeg ( m_waypoints[0].pose.position );
- if (angleToWaypoint < -180)
- {
- angleToWaypoint += 360;
- }
- float angle = deg2Rad( angleToWaypoint );
-
- //linear speed calculation
- if(m_avoided_collision)
- {
- if( std::abs(angleToWaypoint) < 10)
- {
- m_avoided_collision = false;
- }
- }
- else if (abs(angle) > m_max_drive_angle)
- {
- m_act_speed = 0.0;
- }
- else
- {
- float obstacleMapDistance = 1;
- for(int wpi = -1; wpi < std::min((int)m_waypoints.size(),(int) 2); wpi++)
- {
- Eigen::Vector2i robotPixel;
- if(wpi == -1)
- {
- robotPixel = map_tools::toMapCoords(m_robot_pose.position, m_origin, m_resolution);
- }
- else
- {
- robotPixel = map_tools::toMapCoords(m_waypoints[wpi].pose.position, m_origin, m_resolution);
- }
- obstacleMapDistance = std::min((float)obstacleMapDistance,(float)( m_explorer->getObstacleTransform()->getValue( robotPixel.x(), robotPixel.y() ) * m_resolution));
- }
-
- float max_move_distance_speed = m_max_move_speed * m_max_move_distance * m_obstacle_speed_factor;
- float max_map_distance_speed = m_max_move_speed * obstacleMapDistance * m_map_speed_factor;
- m_act_speed = std::min(std::max((float)0.1,m_distance_to_target * m_target_distance_speed_factor) ,std::min(std::min(m_max_move_speed, max_move_distance_speed), std::min(max_map_distance_speed, distanceToWaypoint * m_waypoint_speed_factor)));
- std_msgs::String tmp;
- std::stringstream str;
- str << "m_obstacle_speed " << max_move_distance_speed <<" max_map_distance_speed "<<max_map_distance_speed;
- tmp.data = str.str();
- m_debug_pub.publish(tmp);
- }
-
- //angular speed calculation
- if (angle < 0 )
- {
- angle = std::max(angle * (float) 0.8, -m_max_turn_speed);
- m_act_speed = m_act_speed + angle / 2.0;
- if(m_act_speed < 0 )
- {
- m_act_speed = 0;
- }
- }
- else
- {
- angle = std::min(angle * (float) 0.8, m_max_turn_speed);
- m_act_speed = m_act_speed - angle / 2.0;
- if(m_act_speed < 0 )
- {
- m_act_speed = 0;
- }
- }
- geometry_msgs::Twist cmd_vel_msg;
- cmd_vel_msg.linear.x = m_act_speed;
- cmd_vel_msg.angular.z = angle;
- m_cmd_vel_pub.publish(cmd_vel_msg);
-
- ROS_DEBUG_STREAM("Driving & turning" << std::endl << "linear: " << m_act_speed << " angular: " << angle << std::endl
- << "distanceToWaypoint:" << distanceToWaypoint << "angleToWaypoint: " << angleToWaypoint << std::endl );
- }
- else if(m_MainMachine.state()== AVOIDING_COLLISION)
- {
- if( m_distance_to_target < m_desired_distance && !m_new_target)
- {
- ROS_INFO_STREAM("Collision detected near target. Switch to final turn.");
- targetPositionReached();
- }
- else if ( m_max_move_distance <= m_collision_distance && m_waypoints.size() > 1 || m_max_move_distance <= m_collision_distance_near_target )
- {
- ROS_WARN_STREAM( "Maximum driving distance too short (" << m_max_move_distance << "m)! Moving back." );
- geometry_msgs::Twist cmd_vel_msg;
- if(!HomerNavigationNode::backwardObstacle())
- {
- cmd_vel_msg.linear.x = -0.2;
- }
- else
- {
- if(m_angular_avoidance == 0)
- {
- float angleToWaypoint = angleToPointDeg ( m_waypoints[0].pose.position );
- if(angleToWaypoint < -180)
- {
- angleToWaypoint += 360;
- }
- if (angleToWaypoint < 0 )
- {
- m_angular_avoidance = -0.4;
- }
- else
- {
- m_angular_avoidance = 0.4;
- }
- }
- cmd_vel_msg.angular.z = m_angular_avoidance;
- }
- m_cmd_vel_pub.publish(cmd_vel_msg);
- }
- else
- {
- m_angular_avoidance = 0;
- m_avoided_collision = true;
- ROS_WARN_STREAM( "Collision avoided. Updating path." );
- currentPathFinished();
- }
- }
- else if(m_MainMachine.state()== FINAL_TURN)
- {
- if(m_use_ptu)
- {
- //reset PTU
- homer_ptu_msgs::SetPanTilt msg;
- msg.absolute = true;
- msg.panAngle = 0;
- msg.tiltAngle = 0;
- m_set_pan_tilt_pub.publish(msg);
- }
-
- if ( m_skip_final_turn )
- {
- ROS_INFO_STREAM("Final turn skipped. Target reached.");
- if(m_path_reaches_target)
- {
- sendTargetReachedMsg();
- }
- else
- {
- sendTargetUnreachableMsg(homer_mapnav_msgs::TargetUnreachable::NO_PATH_FOUND);
- }
- return;
- }
-
- float turnAngle = minTurnAngle( tf::getYaw(m_robot_pose.orientation), m_target_orientation );
- ROS_DEBUG_STREAM("homer_navigation::PerformNextMove:: Final Turn. Robot orientation: " << rad2Deg(tf::getYaw(m_robot_pose.orientation)) << ". Target orientation: " << rad2Deg(m_target_orientation) << "homer_navigation::PerformNextMove:: turnAngle: " << rad2Deg(turnAngle));
-
- if (std::fabs(turnAngle) < m_min_turn_angle)
- {
- ROS_INFO_STREAM(":::::::NEAREST WALKABLE TARGET REACHED BECAUSE lower " << m_min_turn_angle);
- ROS_INFO_STREAM("target angle = "<< m_target_orientation );
- ROS_INFO_STREAM("is angle = "<< tf::getYaw(m_robot_pose.orientation));
- ROS_INFO_STREAM("m_distance_to_target = " << m_distance_to_target);
- ROS_INFO_STREAM("m_desired_distance = " << m_desired_distance);
- if(m_path_reaches_target)
- {
- sendTargetReachedMsg();
- }
- else
- {
- sendTargetUnreachableMsg(homer_mapnav_msgs::TargetUnreachable::NO_PATH_FOUND);
- }
- return;
- }
- else
- {
- if (turnAngle< 0 )
- {
- turnAngle= std::max(std::min(turnAngle, -m_min_turn_speed ),-m_max_turn_speed);
- }
- else
- {
- turnAngle = std::min(std::max(turnAngle, m_min_turn_speed ),m_max_turn_speed);
- }
- geometry_msgs::Twist cmd_vel_msg;
- cmd_vel_msg.angular.z = turnAngle;
- m_cmd_vel_pub.publish(cmd_vel_msg);
- }
- }
+void HomerNavigationNode::performNextMove() {
+ if (m_MainMachine.state() == FOLLOWING_PATH) {
+ if (m_distance_to_target < m_desired_distance && !m_new_target) {
+ ROS_INFO_STREAM("Desired distance to target was reached.");
+ targetPositionReached();
+ return;
+ }
+ // if we have accidentaly skipped waypoints, recalculate path
+ float minDistance = FLT_MAX;
+ float distance;
+ unsigned nearestWaypoint = 0;
+ for (unsigned i = 0; i < m_waypoints.size(); i++) {
+ distance = map_tools::distance(m_robot_pose.position,
+ m_waypoints[i].pose.position);
+ if (distance < minDistance) {
+ nearestWaypoint = i;
+ minDistance = distance;
+ }
+ }
+ if (nearestWaypoint != 0) {
+ // if the target is nearer than the waypoint don't recalculate
+ if (m_waypoints.size() != 2) {
+ ROS_WARN_STREAM("Waypoints skipped. Recalculating path!");
+ calculatePath();
+ if (m_MainMachine.state() != FOLLOWING_PATH) {
+ return;
+ }
+ } else {
+ m_waypoints.erase(m_waypoints.begin());
+ }
+ }
+
+ Eigen::Vector2i waypointPixel = map_tools::toMapCoords(
+ m_waypoints[0].pose.position, m_origin, m_resolution);
+ float obstacleDistanceMap = m_explorer->getObstacleTransform()->getValue(
+ waypointPixel.x(), waypointPixel.y()) *
+ m_resolution;
+ float waypointRadius = m_waypoint_radius_factor * obstacleDistanceMap;
+
+ if ((waypointRadius < m_resolution) || (m_waypoints.size() == 1)) {
+ waypointRadius = m_resolution;
+ }
+
+ if (m_waypoints.size() != 0) {
+ // calculate distance between last_pose->current_pose and waypoint
+ Eigen::Vector2f line; // direction of the line
+ line[0] = m_robot_pose.position.x - m_robot_last_pose.position.x;
+ line[1] = m_robot_pose.position.y - m_robot_last_pose.position.y;
+ Eigen::Vector2f
+ point_to_start; // vector from the point to the start of the line
+ point_to_start[0] =
+ m_robot_last_pose.position.x - m_waypoints[0].pose.position.x;
+ point_to_start[1] =
+ m_robot_last_pose.position.y - m_waypoints[0].pose.position.y;
+ float dot =
+ point_to_start[0] * line[0] +
+ point_to_start[1] * line[1]; // dot product of point_to_start * line
+ Eigen::Vector2f distance; // shortest distance vector from point to line
+ distance[0] = point_to_start[0] - dot * line[0];
+ distance[1] = point_to_start[1] - dot * line[1];
+ float distance_to_waypoint =
+ sqrt((double)(distance[0] * distance[0] + distance[1] * distance[1]));
+
+ // check if current waypoint has been reached
+ if ((distance_to_waypoint < waypointRadius && m_waypoints.size() > 1) ||
+ (m_distance_to_target < waypointRadius)) {
+ m_waypoints.erase(m_waypoints.begin());
+ }
+ }
+
+ sendPathData();
+ // last wayoint reached
+ if (m_waypoints.size() == 0) {
+ ROS_INFO_STREAM("Last waypoint reached");
+ currentPathFinished();
+ return;
+ }
+
+ if (m_use_ptu) {
+ ROS_INFO_STREAM("Moving PTU to center next Waypoint");
+ homer_ptu_msgs::CenterWorldPoint ptu_msg;
+ ptu_msg.point.x = m_waypoints[0].pose.position.x;
+ ptu_msg.point.y = m_waypoints[0].pose.position.y;
+ ptu_msg.point.z = 0;
+ ptu_msg.permanent = true;
+ m_ptu_center_world_point_pub.publish(ptu_msg);
+ }
+
+ float distanceToWaypoint = map_tools::distance(
+ m_robot_pose.position, m_waypoints[0].pose.position);
+ float angleToWaypoint = angleToPointDeg(m_waypoints[0].pose.position);
+ if (angleToWaypoint < -180) {
+ angleToWaypoint += 360;
+ }
+ float angle = deg2Rad(angleToWaypoint);
+
+ // linear speed calculation
+ if (m_avoided_collision) {
+ if (std::abs(angleToWaypoint) < 10) {
+ m_avoided_collision = false;
+ }
+ } else if (abs(angle) > m_max_drive_angle) {
+ m_act_speed = 0.0;
+ } else {
+ float obstacleMapDistance = 1;
+ for (int wpi = -1; wpi < std::min((int)m_waypoints.size(), (int)2);
+ wpi++) {
+ Eigen::Vector2i robotPixel;
+ if (wpi == -1) {
+ robotPixel = map_tools::toMapCoords(m_robot_pose.position, m_origin,
+ m_resolution);
+ } else {
+ robotPixel = map_tools::toMapCoords(m_waypoints[wpi].pose.position,
+ m_origin, m_resolution);
+ }
+ obstacleMapDistance =
+ std::min((float)obstacleMapDistance,
+ (float)(m_explorer->getObstacleTransform()->getValue(
+ robotPixel.x(), robotPixel.y()) *
+ m_resolution));
+ }
+
+ float max_move_distance_speed =
+ m_max_move_speed * m_max_move_distance * m_obstacle_speed_factor;
+ float max_map_distance_speed =
+ m_max_move_speed * obstacleMapDistance * m_map_speed_factor;
+ m_act_speed = std::min(
+ std::max((float)0.1,
+ m_distance_to_target * m_target_distance_speed_factor),
+ std::min(std::min(m_max_move_speed, max_move_distance_speed),
+ std::min(max_map_distance_speed,
+ distanceToWaypoint * m_waypoint_speed_factor)));
+ std_msgs::String tmp;
+ std::stringstream str;
+ str << "m_obstacle_speed " << max_move_distance_speed
+ << " max_map_distance_speed " << max_map_distance_speed;
+ tmp.data = str.str();
+ m_debug_pub.publish(tmp);
+ }
+
+ // angular speed calculation
+ if (angle < 0) {
+ angle = std::max(angle * (float)0.8, -m_max_turn_speed);
+ m_act_speed = m_act_speed + angle / 2.0;
+ if (m_act_speed < 0) {
+ m_act_speed = 0;
+ }
+ } else {
+ angle = std::min(angle * (float)0.8, m_max_turn_speed);
+ m_act_speed = m_act_speed - angle / 2.0;
+ if (m_act_speed < 0) {
+ m_act_speed = 0;
+ }
+ }
+ geometry_msgs::Twist cmd_vel_msg;
+ cmd_vel_msg.linear.x = m_act_speed;
+ cmd_vel_msg.angular.z = angle;
+ m_cmd_vel_pub.publish(cmd_vel_msg);
+
+ ROS_DEBUG_STREAM("Driving & turning"
+ << std::endl
+ << "linear: " << m_act_speed << " angular: " << angle
+ << std::endl
+ << "distanceToWaypoint:" << distanceToWaypoint
+ << "angleToWaypoint: " << angleToWaypoint << std::endl);
+ } else if (m_MainMachine.state() == AVOIDING_COLLISION) {
+ if (m_distance_to_target < m_desired_distance && !m_new_target) {
+ ROS_INFO_STREAM("Collision detected near target. Switch to final turn.");
+ targetPositionReached();
+ } else if (m_max_move_distance <= m_collision_distance &&
+ m_waypoints.size() > 1 ||
+ m_max_move_distance <= m_collision_distance_near_target) {
+ ROS_WARN_STREAM("Maximum driving distance too short ("
+ << m_max_move_distance << "m)! Moving back.");
+ geometry_msgs::Twist cmd_vel_msg;
+ if (!HomerNavigationNode::backwardObstacle()) {
+ cmd_vel_msg.linear.x = -0.2;
+ } else {
+ if (m_angular_avoidance == 0) {
+ float angleToWaypoint = angleToPointDeg(m_waypoints[0].pose.position);
+ if (angleToWaypoint < -180) {
+ angleToWaypoint += 360;
+ }
+ if (angleToWaypoint < 0) {
+ m_angular_avoidance = -0.4;
+ } else {
+ m_angular_avoidance = 0.4;
+ }
+ }
+ cmd_vel_msg.angular.z = m_angular_avoidance;
+ }
+ m_cmd_vel_pub.publish(cmd_vel_msg);
+ } else {
+ m_angular_avoidance = 0;
+ m_avoided_collision = true;
+ ROS_WARN_STREAM("Collision avoided. Updating path.");
+ currentPathFinished();
+ }
+ } else if (m_MainMachine.state() == FINAL_TURN) {
+ if (m_use_ptu) {
+ // reset PTU
+ homer_ptu_msgs::SetPanTilt msg;
+ msg.absolute = true;
+ msg.panAngle = 0;
+ msg.tiltAngle = 0;
+ m_set_pan_tilt_pub.publish(msg);
+ }
+
+ if (m_skip_final_turn) {
+ ROS_INFO_STREAM("Final turn skipped. Target reached.");
+ if (m_path_reaches_target) {
+ sendTargetReachedMsg();
+ } else {
+ sendTargetUnreachableMsg(
+ homer_mapnav_msgs::TargetUnreachable::NO_PATH_FOUND);
+ }
+ return;
+ }
+
+ float turnAngle = minTurnAngle(tf::getYaw(m_robot_pose.orientation),
+ m_target_orientation);
+ ROS_DEBUG_STREAM(
+ "homer_navigation::PerformNextMove:: Final Turn. Robot orientation: "
+ << rad2Deg(tf::getYaw(m_robot_pose.orientation))
+ << ". Target orientation: " << rad2Deg(m_target_orientation)
+ << "homer_navigation::PerformNextMove:: turnAngle: "
+ << rad2Deg(turnAngle));
+
+ if (std::fabs(turnAngle) < m_min_turn_angle) {
+ ROS_INFO_STREAM(":::::::NEAREST WALKABLE TARGET REACHED BECAUSE lower "
+ << m_min_turn_angle);
+ ROS_INFO_STREAM("target angle = " << m_target_orientation);
+ ROS_INFO_STREAM("is angle = " << tf::getYaw(m_robot_pose.orientation));
+ ROS_INFO_STREAM("m_distance_to_target = " << m_distance_to_target);
+ ROS_INFO_STREAM("m_desired_distance = " << m_desired_distance);
+ if (m_path_reaches_target) {
+ sendTargetReachedMsg();
+ } else {
+ sendTargetUnreachableMsg(
+ homer_mapnav_msgs::TargetUnreachable::NO_PATH_FOUND);
+ }
+ return;
+ } else {
+ if (turnAngle < 0) {
+ turnAngle =
+ std::max(std::min(turnAngle, -m_min_turn_speed), -m_max_turn_speed);
+ } else {
+ turnAngle =
+ std::min(std::max(turnAngle, m_min_turn_speed), m_max_turn_speed);
+ }
+ geometry_msgs::Twist cmd_vel_msg;
+ cmd_vel_msg.angular.z = turnAngle;
+ m_cmd_vel_pub.publish(cmd_vel_msg);
+ }
+ }
}
-void HomerNavigationNode::currentPathFinished()
-{
- ROS_INFO_STREAM( "Current path was finished, initiating recalculation.");
- m_waypoints.clear();
- stopRobot();
- m_MainMachine.setState( AWAITING_PATHPLANNING_MAP );
+void HomerNavigationNode::currentPathFinished() {
+ ROS_INFO_STREAM("Current path was finished, initiating recalculation.");
+ m_waypoints.clear();
+ stopRobot();
+ m_MainMachine.setState(AWAITING_PATHPLANNING_MAP);
}
-int HomerNavigationNode::angleToPointDeg ( geometry_msgs::Point target )
-{
- double cx = m_robot_pose.position.x;
- double cy = m_robot_pose.position.y;
- int targetAngle = rad2Deg( atan2 ( target.y - cy, target.x - cx ) );
- int currentAngle = rad2Deg( tf::getYaw(m_robot_pose.orientation) );
+int HomerNavigationNode::angleToPointDeg(geometry_msgs::Point target) {
+ double cx = m_robot_pose.position.x;
+ double cy = m_robot_pose.position.y;
+ int targetAngle = rad2Deg(atan2(target.y - cy, target.x - cx));
+ int currentAngle = rad2Deg(tf::getYaw(m_robot_pose.orientation));
- int angleDiff = targetAngle - currentAngle;
- angleDiff = (angleDiff + 180) % 360 - 180;
- return angleDiff;
+ int angleDiff = targetAngle - currentAngle;
+ angleDiff = (angleDiff + 180) % 360 - 180;
+ return angleDiff;
}
-bool HomerNavigationNode::drawPolygon ( std::vector< geometry_msgs::Point > vertices)
-{
- if ( vertices.size() == 0 )
- {
- ROS_INFO_STREAM( "No vertices given!" );
- return false;
- }
- //make temp. map
- std::vector<int> data(m_width * m_height);
- for ( int i = 0; i < data.size(); i++ )
- {
- data[i] = 0;
- }
-
- //draw the lines surrounding the polygon
- for ( unsigned int i = 0; i < vertices.size(); i++ )
- {
- int i2 = ( i+1 ) % vertices.size();
- drawLine ( data, vertices[i].x, vertices[i].y, vertices[i2].x, vertices[i2].y, 30);
- }
- //calculate a point in the middle of the polygon
- float midX = 0;
- float midY = 0;
- for ( unsigned int i = 0; i < vertices.size(); i++ )
- {
- midX += vertices[i].x;
- midY += vertices[i].y;
- }
- midX /= vertices.size();
- midY /= vertices.size();
- //fill polygon
- return fillPolygon ( data, (int)midX, (int)midY, 30 );
+bool HomerNavigationNode::drawPolygon(
+ std::vector<geometry_msgs::Point> vertices) {
+ if (vertices.size() == 0) {
+ ROS_INFO_STREAM("No vertices given!");
+ return false;
+ }
+ // make temp. map
+ std::vector<int> data(m_width * m_height);
+ for (int i = 0; i < data.size(); i++) {
+ data[i] = 0;
+ }
+
+ // draw the lines surrounding the polygon
+ for (unsigned int i = 0; i < vertices.size(); i++) {
+ int i2 = (i + 1) % vertices.size();
+ drawLine(data, vertices[i].x, vertices[i].y, vertices[i2].x, vertices[i2].y,
+ 30);
+ }
+ // calculate a point in the middle of the polygon
+ float midX = 0;
+ float midY = 0;
+ for (unsigned int i = 0; i < vertices.size(); i++) {
+ midX += vertices[i].x;
+ midY += vertices[i].y;
+ }
+ midX /= vertices.size();
+ midY /= vertices.size();
+ // fill polygon
+ return fillPolygon(data, (int)midX, (int)midY, 30);
}
-void HomerNavigationNode::drawLine ( std::vector<int> &data, int startX, int startY, int endX, int endY, int value )
-{
- //bresenham algorithm
- int x, y, t, dist, xerr, yerr, dx, dy, incx, incy;
- // compute distances
- dx = endX - startX;
- dy = endY - startY;
-
- // compute increment
- if ( dx < 0 )
- {
- incx = -1;
- dx = -dx;
- }
- else
- {
- incx = dx ? 1 : 0;
- }
-
- if ( dy < 0 )
- {
- incy = -1;
- dy = -dy;
- }
- else
- {
- incy = dy ? 1 : 0;
- }
-
- // which distance is greater?
- dist = ( dx > dy ) ? dx : dy;
- // initializing
- x = startX;
- y = startY;
- xerr = dx;
- yerr = dy;
-
- // compute cells
- for ( t = 0; t < dist; t++ )
- {
- int index = x + m_width* y;
- if(index < 0 || index > data.size())
- {
- continue;
- }
- data[index] = value;
-
- xerr += dx;
- yerr += dy;
- if ( xerr > dist )
- {
- xerr -= dist;
- x += incx;
- }
- if ( yerr > dist )
- {
- yerr -= dist;
- y += incy;
- }
- }
+void HomerNavigationNode::drawLine(std::vector<int>& data, int startX,
+ int startY, int endX, int endY, int value) {
+ // bresenham algorithm
+ int x, y, t, dist, xerr, yerr, dx, dy, incx, incy;
+ // compute distances
+ dx = endX - startX;
+ dy = endY - startY;
+
+ // compute increment
+ if (dx < 0) {
+ incx = -1;
+ dx = -dx;
+ } else {
+ incx = dx ? 1 : 0;
+ }
+
+ if (dy < 0) {
+ incy = -1;
+ dy = -dy;
+ } else {
+ incy = dy ? 1 : 0;
+ }
+
+ // which distance is greater?
+ dist = (dx > dy) ? dx : dy;
+ // initializing
+ x = startX;
+ y = startY;
+ xerr = dx;
+ yerr = dy;
+
+ // compute cells
+ for (t = 0; t < dist; t++) {
+ int index = x + m_width * y;
+ if (index < 0 || index > data.size()) {
+ continue;
+ }
+ data[index] = value;
+
+ xerr += dx;
+ yerr += dy;
+ if (xerr > dist) {
+ xerr -= dist;
+ x += incx;
+ }
+ if (yerr > dist) {
+ yerr -= dist;
+ y += incy;
+ }
+ }
}
-bool HomerNavigationNode::fillPolygon ( std::vector<int> &data, int x, int y, int value )
-{
- int index = x + m_width * y;
- bool tmp = false;
-
- if ((int)m_last_map_data->at(index)> 90)
- {
- tmp = true;
- }
- if ( data[index] != value )
- {
- data[index] = value;
- if (fillPolygon ( data, x + 1, y, value ))
- {
- tmp = true;
- }
- if(fillPolygon ( data, x - 1, y, value ))
- {
- tmp = true;
- }
- if(fillPolygon ( data, x, y + 1, value ))
- {
- tmp = true;
- }
- if(fillPolygon ( data, x, y - 1, value ))
- {
- tmp = true;
- }
- }
- return tmp;
+bool HomerNavigationNode::fillPolygon(std::vector<int>& data, int x, int y,
+ int value) {
+ int index = x + m_width * y;
+ bool tmp = false;
+
+ if ((int)m_last_map_data->at(index) > 90) {
+ tmp = true;
+ }
+ if (data[index] != value) {
+ data[index] = value;
+ if (fillPolygon(data, x + 1, y, value)) {
+ tmp = true;
+ }
+ if (fillPolygon(data, x - 1, y, value)) {
+ tmp = true;
+ }
+ if (fillPolygon(data, x, y + 1, value)) {
+ tmp = true;
+ }
+ if (fillPolygon(data, x, y - 1, value)) {
+ tmp = true;
+ }
+ }
+ return tmp;
}
-bool HomerNavigationNode::backwardObstacle()
-{
- std::vector<geometry_msgs::Point> vertices;
- geometry_msgs::Point base_link_point;
- geometry_msgs::Point map_point;
- Eigen::Vector2i map_coord;
+bool HomerNavigationNode::backwardObstacle() {
+ std::vector<geometry_msgs::Point> vertices;
+ geometry_msgs::Point base_link_point;
+ geometry_msgs::Point map_point;
+ Eigen::Vector2i map_coord;
- std::vector<float> x;
- std::vector<float> y;
+ std::vector<float> x;
+ std::vector<float> y;
- x.push_back(- m_min_x - m_backward_collision_distance);
- y.push_back(m_min_y);
+ x.push_back(-m_min_x - m_backward_collision_distance);
+ y.push_back(m_min_y);
- x.push_back(- m_min_x - m_backward_collision_distance);
- y.push_back(-m_min_y);
+ x.push_back(-m_min_x - m_backward_collision_distance);
+ y.push_back(-m_min_y);
- x.push_back(-0.1);
- y.push_back(-m_min_y);
+ x.push_back(-0.1);
+ y.push_back(-m_min_y);
- x.push_back(-0.1);
- y.push_back(m_min_y);
+ x.push_back(-0.1);
+ y.push_back(m_min_y);
- for(int i = 0; i<x.size(); i++)
- {
- base_link_point.x = x[i];
- base_link_point.y = y[i];
- map_coord = map_tools::toMapCoords(map_tools::transformPoint(base_link_point, m_transform_listener ,"/base_link", "/map"), m_origin, m_resolution);
- map_point.x = map_coord.x();
- map_point.y = map_coord.y();
- vertices.push_back(map_point);
- }
+ for (int i = 0; i < x.size(); i++) {
+ base_link_point.x = x[i];
+ base_link_point.y = y[i];
+ map_coord = map_tools::toMapCoords(
+ map_tools::transformPoint(base_link_point, m_transform_listener,
+ "/base_link", "/map"),
+ m_origin, m_resolution);
+ map_point.x = map_coord.x();
+ map_point.y = map_coord.y();
+ vertices.push_back(map_point);
+ }
- return drawPolygon(vertices);
+ return drawPolygon(vertices);
}
-void HomerNavigationNode::maskMap()
-{
- //generate bounding box
- ROS_INFO_STREAM("Calculating Bounding box for fast planning");
- Eigen::Vector2i pose_pixel = map_tools::toMapCoords( m_robot_pose.position, m_origin, m_resolution);
- Eigen::Vector2i target_pixel = map_tools::toMapCoords( m_target_point, m_origin, m_resolution);
- Eigen::Vector2i safe_pixel_distance(m_AllowedObstacleDistance.first * 4,
- m_AllowedObstacleDistance.first * 4);
- Eigen::AlignedBox2i planning_box;
- planning_box.extend(pose_pixel);
- planning_box.extend(target_pixel);
- ROS_INFO_STREAM("Bounding Box: (" << planning_box.min() << " " << planning_box.max());
- Eigen::AlignedBox2i safe_planning_box(planning_box.min() - safe_pixel_distance, planning_box.max() + safe_pixel_distance);
- ROS_INFO_STREAM("safe Bounding Box: (" << safe_planning_box.min() << " " << safe_planning_box.max());
- ROS_INFO_STREAM("min in m: " <<map_tools::fromMapCoords(safe_planning_box.min(), m_origin, m_resolution));
- ROS_INFO_STREAM("max in m: "<<map_tools::fromMapCoords(safe_planning_box.max(), m_origin, m_resolution));
- for(size_t x = 0; x < m_width; x++)
- {
- for(size_t y = 0; y < m_width; y++)
- {
- if(!safe_planning_box.contains(Eigen::Vector2i(x, y)))
- {
- m_last_map_data->at(y * m_width + x) = -1;
- }
- }
- }
+void HomerNavigationNode::maskMap() {
+ // generate bounding box
+ ROS_INFO_STREAM("Calculating Bounding box for fast planning");
+ Eigen::Vector2i pose_pixel =
+ map_tools::toMapCoords(m_robot_pose.position, m_origin, m_resolution);
+ Eigen::Vector2i target_pixel =
+ map_tools::toMapCoords(m_target_point, m_origin, m_resolution);
+ Eigen::Vector2i safe_pixel_distance(m_AllowedObstacleDistance.first * 4,
+ m_AllowedObstacleDistance.first * 4);
+ Eigen::AlignedBox2i planning_box;
+ planning_box.extend(pose_pixel);
+ planning_box.extend(target_pixel);
+ ROS_INFO_STREAM("Bounding Box: (" << planning_box.min() << " "
+ << planning_box.max());
+ Eigen::AlignedBox2i safe_planning_box(
+ planning_box.min() - safe_pixel_distance,
+ planning_box.max() + safe_pixel_distance);
+ ROS_INFO_STREAM("safe Bounding Box: (" << safe_planning_box.min() << " "
+ << safe_planning_box.max());
+ ROS_INFO_STREAM("min in m: " << map_tools::fromMapCoords(
+ safe_planning_box.min(), m_origin, m_resolution));
+ ROS_INFO_STREAM("max in m: " << map_tools::fromMapCoords(
+ safe_planning_box.max(), m_origin, m_resolution));
+ for (size_t x = 0; x < m_width; x++) {
+ for (size_t y = 0; y < m_width; y++) {
+ if (!safe_planning_box.contains(Eigen::Vector2i(x, y))) {
+ m_last_map_data->at(y * m_width + x) = -1;
+ }
+ }
+ }
}
-//convenience math functions
-float HomerNavigationNode::minTurnAngle( float angle1, float angle2 )
-{
- angle1 *= 180.0/M_PI;
- angle2 *= 180.0/M_PI;
-
- int diff= angle2 - angle1;
- diff = (diff + 180) % 360 - 180;
- if (diff < -180)
- {
- diff += 360;
- }
+// convenience math functions
+float HomerNavigationNode::minTurnAngle(float angle1, float angle2) {
+ angle1 *= 180.0 / M_PI;
+ angle2 *= 180.0 / M_PI;
+
+ int diff = angle2 - angle1;
+ diff = (diff + 180) % 360 - 180;
+ if (diff < -180) {
+ diff += 360;
+ }
- float ret = static_cast<double>(diff) * M_PI/180.0;
- return ret;
+ float ret = static_cast<double>(diff) * M_PI / 180.0;
+ return ret;
}
-void HomerNavigationNode::refreshParamsCallback(const std_msgs::Empty::ConstPtr& msg)
-{
- ROS_INFO_STREAM("Refreshing Parameters");
- loadParameters();
+void HomerNavigationNode::refreshParamsCallback(
+ const std_msgs::Empty::ConstPtr& msg) {
+ ROS_INFO_STREAM("Refreshing Parameters");
+ loadParameters();
}
-void HomerNavigationNode::mapCallback(const nav_msgs::OccupancyGrid::ConstPtr& msg)
-{
- if(msg->info.height != msg->info.width)
- {
- ROS_ERROR_STREAM("Incoming Map not quadratic. No map update!");
- return;
- }
- if(m_last_map_data)
- {
- delete m_last_map_data;
- }
-
- m_last_map_data = new std::vector<int8_t>(msg->data);
- m_origin = msg->info.origin;
- m_width = msg->info.width;
- m_height = msg->info.height;
- m_resolution = msg->info.resolution;
-
- switch ( m_MainMachine.state() )
- {
- case AWAITING_PATHPLANNING_MAP:
- startNavigation();
- break;
- case FOLLOWING_PATH:
- {
- if ( m_check_path)
- {
- if( !checkPath())
- {
- if(!m_last_check_path_res)
- {
- calculatePath();
- }
- m_last_check_path_res = false;
- }
- else
- {
- m_last_check_path_res = true;
- }
- }
- break;
- }
- }
+void HomerNavigationNode::mapCallback(
+ const nav_msgs::OccupancyGrid::ConstPtr& msg) {
+ if (msg->info.height != msg->info.width) {
+ ROS_ERROR_STREAM("Incoming Map not quadratic. No map update!");
+ return;
+ }
+ if (m_last_map_data) {
+ delete m_last_map_data;
+ }
+
+ m_last_map_data = new std::vector<int8_t>(msg->data);
+ m_origin = msg->info.origin;
+ m_width = msg->info.width;
+ m_height = msg->info.height;
+ m_resolution = msg->info.resolution;
+
+ switch (m_MainMachine.state()) {
+ case AWAITING_PATHPLANNING_MAP:
+ startNavigation();
+ break;
+ case FOLLOWING_PATH: {
+ if (m_check_path) {
+ if (!checkPath()) {
+ if (!m_last_check_path_res) {
+ calculatePath();
+ }
+ m_last_check_path_res = false;
+ } else {
+ m_last_check_path_res = true;
+ }
+ }
+ break;
+ }
+ }
}
-void HomerNavigationNode::poseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
-{
- m_robot_last_pose = m_robot_pose;
- m_robot_pose = msg->pose;
- m_last_pose_time = ros::Time::now();
- m_distance_to_target = map_tools::distance( m_robot_pose.position, m_target_point );
- m_new_target = false;
- performNextMove();
+void HomerNavigationNode::poseCallback(
+ const geometry_msgs::PoseStamped::ConstPtr& msg) {
+ m_robot_last_pose = m_robot_pose;
+ m_robot_pose = msg->pose;
+ m_last_pose_time = ros::Time::now();
+ m_distance_to_target =
+ map_tools::distance(m_robot_pose.position, m_target_point);
+ m_new_target = false;
+ performNextMove();
}
-void HomerNavigationNode::calcMaxMoveDist()
-{
- m_max_move_distance = std::min (m_max_move_sick, std::min(m_max_move_down, m_max_move_depth));
- if(m_max_move_distance <= m_collision_distance && std::fabs(m_act_speed) > 0.1 && m_waypoints.size() > 1 ||
- m_max_move_distance <= m_collision_distance_near_target && std::fabs(m_act_speed) > 0.1 && m_waypoints.size() == 1 ||
- m_max_move_distance <= 0.1 )
- {
- handleCollision();
- }
+void HomerNavigationNode::calcMaxMoveDist() {
+ m_max_move_distance =
+ std::min(m_max_move_sick, std::min(m_max_move_down, m_max_move_depth));
+ if (m_max_move_distance <= m_collision_distance &&
+ std::fabs(m_act_speed) > 0.1 && m_waypoints.size() > 1 ||
+ m_max_move_distance <= m_collision_distance_near_target &&
+ std::fabs(m_act_speed) > 0.1 && m_waypoints.size() == 1 ||
+ m_max_move_distance <= 0.1) {
+ handleCollision();
+ }
}
-void HomerNavigationNode::maxDepthMoveDistanceCallback(const std_msgs::Float32::ConstPtr& msg)
-{
- m_max_move_depth = msg->data;
- calcMaxMoveDist();
+void HomerNavigationNode::maxDepthMoveDistanceCallback(
+ const std_msgs::Float32::ConstPtr& msg) {
+ m_max_move_depth = msg->data;
+ calcMaxMoveDist();
}
-void HomerNavigationNode::laserDataCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
-{
- m_last_laser_time = ros::Time::now();
- m_max_move_sick = map_tools::get_max_move_distance ( map_tools::laser_ranges_to_points(msg->ranges, msg->angle_min, msg->angle_increment, msg->range_min, msg->range_max, m_transform_listener, msg->header.frame_id, "/base_link"), m_min_x, m_min_y);
- calcMaxMoveDist();
+void HomerNavigationNode::laserDataCallback(
+ const sensor_msgs::LaserScan::ConstPtr& msg) {
+ m_last_laser_time = ros::Time::now();
+ m_max_move_sick = map_tools::get_max_move_distance(
+ map_tools::laser_ranges_to_points(msg->ranges, msg->angle_min,
+ msg->angle_increment, msg->range_min,
+ msg->range_max, m_transform_listener,
+ msg->header.frame_id, "/base_link"),
+ m_min_x, m_min_y);
+ calcMaxMoveDist();
}
-
-void HomerNavigationNode::downlaserDataCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
-{
- m_max_move_down = map_tools::get_max_move_distance ( map_tools::laser_ranges_to_points(msg->ranges, msg->angle_min, msg->angle_increment, msg->range_min, msg->range_max, m_transform_listener, msg->header.frame_id, "/base_link"), m_min_x, m_min_y);
- calcMaxMoveDist();
+void HomerNavigationNode::downlaserDataCallback(
+ const sensor_msgs::LaserScan::ConstPtr& msg) {
+ m_max_move_down = map_tools::get_max_move_distance(
+ map_tools::laser_ranges_to_points(msg->ranges, msg->angle_min,
+ msg->angle_increment, msg->range_min,
+ msg->range_max, m_transform_listener,
+ msg->header.frame_id, "/base_link"),
+ m_min_x, m_min_y);
+ calcMaxMoveDist();
}
-void HomerNavigationNode::startNavigationCallback(const homer_mapnav_msgs::StartNavigation::ConstPtr& msg)
-{
- m_avoided_collision = false;
- m_target_point = msg->goal.position;
- m_target_orientation = tf::getYaw(msg->goal.orientation);
- m_desired_distance = msg->distance_to_target < 0.1 ? 0.1 : msg->distance_to_target;
- m_skip_final_turn = msg->skip_final_turn;
- m_fast_path_planning = msg->fast_planning;
- m_new_target = true;
- m_target_name = "";
-
- ROS_INFO_STREAM("Navigating to target " << m_target_point.x << ", " << m_target_point.y
- << "\nTarget orientation: " << m_target_orientation
- << "Desired distance to target: " << m_desired_distance);
-
- m_MainMachine.setState( AWAITING_PATHPLANNING_MAP );
+void HomerNavigationNode::startNavigationCallback(
+ const homer_mapnav_msgs::StartNavigation::ConstPtr& msg) {
+ ROS_INFO_STREAM("This is MY node.");
+ m_avoided_collision = false;
+ m_target_point = msg->goal.position;
+ m_target_orientation = tf::getYaw(msg->goal.orientation);
+ m_desired_distance =
+ msg->distance_to_target < 0.1 ? 0.1 : msg->distance_to_target;
+ m_skip_final_turn = msg->skip_final_turn;
+ m_fast_path_planning = msg->fast_planning;
+ m_new_target = true;
+ m_target_name = "";
+ m_initial_path_reaches_target = false;
+
+ ROS_INFO_STREAM("Navigating to target "
+ << m_target_point.x << ", " << m_target_point.y
+ << "\nTarget orientation: " << m_target_orientation
+ << "Desired distance to target: " << m_desired_distance);
+
+ m_MainMachine.setState(AWAITING_PATHPLANNING_MAP);
}
-void HomerNavigationNode::moveBaseSimpleGoalCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
-{
- if(msg->header.frame_id != "map")
- {
- tf::StampedTransform transform;
- if(m_transform_listener.waitForTransform("map", msg->header.frame_id, msg->header.stamp, ros::Duration(1.0)))
- {
- try
- {
- m_transform_listener.lookupTransform("map", msg->header.frame_id, msg->header.stamp, transform);
- tf::Vector3 targetPos(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);
- targetPos = transform * targetPos;
- m_target_point.x = targetPos.getX();
- m_target_point.y = targetPos.getY();
- m_target_orientation = tf::getYaw(transform*tf::Quaternion(msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z, msg->pose.orientation.w));
- }
- catch(tf::TransformException ex)
- {
- ROS_ERROR_STREAM(ex.what());
- return;
- }
- }
- else
- {
- try
- {
- m_transform_listener.lookupTransform("map", msg->header.frame_id, ros::Time(0), transform);
- tf::Vector3 targetPos(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);
- targetPos = transform * targetPos;
- m_target_point.x = targetPos.getX();
- m_target_point.y = targetPos.getY();
- m_target_orientation = tf::getYaw(transform*tf::Quaternion(msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z, msg->pose.orientation.w));
- }
- catch(tf::TransformException ex)
- {
- ROS_ERROR_STREAM(ex.what());
- return;
- }
- }
- }
- else
- {
- m_target_point = msg->pose.position;
- m_target_orientation = tf::getYaw(msg->pose.orientation);
- }
- m_avoided_collision = false;
- m_desired_distance = 0.1;
- m_skip_final_turn = false;
- m_fast_path_planning = false;
- m_new_target = true;
-
- ROS_INFO_STREAM("Navigating to target via Move Base Simple x: " << m_target_point.x << ", y: " << m_target_point.y
- << "\nTarget orientation: " << m_target_orientation
- << " Desired distance to target: " << m_desired_distance
- << "\nframe_id: " << msg->header.frame_id);
-
- m_MainMachine.setState( AWAITING_PATHPLANNING_MAP );
+void HomerNavigationNode::moveBaseSimpleGoalCallback(
+ const geometry_msgs::PoseStamped::ConstPtr& msg) {
+ if (msg->header.frame_id != "map") {
+ tf::StampedTransform transform;
+ if (m_transform_listener.waitForTransform("map", msg->header.frame_id,
+ msg->header.stamp,
+ ros::Duration(1.0))) {
+ try {
+ m_transform_listener.lookupTransform("map", msg->header.frame_id,
+ msg->header.stamp, transform);
+ tf::Vector3 targetPos(msg->pose.position.x, msg->pose.position.y,
+ msg->pose.position.z);
+ targetPos = transform * targetPos;
+ m_target_point.x = targetPos.getX();
+ m_target_point.y = targetPos.getY();
+ m_target_orientation = tf::getYaw(
+ transform *
+ tf::Quaternion(msg->pose.orientation.x, msg->pose.orientation.y,
+ msg->pose.orientation.z, msg->pose.orientation.w));
+ } catch (tf::TransformException ex) {
+ ROS_ERROR_STREAM(ex.what());
+ return;
+ }
+ } else {
+ try {
+ m_transform_listener.lookupTransform("map", msg->header.frame_id,
+ ros::Time(0), transform);
+ tf::Vector3 targetPos(msg->pose.position.x, msg->pose.position.y,
+ msg->pose.position.z);
+ targetPos = transform * targetPos;
+ m_target_point.x = targetPos.getX();
+ m_target_point.y = targetPos.getY();
+ m_target_orientation = tf::getYaw(
+ transform *
+ tf::Quaternion(msg->pose.orientation.x, msg->pose.orientation.y,
+ msg->pose.orientation.z, msg->pose.orientation.w));
+ } catch (tf::TransformException ex) {
+ ROS_ERROR_STREAM(ex.what());
+ return;
+ }
+ }
+ } else {
+ m_target_point = msg->pose.position;
+ m_target_orientation = tf::getYaw(msg->pose.orientation);
+ }
+ m_avoided_collision = false;
+ m_desired_distance = 0.1;
+ m_skip_final_turn = false;
+ m_fast_path_planning = false;
+ m_new_target = true;
+
+ ROS_INFO_STREAM("Navigating to target via Move Base Simple x: "
+ << m_target_point.x << ", y: " << m_target_point.y
+ << "\nTarget orientation: " << m_target_orientation
+ << " Desired distance to target: " << m_desired_distance
+ << "\nframe_id: " << msg->header.frame_id);
+
+ m_MainMachine.setState(AWAITING_PATHPLANNING_MAP);
}
-void HomerNavigationNode::navigateToPOICallback(const homer_mapnav_msgs::NavigateToPOI::ConstPtr &msg)
-{
- homer_mapnav_msgs::GetPointsOfInterest srv;
- m_get_POIs_client.call(srv);
- std::vector<homer_mapnav_msgs::PointOfInterest>::iterator it;
- for(it = srv.response.poi_list.pois.begin(); it != srv.response.poi_list.pois.end(); ++it)
- {
- if(it->name == msg->poi_name)
- {
- m_avoided_collision = false;
- m_target_point = it->pose.position;
- m_target_orientation = tf::getYaw(it->pose.orientation);
- m_desired_distance = msg->distance_to_target < 0.1 ? 0.1 : msg->distance_to_target;
- m_skip_final_turn = msg->skip_final_turn;
- m_fast_path_planning = false;
- m_new_target = true;
- m_target_name = msg->poi_name;
-
- ROS_INFO_STREAM("Navigating to target " << m_target_point.x << ", " << m_target_point.y
- << "\nTarget orientation: " << m_target_orientation
- << "Desired distance to target: " << m_desired_distance);
-
- m_MainMachine.setState( AWAITING_PATHPLANNING_MAP );
- return;
- }
- }
-
- ROS_ERROR_STREAM("No point of interest with name '" << msg->poi_name << "' found in current poi list");
- sendTargetUnreachableMsg(homer_mapnav_msgs::TargetUnreachable::UNKNOWN);
+void HomerNavigationNode::navigateToPOICallback(
+ const homer_mapnav_msgs::NavigateToPOI::ConstPtr& msg) {
+ homer_mapnav_msgs::GetPointsOfInterest srv;
+ m_get_POIs_client.call(srv);
+ std::vector<homer_mapnav_msgs::PointOfInterest>::iterator it;
+ for (it = srv.response.poi_list.pois.begin();
+ it != srv.response.poi_list.pois.end(); ++it) {
+ if (it->name == msg->poi_name) {
+ m_avoided_collision = false;
+ m_target_point = it->pose.position;
+ m_target_orientation = tf::getYaw(it->pose.orientation);
+ m_desired_distance =
+ msg->distance_to_target < 0.1 ? 0.1 : msg->distance_to_target;
+ m_skip_final_turn = msg->skip_final_turn;
+ m_fast_path_planning = false;
+ m_new_target = true;
+ m_target_name = msg->poi_name;
+ m_stop_before_obstacle = msg->stop_before_obstacle;
+
+ ROS_INFO_STREAM("Navigating to target "
+ << m_target_point.x << ", " << m_target_point.y
+ << "\nTarget orientation: " << m_target_orientation
+ << "Desired distance to target: " << m_desired_distance);
+
+ m_MainMachine.setState(AWAITING_PATHPLANNING_MAP);
+ return;
+ }
+ }
+
+ ROS_ERROR_STREAM("No point of interest with name '"
+ << msg->poi_name << "' found in current poi list");
+ sendTargetUnreachableMsg(homer_mapnav_msgs::TargetUnreachable::UNKNOWN);
}
-void HomerNavigationNode::stopNavigationCallback(const std_msgs::Empty::ConstPtr& msg)
-{
- ROS_INFO_STREAM("Stopping navigation." );
- m_MainMachine.setState( IDLE );
- m_avoided_collision = false;
- stopRobot();
-
- m_waypoints.clear();
- nav_msgs::Path empty_path_msg;
- empty_path_msg.poses = m_waypoints;
- m_path_pub.publish(empty_path_msg);
+void HomerNavigationNode::stopNavigationCallback(
+ const std_msgs::Empty::ConstPtr& msg) {
+ ROS_INFO_STREAM("Stopping navigation.");
+ m_MainMachine.setState(IDLE);
+ m_avoided_collision = false;
+ stopRobot();
+
+ m_waypoints.clear();
+ nav_msgs::Path empty_path_msg;
+ empty_path_msg.poses = m_waypoints;
+ m_path_pub.publish(empty_path_msg);
}
-void HomerNavigationNode::unknownThresholdCallback(const std_msgs::Int8::ConstPtr &msg)
-{
- m_explorer->setUnknownThreshold(static_cast<int>(msg->data));
+void HomerNavigationNode::unknownThresholdCallback(
+ const std_msgs::Int8::ConstPtr& msg) {
+ m_explorer->setUnknownThreshold(static_cast<int>(msg->data));
}
-int main(int argc, char **argv)
-{
- ros::init(argc, argv, "homer_navigation");
+int main(int argc, char** argv) {
+ ros::init(argc, argv, "homer_navigation");
- HomerNavigationNode node;
+ HomerNavigationNode node;
- ros::Rate rate(50);
+ ros::Rate rate(50);
- while(ros::ok())
- {
- ros::spinOnce();
- node.idleProcess();
- rate.sleep();
- }
+ while (ros::ok()) {
+ ros::spinOnce();
+ node.idleProcess();
+ rate.sleep();
+ }
- return 0;
+ return 0;
}
diff --git a/tracks.yaml b/tracks.yaml
index b20d83b1..d280d6a0 100644
--- a/tracks.yaml
+++ b/tracks.yaml
@@ -13,7 +13,7 @@ tracks:
- git-bloom-generate -y rosrpm --prefix release/:{ros_distro} :{ros_distro} -i
:{release_inc}
devel_branch: master
- last_version: 0.1.5
+ last_version: 0.1.6
name: upstream
patches: null
release_inc: '0'
--
GitLab