homer_navigation_node.cpp 57.1 KB
Newer Older
Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
1
2
#include "homer_navigation/homer_navigation_node.h"

3
4
#include <homer_ptu/HomerPtuLib.h>

5
#define DUMP_OBSTACLE_AVOIDANCE_IMAGE 0 
6
7
8
9
#if DUMP_OBSTACLE_AVOIDANCE_IMAGE
#include <opencv2/opencv.hpp>
#endif

Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
10
11
HomerNavigationNode::HomerNavigationNode()
{
12
    ros::NodeHandle nh;
13
14
    homer_ptu::init(nh);
    
15
16
17
18
19
    // subscribers
    m_map_sub
        = nh.subscribe<nav_msgs::OccupancyGrid>("/map", 3, &HomerNavigationNode::mapCallback, this);
    m_pose_sub = nh.subscribe<geometry_msgs::PoseStamped>(
        "/pose", 3, &HomerNavigationNode::poseCallback, this);
20
21
    std::string scan_topic;
    nh.param<std::string>("/homer_scan_topic", scan_topic, "/scan");
22
    m_laser_data_sub = nh.subscribe<sensor_msgs::LaserScan>(
23
        scan_topic, 3, &HomerNavigationNode::laserDataCallback, this);
24
25
    std::string front_scan_topic;
    nh.param<std::string>("/homer_front_scan_topic", front_scan_topic, "/front_scan");
26
    m_down_laser_data_sub = nh.subscribe<sensor_msgs::LaserScan>(
27
        front_scan_topic, 3, &HomerNavigationNode::laserDataCallback, this);
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
    m_start_navigation_sub
        = nh.subscribe<homer_mapnav_msgs::StartNavigation>("/homer_navigation/start_navigation", 3,
            &HomerNavigationNode::startNavigationCallback, this);
    m_move_base_simple_goal_sub = nh.subscribe<geometry_msgs::PoseStamped>("/move_base_simple/goal",
        3, &HomerNavigationNode::moveBaseSimpleGoalCallback,
        this); // for RVIZ usage
    m_stop_navigation_sub = nh.subscribe<std_msgs::Empty>(
        "/homer_navigation/stop_navigation", 3, &HomerNavigationNode::stopNavigationCallback, this);
    m_navigate_to_poi_sub = nh.subscribe<homer_mapnav_msgs::NavigateToPOI>(
        "/homer_navigation/navigate_to_POI", 3, &HomerNavigationNode::navigateToPOICallback, this);
    m_unknown_threshold_sub = nh.subscribe<std_msgs::Int8>("/homer_navigation/unknown_threshold", 3,
        &HomerNavigationNode::unknownThresholdCallback, this);
    m_refresh_param_sub = nh.subscribe<std_msgs::Empty>(
        "/homer_navigation/refresh_params", 3, &HomerNavigationNode::refreshParamsCallback, this);
    m_max_move_depth_sub
        = nh.subscribe<std_msgs::Float32>("/homer_navigation/max_depth_move_distance", 3,
            &HomerNavigationNode::maxDepthMoveDistanceCallback, this);

    std::string cmd_vel_topic;
    nh.param<std::string>("/homer_cmd_vel_topic", cmd_vel_topic, "/robot_platform/cmd_vel");
    m_cmd_vel_pub = nh.advertise<geometry_msgs::Twist>(cmd_vel_topic, 3);
    m_target_reached_string_pub
        = nh.advertise<std_msgs::String>("/homer_navigation/target_reached", 3);
    // m_target_reached_empty_pub 	=
    // nh.advertise<std_msgs::Empty>("/homer_navigation/target_reached", 3);
    m_target_unreachable_pub = nh.advertise<homer_mapnav_msgs::TargetUnreachable>(
        "/homer_navigation/target_unreachable", 3);
    m_path_pub = nh.advertise<nav_msgs::Path>("/homer_navigation/path", 3);

    m_get_POIs_client
        = nh.serviceClient<homer_mapnav_msgs::GetPointsOfInterest>("/map_manager/get_pois");

    init();
Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
61
62
}

Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
63
64
void HomerNavigationNode::loadParameters()
{
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
    // 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);

    // 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);
    ros::param::param("/homer_navigation/speed_ramp", m_speed_ramp, (float)0.1);

    // 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/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;
        m_explorer = 0;
    }
    initExplorer();
Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
126
127
}

Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
128
129
void HomerNavigationNode::init()
{
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
    m_explorer = 0;
    m_robot_pose = geometry_msgs::Pose();
    m_robot_pose.orientation = tf::createQuaternionMsgFromYaw(0.0);
    m_robot_last_pose = m_robot_pose;
    m_avoided_collision = false;
    m_angular_avoidance = 0;
    m_last_check_path_time = ros::Time::now();
    m_unreachable_delay = ros::Time::now();
    m_fast_path_planning = false;

    m_cmd_vel = geometry_msgs::Twist();
    m_last_cmd_vel = m_cmd_vel;
    m_last_cmd_vel_time = ros::Time::now();

    m_obstacle_on_path = false;
    m_obstacle_position = geometry_msgs::Point();

    nav_msgs::OccupancyGrid tmp_map = nav_msgs::OccupancyGrid();
148
    tmp_map.header.frame_id = "map";
149
150
151
152
153
    m_map = boost::make_shared<nav_msgs::OccupancyGrid>(tmp_map);

    loadParameters();

    m_state = IDLE;
Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
154
155
}

Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
156
157
HomerNavigationNode::~HomerNavigationNode()
{
158
159
160
161
    if (m_explorer)
    {
        delete m_explorer;
    }
Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
162
163
}

Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
164
165
void HomerNavigationNode::stopRobot()
{
166
167
168
169
170
    ROS_DEBUG_STREAM("stopRobot()");
    m_cmd_vel = geometry_msgs::Twist();
    m_cmd_vel_pub.publish(m_cmd_vel);
    m_cmd_vel_pub.publish(m_cmd_vel);
    m_cmd_vel_pub.publish(m_cmd_vel);
Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
171
172
}

Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
173
174
void HomerNavigationNode::idleProcess()
{
175
    if (m_state != IDLE)
Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
176
    {
177
178
179
180
181
182
183
184
185
186
187
188
        if ((ros::Time::now() - m_last_laser_time) > ros::Duration(m_callback_error_duration))
        {
            ROS_ERROR_STREAM("Laser data timeout!\n");
            stopRobot();
            ros::Duration(0.1).sleep();
        }
        if ((ros::Time::now() - m_last_pose_time) > ros::Duration(m_callback_error_duration))
        {
            ROS_ERROR_STREAM("Pose timeout!\n");
            stopRobot();
            ros::Duration(0.1).sleep();
        }
189
    }
Florian Polster's avatar
Florian Polster committed
190
191
}

Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
192
193
void HomerNavigationNode::setExplorerMap()
{
194
195
196
197
    // adding lasers to map
    nav_msgs::OccupancyGrid temp_map = *m_map;
    for (const auto& scan : m_scan_map)
    {
Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
198
        if (ros::Time::now() - scan.second->header.stamp < ros::Duration(1))
Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
199
        {
200
            std::vector<geometry_msgs::Point> scan_points;
201
            scan_points = map_tools::laser_msg_to_points(scan.second, m_transform_listener, "map");
202
203
204
205
206
207
208
209
210
            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;
                }
            }
Florian Polster's avatar
Florian Polster committed
211
        }
212
213
214
215
216
217
    }
    if (m_fast_path_planning)
    {
        // maskMap(temp_map);
    }
    m_explorer->setOccupancyMap(boost::make_shared<nav_msgs::OccupancyGrid>(temp_map));
Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
218
219
}

220
void HomerNavigationNode::calculatePath(bool setMap)
Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
221
{
222
223
224
    if (!m_explorer)
    {
        return;
225
    }
226
    if (isTargetPositionReached())
Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
227
    {
228
        return;
229
230
    }

231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
    ROS_DEBUG_STREAM("calculatePath(" << setMap << ")");

    if (setMap)
    {
        setExplorerMap();
    }
    m_explorer->setStart(map_tools::toMapCoords(m_robot_pose.position, m_map));

    bool success;
    std::vector<Eigen::Vector2i> tmp_pixel_path = m_explorer->getPath(success);
    if (!success)
    {
        ROS_WARN_STREAM("no path to target possible - drive to obstacle");
        m_obstacle_on_path = true;
    }
    else
    {
248
        ROS_DEBUG_STREAM("Found tmp path with " << tmp_pixel_path.size() << " points.");
249
        m_obstacle_on_path = false;
250
251
252
253
254
255
        
        m_sample_path = false;
        ros::param::getCached("/homer_navigation/sample_path", m_sample_path); 
        const std::vector<Eigen::Vector2i>& waypoint_pixels = 
            (m_sample_path) ? m_explorer->sampleWaypointsFromPath(tmp_pixel_path, m_waypoint_sampling_threshold) : tmp_pixel_path;
        
256
257
258
259
        m_waypoints.clear();
        ROS_INFO_STREAM("homer_navigation::calculatePath - Path Size: " << waypoint_pixels.size());
        if (waypoint_pixels.size() > 0)
        {
260
261
262
263
            geometry_msgs::PoseStamped poseStamped;
            poseStamped.header.frame_id = "map";
            poseStamped.pose.orientation.w = 1.0;
            for(const auto& wp : waypoint_pixels)
264
            {
265
                poseStamped.pose.position = map_tools::fromMapCoords(wp, m_map);
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
                m_waypoints.push_back(poseStamped);
            }
            if (m_path_reaches_target)
            {
                poseStamped.pose.position = m_target_point;
                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();
    }
Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
283
284
285
286
}

void HomerNavigationNode::startNavigation()
{
287
288
289
290
291
292
293
294
295
296
297
298
    if (!m_explorer)
    {
        return;
    }
    if (isTargetPositionReached())
    {
        return;
    }
    // CHECK IF THERE IS A DIRECT PATH TO THE TARGET
    if (m_fast_path_planning)
    {
        m_waypoints.clear();
299

300
        geometry_msgs::PoseStamped poseStamped = geometry_msgs::PoseStamped();
301
        poseStamped.header.frame_id = "map";
302
303
304
        poseStamped.pose.position = m_target_point;
        poseStamped.pose.orientation.w = 1;
        m_waypoints.push_back(poseStamped);
305

306
307
308
309
310
        if (!obstacleOnPath())
        {
            m_state = FOLLOWING_PATH;
            return;
        }
311
    }
Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
312

313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
    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;
        std::vector<Eigen::Vector2i> tmp_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;
        }
    }
Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
332

Florian Polster's avatar
Florian Polster committed
333
    m_explorer->setStart(map_tools::toMapCoords(m_robot_pose.position, m_map));
334
335
    Eigen::Vector2i new_target_approx
        = m_explorer->getNearestAccessibleTarget(map_tools::toMapCoords(m_target_point, m_map));
Florian Polster's avatar
Florian Polster committed
336

337
338
339
340
341
342
343
344
345
346
    geometry_msgs::Point new_target_approx_world
        = map_tools::fromMapCoords(new_target_approx, m_map);
    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;

    if (map_tools::distance(m_robot_pose.position, new_target_approx_world)
        < m_map->info.resolution * 1.5)
Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
347
    {
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
        ROS_WARN_STREAM("close to aprox target - final turn");
        m_state = 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_state = FINAL_TURN;
    }
    else
    {
        m_explorer->setTarget(new_target_approx);
        m_state = FOLLOWING_PATH;

        calculatePath(false);
    }
Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
372
373
}

Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
374
375
void HomerNavigationNode::sendPathData()
{
376
    nav_msgs::Path msg;
377
    msg.header.frame_id = "map";
378
379
380
381
382
383
    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;
384
        pose_stamped.header.frame_id = "map";
385
386
387
388
        pose_stamped.header.stamp = ros::Time::now();
        msg.poses.insert(msg.poses.begin(), pose_stamped);
    }
    m_path_pub.publish(msg);
Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
389
390
}

Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
391
392
void HomerNavigationNode::sendTargetReachedMsg()
{
393
394
395
396
397
398
399
    stopRobot();
    m_state = 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 << " ===");
Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
400
401
}

Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
402
403
void HomerNavigationNode::sendTargetUnreachableMsg(int8_t reason)
{
404
405
406
407
    stopRobot();
    double unreachable_duration = 3;
    ros::param::getCached("/homer_navigation/unreachable_duration", unreachable_duration);
    if (ros::Time::now() - m_unreachable_delay > ros::Duration(5))
408
    {
409
        m_unreachable_delay = ros::Time::now();
410
    }
411
412
    else if (ros::Time::now() - m_unreachable_delay > ros::Duration(unreachable_duration))
    {
413
        m_use_ptu = false;
414
415
416
417
        ros::param::getCached("/homer_navigation/use_ptu", m_use_ptu);
        if (m_use_ptu)
        {
            // reset PTU
418
419
            homer_ptu::stop_focussing_view();
            homer_ptu::set_ptu(0, 0);
420
        }
421

422
423
424
425
        m_state = IDLE;
        homer_mapnav_msgs::TargetUnreachable unreachable_msg;
        unreachable_msg.reason = reason;
        unreachable_msg.point = geometry_msgs::PointStamped();
426
        unreachable_msg.point.header.frame_id = "map";
427
428
429
        unreachable_msg.point.point = m_obstacle_position;
        m_target_unreachable_pub.publish(unreachable_msg);
        m_waypoints.clear();
430
        ROS_INFO_STREAM("=== TargetUnreachableMsg === (reason: " << (int)reason << ")");
431
    }
Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
432
433
}

Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
434
435
bool HomerNavigationNode::isTargetPositionReached()
{
436
437
438
439
440
441
442
443
444
445
446
447
448
449
    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_state = FINAL_TURN;
        ROS_INFO_STREAM("Turning to look-at point");
        return true;
    }
    else
    {
        return false;
    }
Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
450
451
}

452
void HomerNavigationNode::filterScanPoints(std::vector<geometry_msgs::Point>& points)
Projekt Robbie's avatar
testing    
Projekt Robbie committed
453
{
454
455
    std::vector<int> remove_list;
    for (int i = 0; i < points.size(); i++)
Projekt Robbie's avatar
testing    
Projekt Robbie committed
456
    {
457
458
459
460
461
462
463
464
465
466
467
468
469
        bool found = false;
        for (int j = i; j < points.size(); j++)
        {
            if (map_tools::distance(points[i], points[j]) < 0.03)
            {
                found = true;
            }
        }

        if (!found)
        {
            remove_list.push_back(i);
        }
Projekt Robbie's avatar
testing    
Projekt Robbie committed
470
471
    }

472
    for (int i = remove_list.size() - 1; i >= 0; i--)
Projekt Robbie's avatar
testing    
Projekt Robbie committed
473
    {
474
        points.erase(points.begin() + remove_list[i]);
Projekt Robbie's avatar
testing    
Projekt Robbie committed
475
476
477
    }
}

478
geometry_msgs::Point HomerNavigationNode::calculateMeanPoint(
Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
479
480
    const std::vector<geometry_msgs::Point>& points)
{
481
482
483
    geometry_msgs::Point mean_point;
    if (points.empty())
        return mean_point;
484
485
486
487
488
    for (auto const& point : points)
    {
        mean_point.x += point.x;
        mean_point.y += point.y;
    }
489
490
    mean_point.x /= points.size();
    mean_point.y /= points.size();
491
    return mean_point;
492
493
}

Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
494
495
bool HomerNavigationNode::obstacleOnPath()
{
496
497
498
499
500
501
502
    m_last_check_path_time = ros::Time::now();
    if (m_waypoints.size() == 0)
    {
        ROS_DEBUG_STREAM("no path found for finding an obstacle");
        return false;
    }

503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
    // Lookup map grid cell states around current position and waypoints in search of possible obstacles
    
    std::vector<geometry_msgs::Point> waypoints;
    waypoints.push_back(m_robot_pose.position);
  
    // Only check if path is blocked from current robot position to the next waypoints 
    int count = 0; 
    for (const auto& p : m_waypoints)
    {
        waypoints.push_back(p.pose.position);
        count++;
        if (count > 0)
            break;
    }
    bool path_blocked = false;
    auto collision_coordinate = Eigen::Vector2i(0,0);
519
    std::vector<Eigen::Vector2i> coordinates;
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
    std::vector<Eigen::Vector2i> checked_coordinates;
    for(auto i = 0; i < waypoints.size()-1; i++)
    {
        const auto& wp0 = waypoints[i];
        const auto& wp1 = waypoints[i+1];

        const auto p0 = Eigen::Vector2d(wp0.x, wp0.y);
        const auto p1 = Eigen::Vector2d(wp1.x, wp1.y);

        Eigen::Vector2d direction = p1 - p0; 
        const double distance = direction.norm();
        direction /= distance;
        const auto normal = Eigen::Vector2d(direction.y(), -direction.x());


535
        // Oriented rectangle region to check in map coordinates, region ends on next waypoint
536
537
538
539
540
541
542
543
544
545
546
        const auto leftbottom = p0 - normal * m_AllowedObstacleDistance.first; 
        const auto rightbottom = p0 + normal * m_AllowedObstacleDistance.first; 
        const auto lefttop = p1 - normal * m_AllowedObstacleDistance.first; 
        const auto righttop = p1 + normal * m_AllowedObstacleDistance.first; 

        const auto lb = map_tools::toMapCoords(leftbottom, m_map->info.origin, m_map->info.resolution);
        const auto lt = map_tools::toMapCoords(lefttop, m_map->info.origin, m_map->info.resolution);
        const auto rb = map_tools::toMapCoords(rightbottom, m_map->info.origin, m_map->info.resolution);
        const auto rt = map_tools::toMapCoords(righttop, m_map->info.origin, m_map->info.resolution);

        // TODO: There could be a slight error due to pixelation effects with the to enforce min distance (depends on map resolution)
547
        std::vector<Eigen::Vector2i> left, right, top, bottom;
548
549
550
551
552
553
        generateLine(left, lb, lt);
        generateLine(right, rb, rt);
        generateLine(bottom, lb, rb);
        generateLine(top, lt, rt);

        std::map<int, std::pair<int, int>> row_ranges;
554
        const auto update_ranges = [&row_ranges](const Eigen::Vector2i& c)
555
        {
556
557
558
559
560
561
562
563
564
565
566
567
            const auto x = c.x();
            const auto y = c.y();
            if (row_ranges.find(y) == row_ranges.end())
                row_ranges.emplace(y, std::make_pair(std::numeric_limits<int>::max(), std::numeric_limits<int>::lowest()));
           auto& range = row_ranges[y];
           if(x < range.first)
               range.first = x;
           if (x > range.second)
               range.second = x;
        };

        for (const auto& line : {left, right, bottom, top})
568
            for(const auto& c : line)
569
570
                update_ranges(c);

571
        // Also check circle arround next waypoint and current position
572
        const float TWO_PI = 2.f * M_PI;
573
        for(const auto& origin : {p0, p1})
574
        {
575
576
577
578
579
580
581
            for (float angle = 0.f; angle < TWO_PI; angle += 0.1)
            {
                const Eigen::Vector2d v = m_AllowedObstacleDistance.first * Eigen::Vector2d(std::cos(angle), std::sin(angle));
                Eigen::Vector2d p = origin + v;
                const auto px = map_tools::toMapCoords(p, m_map->info.origin, m_map->info.resolution);
                update_ranges(px);
            }
582
        }
583
584
        // Fill polygon from left to right for each row
        coordinates.clear();
585
586
587
588
589
        for (const auto& row : row_ranges)
        {
            const auto y = row.first;
            const auto& range = row.second;
            for(auto x = range.first; x <= range.second; x++)
590
591
592
593
            {
                Eigen::Vector2i px(x, y);
                coordinates.push_back(px);
            }
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
        }


        for (const auto& c : coordinates)
        {
            checked_coordinates.push_back(c);
            const auto index = c.x() + m_map->info.width * c.y();
            if (index < 0 || index >= m_map->data.size())
                continue;
            // If obstacle is in coordinate's cell
            if(m_map->data[index] > 80)
            {
                m_obstacle_position = map_tools::fromMapCoords(c, m_map->info.origin, m_map->info.resolution);
                ROS_DEBUG_STREAM("found obstacle at map position:\n" << m_obstacle_position);
                path_blocked = true;
                collision_coordinate = c;
                break;
            }
        }
        if (path_blocked)
            break;
    }

#if DUMP_OBSTACLE_AVOIDANCE_IMAGE
    const auto h = m_map->info.height;
    const auto w = m_map->info.width;
    cv::Mat img(h, w, CV_8UC3);
    img.setTo(0);
    for (auto y = 0; y < h; y++)
    {
        for(auto x = 0; x < w; x++)
        {
            const auto index = y * w + x;
            const auto value = m_map->data[index];
            
            if(value == 102)
                img.at<cv::Vec3b>(y, x) [1] = 255;
            if(value == 99 || value == 100)
                img.at<cv::Vec3b>(y, x)[0] = 255;
            //if(value == 101)
            //    img.at<cv::Vec3b>(y, x0)[0] = 255;
            if(value == 0)
                img.at<cv::Vec3b>(y, x) = cv::Vec3b(255, 255, 255);
        }
    }
    // Searched area
640
    for(const auto& c : coordinates)
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
    {
        auto& color = img.at<cv::Vec3b>(c.y(), c.x());
        color *= 0.8;
        color[2] = std::min(255, int(color[2] / 0.8 * 1.2));
    }

    for(const auto& wp : m_waypoints)
    {
        const auto p = map_tools::toMapCoords(wp.pose.position, m_map->info.origin, m_map->info.resolution);
        cv::circle(img, cv::Point2i(p.x(), p.y()), 2, cv::Vec3b(0, 120, 200), 1, cv::LINE_8);
    }
    // Current robot position
    const auto p = map_tools::toMapCoords(waypoints[0], m_map->info.origin, m_map->info.resolution);
    cv::circle(img, cv::Point2i(p.x(), p.y()), 2, cv::Vec3b(0, 120, 200), 1, cv::LINE_8);
   

    if(path_blocked)
    {
        const auto x = collision_coordinate.x();
        const auto y = collision_coordinate.y();
        img.at<cv::Vec3b>(y, x) = cv::Vec3b(0, 0, 255);
        cv::circle(img, cv::Point2i(x, y), 4, cv::Vec3b(0, 0, 255), 1, cv::LINE_8);
    }
    // Flip y axis
    cv::flip(img, img, 1);
    cv::imwrite("/tmp/obstacles.png", img);
#endif

    return path_blocked;
}
/*
672
673
674
675
    std::vector<geometry_msgs::Point> obstacle_vec;

    for (auto const& scan : m_scan_map)
    {
Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
676
        if (ros::Time::now() - scan.second->header.stamp < ros::Duration(1))
Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
677
        {
678
            std::vector<geometry_msgs::Point> scan_points;
679
            scan_points = map_tools::laser_msg_to_points(scan.second, m_transform_listener, "map");
680
681
682
683
684
685

            filterScanPoints(scan_points);
            geometry_msgs::Point lp;
            geometry_msgs::Point p;

            for (unsigned i = 0; i < m_waypoints.size(); i++)
Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
686
            {
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
                if (i == 0)
                {
                    lp = m_robot_pose.position;
                    p = m_waypoints.at(i).pose.position;
                }
                else
                {
                    lp = m_waypoints.at(i - 1).pose.position;
                    p = m_waypoints.at(i).pose.position;
                }
                if (map_tools::distance(m_robot_pose.position, p) > m_check_path_max_distance * 2)
                {
                    if (obstacle_vec.size() > 0)
                    {
                        m_obstacle_position = calculateMeanPoint(obstacle_vec);
                        ROS_DEBUG_STREAM("found obstacle at: " << m_obstacle_position);
                        return true;
                    }
                    else
                    {
                        continue;
                    }
                }
710
711

                // Generate points to check along path to next waypoint
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
                for (int k = 0; k < 4; k++)
                {
                    geometry_msgs::Point t;
                    t.x = lp.x + (p.x - lp.x) * k / 4.0;
                    t.y = lp.y + (p.y - lp.y) * k / 4.0;
                    for (const auto& sp : scan_points)
                    {
                        if (map_tools::distance(sp, t) < m_AllowedObstacleDistance.first)
                        {
                            if (map_tools::distance(m_robot_pose.position, sp)
                                < m_check_path_max_distance)
                            {
                                obstacle_vec.push_back(sp);
                            }
                        }
                    }
                }
Florian Polster's avatar
Florian Polster committed
729
730
731
            }
        }
    }
732
733
734
735
736
737
738
739
740
741
    if (obstacle_vec.size() > 0)
    {
        m_obstacle_position = calculateMeanPoint(obstacle_vec);
        ROS_DEBUG_STREAM("found obstacle at: " << m_obstacle_position);
        return true;
    }
    else
    {
        return false;
    }
Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
742
}
743
*/
Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
744

Florian Polster's avatar
Florian Polster committed
745
float HomerNavigationNode::getMinLaserDistance()
746
{
747
748
749
    float min = 10;
    for (auto const& scan : m_scan_map)
    {
Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
750
        if (ros::Time::now() - scan.second->header.stamp < ros::Duration(1))
751
        {
752
753
754
755
756
757
758
            for (auto const& range : scan.second->ranges)
            {
                if (range < min && range > scan.second->range_min && range < scan.second->range_max)
                {
                    min = range;
                }
            }
759
760
        }
    }
761
    return min;
762
763
}

Florian Polster's avatar
Florian Polster committed
764
bool HomerNavigationNode::checkWaypoints()
Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
765
{
766
767
768
769
770
    if (m_waypoints.size() == 0)
    {
        ROS_WARN_STREAM("No waypoints but trying to perform next move! "
                        "Skipping.");
        startNavigation();
Florian Polster's avatar
Florian Polster committed
771
        return false;
Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
772
    }
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
    // 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_state != FOLLOWING_PATH)
            {
                return false;
            }
        }
        else
        {
            m_waypoints.erase(m_waypoints.begin());
        }
    }

    Eigen::Vector2i waypointPixel = map_tools::toMapCoords(m_waypoints[0].pose.position, m_map);
    float obstacleDistanceMap = getMinLaserDistance();
    float waypointRadius = m_waypoint_radius_factor * obstacleDistanceMap;
    float distanceToWaypoint
        = map_tools::distance(m_robot_pose.position, m_waypoints[0].pose.position);

    waypointRadius = std::min((float)0.3, waypointRadius);
    if ((waypointRadius < m_map->info.resolution) || (m_waypoints.size() == 1))
Niklas Yann Wettengel's avatar
Niklas Yann Wettengel committed
812
    {
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
        waypointRadius = m_map->info.resolution;
    }

    if (m_waypoints.size() != 0)
    {
        Eigen::Vector2f wp_to_robot;
        wp_to_robot[0] = m_robot_pose.position.x - m_waypoints[0].pose.position.x;
        wp_to_robot[1] = m_robot_pose.position.y - m_waypoints[0].pose.position.y;

        Eigen::Vector2f wp_0_to_wp_1;
        wp_0_to_wp_1[0] = m_waypoints[1].pose.position.x - m_waypoints[0].pose.position.x;
        wp_0_to_wp_1[1] = m_waypoints[1].pose.position.y - m_waypoints[0].pose.position.y;

        float dot = wp_to_robot[0] * wp_0_to_wp_1[0]
            + wp_to_robot[1] * wp_0_to_wp_1[1]; // dot product of point_to_start * line

        // check if current waypoint has been reached
        bool waypoint_reached = (dot >= 0 && distanceToWaypoint < 2 * waypointRadius
            || distanceToWaypoint < waypointRadius);
        if (waypoint_reached && m_waypoints.size() > 1)
        {
            m_waypoints.erase(m_waypoints.begin());
        }
        if (m_waypoints.size() == 1 && distanceToWaypoint < m_map->info.resolution)
        {
            m_waypoints.erase(m_waypoints.begin());
        }
    }

    if (m_waypoints.size() == 0)
    {
        ROS_INFO_STREAM("Last waypoint reached");
        currentPathFinished();
        return false;
    }

    sendPathData();

851
    m_use_ptu = false;
852
853
854
    ros::param::getCached("/homer_navigation/use_ptu", m_use_ptu);
    if (m_use_ptu)
    {
855
856
        ROS_DEBUG_STREAM("Permanently focussing PTU on center next navigation waypoint.");
        geometry_msgs::PointStamped ps;
857
858
859
860
861
862
        float distanceToWaypoint
        = map_tools::distance(m_robot_pose.position, m_waypoints[0].pose.position);
        const auto& wp = (distanceToWaypoint > 0.5) ? m_waypoints[0] : m_waypoints[1];
        ps.point = wp.pose.position;
        ps.point.z = 0.3;
        ps.header = wp.header;
863
        homer_ptu::focus_view(ps, true);
864
865
866
    }

    return true;
Florian Polster's avatar
Florian Polster committed
867
868
869
870
}

bool HomerNavigationNode::updateSpeeds()
{
871
872
873
874
875
876
877
878
879
880
881
882
    float distanceToWaypoint
        = map_tools::distance(m_robot_pose.position, m_waypoints[0].pose.position);
    float angleToWaypoint = angleToPointDeg(m_waypoints[0].pose.position);
    float angle = deg2Rad(angleToWaypoint);

    // LINEAR SPEED CALCULATION
    if (m_avoided_collision)
    {
        if (std::fabs(angleToWaypoint) < 10)
        {
            m_avoided_collision = false;
        }
883
    }
884
885
    // Angle is bigger than maximal driving angle
    else if (std::fabs(angle) > m_max_drive_angle)
886
    {
887
888
889
890
891
892
893
894
        m_cmd_vel.linear.x = 0.0;
    }
    // There is an obstacle on the path and we are one meter away
    else if (m_obstacle_on_path
        && map_tools::distance(m_robot_pose.position, m_obstacle_position) < 1.0)
    {
        m_cmd_vel.linear.x = 0.0;
        angle = deg2Rad(angleToPointDeg(m_obstacle_position));
Florian Polster's avatar
Florian Polster committed
895

896
897
        if (std::fabs(angle) < m_min_turn_angle)
        {
898
            // ROS_INFO_STREAM("angle = " << angle);
899
900
901
902
            m_avoided_collision = true;
            m_initial_path_reaches_target = true;
            m_stop_before_obstacle = true;
            startNavigation();
903
            return false;
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
        }
    }
    // Normal speed calculation
    else
    {
        std::map<std::string, float> m_min_speed_map;

        m_min_speed_map["move_dist"]
            = m_max_move_speed * m_max_move_distance * m_obstacle_speed_factor;
        m_min_speed_map["laser"] = getMinLaserDistance() * m_map_speed_factor * m_max_move_speed;
        m_min_speed_map["waypoint"] = 1;
        m_min_speed_map["target"] = std::max((float)0.2,
            m_distance_to_target * m_distance_to_target * m_target_distance_speed_factor);
        m_min_speed_map["max_speed"] = m_max_move_speed;

        float new_speed = 200;
        std::string mins = "";

        for (auto& speed : m_min_speed_map)
        {
            if (speed.second < new_speed)
            {
                new_speed = speed.second;
                mins = speed.first;
            }
        }

        ROS_DEBUG_STREAM("max speed " << new_speed << " because of " << mins);

        // RAMP FOR SMOOTHER ACCELERATION
        if (new_speed < m_cmd_vel.linear.x)
Florian Polster's avatar
Florian Polster committed
935
        {
936
            m_cmd_vel.linear.x = new_speed;
Florian Polster's avatar
Florian Polster committed
937
938
939
        }
        else
        {
940
            m_cmd_vel.linear.x += std::min((float)(new_speed - m_cmd_vel.linear.x), m_speed_ramp);
Florian Polster's avatar
Florian Polster committed
941
        }
942
943
944
945
946
947
948
949
950
    }

    m_cmd_vel.linear.x *= std::fabs((m_max_turn_speed - std::fabs(angle)) / m_max_turn_speed);

    // angular speed calculation
    angle *= std::fabs(angle) * 3.0 + std::fabs(m_cmd_vel.linear.x / 2.0);
    if (angle < 0)
    {
        m_cmd_vel.angular.z = std::max(angle, -m_max_turn_speed);
Florian Polster's avatar
Florian Polster committed
951
952
953
    }
    else
    {
954
        m_cmd_vel.angular.z = std::min(angle, m_max_turn_speed);
Florian Polster's avatar
Florian Polster committed
955
    }
956
957
958
959
960
961
962

    ROS_DEBUG_STREAM("Driving & turning"
        << std::endl
        << "linear: " << m_cmd_vel.linear.x << " angular: " << m_cmd_vel.angular.z << std::endl
        << "distanceToWaypoint: " << distanceToWaypoint << std::endl
        << " angleToWaypoint: " << angleToWaypoint << std::endl);
    return true;
Florian Polster's avatar
Florian Polster committed
963
964
}

965
bool HomerNavigationNode::checkForObstacles()
Florian Polster's avatar
Florian Polster committed
966
{
967
968
969
970
971
972
973
974
975
976
    if ((ros::Time::now() - m_last_check_path_time) > ros::Duration(0.2))
    {
        ROS_DEBUG_STREAM("checkForObstacles()");
        if (obstacleOnPath())
        {
            ROS_DEBUG_STREAM("obstacle on path");
            // Message stops the robot and triggers the
            // targetUnreachableCallback of the drive_to_node.
            if (m_stop_before_obstacle)
            {
977
                ROS_INFO_STREAM("Stopping in front of obstacle.");
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
                sendTargetUnreachableMsg(homer_mapnav_msgs::TargetUnreachable::LASER_OBSTACLE);
                if (m_state == IDLE)
                    return false;
            }

            if (m_seen_obstacle_before)
            {
                ROS_DEBUG_STREAM("seen obstacle before");
                float distanceToObstacle
                    = map_tools::distance(m_robot_pose.position, m_obstacle_position);
                if (!m_obstacle_on_path && distanceToObstacle < 1.0)
                {
                    ROS_DEBUG_STREAM("distanceToObstacle: "
                        << distanceToObstacle << " m_desired_distance: " << m_desired_distance);
                    if (m_distance_to_target - m_desired_distance < distanceToObstacle)
                    {
                        ROS_INFO_STREAM("distance to obstacle smaller than distance to "
                                        "target; skipping path calculation");
                        return true;
                    }
                    // stopRobot();
                    calculatePath();
                    return false;
For faster browsing, not all history is shown. View entire blame