Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions nav2_util/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -87,6 +87,9 @@ install(DIRECTORY include/

if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
set(ament_cmake_copyright_FOUND TRUE)
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()

Expand Down
14 changes: 8 additions & 6 deletions nav2_util/include/nav2_util/duration_conversions.h
Original file line number Diff line number Diff line change
Expand Up @@ -19,12 +19,14 @@

namespace nav2_util
{
// TODO(crdelsey): This functionality should be part of the RCLCPP Duration
// interface. Once that gets integrated, this function can be removed.
inline rclcpp::Duration durationFromSeconds(double seconds)
{
return rclcpp::Duration(static_cast<long>(seconds * 1e9)); // convert to ns
}

// TODO(crdelsey): This functionality should be part of the RCLCPP Duration
// interface. Once that gets integrated, this function can be removed.
inline rclcpp::Duration durationFromSeconds(double seconds)
{
return rclcpp::Duration(static_cast<int64_t>(seconds * 1e9)); // convert to ns
}

} // namespace nav2_util

#endif // NAV2_UTIL__DURATION_CONVERSIONS_H_
6 changes: 3 additions & 3 deletions nav2_util/include/nav2_util/map_loader/map_loader.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -29,8 +29,8 @@

// Author: Brian Gerkey

#ifndef MAP_LOADER__MAPLOADER_HPP_
#define MAP_LOADER__MAPLOADER_HPP_
#ifndef NAV2_UTIL__MAP_LOADER__MAP_LOADER_HPP_
#define NAV2_UTIL__MAP_LOADER__MAP_LOADER_HPP_

#include <string>
#include "nav_msgs/msg/occupancy_grid.hpp"
Expand Down Expand Up @@ -75,4 +75,4 @@ nav_msgs::msg::OccupancyGrid loadMapFromFile(
const geometry_msgs::msg::Twist origin, const MapMode mode = TRINARY);
} // namespace map_loader

#endif // MAP_LOADER__MAPLOADER_HPP_
#endif // NAV2_UTIL__MAP_LOADER__MAP_LOADER_HPP_
7 changes: 2 additions & 5 deletions nav2_util/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -18,12 +18,9 @@
<exec_depend>nav2_msgs</exec_depend>
<exec_depend>nav_msgs</exec_depend>

<test_depend>ament_cmake_cppcheck</test_depend>
<test_depend>ament_cmake_cpplint</test_depend>
<test_depend>ament_cmake_lint_cmake</test_depend>
<test_depend>ament_cmake_pep257</test_depend>
<test_depend>ament_cmake_uncrustify</test_depend>
<test_depend>ament_lint_common</test_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_cmake_pytest</test_depend>

<export>
<build_type>ament_cmake</build_type>
Expand Down
132 changes: 66 additions & 66 deletions nav2_util/src/costmap.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -135,82 +135,82 @@ vector<uint8_t> Costmap::getTestData(const TestCostmap testCostmapType)
const uint8_t o = free_space;

vector<uint8_t> costmapFree =
// 0 1 2 3 4 5 6 7 8 9
{o, o, o, o, o, o, o, o, o, o, // 0
o, o, o, o, o, o, o, o, o, o, // 1
o, o, o, o, o, o, o, o, o, o, // 2
o, o, o, o, o, o, o, o, o, o, // 3
o, o, o, o, o, o, o, o, o, o, // 4
o, o, o, o, o, o, o, o, o, o, // 5
o, o, o, o, o, o, o, o, o, o, // 6
o, o, o, o, o, o, o, o, o, o, // 7
o, o, o, o, o, o, o, o, o, o, // 8
o, o, o, o, o, o, o, o, o, o}; // 9
// 0 1 2 3 4 5 6 7 8 9
{o, o, o, o, o, o, o, o, o, o, // 0
o, o, o, o, o, o, o, o, o, o, // 1
o, o, o, o, o, o, o, o, o, o, // 2
o, o, o, o, o, o, o, o, o, o, // 3
o, o, o, o, o, o, o, o, o, o, // 4
o, o, o, o, o, o, o, o, o, o, // 5
o, o, o, o, o, o, o, o, o, o, // 6
o, o, o, o, o, o, o, o, o, o, // 7
o, o, o, o, o, o, o, o, o, o, // 8
o, o, o, o, o, o, o, o, o, o}; // 9

vector<uint8_t> costmapBounded =
// 0 1 2 3 4 5 6 7 8 9
{n, n, n, n, n, n, n, n, n, n, // 0
n, o, o, o, o, o, o, o, o, n, // 1
n, o, o, o, o, o, o, o, o, n, // 2
n, o, o, o, o, o, o, o, o, n, // 3
n, o, o, o, o, o, o, o, o, n, // 4
n, o, o, o, o, o, o, o, o, n, // 5
n, o, o, o, o, o, o, o, o, n, // 6
n, o, o, o, o, o, o, o, o, n, // 7
n, o, o, o, o, o, o, o, o, n, // 8
n, n, n, n, n, n, n, n, n, n}; // 9
// 0 1 2 3 4 5 6 7 8 9
{n, n, n, n, n, n, n, n, n, n, // 0
n, o, o, o, o, o, o, o, o, n, // 1
n, o, o, o, o, o, o, o, o, n, // 2
n, o, o, o, o, o, o, o, o, n, // 3
n, o, o, o, o, o, o, o, o, n, // 4
n, o, o, o, o, o, o, o, o, n, // 5
n, o, o, o, o, o, o, o, o, n, // 6
n, o, o, o, o, o, o, o, o, n, // 7
n, o, o, o, o, o, o, o, o, n, // 8
n, n, n, n, n, n, n, n, n, n}; // 9

vector<uint8_t> costmapObstacleBL =
// 0 1 2 3 4 5 6 7 8 9
{n, n, n, n, n, n, n, n, n, n, // 0
n, o, o, o, o, o, o, o, o, n, // 1
n, o, o, o, o, o, o, o, o, n, // 2
n, o, o, o, o, o, o, o, o, n, // 3
n, o, o, o, o, o, o, o, o, n, // 4
n, o, x, x, x, o, o, o, o, n, // 5
n, o, x, x, x, o, o, o, o, n, // 6
n, o, x, x, x, o, o, o, o, n, // 7
n, o, o, o, o, o, o, o, o, n, // 8
n, n, n, n, n, n, n, n, n, n}; // 9
// 0 1 2 3 4 5 6 7 8 9
{n, n, n, n, n, n, n, n, n, n, // 0
n, o, o, o, o, o, o, o, o, n, // 1
n, o, o, o, o, o, o, o, o, n, // 2
n, o, o, o, o, o, o, o, o, n, // 3
n, o, o, o, o, o, o, o, o, n, // 4
n, o, x, x, x, o, o, o, o, n, // 5
n, o, x, x, x, o, o, o, o, n, // 6
n, o, x, x, x, o, o, o, o, n, // 7
n, o, o, o, o, o, o, o, o, n, // 8
n, n, n, n, n, n, n, n, n, n}; // 9

vector<uint8_t> costmapObstacleTL =
// 0 1 2 3 4 5 6 7 8 9
{n, n, n, n, n, n, n, n, n, n, // 0
n, o, o, o, o, o, o, o, o, n, // 1
n, o, x, x, x, o, o, o, o, n, // 2
n, o, x, x, x, o, o, o, o, n, // 3
n, o, x, x, x, o, o, o, o, n, // 4
n, o, o, o, o, o, o, o, o, n, // 5
n, o, o, o, o, o, o, o, o, n, // 6
n, o, o, o, o, o, o, o, o, n, // 7
n, o, o, o, o, o, o, o, o, n, // 8
n, n, n, n, n, n, n, n, n, n}; // 9
// 0 1 2 3 4 5 6 7 8 9
{n, n, n, n, n, n, n, n, n, n, // 0
n, o, o, o, o, o, o, o, o, n, // 1
n, o, x, x, x, o, o, o, o, n, // 2
n, o, x, x, x, o, o, o, o, n, // 3
n, o, x, x, x, o, o, o, o, n, // 4
n, o, o, o, o, o, o, o, o, n, // 5
n, o, o, o, o, o, o, o, o, n, // 6
n, o, o, o, o, o, o, o, o, n, // 7
n, o, o, o, o, o, o, o, o, n, // 8
n, n, n, n, n, n, n, n, n, n}; // 9

vector<uint8_t> costmapMaze =
// 0 1 2 3 4 5 6 7 8 9
{n, n, n, n, n, n, n, n, n, n, // 0
n, o, o, o, o, o, o, o, o, n, // 1
n, x, x, o, x, x, x, o, x, n, // 2
n, o, o, o, o, x, o, o, o, n, // 3
n, o, x, x, o, x, o, x, o, n, // 4
n, o, x, x, o, x, o, x, o, n, // 5
n, o, o, x, o, x, o, x, o, n, // 6
n, x, o, x, o, x, o, x, o, n, // 7
n, o, o, o, o, o, o, x, o, n, // 8
n, n, n, n, n, n, n, n, n, n}; // 9
// 0 1 2 3 4 5 6 7 8 9
{n, n, n, n, n, n, n, n, n, n, // 0
n, o, o, o, o, o, o, o, o, n, // 1
n, x, x, o, x, x, x, o, x, n, // 2
n, o, o, o, o, x, o, o, o, n, // 3
n, o, x, x, o, x, o, x, o, n, // 4
n, o, x, x, o, x, o, x, o, n, // 5
n, o, o, x, o, x, o, x, o, n, // 6
n, x, o, x, o, x, o, x, o, n, // 7
n, o, o, o, o, o, o, x, o, n, // 8
n, n, n, n, n, n, n, n, n, n}; // 9

vector<uint8_t> costmapMaze2 =
// 0 1 2 3 4 5 6 7 8 9
{n, n, n, n, n, n, n, n, n, n, // 0
n, o, o, o, o, o, o, o, o, n, // 1
n, x, x, u, x, x, x, o, x, n, // 2
n, o, o, o, o, o, o, o, u, n, // 3
n, o, x, x, o, x, x, x, u, n, // 4
n, o, x, x, o, o, o, x, u, n, // 5
n, o, o, x, u, x, o, x, u, n, // 6
n, x, o, x, u, x, i, x, u, n, // 7
n, o, o, o, o, o, o, o, o, n, // 8
n, n, n, n, n, n, n, n, n, n}; // 9
// 0 1 2 3 4 5 6 7 8 9
{n, n, n, n, n, n, n, n, n, n, // 0
n, o, o, o, o, o, o, o, o, n, // 1
n, x, x, u, x, x, x, o, x, n, // 2
n, o, o, o, o, o, o, o, u, n, // 3
n, o, x, x, o, x, x, x, u, n, // 4
n, o, x, x, o, o, o, x, u, n, // 5
n, o, o, x, u, x, o, x, u, n, // 6
n, x, o, x, u, x, i, x, u, n, // 7
n, o, o, o, o, o, o, o, o, n, // 8
n, n, n, n, n, n, n, n, n, n}; // 9

switch (testCostmapType) {
case TestCostmap::open_space:
Expand Down
10 changes: 5 additions & 5 deletions nav2_util/src/map_loader/map_loader.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -29,15 +29,15 @@

// Author: Brian Gerkey

#include <cstring>
#include <stdexcept>
#include "nav2_util/map_loader/map_loader.hpp"
#include <stdlib.h>
#include <stdio.h>
#include "nav2_util/map_loader/map_loader.hpp"
#include "tf2/LinearMath/Quaternion.h"

// We use SDL_image to load the image from disk
#include <SDL/SDL_image.h>
#include <string>
#include <stdexcept>
#include "tf2/LinearMath/Quaternion.h"


// compute linear index for given map coords
#define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
Expand Down