trajectory_utils.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2018, Locus Robotics
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the copyright holder nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
36 #include <nav_core2/exceptions.h>
37 
38 namespace dwb_local_planner
39 {
40 const geometry_msgs::Pose2D& getClosestPose(const dwb_msgs::Trajectory2D& trajectory, const double time_offset)
41 {
42  ros::Duration goal_time(time_offset);
43  const unsigned int num_poses = trajectory.poses.size();
44  if (num_poses == 0)
45  {
46  throw nav_core2::PlannerException("Cannot call getClosestPose on empty trajectory.");
47  }
48  unsigned int closest_index = num_poses;
49  double closest_diff = 0.0;
50  for (unsigned int i=0; i < num_poses; ++i)
51  {
52  double diff = fabs((trajectory.time_offsets[i] - goal_time).toSec());
53  if (closest_index == num_poses || diff < closest_diff)
54  {
55  closest_index = i;
56  closest_diff = diff;
57  }
58  if (goal_time < trajectory.time_offsets[i])
59  {
60  break;
61  }
62  }
63  return trajectory.poses[closest_index];
64 }
65 
66 geometry_msgs::Pose2D projectPose(const dwb_msgs::Trajectory2D& trajectory, const double time_offset)
67 {
68  ros::Duration goal_time(time_offset);
69  const unsigned int num_poses = trajectory.poses.size();
70  if (num_poses == 0)
71  {
72  throw nav_core2::PlannerException("Cannot call projectPose on empty trajectory.");
73  }
74  if (goal_time <= trajectory.time_offsets[0])
75  {
76  return trajectory.poses[0];
77  }
78  else if (goal_time >= trajectory.time_offsets[num_poses - 1])
79  {
80  return trajectory.poses[num_poses - 1];
81  }
82 
83  for (unsigned int i=0; i < num_poses - 1; ++i)
84  {
85  if (goal_time >= trajectory.time_offsets[i] && goal_time < trajectory.time_offsets[i+1])
86  {
87  double time_diff = (trajectory.time_offsets[i+1] - trajectory.time_offsets[i]).toSec();
88  double ratio = (goal_time - trajectory.time_offsets[i]).toSec() / time_diff;
89  double inv_ratio = 1.0 - ratio;
90  const geometry_msgs::Pose2D& pose_a = trajectory.poses[i];
91  const geometry_msgs::Pose2D& pose_b = trajectory.poses[i + 1];
92  geometry_msgs::Pose2D projected;
93  projected.x = pose_a.x * inv_ratio + pose_b.x * ratio;
94  projected.y = pose_a.y * inv_ratio + pose_b.y * ratio;
95  projected.theta = pose_a.theta * inv_ratio + pose_b.theta * ratio;
96  return projected;
97  }
98  }
99 
100  // Should not reach this point
101  return trajectory.poses[num_poses - 1];
102 }
103 
104 
105 } // namespace dwb_local_planner
const geometry_msgs::Pose2D & getClosestPose(const dwb_msgs::Trajectory2D &trajectory, const double time_offset)
Helper function to find a pose in the trajectory with a particular time time_offset.
geometry_msgs::Pose2D projectPose(const dwb_msgs::Trajectory2D &trajectory, const double time_offset)
Helper function to create a pose with an exact time_offset by linearly interpolating between existing...


dwb_local_planner
Author(s): David V. Lu!!
autogenerated on Sun Jan 10 2021 04:08:34