-
Notifications
You must be signed in to change notification settings - Fork 682
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
- Loading branch information
1 parent
4c54cbc
commit 0c267be
Showing
12 changed files
with
486 additions
and
389 deletions.
There are no files selected for viewing
203 changes: 109 additions & 94 deletions
203
planning/obstacle_velocity_planner/include/obstacle_velocity_planner/box2d.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -1,120 +1,135 @@ | ||
#ifndef OBSTACLE_VELOCITY_PLANNER_BOX2D_HPP_ | ||
#define OBSTACLE_VELOCITY_PLANNER_BOX2D_HPP_ | ||
// Copyright 2022 Tier IV, Inc. | ||
// | ||
// Licensed under the Apache License, Version 2.0 (the "License"); | ||
// you may not use this file except in compliance with the License. | ||
// You may obtain a copy of the License at | ||
// | ||
// http://www.apache.org/licenses/LICENSE-2.0 | ||
// | ||
// Unless required by applicable law or agreed to in writing, software | ||
// distributed under the License is distributed on an "AS IS" BASIS, | ||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
// See the License for the specific language governing permissions and | ||
// limitations under the License. | ||
|
||
#ifndef OBSTACLE_VELOCITY_PLANNER__BOX2D_HPP_ | ||
#define OBSTACLE_VELOCITY_PLANNER__BOX2D_HPP_ | ||
|
||
#include <geometry_msgs/msg/point.hpp> | ||
#include <geometry_msgs/msg/pose.hpp> | ||
|
||
#include <cmath> | ||
#include <limits> | ||
#include <vector> | ||
|
||
class Box2d | ||
{ | ||
public: | ||
Box2d(const geometry_msgs::msg::Pose & center_pose, const double length, const double width); | ||
Box2d(const geometry_msgs::msg::Pose & center_pose, const double length, const double width); | ||
|
||
bool HasOverlap(const Box2d & box) const ; | ||
bool HasOverlap(const Box2d & box) const; | ||
|
||
void InitCorners(); | ||
void InitCorners(); | ||
|
||
/** | ||
/** | ||
* @brief Getter of the center of the box | ||
* @return The center of the box | ||
*/ | ||
const geometry_msgs::msg::Point& center() const { return center_; } | ||
|
||
/** | ||
* @brief Getter of the x-coordinate of the center of the box | ||
* @return The x-coordinate of the center of the box | ||
*/ | ||
double center_x() const { return center_.x; } | ||
|
||
/** | ||
* @brief Getter of the y-coordinate of the center of the box | ||
* @return The y-coordinate of the center of the box | ||
*/ | ||
double center_y() const { return center_.y; } | ||
|
||
/** | ||
* @brief Getter of the length | ||
* @return The length of the heading-axis | ||
*/ | ||
double length() const { return length_; } | ||
|
||
/** | ||
* @brief Getter of the width | ||
* @return The width of the box taken perpendicularly to the heading | ||
*/ | ||
double width() const { return width_; } | ||
|
||
/** | ||
* @brief Getter of half the length | ||
* @return Half the length of the heading-axis | ||
*/ | ||
double half_length() const { return half_length_; } | ||
|
||
/** | ||
* @brief Getter of half the width | ||
* @return Half the width of the box taken perpendicularly to the heading | ||
*/ | ||
double half_width() const { return half_width_; } | ||
|
||
/** | ||
* @brief Getter of the heading | ||
* @return The counter-clockwise angle between the x-axis and the heading-axis | ||
*/ | ||
double heading() const { return heading_; } | ||
|
||
/** | ||
* @brief Getter of the cosine of the heading | ||
* @return The cosine of the heading | ||
*/ | ||
double cos_heading() const { return cos_heading_; } | ||
|
||
/** | ||
* @brief Getter of the sine of the heading | ||
* @return The sine of the heading | ||
*/ | ||
double sin_heading() const { return sin_heading_; } | ||
|
||
/** | ||
const geometry_msgs::msg::Point & center() const { return center_; } | ||
|
||
/** | ||
* @brief Getter of the x-coordinate of the center of the box | ||
* @return The x-coordinate of the center of the box | ||
*/ | ||
double center_x() const { return center_.x; } | ||
|
||
/** | ||
* @brief Getter of the y-coordinate of the center of the box | ||
* @return The y-coordinate of the center of the box | ||
*/ | ||
double center_y() const { return center_.y; } | ||
|
||
/** | ||
* @brief Getter of the length | ||
* @return The length of the heading-axis | ||
*/ | ||
double length() const { return length_; } | ||
|
||
/** | ||
* @brief Getter of the width | ||
* @return The width of the box taken perpendicularly to the heading | ||
*/ | ||
double width() const { return width_; } | ||
|
||
/** | ||
* @brief Getter of half the length | ||
* @return Half the length of the heading-axis | ||
*/ | ||
double half_length() const { return half_length_; } | ||
|
||
/** | ||
* @brief Getter of half the width | ||
* @return Half the width of the box taken perpendicularly to the heading | ||
*/ | ||
double half_width() const { return half_width_; } | ||
|
||
/** | ||
* @brief Getter of the heading | ||
* @return The counter-clockwise angle between the x-axis and the heading-axis | ||
*/ | ||
double heading() const { return heading_; } | ||
|
||
/** | ||
* @brief Getter of the cosine of the heading | ||
* @return The cosine of the heading | ||
*/ | ||
double cos_heading() const { return cos_heading_; } | ||
|
||
/** | ||
* @brief Getter of the sine of the heading | ||
* @return The sine of the heading | ||
*/ | ||
double sin_heading() const { return sin_heading_; } | ||
|
||
/** | ||
* @brief Getter of the area of the box | ||
* @return The product of its length and width | ||
*/ | ||
double area() const { return length_ * width_; } | ||
double area() const { return length_ * width_; } | ||
|
||
/** | ||
* @brief Getter of the size of the diagonal of the box | ||
* @return The diagonal size of the box | ||
*/ | ||
double diagonal() const { return std::hypot(length_, width_); } | ||
/** | ||
* @brief Getter of the size of the diagonal of the box | ||
* @return The diagonal size of the box | ||
*/ | ||
double diagonal() const { return std::hypot(length_, width_); } | ||
|
||
/** | ||
* @brief Getter of the corners of the box | ||
* @param corners The vector where the corners are listed | ||
*/ | ||
std::vector<geometry_msgs::msg::Point> GetAllCorners() const { return corners_; } | ||
/** | ||
* @brief Getter of the corners of the box | ||
* @param corners The vector where the corners are listed | ||
*/ | ||
std::vector<geometry_msgs::msg::Point> GetAllCorners() const { return corners_; } | ||
|
||
double max_x() const { return max_x_; } | ||
double min_x() const { return min_x_; } | ||
double max_y() const { return max_y_; } | ||
double min_y() const { return min_y_; } | ||
double max_x() const { return max_x_; } | ||
double min_x() const { return min_x_; } | ||
double max_y() const { return max_y_; } | ||
double min_y() const { return min_y_; } | ||
|
||
private: | ||
geometry_msgs::msg::Point center_; | ||
double length_ = 0.0; | ||
double width_ = 0.0; | ||
double half_length_ = 0.0; | ||
double half_width_ = 0.0; | ||
double heading_ = 0.0; | ||
double cos_heading_ = 1.0; | ||
double sin_heading_ = 0.0; | ||
|
||
std::vector<geometry_msgs::msg::Point> corners_; | ||
|
||
double max_x_ = std::numeric_limits<double>::lowest(); | ||
double min_x_ = std::numeric_limits<double>::max(); | ||
double max_y_ = std::numeric_limits<double>::lowest(); | ||
double min_y_ = std::numeric_limits<double>::max(); | ||
geometry_msgs::msg::Point center_; | ||
double length_ = 0.0; | ||
double width_ = 0.0; | ||
double half_length_ = 0.0; | ||
double half_width_ = 0.0; | ||
double heading_ = 0.0; | ||
double cos_heading_ = 1.0; | ||
double sin_heading_ = 0.0; | ||
|
||
std::vector<geometry_msgs::msg::Point> corners_; | ||
|
||
double max_x_ = std::numeric_limits<double>::lowest(); | ||
double min_x_ = std::numeric_limits<double>::max(); | ||
double max_y_ = std::numeric_limits<double>::lowest(); | ||
double min_y_ = std::numeric_limits<double>::max(); | ||
}; | ||
|
||
#endif // OBSTACLE_VELOCITY_PLANNER_BOX2D_HPP_ | ||
#endif // OBSTACLE_VELOCITY_PLANNER__BOX2D_HPP_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.