Skip to content

Commit

Permalink
apply pre-commit
Browse files Browse the repository at this point in the history
Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
  • Loading branch information
takayuki5168 committed Mar 7, 2022
1 parent 4c54cbc commit 0c267be
Show file tree
Hide file tree
Showing 12 changed files with 486 additions and 389 deletions.
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_
Original file line number Diff line number Diff line change
Expand Up @@ -11,23 +11,23 @@
// 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_AVOIDANCE_PLANNER_ST_BOUNDARY_HPP_
#define OBSTACLE_AVOIDANCE_PLANNER_ST_BOUNDARY_HPP_
#ifndef OBSTACLE_VELOCITY_PLANNER__COMMON__S_BOUNDARY_HPP_
#define OBSTACLE_VELOCITY_PLANNER__COMMON__S_BOUNDARY_HPP_

#include <limits>
#include <vector>

class SBoundary
{
public:
SBoundary(const double _max_s, const double _min_s) : max_s(_max_s), min_s(_min_s) {}
SBoundary() : max_s(std::numeric_limits<double>::max()), min_s(0.0) {}
SBoundary(const double _max_s, const double _min_s) : max_s(_max_s), min_s(_min_s) {}
SBoundary() : max_s(std::numeric_limits<double>::max()), min_s(0.0) {}

double max_s = std::numeric_limits<double>::max();
double min_s = 0.0;
bool is_object = false;
double max_s = std::numeric_limits<double>::max();
double min_s = 0.0;
bool is_object = false;
};

using SBoundaries = std::vector<SBoundary>;

#endif // OBSTACLE_AVOIDANCE_PLANNER_ST_BOUNDARY_HPP_
#endif // OBSTACLE_VELOCITY_PLANNER__COMMON__S_BOUNDARY_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -11,21 +11,21 @@
// 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_AVOIDANCE_PLANNER_ST_POINT_HPP_
#define OBSTACLE_AVOIDANCE_PLANNER_ST_POINT_HPP_
#ifndef OBSTACLE_VELOCITY_PLANNER__COMMON__ST_POINT_HPP_
#define OBSTACLE_VELOCITY_PLANNER__COMMON__ST_POINT_HPP_

#include <vector>

class STPoint
{
public:
STPoint(const double _s, const double _t) : s(_s), t(_t) {}
STPoint() : s(0.0), t(0.0) {}
STPoint(const double _s, const double _t) : s(_s), t(_t) {}
STPoint() : s(0.0), t(0.0) {}

double s;
double t;
double s;
double t;
};

using STPoints = std::vector<STPoint>;

#endif // OBSTACLE_AVOIDANCE_PLANNER_ST_POINT_HPP_
#endif // OBSTACLE_VELOCITY_PLANNER__COMMON__ST_POINT_HPP_
Loading

0 comments on commit 0c267be

Please sign in to comment.