diff --git a/homer_navigation/include/homer_navigation/homer_navigation_node.h b/homer_navigation/include/homer_navigation/homer_navigation_node.h
index 53447adf4a13e21d6e02a60cf679be2166916e6d..d452feb40237fe03ab2c75ec60f47a0adb1c4a88 100644
--- a/homer_navigation/include/homer_navigation/homer_navigation_node.h
+++ b/homer_navigation/include/homer_navigation/homer_navigation_node.h
@@ -40,304 +40,299 @@ 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, 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();
+
+ void initNewTarget();
+
+ 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 obstacleOnPath();
+
+ /** @brief calculate path from current robot position to target
+ * approximation
+ */
+ void calculatePath();
+
+ void setExplorerMap();
+
+ /** @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 */
+ bool isTargetPositionReached();
+ bool m_new_target;
+
+ /** @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(nav_msgs::OccupancyGrid& cmap);
+
+ /**
+ * @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();
- /** @brief calcs the maximal move distance from Laser and DepthData */
- void calcMaxMoveDist();
+ // 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);
- /// @brief Worker instances
- Explorer* m_explorer;
+ /**
+ * 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 State machine
- StateMachine<ProcessState> m_MainMachine;
+ 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 Navigation options & data
+ /** @brief calcs the maximal move distance from Laser and DepthData */
+ void calcMaxMoveDist();
- /** list of waypoints subsampled from m_PixelPath */
- std::vector<geometry_msgs::PoseStamped> m_waypoints;
+ /// @brief Worker instances
+ Explorer* m_explorer;
- /** Path planned by Explorer, pixel accuracy */
- std::vector<Eigen::Vector2i> m_pixel_path;
-
- /** target point */
- geometry_msgs::Point m_target_point;
+ /// @brief State machine
+ StateMachine<ProcessState> m_MainMachine;
- /** target name if called via Navigate_to_POI */
- std::string m_target_name;
+ /// @brief Navigation options & data
- /** orientation the robot should have at the target point */
- double m_target_orientation;
+ /** list of waypoints subsampled from m_PixelPath */
+ std::vector<geometry_msgs::PoseStamped> m_waypoints;
- /** allowed distance to target */
- float m_desired_distance;
+ /** Path planned by Explorer, pixel accuracy */
+ std::vector<Eigen::Vector2i> m_pixel_path;
- /** check if the final turn should be skipped */
- bool m_skip_final_turn;
+ /** target point */
+ geometry_msgs::Point m_target_point;
- /**
- * 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;
+ /** target name if called via Navigate_to_POI */
+ std::string m_target_name;
- /** current pose of the robot */
- geometry_msgs::Pose m_robot_pose;
+ /** orientation the robot should have at the target point */
+ double m_target_orientation;
- /** 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;
+ /** allowed distance to target */
+ float m_desired_distance;
- bool m_avoided_collision;
+ /** check if the final turn should be skipped */
+ bool m_skip_final_turn;
- 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;
+ /**
+ * 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;
+
+ std::map<std::string, sensor_msgs::LaserScan::ConstPtr> m_scan_map;
+
+ /** 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;
+
+ /// @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;
+ ros::Time m_last_check_path_time;
+
+ /** do not replan if lisa avoids an obstacle, instead send target
+ * unreachable*/
+ bool m_stop_before_obstacle;
+
+ 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;
+
+ 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_seen_obstacle_before;
+ bool m_use_ptu;
+
+ bool m_path_reaches_target;
+ bool m_initial_path_reaches_target;
+ int m_last_calculations_failed;
+ int m_unknown_threshold;
+
+ /** last map data */
+ nav_msgs::OccupancyGrid::ConstPtr m_map;
+
+ // 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/src/homer_navigation_node.cpp b/homer_navigation/src/homer_navigation_node.cpp
index c5de4ecce49f98698f240ff55adf9231a9a6560e..4f7d241f2807f0b6f8df1952158e04e84b680f21 100644
--- a/homer_navigation/src/homer_navigation_node.cpp
+++ b/homer_navigation/src/homer_navigation_node.cpp
@@ -1,1126 +1,1162 @@
#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();
+ 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", 3);
+ 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, 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);
+ // Explorer constructor
+ 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_AllowedObstacleDistance.second;
+ m_SafeObstacleDistance.first;
+ m_SafeObstacleDistance.second;
+
+ // 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);
+ if (m_explorer) {
+ delete m_explorer;
+ }
}
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);
+ m_explorer = 0;
+ 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_time = ros::Time::now();
+ nav_msgs::OccupancyGrid tmp_map;
+ tmp_map.header.frame_id = "/map";
+ tmp_map.info.resolution = 0;
+ tmp_map.info.width = 0;
+ tmp_map.info.height = 0;
+ tmp_map.info.origin.position.x = 0;
+ tmp_map.info.origin.position.y = 0;
+ tmp_map.info.origin.position.z = 0;
+ m_map = boost::make_shared<nav_msgs::OccupancyGrid>(tmp_map);
+
+ loadParameters();
+
+ m_MainMachine.setState(IDLE);
}
HomerNavigationNode::~HomerNavigationNode() {
- if (m_explorer) {
- delete m_explorer;
- }
- if (m_last_map_data) {
- delete m_last_map_data;
- }
+ if (m_explorer) {
+ delete m_explorer;
+ }
}
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);
+ 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.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 (m_MainMachine.state() != IDLE) {
+ 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();
+ }
}
- if ((ros::Time::now() - m_last_pose_time) >
- ros::Duration(m_callback_error_duration)) {
- ROS_ERROR_STREAM("Pose timeout!\n");
- stopRobot();
+}
+
+void HomerNavigationNode::setExplorerMap() {
+ // adding lasers to map
+ nav_msgs::OccupancyGrid temp_map = *m_map;
+ for (const auto& scan : m_scan_map) {
+ std::vector<geometry_msgs::Point> scan_points;
+ scan_points = map_tools::laser_msg_to_points(
+ scan.second, m_transform_listener, "/map");
+ for (const auto& point : scan_points) {
+ Eigen::Vector2i map_point = map_tools::toMapCoords(point, m_map);
+ int index = map_point.y() * m_map->info.width + map_point.x();
+ if (index > 0 && index < m_map->info.width * m_map->info.height) {
+ temp_map.data[index] = (int8_t)100;
+ }
+ }
+ }
+ if (m_fast_path_planning) {
+ maskMap(temp_map);
}
- }
+ m_explorer->setOccupancyMap(
+ boost::make_shared<nav_msgs::OccupancyGrid>(temp_map));
}
void HomerNavigationNode::calculatePath() {
- if (m_distance_to_target < m_desired_distance && !m_new_target) {
- m_path_reaches_target = true;
- return;
- }
- 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);
+ if (!m_explorer) {
+ return;
}
- } 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);
+ if (isTargetPositionReached()) {
+ return;
}
- m_last_laser_time = ros::Time::now();
- m_last_pose_time = ros::Time::now();
- }
+ // setExplorerMap();
+ m_explorer->setStart(map_tools::toMapCoords(m_robot_pose.position, m_map));
+
+ 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_map);
+ 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);
+ }
+ if (m_path_reaches_target) {
+ geometry_msgs::PoseStamped poseStamped;
+ poseStamped.header.frame_id = "/map";
+ poseStamped.pose.position = m_target_point;
+ poseStamped.pose.orientation.x = 0;
+ poseStamped.pose.orientation.y = 0;
+ poseStamped.pose.orientation.z = 0;
+ poseStamped.pose.orientation.w = 1;
+ 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) {
+ if (!m_explorer) {
+ return;
+ }
+ if (isTargetPositionReached()) {
+ return;
+ }
+
+ ROS_INFO_STREAM("Distance to target still too large ("
+ << m_distance_to_target
+ << "m; requested: " << m_desired_distance << "m)");
+ setExplorerMap();
+
+ // 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_map));
+
+ 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_map));
+ Eigen::Vector2i new_target_approx = m_explorer->getNearestAccessibleTarget(
+ map_tools::toMapCoords(m_target_point, m_map));
+
+ geometry_msgs::Point new_target_approx_world =
+ map_tools::fromMapCoords(new_target_approx, m_map);
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();
- }
-
-
- // 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));
+ "start Navigation: Approx target: " << new_target_approx_world);
- 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_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;
+
+ if (map_tools::distance(m_robot_pose.position, new_target_approx_world) <
+ m_map->info.resolution * 1.5) {
+ ROS_WARN_STREAM("close to aprox target - final turn");
+ m_MainMachine.setState(FINAL_TURN);
+ }
+
+ 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();
}
- }
-
- 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);
+ 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 << " ===");
+ 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();
+ 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::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");
+ 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();
+ ROS_INFO_STREAM("=== TargetUnreachableMsg ===");
}
-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) {
+bool HomerNavigationNode::isTargetPositionReached() {
+ if (m_distance_to_target < m_desired_distance && !m_new_target) {
+ ROS_INFO_STREAM("Target position reached. Distance to target: "
+ << m_distance_to_target
+ << "m. Desired distance:" << m_desired_distance << "m");
+ stopRobot();
+ m_waypoints.clear();
+ m_MainMachine.setState(FINAL_TURN);
+ ROS_INFO_STREAM("Turning to look-at point");
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;
- }
- }
+ } else {
+ 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!");
- }
+bool HomerNavigationNode::obstacleOnPath() {
+ m_last_check_path_time = ros::Time::now();
+ if (m_pixel_path.size() != 0) {
+ for (auto const& scan : m_scan_map) {
+ std::vector<geometry_msgs::Point> scan_points;
+ scan_points = map_tools::laser_msg_to_points(
+ scan.second, m_transform_listener, "/map");
+
+ for (unsigned i = 0; i < m_pixel_path.size() - 1; i++) {
+ geometry_msgs::Point p =
+ map_tools::fromMapCoords(m_pixel_path.at(i), m_map);
+ if (map_tools::distance(m_robot_pose.position, p) >
+ m_check_path_max_distance) {
+ return false;
+ }
+ for (auto const& sp : scan_points) {
+ if (map_tools::distance(sp, p) <
+ m_AllowedObstacleDistance.first) {
+ ROS_INFO_STREAM("Found obstacle");
+ return true;
+ }
+ }
+ }
+ }
+ }
+ return false;
}
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 (m_waypoints.size() == 0) {
- ROS_WARN_STREAM(
- "No waypoints but trying to perform next move! Skipping.");
- 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;
+ if (m_MainMachine.state() == FOLLOWING_PATH) {
+ if (isTargetPositionReached()) {
+ return;
+ }
+ // check if an obstacle is on the current path
+ if (m_check_path &&
+ (ros::Time::now() - m_last_check_path_time) > ros::Duration(0.2)) {
+ if (obstacleOnPath()) {
+ if (m_seen_obstacle_before) {
+ stopRobot();
+ calculatePath();
+ return;
+ }
+ m_seen_obstacle_before = true;
+ } else {
+ m_seen_obstacle_before = false;
+ }
}
- } 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 (m_waypoints.size() == 0) {
+ ROS_WARN_STREAM(
+ "No waypoints but trying to perform next move! Skipping.");
+ 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());
+ }
+ }
- if ((waypointRadius < m_resolution) || (m_waypoints.size() == 1)) {
- waypointRadius = m_resolution;
- }
+ Eigen::Vector2i waypointPixel =
+ map_tools::toMapCoords(m_waypoints[0].pose.position, m_map);
+ float obstacleDistanceMap =
+ m_explorer->getObstacleTransform()->getValue(waypointPixel.x(),
+ waypointPixel.y()) *
+ m_map->info.resolution;
+ float waypointRadius = m_waypoint_radius_factor * obstacleDistanceMap;
+
+ if ((waypointRadius < m_map->info.resolution) ||
+ (m_waypoints.size() == 1)) {
+ waypointRadius = m_map->info.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());
- }
- }
+ 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_waypoints.erase(m_waypoints.begin());
+ }
+ }
- sendPathData();
- // last wayoint reached
- if (m_waypoints.size() == 0) {
- ROS_INFO_STREAM("Last waypoint reached");
- currentPathFinished();
- return;
- }
+ 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);
- }
+ sendPathData();
- 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));
- if (obstacleMapDistance <= 0.00001) {
- ROS_ERROR_STREAM(
- "obstacleMapDistance is below threshold to 0 setting to 1");
- obstacleMapDistance = 1;
+ 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 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) {
+ 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;
- }
- 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);
- }
+ 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_map);
+ } else {
+ robotPixel = map_tools::toMapCoords(
+ m_waypoints[wpi].pose.position, m_map);
+ }
+ obstacleMapDistance = std::min(
+ (float)obstacleMapDistance,
+ (float)(m_explorer->getObstacleTransform()->getValue(
+ robotPixel.x(), robotPixel.y()) *
+ m_map->info.resolution));
+ if (obstacleMapDistance <= 0.00001) {
+ ROS_ERROR_STREAM(
+ "obstacleMapDistance is below threshold to 0 setting "
+ "to 1");
+ obstacleMapDistance = 1;
+ }
+ }
+
+ 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),
+ m_max_move_speed, max_move_distance_speed,
+ 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);
+ }
- 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;
- }
+ // 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 (isTargetPositionReached()) {
+ return;
+ } 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);
+ }
- 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);
+ 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);
+ ROS_INFO_STREAM("Current path was finished, initiating recalculation.");
+ m_waypoints.clear();
+ stopRobot();
+ startNavigation();
}
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;
+ 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;
}
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);
+ if (vertices.size() == 0) {
+ ROS_INFO_STREAM("No vertices given!");
+ return false;
+ }
+ // make temp. map
+ std::vector<int> data(m_map->info.width * m_map->info.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;
+ // 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;
}
- data[index] = value;
- xerr += dx;
- yerr += dy;
- if (xerr > dist) {
- xerr -= dist;
- x += incx;
+ if (dy < 0) {
+ incy = -1;
+ dy = -dy;
+ } else {
+ incy = dy ? 1 : 0;
}
- if (yerr > dist) {
- yerr -= dist;
- y += incy;
+
+ // 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_map->info.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;
+ int index = x + m_map->info.width * y;
+ bool tmp = false;
+
+ if ((int)m_map->data.at(index) > 90) {
+ tmp = true;
}
- if (fillPolygon(data, x, y - 1, value)) {
- 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;
+ 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;
-
- 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(-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);
- }
-
- return drawPolygon(vertices);
+ 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;
+
+ 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);
+
+ 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_map);
+ map_point.x = map_coord.x();
+ map_point.y = map_coord.y();
+ vertices.push_back(map_point);
+ }
+
+ 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_height; y++){
- if (!safe_planning_box.contains(Eigen::Vector2i(x, y))) {
- m_last_map_data->at(y * m_width + x) = -1;
- }
+void HomerNavigationNode::maskMap(nav_msgs::OccupancyGrid& cmap) {
+ // generate bounding box
+ ROS_INFO_STREAM("Calculating Bounding box for fast planning");
+ Eigen::Vector2i pose_pixel =
+ map_tools::toMapCoords(m_robot_pose.position, m_map);
+ Eigen::Vector2i target_pixel =
+ map_tools::toMapCoords(m_target_point, m_map);
+ 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_map));
+ ROS_INFO_STREAM("max in m: " << map_tools::fromMapCoords(
+ safe_planning_box.max(), m_map));
+ for (size_t x = 0; x < m_map->info.width; x++) {
+ for (size_t y = 0; y < m_map->info.height; y++) {
+ if (!safe_planning_box.contains(Eigen::Vector2i(x, y))) {
+ cmap.data.at(y * m_map->info.width + x) = -1;
+ }
+ }
}
- }
}
// convenience math functions
float HomerNavigationNode::minTurnAngle(float angle1, float angle2) {
- angle1 *= 180.0 / M_PI;
- angle2 *= 180.0 / M_PI;
+ angle1 *= 180.0 / M_PI;
+ angle2 *= 180.0 / M_PI;
- int diff = angle2 - angle1;
- diff = (diff + 180) % 360 - 180;
- if (diff < -180) {
- diff += 360;
- }
+ 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();
+ ROS_INFO_STREAM("Refreshing Parameters");
+ loadParameters();
}
void HomerNavigationNode::mapCallback(
const nav_msgs::OccupancyGrid::ConstPtr& msg) {
- 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;
- m_explorer->setOccupancyMap(m_width, m_height, m_origin,
- &(*m_last_map_data)[0]);
-
- 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;
+ if (!m_explorer) {
+ float resolution = msg->info.resolution;
+ m_explorer = new Explorer(m_AllowedObstacleDistance.first / resolution,
+ m_AllowedObstacleDistance.second / resolution,
+ m_SafeObstacleDistance.first / resolution,
+ m_SafeObstacleDistance.second / resolution,
+ m_SafePathWeight, m_FrontierSafenessFactor,
+ m_unknown_threshold);
+ }
+ m_map = msg;
+ if (m_MainMachine.state() != IDLE) {
+ setExplorerMap();
}
- }
}
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();
+ m_robot_last_pose = m_robot_pose;
+ m_robot_pose = msg->pose;
+ m_last_pose_time = ros::Time::now();
+ m_new_target = false;
+ m_distance_to_target =
+ map_tools::distance(m_robot_pose.position, m_target_point);
+ 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();
- }
+ m_max_move_distance =
+ std::min({m_max_move_sick, 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) {
+ if (m_MainMachine.state() == FOLLOWING_PATH) {
+ stopRobot();
+ m_MainMachine.setState(AVOIDING_COLLISION);
+ ROS_WARN_STREAM("Collision detected while following path!");
+ }
+ }
}
void HomerNavigationNode::maxDepthMoveDistanceCallback(
const std_msgs::Float32::ConstPtr& msg) {
- m_max_move_depth = msg->data;
- calcMaxMoveDist();
+ 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();
+ m_scan_map[msg->header.frame_id] = msg;
+ m_last_laser_time = ros::Time::now();
+ m_max_move_sick = map_tools::get_max_move_distance(
+ map_tools::laser_msg_to_points(msg, m_transform_listener, "/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();
+ m_scan_map[msg->header.frame_id] = msg;
+ m_max_move_down = map_tools::get_max_move_distance(
+ map_tools::laser_msg_to_points(msg, m_transform_listener, "/base_link"),
+ m_min_x, m_min_y);
+ calcMaxMoveDist();
}
+void HomerNavigationNode::initNewTarget() {
+ m_initial_path_reaches_target = false;
+ m_avoided_collision = false;
+ m_new_target = true;
+}
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);
+ 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_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);
+ initNewTarget();
+ startNavigation();
}
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;
- }
+ 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 {
- 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;
- }
+ m_target_point = msg->pose.position;
+ m_target_orientation = tf::getYaw(msg->pose.orientation);
}
- } 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);
+ m_desired_distance = 0.1;
+ m_skip_final_turn = false;
+ m_fast_path_planning = false;
+
+ 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);
+ initNewTarget();
+ startNavigation();
}
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;
+ 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_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_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);
+ initNewTarget();
+ startNavigation();
+ return;
+ }
}
- }
- ROS_ERROR_STREAM("No point of interest with name '"
- << msg->poi_name << "' found in current poi list");
- sendTargetUnreachableMsg(homer_mapnav_msgs::TargetUnreachable::UNKNOWN);
+ 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);
+ ROS_INFO_STREAM("Stopping navigation.");
+ m_MainMachine.setState(IDLE);
+ stopRobot();
+
+ m_waypoints.clear();
}
void HomerNavigationNode::unknownThresholdCallback(
const std_msgs::Int8::ConstPtr& msg) {
- m_explorer->setUnknownThreshold(static_cast<int>(msg->data));
+ if (m_explorer) {
+ m_explorer->setUnknownThreshold(static_cast<int>(msg->data));
+ }
}
int main(int argc, char** argv) {
- ros::init(argc, argv, "homer_navigation");
+ 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;
}