Skip to content
Snippets Groups Projects
Commit 01f9cfa0 authored by Niklas Yann Wettengel's avatar Niklas Yann Wettengel
Browse files

Rebase from 'upstream'

parent e76f25fb
No related branches found
No related tags found
No related merge requests found
Showing with 1396 additions and 1290 deletions
## 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`
......
......@@ -2,6 +2,9 @@
Changelog for package homer_map_manager
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.1.7 (2016-11-17)
------------------
0.1.6 (2016-11-04)
------------------
......
<package>
<name>homer_map_manager</name>
<version>0.1.6</version>
<version>0.1.7</version>
<description>
map_manager
</description>
......
......@@ -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)
------------------
......
string poi_name
float32 distance_to_target
bool skip_final_turn
bool stop_before_obstacle
<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>
......
......@@ -2,6 +2,9 @@
Changelog for package homer_mapping
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.1.7 (2016-11-17)
------------------
0.1.6 (2016-11-04)
------------------
......
<package>
<name>homer_mapping</name>
<version>0.1.6</version>
<version>0.1.7</version>
<description>
homer_mapping
......
......@@ -2,6 +2,9 @@
Changelog for package homer_nav_libs
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.1.7 (2016-11-17)
------------------
0.1.6 (2016-11-04)
------------------
......
<?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>
......
......@@ -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)
------------------
......
#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
<?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>
......
This diff is collapsed.
......@@ -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'
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment