Skip to content

Commit d802c23

Browse files
chiragmakwana0296chiragmakwana0296
committed
add util pkg
1 parent a661169 commit d802c23

File tree

5 files changed

+277
-0
lines changed

5 files changed

+277
-0
lines changed

‎nav2_dynamic_util/CMakeLists.txt‎

Lines changed: 73 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,73 @@
1+
cmake_minimum_required(VERSION 3.8)
2+
project(nav2_dynamic_util)
3+
4+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
5+
add_compile_options(-Wall -Wextra -Wpedantic)
6+
endif()
7+
8+
# find dependencies
9+
find_package(ament_cmake REQUIRED)
10+
find_package(rclcpp REQUIRED)
11+
find_package(std_msgs REQUIRED)
12+
find_package(geometry_msgs REQUIRED)
13+
find_package(unique_identifier_msgs REQUIRED)
14+
find_package(nav2_dynamic_msgs REQUIRED)
15+
find_package(nav2_dynamic_motion_model REQUIRED)
16+
find_package(visualization_msgs REQUIRED)
17+
18+
include_directories(include)
19+
20+
install(DIRECTORY include/
21+
DESTINATION include/${PROJECT_NAME}
22+
)
23+
24+
add_library(${PROJECT_NAME} SHARED
25+
src/tracked_obstacle_utils.cpp
26+
)
27+
# Link dependencies
28+
ament_target_dependencies(${PROJECT_NAME}
29+
rclcpp
30+
geometry_msgs
31+
nav2_dynamic_msgs
32+
nav2_dynamic_motion_model
33+
)
34+
35+
# Export headers
36+
target_include_directories(${PROJECT_NAME} PUBLIC
37+
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
38+
$<INSTALL_INTERFACE:include>
39+
)
40+
41+
42+
add_executable(tracked_obstacle_publisher src/tracked_obstacle_publisher.cpp)
43+
ament_target_dependencies(tracked_obstacle_publisher
44+
rclcpp
45+
nav2_dynamic_msgs
46+
std_msgs
47+
geometry_msgs
48+
unique_identifier_msgs
49+
visualization_msgs)
50+
51+
52+
install(TARGETS tracked_obstacle_publisher
53+
DESTINATION lib/${PROJECT_NAME})
54+
55+
if(BUILD_TESTING)
56+
find_package(ament_lint_auto REQUIRED)
57+
# the following line skips the linter which checks for copyrights
58+
# comment the line when a copyright and license is added to all source files
59+
set(ament_cmake_copyright_FOUND TRUE)
60+
# the following line skips cpplint (only works in a git repo)
61+
# comment the line when this package is in a git repo and when
62+
# a copyright and license is added to all source files
63+
set(ament_cmake_cpplint_FOUND TRUE)
64+
ament_lint_auto_find_test_dependencies()
65+
endif()
66+
67+
68+
# Export package
69+
ament_export_include_directories(include)
70+
ament_export_libraries(${PROJECT_NAME})
71+
72+
73+
ament_package()

‎nav2_dynamic_util/include/nav2_dynamic_util/tracked_obstacle_utils.hpp‎

Lines changed: 24 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,24 @@
1+
#ifndef NAV2_DYNAMIC_UTIL__TRACKED_OBSTACLE_UTILS_HPP_
2+
#define NAV2_DYNAMIC_UTIL__TRACKED_OBSTACLE_UTILS_HPP_
3+
4+
#include <rclcpp/rclcpp.hpp>
5+
#include <std_msgs/msg/header.hpp>
6+
#include <geometry_msgs/msg/pose.hpp>
7+
8+
#include <nav2_dynamic_msgs/msg/obstacle.hpp>
9+
#include <nav2_dynamic_msgs/msg/obstacle_array.hpp>
10+
#include "nav2_dynamic_motion_model/motion_model_base.hpp"
11+
12+
#include <random>
13+
#include <vector>
14+
#include <cmath>
15+
16+
namespace nav2_dynamic_util
17+
{
18+
geometry_msgs::msg::Pose getObstaclePoseAt(const double dt,
19+
const nav2_dynamic_msgs::msg::Obstacle obstacle,
20+
const std::shared_ptr<nav2_dynamic_motion_model::MotionModelInterface> &motion_model);
21+
22+
23+
} // namespace
24+
#endif // NAV2_DYNAMIC_UTIL__TRACKED_OBSTACLE_UTILS_HPP_

‎nav2_dynamic_util/package.xml‎

Lines changed: 26 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,26 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>nav2_dynamic_util</name>
5+
<version>0.0.0</version>
6+
<description>TODO: Package description</description>
7+
<maintainer email="[email protected]">Chirag Makwana</maintainer>
8+
<license>Apache-2.0</license>
9+
10+
<buildtool_depend>ament_cmake</buildtool_depend>
11+
12+
<depend>rclcpp</depend>
13+
<depend>std_msgs</depend>
14+
<depend>geometry_msgs</depend>
15+
<depend>unique_identifier_msgs</depend>
16+
<depend>nav2_dynamic_msgs</depend>
17+
<depend>nav2_dynamic_motion_model</depend>
18+
<depend>visualization_msgs</depend>
19+
20+
<test_depend>ament_lint_auto</test_depend>
21+
<test_depend>ament_lint_common</test_depend>
22+
23+
<export>
24+
<build_type>ament_cmake</build_type>
25+
</export>
26+
</package>

‎nav2_dynamic_util/src/tracked_obstacle_publisher.cpp‎

Lines changed: 139 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,139 @@
1+
#include <rclcpp/rclcpp.hpp>
2+
#include <std_msgs/msg/header.hpp>
3+
#include <unique_identifier_msgs/msg/uuid.hpp>
4+
#include <geometry_msgs/msg/point.hpp>
5+
#include <geometry_msgs/msg/vector3.hpp>
6+
#include <visualization_msgs/msg/marker_array.hpp>
7+
#include "nav2_dynamic_msgs/msg/obstacle.hpp"
8+
#include "nav2_dynamic_msgs/msg/obstacle_array.hpp"
9+
10+
#include <random>
11+
#include <vector>
12+
#include <cmath>
13+
14+
class ObstacleTracker : public rclcpp::Node {
15+
public:
16+
ObstacleTracker() : Node("obstacle_tracker") {
17+
publisher_ = this->create_publisher<nav2_dynamic_msgs::msg::ObstacleArray>("/tracked_obstacles", 10);
18+
marker_publisher_ = this->create_publisher<visualization_msgs::msg::MarkerArray>("/obstacle_markers", 10);
19+
20+
timer_ = this->create_wall_timer(std::chrono::milliseconds(100),
21+
std::bind(&ObstacleTracker::update_obstacles, this));
22+
initialize_waypoints();
23+
initialize_obstacles();
24+
}
25+
26+
private:
27+
rclcpp::Publisher<nav2_dynamic_msgs::msg::ObstacleArray>::SharedPtr publisher_;
28+
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr marker_publisher_;
29+
rclcpp::TimerBase::SharedPtr timer_;
30+
std::vector<nav2_dynamic_msgs::msg::Obstacle> obstacles_;
31+
std::vector<std::vector<geometry_msgs::msg::Point>> waypoints_;
32+
std::vector<size_t> waypoint_indices_;
33+
std::vector<double> progresses_;
34+
35+
void initialize_waypoints() {
36+
std::random_device rd;
37+
std::mt19937 gen(rd());
38+
std::uniform_real_distribution<float> pos_dist(-10.0, 10.0);
39+
40+
waypoints_.resize(4);
41+
waypoint_indices_.resize(4, 0);
42+
progresses_.resize(4, 0.0);
43+
44+
for (int i = 0; i < 4; ++i) {
45+
for (int j = 0; j < 5; ++j) {
46+
geometry_msgs::msg::Point p;
47+
p.x = pos_dist(gen);
48+
p.y = pos_dist(gen);
49+
p.z = 0.0;
50+
waypoints_[i].push_back(p);
51+
}
52+
}
53+
}
54+
55+
void initialize_obstacles() {
56+
std::random_device rd;
57+
std::mt19937 gen(rd());
58+
std::uniform_real_distribution<float> pos_dist(-10.0, 10.0);
59+
60+
for (int i = 0; i < 4; ++i) {
61+
nav2_dynamic_msgs::msg::Obstacle obs;
62+
obs.uuid = generate_uuid();
63+
obs.score = 1.0;
64+
obs.position = waypoints_[i][0];
65+
obs.velocity.x = 0.0;
66+
obs.velocity.y = 0.0;
67+
obs.velocity.z = 0.0;
68+
obs.size.x = 1.0;
69+
obs.size.y = 1.0;
70+
obs.size.z = 1.0;
71+
std::fill(std::begin(obs.position_covariance), std::end(obs.position_covariance), 0.1);
72+
std::fill(std::begin(obs.velocity_covariance), std::end(obs.velocity_covariance), 0.05);
73+
74+
obstacles_.push_back(obs);
75+
}
76+
}
77+
78+
unique_identifier_msgs::msg::UUID generate_uuid() {
79+
unique_identifier_msgs::msg::UUID uuid;
80+
std::random_device rd;
81+
for (auto &byte : uuid.uuid) {
82+
byte = static_cast<uint8_t>(rd());
83+
}
84+
return uuid;
85+
}
86+
87+
void update_obstacles() {
88+
if (obstacles_.empty() || waypoints_.empty()) return;
89+
90+
nav2_dynamic_msgs::msg::ObstacleArray msg;
91+
msg.header.stamp = this->now();
92+
msg.header.frame_id = "map";
93+
94+
visualization_msgs::msg::MarkerArray marker_array;
95+
int id = 0;
96+
97+
for (size_t i = 0; i < obstacles_.size(); ++i) {
98+
geometry_msgs::msg::Point start = waypoints_[i][waypoint_indices_[i]];
99+
geometry_msgs::msg::Point end = waypoints_[i][(waypoint_indices_[i] + 1) % waypoints_[i].size()];
100+
101+
progresses_[i] += 0.02; // Adjust speed of movement
102+
if (progresses_[i] >= 1.0) {
103+
progresses_[i] = 0.0;
104+
waypoint_indices_[i] = (waypoint_indices_[i] + 1) % waypoints_[i].size();
105+
}
106+
107+
obstacles_[i].position.x = start.x + (end.x - start.x) * progresses_[i];
108+
obstacles_[i].position.y = start.y + (end.y - start.y) * progresses_[i];
109+
110+
visualization_msgs::msg::Marker marker;
111+
marker.header.frame_id = "map";
112+
marker.header.stamp = this->now();
113+
marker.ns = "obstacles";
114+
marker.id = id++;
115+
marker.type = visualization_msgs::msg::Marker::SPHERE;
116+
marker.action = visualization_msgs::msg::Marker::ADD;
117+
marker.pose.position = obstacles_[i].position;
118+
marker.scale.x = obstacles_[i].size.x;
119+
marker.scale.y = obstacles_[i].size.y;
120+
marker.scale.z = obstacles_[i].size.z;
121+
marker.color.a = 1.0;
122+
marker.color.r = 1.0;
123+
marker.color.g = 0.0;
124+
marker.color.b = 0.0;
125+
marker_array.markers.push_back(marker);
126+
}
127+
128+
msg.obstacles = obstacles_;
129+
publisher_->publish(msg);
130+
marker_publisher_->publish(marker_array);
131+
}
132+
};
133+
134+
int main(int argc, char **argv) {
135+
rclcpp::init(argc, argv);
136+
rclcpp::spin(std::make_shared<ObstacleTracker>());
137+
rclcpp::shutdown();
138+
return 0;
139+
}

‎nav2_dynamic_util/src/tracked_obstacle_utils.cpp‎

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,15 @@
1+
#include "nav2_dynamic_util/tracked_obstacle_utils.hpp"
2+
3+
namespace nav2_dynamic_util
4+
{
5+
6+
geometry_msgs::msg::Pose getObstaclePoseAt(const double dt,
7+
const nav2_dynamic_msgs::msg::Obstacle obstacle,
8+
const std::shared_ptr<nav2_dynamic_motion_model::MotionModelInterface> &motion_model)
9+
{
10+
11+
geometry_msgs::msg::Pose predicted_pose = motion_model->predictObstaclePose(obstacle, dt);
12+
return predicted_pose;
13+
14+
}
15+
}

0 commit comments

Comments
 (0)