Skip to content

Commit 9259493

Browse files
committed
refactor(costmap_generator): add autoware prefix
Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
1 parent 7d69e00 commit 9259493

20 files changed

+64
-37
lines changed

.github/CODEOWNERS

+1-1
Original file line numberDiff line numberDiff line change
@@ -176,7 +176,7 @@ planning/behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakab
176176
planning/behavior_velocity_stop_line_module/** fumiya.watanabe@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp zhe.shen@tier4.jp
177177
planning/behavior_velocity_traffic_light_module/** mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp
178178
planning/behavior_velocity_walkway_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp
179-
planning/costmap_generator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp
179+
planning/autoware_costmap_generator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp
180180
planning/autoware_external_velocity_limit_selector/** satoshi.ota@tier4.jp shinnosuke.hirakawa@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp
181181
planning/freespace_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp
182182
planning/freespace_planning_algorithms/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp

launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml

+1-1
Original file line numberDiff line numberDiff line change
@@ -4,7 +4,7 @@
44
<push-ros-namespace namespace="parking"/>
55

66
<node_container pkg="rclcpp_components" exec="$(var container_type)" name="parking_container" namespace="" output="screen">
7-
<composable_node pkg="costmap_generator" plugin="CostmapGenerator" name="costmap_generator" namespace="">
7+
<composable_node pkg="autoware_costmap_generator" plugin="autoware::costmap_generator::CostmapGenerator" name="costmap_generator" namespace="">
88
<!-- topic remap -->
99
<remap from="~/input/objects" to="/perception/object_recognition/objects"/>
1010
<remap from="~/input/points_no_ground" to="/perception/obstacle_segmentation/pointcloud"/>

launch/tier4_planning_launch/package.xml

+1-1
Original file line numberDiff line numberDiff line change
@@ -58,12 +58,12 @@
5858
<buildtool_depend>autoware_cmake</buildtool_depend>
5959

6060
<exec_depend>autoware_behavior_velocity_planner</exec_depend>
61+
<exec_depend>autoware_costmap_generator</exec_depend>
6162
<exec_depend>autoware_external_velocity_limit_selector</exec_depend>
6263
<exec_depend>autoware_path_optimizer</exec_depend>
6364
<exec_depend>autoware_remaining_distance_time_calculator</exec_depend>
6465
<exec_depend>autoware_velocity_smoother</exec_depend>
6566
<exec_depend>behavior_path_planner</exec_depend>
66-
<exec_depend>costmap_generator</exec_depend>
6767
<exec_depend>external_cmd_selector</exec_depend>
6868
<exec_depend>freespace_planner</exec_depend>
6969
<exec_depend>glog_component</exec_depend>

planning/costmap_generator/CMakeLists.txt planning/autoware_costmap_generator/CMakeLists.txt

+6-6
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
cmake_minimum_required(VERSION 3.14)
2-
project(costmap_generator)
2+
project(autoware_costmap_generator)
33

44
find_package(autoware_cmake REQUIRED)
55
autoware_package()
@@ -16,9 +16,9 @@ include_directories(
1616
)
1717

1818
ament_auto_add_library(costmap_generator_lib SHARED
19-
nodes/costmap_generator/points_to_costmap.cpp
20-
nodes/costmap_generator/objects_to_costmap.cpp
21-
nodes/costmap_generator/object_map_utils.cpp
19+
nodes/autoware_costmap_generator/points_to_costmap.cpp
20+
nodes/autoware_costmap_generator/objects_to_costmap.cpp
21+
nodes/autoware_costmap_generator/object_map_utils.cpp
2222
)
2323
target_link_libraries(costmap_generator_lib
2424
${PCL_LIBRARIES}
@@ -33,15 +33,15 @@ if(${PCL_VERSION} GREATER_EQUAL 1.12.1)
3333
endif()
3434

3535
ament_auto_add_library(costmap_generator_node SHARED
36-
nodes/costmap_generator/costmap_generator_node.cpp
36+
nodes/autoware_costmap_generator/costmap_generator_node.cpp
3737
)
3838
target_link_libraries(costmap_generator_node
3939
${PCL_LIBRARIES}
4040
costmap_generator_lib
4141
)
4242

4343
rclcpp_components_register_node(costmap_generator_node
44-
PLUGIN "CostmapGenerator"
44+
PLUGIN "autoware::costmap_generator::CostmapGenerator"
4545
EXECUTABLE costmap_generator
4646
)
4747

planning/costmap_generator/include/costmap_generator/costmap_generator.hpp planning/autoware_costmap_generator/include/autoware_costmap_generator/costmap_generator.hpp

+8-5
Original file line numberDiff line numberDiff line change
@@ -42,11 +42,11 @@
4242
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
4343
********************/
4444

45-
#ifndef COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_
46-
#define COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_
45+
#ifndef AUTOWARE_COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_
46+
#define AUTOWARE_COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_
4747

48-
#include "costmap_generator/objects_to_costmap.hpp"
49-
#include "costmap_generator/points_to_costmap.hpp"
48+
#include "autoware_costmap_generator/objects_to_costmap.hpp"
49+
#include "autoware_costmap_generator/points_to_costmap.hpp"
5050

5151
#include <grid_map_ros/GridMapRosConverter.hpp>
5252
#include <grid_map_ros/grid_map_ros.hpp>
@@ -72,6 +72,8 @@
7272
#include <string>
7373
#include <vector>
7474

75+
namespace autoware::costmap_generator
76+
{
7577
class CostmapGenerator : public rclcpp::Node
7678
{
7779
public:
@@ -197,5 +199,6 @@ class CostmapGenerator : public rclcpp::Node
197199
/// \brief calculate cost for final output
198200
grid_map::Matrix generateCombinedCostmap();
199201
};
202+
} // namespace autoware::costmap_generator
200203

201-
#endif // COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_
204+
#endif // AUTOWARE_COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_

planning/costmap_generator/include/costmap_generator/object_map_utils.hpp planning/autoware_costmap_generator/include/autoware_costmap_generator/object_map_utils.hpp

+3-3
Original file line numberDiff line numberDiff line change
@@ -30,8 +30,8 @@
3030
*
3131
*/
3232

33-
#ifndef COSTMAP_GENERATOR__OBJECT_MAP_UTILS_HPP_
34-
#define COSTMAP_GENERATOR__OBJECT_MAP_UTILS_HPP_
33+
#ifndef AUTOWARE_COSTMAP_GENERATOR__OBJECT_MAP_UTILS_HPP_
34+
#define AUTOWARE_COSTMAP_GENERATOR__OBJECT_MAP_UTILS_HPP_
3535

3636
#include <grid_map_cv/grid_map_cv.hpp>
3737
#include <grid_map_ros/grid_map_ros.hpp>
@@ -98,4 +98,4 @@ void FillPolygonAreas(
9898

9999
} // namespace object_map
100100

101-
#endif // COSTMAP_GENERATOR__OBJECT_MAP_UTILS_HPP_
101+
#endif // AUTOWARE_COSTMAP_GENERATOR__OBJECT_MAP_UTILS_HPP_

planning/costmap_generator/include/costmap_generator/objects_to_costmap.hpp planning/autoware_costmap_generator/include/autoware_costmap_generator/objects_to_costmap.hpp

+6-4
Original file line numberDiff line numberDiff line change
@@ -42,8 +42,8 @@
4242
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
4343
********************/
4444

45-
#ifndef COSTMAP_GENERATOR__OBJECTS_TO_COSTMAP_HPP_
46-
#define COSTMAP_GENERATOR__OBJECTS_TO_COSTMAP_HPP_
45+
#ifndef AUTOWARE_COSTMAP_GENERATOR__OBJECTS_TO_COSTMAP_HPP_
46+
#define AUTOWARE_COSTMAP_GENERATOR__OBJECTS_TO_COSTMAP_HPP_
4747

4848
#include <grid_map_ros/grid_map_ros.hpp>
4949

@@ -57,6 +57,8 @@
5757

5858
#include <string>
5959

60+
namespace autoware::costmap_generator
61+
{
6062
class ObjectsToCostmap
6163
{
6264
public:
@@ -123,5 +125,5 @@ class ObjectsToCostmap
123125
const grid_map::Polygon & polygon, const std::string & gridmap_layer_name, const float score,
124126
grid_map::GridMap & objects_costmap);
125127
};
126-
127-
#endif // COSTMAP_GENERATOR__OBJECTS_TO_COSTMAP_HPP_
128+
} // namespace autoware::costmap_generator
129+
#endif // AUTOWARE_COSTMAP_GENERATOR__OBJECTS_TO_COSTMAP_HPP_

planning/costmap_generator/include/costmap_generator/points_to_costmap.hpp planning/autoware_costmap_generator/include/autoware_costmap_generator/points_to_costmap.hpp

+7-3
Original file line numberDiff line numberDiff line change
@@ -42,8 +42,8 @@
4242
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
4343
********************/
4444

45-
#ifndef COSTMAP_GENERATOR__POINTS_TO_COSTMAP_HPP_
46-
#define COSTMAP_GENERATOR__POINTS_TO_COSTMAP_HPP_
45+
#ifndef AUTOWARE_COSTMAP_GENERATOR__POINTS_TO_COSTMAP_HPP_
46+
#define AUTOWARE_COSTMAP_GENERATOR__POINTS_TO_COSTMAP_HPP_
4747

4848
#include <grid_map_ros/grid_map_ros.hpp>
4949

@@ -52,6 +52,9 @@
5252
#include <string>
5353
#include <vector>
5454

55+
namespace autoware::costmap_generator
56+
57+
{
5558
class PointsToCostmap
5659
{
5760
public:
@@ -115,5 +118,6 @@ class PointsToCostmap
115118
const std::string & gridmap_layer_name,
116119
const std::vector<std::vector<std::vector<double>>> grid_vec);
117120
};
121+
} // namespace autoware::costmap_generator
118122

119-
#endif // COSTMAP_GENERATOR__POINTS_TO_COSTMAP_HPP_
123+
#endif // AUTOWARE_COSTMAP_GENERATOR__POINTS_TO_COSTMAP_HPP_

planning/costmap_generator/launch/costmap_generator.launch.xml planning/autoware_costmap_generator/launch/costmap_generator.launch.xml

+2-2
Original file line numberDiff line numberDiff line change
@@ -6,9 +6,9 @@
66
<arg name="output_grid_map" default="~/output/grid_map"/>
77
<arg name="output_occupancy_grid" default="~/output/occupancy_grid"/>
88

9-
<arg name="costmap_generator_param_file" default="$(find-pkg-share costmap_generator)/config/costmap_generator.param.yaml"/>
9+
<arg name="costmap_generator_param_file" default="$(find-pkg-share autoware_costmap_generator)/config/costmap_generator.param.yaml"/>
1010

11-
<node pkg="costmap_generator" exec="costmap_generator" name="costmap_generator" output="screen">
11+
<node pkg="autoware_costmap_generator" exec="costmap_generator" name="costmap_generator" output="screen">
1212
<remap from="~/input/objects" to="$(var input_objects)"/>
1313
<remap from="~/input/points_no_ground" to="$(var input_points_no_ground)"/>
1414
<remap from="~/input/vector_map" to="$(var input_lanelet_map)"/>

planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp planning/autoware_costmap_generator/nodes/autoware_costmap_generator/costmap_generator_node.cpp

+6-3
Original file line numberDiff line numberDiff line change
@@ -42,8 +42,8 @@
4242
* OF private_node SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
4343
********************/
4444

45-
#include "costmap_generator/costmap_generator.hpp"
46-
#include "costmap_generator/object_map_utils.hpp"
45+
#include "autoware_costmap_generator/costmap_generator.hpp"
46+
#include "autoware_costmap_generator/object_map_utils.hpp"
4747

4848
#include <lanelet2_extension/utility/message_conversion.hpp>
4949
#include <lanelet2_extension/utility/query.hpp>
@@ -157,6 +157,8 @@ pcl::PointCloud<pcl::PointXYZ> getTransformedPointCloud(
157157

158158
} // namespace
159159

160+
namespace autoware::costmap_generator
161+
{
160162
CostmapGenerator::CostmapGenerator(const rclcpp::NodeOptions & node_options)
161163
: Node("costmap_generator", node_options), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_)
162164
{
@@ -476,6 +478,7 @@ void CostmapGenerator::publishCostmap(const grid_map::GridMap & costmap)
476478
out_gridmap_msg->header = header;
477479
pub_costmap_->publish(*out_gridmap_msg);
478480
}
481+
} // namespace autoware::costmap_generator
479482

480483
#include <rclcpp_components/register_node_macro.hpp>
481-
RCLCPP_COMPONENTS_REGISTER_NODE(CostmapGenerator)
484+
RCLCPP_COMPONENTS_REGISTER_NODE(autoware::costmap_generator::CostmapGenerator)

planning/costmap_generator/nodes/costmap_generator/object_map_utils.cpp planning/autoware_costmap_generator/nodes/autoware_costmap_generator/object_map_utils.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -30,7 +30,7 @@
3030
*
3131
*/
3232

33-
#include "costmap_generator/object_map_utils.hpp"
33+
#include "autoware_costmap_generator/object_map_utils.hpp"
3434

3535
#include <string>
3636
#include <vector>

planning/costmap_generator/nodes/costmap_generator/objects_to_costmap.cpp planning/autoware_costmap_generator/nodes/autoware_costmap_generator/objects_to_costmap.cpp

+4-1
Original file line numberDiff line numberDiff line change
@@ -42,13 +42,15 @@
4242
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
4343
********************/
4444

45-
#include "costmap_generator/objects_to_costmap.hpp"
45+
#include "autoware_costmap_generator/objects_to_costmap.hpp"
4646

4747
#include <tf2/utils.h>
4848

4949
#include <cmath>
5050
#include <string>
5151

52+
namespace autoware::costmap_generator
53+
{
5254
// Constructor
5355
ObjectsToCostmap::ObjectsToCostmap()
5456
: NUMBER_OF_POINTS(4),
@@ -196,3 +198,4 @@ grid_map::Matrix ObjectsToCostmap::makeCostmapFromObjects(
196198

197199
return objects_costmap[OBJECTS_COSTMAP_LAYER_];
198200
}
201+
} // namespace autoware::costmap_generator

planning/costmap_generator/nodes/costmap_generator/points_to_costmap.cpp planning/autoware_costmap_generator/nodes/autoware_costmap_generator/points_to_costmap.cpp

+6-1
Original file line numberDiff line numberDiff line change
@@ -42,11 +42,14 @@
4242
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
4343
********************/
4444

45-
#include "costmap_generator/points_to_costmap.hpp"
45+
#include "autoware_costmap_generator/points_to_costmap.hpp"
4646

4747
#include <string>
4848
#include <vector>
4949

50+
namespace autoware::costmap_generator
51+
{
52+
5053
void PointsToCostmap::initGridmapParam(const grid_map::GridMap & gridmap)
5154
{
5255
grid_length_x_ = gridmap.getLength().x();
@@ -140,3 +143,5 @@ grid_map::Matrix PointsToCostmap::makeCostmapFromPoints(
140143
gridmap_layer_name, grid_vec);
141144
return costmap;
142145
}
146+
147+
} // namespace autoware::costmap_generator

planning/costmap_generator/package.xml planning/autoware_costmap_generator/package.xml

+2-2
Original file line numberDiff line numberDiff line change
@@ -1,9 +1,9 @@
11
<?xml version="1.0"?>
22
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
33
<package format="3">
4-
<name>costmap_generator</name>
4+
<name>autoware_costmap_generator</name>
55
<version>0.1.0</version>
6-
<description>The costmap_generator package</description>
6+
<description>The autoware_costmap_generator package</description>
77
<maintainer email="kosuke.takeuchi@tier4.jp">Kosuke Takeuchi</maintainer>
88
<maintainer email="takamasa.horibe@tier4.jp">Takamasa Horibe</maintainer>
99
<maintainer email="takayuki.murooka@tier4.jp">Takayuki Murooka</maintainer>

planning/costmap_generator/test/test_objects_to_costmap.cpp planning/autoware_costmap_generator/test/test_objects_to_costmap.cpp

+4-1
Original file line numberDiff line numberDiff line change
@@ -12,11 +12,13 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#include <costmap_generator/objects_to_costmap.hpp>
15+
#include <autoware_costmap_generator/objects_to_costmap.hpp>
1616
#include <rclcpp/rclcpp.hpp>
1717

1818
#include <gtest/gtest.h>
1919

20+
namespace autoware::costmap_generator
21+
{
2022
using LABEL = autoware_perception_msgs::msg::ObjectClassification;
2123

2224
class ObjectsToCostMapTest : public ::testing::Test
@@ -134,3 +136,4 @@ TEST_F(ObjectsToCostMapTest, TestMakeCostmapFromObjects)
134136
}
135137
}
136138
}
139+
} // namespace autoware::costmap_generator

planning/costmap_generator/test/test_points_to_costmap.cpp planning/autoware_costmap_generator/test/test_points_to_costmap.cpp

+5-1
Original file line numberDiff line numberDiff line change
@@ -12,9 +12,12 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#include <costmap_generator/points_to_costmap.hpp>
15+
#include <autoware_costmap_generator/points_to_costmap.hpp>
1616

1717
#include <gtest/gtest.h>
18+
19+
namespace autoware::costmap_generator
20+
{
1821
using pointcloud = pcl::PointCloud<pcl::PointXYZ>;
1922
class PointsToCostmapTest : public ::testing::Test
2023
{
@@ -216,3 +219,4 @@ TEST_F(PointsToCostmapTest, TestMakeCostmapFromPoints_invalidPoints_outOfGrid)
216219

217220
EXPECT_EQ(nonempty_grid_cell_num, 0);
218221
}
222+
} // namespace autoware::costmap_generator

planning/behavior_path_start_planner_module/README.md

+1-1
Original file line numberDiff line numberDiff line change
@@ -526,7 +526,7 @@ If a safe path cannot be generated from the current position, search backwards f
526526
### **freespace pull out**
527527

528528
If the vehicle gets stuck with pull out along lanes, execute freespace pull out.
529-
To run this feature, you need to set `parking_lot` to the map, `activate_by_scenario` of [costmap_generator](../costmap_generator/README.md) to `false` and `enable_freespace_planner` to `true`
529+
To run this feature, you need to set `parking_lot` to the map, `activate_by_scenario` of [costmap_generator](../autoware_costmap_generator/README.md) to `false` and `enable_freespace_planner` to `true`
530530

531531
<img src="https://user-images.githubusercontent.com/39142679/270964106-ae688bca-1709-4e06-98c4-90f671bb8246.png" width="600">
532532

0 commit comments

Comments
 (0)