Skip to content

Commit 24420e7

Browse files
aquintan4aquintan4
committed
Merge remote-tracking branch 'upstream/humble-devel' into odometry-publisher-gzharmonic
2 parents e8f988e + 41945ec commit 24420e7

8 files changed

Lines changed: 565 additions & 40 deletions

File tree

‎CustomRobots/turtlebot3/models/turtlebot3_waffle/model.sdf‎

Lines changed: 0 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -554,19 +554,6 @@
554554
<odom_publish_frequency>50</odom_publish_frequency>
555555
<dimensions>3</dimensions>
556556
</plugin>
557-
558-
<plugin filename="libNoisyOdometryPlugin.so" name="custom_plugins::NoisyOdometryPlugin">
559-
<ros_topic>/turtlebot3/odom_noisy</ros_topic>
560-
<gz_cmd_vel_topic>/turtlebot3/cmd_vel</gz_cmd_vel_topic>
561-
<frame_id>odom</frame_id>
562-
<child_frame_id>base_footprint</child_frame_id>
563-
564-
<alpha1>0.0005</alpha1>
565-
<alpha2>0.0005</alpha2>
566-
<alpha3>0.0002</alpha3>
567-
<alpha4>0.0002</alpha4>
568-
</plugin>
569-
570557
</model>
571558

572559

‎Docs/robot-world-status.md‎

Lines changed: 58 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,58 @@
1+
# Table of robots, worlds and universes
2+
3+
## Robots
4+
5+
| Robot ID | Launch path | Entity name | Configs available | URDF/SDF | Notes |
6+
| ------------------ | :------------------------------------------------------------------------------------------: | :----------------------: | :----------------------------------------------------: | :------: | -------------------- |
7+
| Ur5 | /opt/jderobot/Launchers/pick_place_harmonic/ur5.launch.py | ur5_robotiq | | URDF | Broken Reset |
8+
| F1 car | /home/ws/src/CustomRobots/f1/launch/f1.launch.py | f1 | mode=(holo/ackermann) sensor=(camera/laser) | URDF | |
9+
| Autonomous car | /home/ws/src/CustomRobots/autonomous_car/launch/autonomous_car.launch.py | autonomous_car | mode=(holonomic/ackermann) sensor=(camera/laser/lidar) | URDF | |
10+
| Vacuum cleaner | /home/ws/src/CustomRobots/vacuum_cleaner/launch/vacuum_cleaner.launch.py | vacuum_cleaner | sensor=(camera/laser) | SDF | Broken bumper in SDF |
11+
| Quadrotor | /home/ws/src/CustomRobots/quadrotor/launch/quadrotor.launch.py | quadrotor | sensor=(camera) namespace | URDF | Ros error |
12+
| Rover 4wd | /home/ws/src/CustomRobots/quadrotor/launch/rover_4wd.launch.py | rover_4wd | noise=(none/low/medium/high) | URDF | |
13+
| Logistic Holonomic | /home/ws/src/CustomRobots/logistic_holonomic_robot/launch/logistic_holonomic_robot.launch.py | logistic_holonomic_robot | | URDF | Ball joint fails |
14+
| Logistic Ackermann | /home/ws/src/CustomRobots/logistic_ackermann_robot/launch/logistic_ackermann_robot.launch.py | logistic_ackermann_robot | | - | |
15+
| TurtleBot 2 | /home/ws/src/CustomRobots/Turtlebot2/launch/turtlebot2.launch.py | turtlebot2 | sensor=(camera/stereo) | URDF | |
16+
| TurtleBot 3 | /home/ws/src/CustomRobots/Turtlebot2/launch/turtlebot2.launch.py | turtlebot3 | | - | |
17+
18+
## Worlds
19+
20+
"Robot included" means that the world launcher only launches the world and not the robot.
21+
22+
| World ID | Launch path (/opt/jderobot/Launchers) | Spawn point | Robot included | 🟨 Gazebo Classic | 🟧 Gazebo Harmonic | Notes |
23+
| ------------------------------------ | :-------------------------------------: | :----------------------------: | :------------: | :---------------: | :----------------: | ------------ |
24+
| 3d Reconstruction | 3d_reconstruction.launch.py | 0,0,0,0,0,0 | No | ok | ok | |
25+
| City Large | taxi_navigator.launch.py | 0,0,0.1,0,0,0 | No | ok | ok | |
26+
| Autopark_line | autopark_line.launch.py | -7,2.5,0.004,0,0,0 | No | ok | ok | |
27+
| Autopark_battery | autopark_battery.launch.py | -7,2.5,0.004,0,0,0 | No | ok | ok | |
28+
| Autopark_sideways | autopark_sideways.launch.py | -7,2.5,0.004,0,0,0 | No | ok | ok | |
29+
| Autoparking Gas Station: In battery | gas_station_battery_ackermann.launch.py | | Yes | ok | | |
30+
| Autoparking Gas Station: In line | gas_station_line_ackermann.launch.py | | Yes | ok | | |
31+
| Autoparking Gas Station: Parking lot | gas_station_parking_ackermann.launch.py | | Yes | ok | | |
32+
| Follow Person | follow_person_harmonic.launch.py | -1.0,10.0,0.1 | No | ok | ok | |
33+
| Follow Person Teleop | follow_person_teleop_harmonic.launch.py | -1.0,10.0,0.1 | No | ok | ok | |
34+
| Laser Mapping Warehouse | laser_mapping.launch.py | 0,0,0,0,0,0 | Yes | | ok | |
35+
| Small Laser Mapping Warehouse | small_laser_mapping.launch.py | 0,0,0,0,0,0 | Yes | | ok | |
36+
| Simple Circuit | simple_circuit.launch.py | 53.462,-10.734,0.004,0,0,-1.57 | No | ok | ok | |
37+
| Montmelo Circuit | montmelo_circuit.launch.py | 27.18,-31.55,0,0,0.01,-3.12 | No | ok | ok | |
38+
| Montreal Circuit | montreal_circuit.launch.py | -200.88,-90.72,0,0,0,-2.83 | No | ok | ok | |
39+
| Nurburgring Circuit | nurburgring_circuit.launch.py | -74.29,37.74,0,0,0,-0.51 | No | ok | ok | |
40+
| Monaco Circuit | monaco_circuit.launch.py | -105.223,-70.77,-1.8,0,0,1.69 | No | ok | ok | |
41+
| Spa Circuit | spa_circuit.launch.py | | Yes | | | |
42+
| Obstacle Avoidance | nurburgring_circuit.launch.py | 53.462,-10.734,0.004,0,0,-1.57 | No | ok | ok | |
43+
| Rescue People | rescue_people.launch.py | 0,0,1.45,0,0,0 | No | ok | ok | |
44+
| Warehouse 1 | small_warehouse.launch.py | 0,0,0.1,0,0,0 | No | ok | ok | |
45+
| Warehouse 2 | pallet_warehouse.launch.py | 0,0,0.1,0,0,0 | No | ok | ok | |
46+
| Restaurant | restaurant.launch.py | | Yes | ok | | |
47+
| Vacuums House | vacuum_cleaner.launch.py | -1,1.5,0,0,0,0 | No | ok | ok | |
48+
| Vacuums House Markers | marker_visual_loc.launch.py | 1,-1.5,0.6,0,0,0 | Yes | | ok | |
49+
| Vacuums House Roof | montecarlo_visual_loc.launch.py | 0,0,0,0,0,0 | No | ok | ok | |
50+
| Follow Road | follow_road.launch.py | 0,0,0,0,0,0 | No | | ok | |
51+
| Car Junction | car_junction.launch.py | | Yes | | ok | |
52+
| Drone Gymkhana | drone_gymkhana.launch.py | | Yes | | ok | |
53+
| Tower Inspection | power_tower_inspection.launch.py | | Yes | | ok | |
54+
| Labyrinth Escape | labyrinth_escape.launch.py | | Yes | | ok | |
55+
| Package delivery world | package_delivery.launch.py | | Yes | | ok | |
56+
| Rover 4wd Warehouse | rover_4wd_warehouse.launch.py | 0,0,0.15,0,0,0 | No | | ok | |
57+
| Pick And Place | pick_place_harmonic/world.launch.py | 0,0,0.9,0,0,0 | No | ok | ok | Reset broken |
58+
| Machine Vision | | | Yes | | | Reset broken |

‎common_interfaces_cpp/include/common_interfaces_cpp/hal/classnet.hpp‎

Lines changed: 14 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -4,19 +4,28 @@
44
#include <string>
55
#include <vector>
66
#include <map>
7+
#include <mutex>
78
#include <opencv2/opencv.hpp>
89
#include <opencv2/dnn.hpp>
910

1011
extern const std::string FROZEN_GRAPH;
1112
extern const std::string PB_TXT;
1213
extern const int SIZE;
1314

14-
extern std::map<int, std::string> LABEL_MAP;
15+
extern const std::map<int, std::string> LABEL_MAP;
1516

1617
class BoundingBox {
1718
public:
18-
BoundingBox(int identifier, const std::string& class_id_str, float score, float xmin, float ymin, float xmax, float ymax);
19-
19+
BoundingBox(
20+
int identifier,
21+
const std::string& class_id_str,
22+
float score,
23+
float xmin,
24+
float ymin,
25+
float xmax,
26+
float ymax
27+
);
28+
2029
int id;
2130
std::string class_id;
2231
float score;
@@ -31,13 +40,14 @@ class BoundingBox {
3140
class NeuralNetwork {
3241
public:
3342
NeuralNetwork();
43+
3444
cv::Mat detect(const cv::Mat& img);
35-
3645
// Get bounding boxes function
3746
std::vector<BoundingBox> getBoundingBoxes(const cv::Mat& img);
3847

3948
private:
4049
cv::dnn::Net net;
50+
std::mutex net_mutex_;
4151
};
4252

4353
#endif

‎common_interfaces_cpp/src/hal/classnet.cpp‎

Lines changed: 75 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -1,11 +1,13 @@
11
#include "common_interfaces_cpp/hal/classnet.hpp"
2+
23
#include <sstream>
4+
#include <stdexcept>
35

46
const std::string FROZEN_GRAPH = "/resources/models/ssd_inception_v2_coco.pb";
57
const std::string PB_TXT = "/resources/models/ssd_inception_v2_coco.pbtxt";
68
const int SIZE = 300;
79

8-
std::map<int, std::string> LABEL_MAP = {
10+
const std::map<int, std::string> LABEL_MAP = {
911
{0, "unlabeled"}, {1, "person"}, {2, "bicycle"}, {3, "car"}, {4, "motorcycle"},
1012
{5, "airplane"}, {6, "bus"}, {7, "train"}, {8, "truck"}, {9, "boat"},
1113
{10, "traffic"}, {11, "fire"}, {12, "street"}, {13, "stop"}, {14, "parking"},
@@ -45,57 +47,106 @@ std::map<int, std::string> LABEL_MAP = {
4547
{180, "window"}, {181, "window"}, {182, "wood"}
4648
};
4749

48-
BoundingBox::BoundingBox(int identifier, const std::string& class_id_str, float score, float xmin, float ymin, float xmax, float ymax)
49-
: id(identifier), class_id(class_id_str), score(score), xmin(xmin), ymin(ymin), xmax(xmax), ymax(ymax) {}
50+
BoundingBox::BoundingBox(
51+
int identifier,
52+
const std::string& class_id_str,
53+
float score,
54+
float xmin,
55+
float ymin,
56+
float xmax,
57+
float ymax
58+
)
59+
: id(identifier),
60+
class_id(class_id_str),
61+
score(score),
62+
xmin(xmin),
63+
ymin(ymin),
64+
xmax(xmax),
65+
ymax(ymax)
66+
{
67+
}
5068

51-
std::string BoundingBox::to_string() const {
69+
std::string BoundingBox::to_string() const
70+
{
5271
std::ostringstream oss;
53-
oss << "[id:" << id << "\nclass:" << class_id << "\nscore:" << score
54-
<< "\nxmin:" << xmin << "\nymin:" << ymin << "\nxmax:" << xmax << "\nymax:" << ymax << "\n";
72+
oss << "[id:" << id
73+
<< "\nclass:" << class_id
74+
<< "\nscore:" << score
75+
<< "\nxmin:" << xmin
76+
<< "\nymin:" << ymin
77+
<< "\nxmax:" << xmax
78+
<< "\nymax:" << ymax
79+
<< "\n]";
5580
return oss.str();
5681
}
5782

58-
NeuralNetwork::NeuralNetwork() {
83+
NeuralNetwork::NeuralNetwork()
84+
{
5985
net = cv::dnn::readNetFromTensorflow(FROZEN_GRAPH, PB_TXT);
86+
87+
if (net.empty()) {
88+
throw std::runtime_error("Failed to load neural network model");
89+
}
6090
}
6191

62-
cv::Mat NeuralNetwork::detect(const cv::Mat& img) {
63-
int rows = img.rows;
64-
int cols = img.cols;
65-
92+
cv::Mat NeuralNetwork::detect(const cv::Mat& img)
93+
{
94+
if (img.empty()) {
95+
return cv::Mat();
96+
}
97+
6698
cv::Mat blob = cv::dnn::blobFromImage(
6799
img,
68100
1.0 / 127.5,
69101
cv::Size(SIZE, SIZE),
70102
cv::Scalar(127.5, 127.5, 127.5),
71-
true, // swapRB
72-
false // crop
103+
true,
104+
false
73105
);
74-
106+
107+
std::lock_guard<std::mutex> lock(net_mutex_);
108+
75109
net.setInput(blob);
76-
cv::Mat cvOut = net.forward();
77-
110+
cv::Mat cv_out = net.forward();
111+
112+
if (cv_out.empty() || cv_out.dims < 4) {
113+
return cv::Mat();
114+
}
115+
78116
// cvOut dimension is typically [1, 1, N, 7]. Reshape to [N, 7] to mimic cvOut[0, 0, :, :]
79-
cv::Mat detections(cvOut.size[2], cvOut.size[3], CV_32F, cvOut.ptr<float>());
80-
return detections;
117+
cv::Mat detections(cv_out.size[2], cv_out.size[3], CV_32F, cv_out.ptr<float>());
118+
return detections.clone();
81119
}
82120

83121
// Get bounding boxes function
84-
std::vector<BoundingBox> NeuralNetwork::getBoundingBoxes(const cv::Mat& img) {
85-
int rows = img.rows;
86-
int cols = img.cols;
87-
cv::Mat detections = detect(img);
122+
std::vector<BoundingBox> NeuralNetwork::getBoundingBoxes(const cv::Mat& img)
123+
{
88124
std::vector<BoundingBox> bounding_boxes;
89125

126+
if (img.empty()) {
127+
return bounding_boxes;
128+
}
129+
130+
const int rows = img.rows;
131+
const int cols = img.cols;
132+
133+
cv::Mat detections = detect(img);
134+
135+
if (detections.empty()) {
136+
return bounding_boxes;
137+
}
138+
90139
for (int i = 0; i < detections.rows; ++i) {
91140
int identifier = static_cast<int>(detections.at<float>(i, 1));
92141
float score = detections.at<float>(i, 2);
142+
93143
float xmin = detections.at<float>(i, 3) * cols;
94144
float ymin = detections.at<float>(i, 4) * rows;
95145
float xmax = detections.at<float>(i, 5) * cols;
96146
float ymax = detections.at<float>(i, 6) * rows;
97147

98-
std::string class_name = "";
148+
std::string class_name;
149+
99150
auto it = LABEL_MAP.find(identifier);
100151
if (it != LABEL_MAP.end()) {
101152
class_name = it->second;
@@ -110,6 +161,7 @@ std::vector<BoundingBox> NeuralNetwork::getBoundingBoxes(const cv::Mat& img) {
110161
xmax,
111162
ymax
112163
);
164+
113165
bounding_boxes.push_back(bounding_box);
114166
}
115167

‎jderobot_drones_cpp/CMakeLists.txt‎

Lines changed: 43 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,43 @@
1+
cmake_minimum_required(VERSION 3.8)
2+
project(jderobot_drones_cpp)
3+
4+
if(NOT CMAKE_CXX_STANDARD)
5+
set(CMAKE_CXX_STANDARD 17)
6+
endif()
7+
8+
set(CMAKE_POSITION_INDEPENDENT_CODE ON)
9+
10+
list(APPEND CMAKE_PREFIX_PATH "/home/drones_ws/install")
11+
list(APPEND CMAKE_PREFIX_PATH "/home/dev_ws/install")
12+
13+
find_package(ament_cmake REQUIRED)
14+
find_package(rclcpp REQUIRED)
15+
find_package(as2_core REQUIRED)
16+
find_package(as2_msgs REQUIRED)
17+
find_package(as2_motion_reference_handlers REQUIRED)
18+
find_package(geometry_msgs REQUIRED)
19+
find_package(sensor_msgs REQUIRED)
20+
find_package(cv_bridge REQUIRED)
21+
find_package(image_transport REQUIRED)
22+
find_package(OpenCV REQUIRED)
23+
find_package(std_srvs REQUIRED)
24+
25+
include_directories(include)
26+
27+
add_library(drone_lib_cpp SHARED src/drone_wrapper.cpp)
28+
29+
ament_target_dependencies(drone_lib_cpp
30+
rclcpp as2_core as2_msgs as2_motion_reference_handlers
31+
geometry_msgs sensor_msgs std_srvs cv_bridge image_transport OpenCV)
32+
33+
install(TARGETS drone_lib_cpp
34+
ARCHIVE DESTINATION lib
35+
LIBRARY DESTINATION lib
36+
RUNTIME DESTINATION bin
37+
)
38+
39+
install(DIRECTORY include/ DESTINATION include)
40+
41+
ament_export_include_directories(include)
42+
ament_export_libraries(drone_lib_cpp)
43+
ament_package()

0 commit comments

Comments
 (0)