9 #include <gtest/gtest.h> 13 #include "../../../../include/cost_map_demos/from_ros_costmaps.hpp" 19 TEST(Costmap2DROS, full_window) {
20 std::cout << std::endl;
21 ROS_INFO(
"***********************************************************");
23 ROS_INFO(
"***********************************************************");
26 std::string layer_name =
"obstacle_costs";
33 ros_costmap_5x5.
getROSCostmap()->getCostmap()->getSizeInCellsX()*ros_costmap_5x5.
getROSCostmap()->getCostmap()->getResolution()
36 ros_costmap_5x5.
getROSCostmap()->getCostmap()->getSizeInCellsY()*ros_costmap_5x5.
getROSCostmap()->getCostmap()->getResolution()
38 ASSERT_EQ(cost_map_5x5.
getSize()[0], ros_costmap_5x5.
getROSCostmap()->getCostmap()->getSizeInCellsX());
39 ASSERT_EQ(cost_map_5x5.
getSize()[1], ros_costmap_5x5.
getROSCostmap()->getCostmap()->getSizeInCellsY());
41 ASSERT_EQ(position.x(), ros_costmap_5x5.
getROSCostmap()->getCostmap()->getOriginX());
42 ASSERT_EQ(position.y(), ros_costmap_5x5.
getROSCostmap()->getCostmap()->getOriginY());
58 std::cout << std::endl;
61 TEST(Costmap2DROS, cost_map_centres) {
62 std::cout << std::endl;
63 ROS_INFO(
"***********************************************************");
64 ROS_INFO(
" Check Subwindow Centres");
65 ROS_INFO(
"***********************************************************");
66 ROS_INFO(
"Subwindows are centred as closely as possible to the robot");
67 ROS_INFO(
"pose, though not exactly. They still need to align with");
68 ROS_INFO(
"the underlying ros costmap so that they don't introduce a");
69 ROS_INFO(
"new kind of error. As a result, the centre is shifted from");
70 ROS_INFO(
"the robot pose to the nearest appropriate point which aligns");
71 ROS_INFO(
"the new cost map exactly on top of the original ros costmap.");
72 std::cout << std::endl;
73 std::string layer_name =
"obstacle_costs";
77 cost_map::CostMap cost_map_5x5_3x3_offset, cost_map_5x5_3x3_centre, cost_map_5x5_2_5x2_5_offset;
86 ASSERT_EQ(-3.5, cost_map_5x5_3x3_offset.
getPosition().x());
87 ASSERT_EQ(2.5, cost_map_5x5_3x3_offset.
getPosition().y());
88 ASSERT_EQ(-3.5, cost_map_5x5_3x3_centre.
getPosition().x());
89 ASSERT_EQ(-3.5, cost_map_5x5_3x3_centre.
getPosition().y());
90 ASSERT_EQ(-9.75, cost_map_5x5_2_5x2_5_offset.
getPosition().x());
91 ASSERT_EQ(2.25, cost_map_5x5_2_5x2_5_offset.
getPosition().y());
92 std::cout << std::endl;
99 int main(
int argc,
char **argv) {
101 ros::init(argc, argv,
"test_from_ros_costmaps");
106 testing::InitGoogleTest(&argc,argv);
107 int result = RUN_ALL_TESTS();
const Length & getLength() const
int main(int argc, char **argv)
DataType & at(const std::string &layer, const Index &index)
const Size & getSize() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
const std::string & getFrameId() const
bool getPosition(const Index &index, Position &position) const
TEST(Costmap2DROS, full_window)
bool fromCostmap2DROS(costmap_2d::Costmap2DROS &ros_costmap, const std::string &layer_name, cost_map::CostMap &cost_map)
#define ROS_INFO_STREAM(args)
grid_map::Position Position
void broadcastCostmap2DROSTestSuiteTransforms(TransformBroadcaster &broadcaster)
ROSCostmapPtr getROSCostmap()
bool fromCostmap2DROSAtRobotPose(costmap_2d::Costmap2DROS &ros_costmap, const cost_map::Length &geometry, const std::string &layer_name, cost_map::CostMap &cost_map)