Skip to content

Commit

Permalink
Renamed Point class to Point2D
Browse files Browse the repository at this point in the history
  • Loading branch information
orduno committed Jan 11, 2019
1 parent 97b0cd2 commit c1b4c36
Show file tree
Hide file tree
Showing 4 changed files with 12 additions and 13 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -22,14 +22,14 @@
namespace nav2_util
{

struct Point
struct Point2D
{
double x, y;

Point(const double x, const double y)
Point2D(const double x, const double y)
: x(x), y(y) { }

bool operator==(const Point & obj) const
bool operator==(const Point2D & obj) const
{
if (x == obj.x && y == obj.y) {
return true;
Expand All @@ -38,14 +38,14 @@ struct Point
}
}

friend std::ostream & operator<<(std::ostream & os, const Point & point)
friend std::ostream & operator<<(std::ostream & os, const Point2D & point)
{
return os <<
"x: " << point.x <<
" y: " << point.y;
}

void rotateAroundPoint(const double theta, const Point & reference)
void rotateAroundPoint(const double theta, const Point2D & reference)
{
// translate point such that the reference is now the origin
double x_t = x - reference.x;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@
#include "nav2_world_model/world_representation.hpp"
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
#include "tf2_ros/transform_listener.h"
#include "nav2_util/point.hpp"
#include "nav2_util/point2d.hpp"
#include "nav2_world_model/region_visualizer.hpp"

namespace nav2_world_model
Expand Down Expand Up @@ -64,7 +64,7 @@ class CostmapRepresentation : public WorldRepresentation

bool addToMapLocations(
std::vector<nav2_costmap_2d::MapLocation> & vertices,
const nav2_util::Point & vertex) const;
const nav2_util::Point2D & vertex) const;

bool isFree(const nav2_costmap_2d::MapLocation & location) const;
};
Expand Down
1 change: 0 additions & 1 deletion nav2_world_model/include/nav2_world_model/world_model.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,6 @@

#include "rclcpp/rclcpp.hpp"
#include "nav2_world_model/world_representation.hpp"
#include "nav2_world_model/rectangular_region.hpp"
#include "geometry_msgs/msg/polygon_stamped.hpp"

namespace nav2_world_model
Expand Down
10 changes: 5 additions & 5 deletions nav2_world_model/src/costmap_representation.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@ namespace nav2_world_model
{

using nav2_costmap_2d::MapLocation;
using nav2_util::Point;
using nav2_util::Point2D;

CostmapRepresentation::CostmapRepresentation(
const std::string name,
Expand Down Expand Up @@ -137,8 +137,8 @@ bool CostmapRepresentation::generateRectangleVertices(
double right = request.reference.x + request.width / 2 - request.offset.x;
double left = request.reference.x - request.width / 2 - request.offset.x;

std::vector<Point> vertices = {
Point{left, down}, Point{left, top}, Point{right, top}, Point{right, down}};
std::vector<Point2D> vertices = {
Point2D{left, down}, Point2D{left, top}, Point2D{right, top}, Point2D{right, down}};

// TODO(orduno) the RVIZ world frame is rotated 90 deg with respect to Gazebo.
const double rvizToGazeboOffset = M_PI / 2;
Expand All @@ -147,7 +147,7 @@ bool CostmapRepresentation::generateRectangleVertices(
for (auto & point : vertices) {
point.rotateAroundPoint(
angleutils::normalize(request.rotation + rvizToGazeboOffset),
Point{request.reference.x, request.reference.y});
Point2D{request.reference.x, request.reference.y});
}

// Convert the vertices to map coordinates
Expand All @@ -162,7 +162,7 @@ bool CostmapRepresentation::generateRectangleVertices(
}

bool CostmapRepresentation::addToMapLocations(
std::vector<MapLocation> & locations, const Point & point) const
std::vector<MapLocation> & locations, const Point2D & point) const
{
unsigned int mx, my;

Expand Down

0 comments on commit c1b4c36

Please sign in to comment.