Skip to content

Commit d8bdb7d

Browse files
Mathi4455Mathi4455
committed
Created node for subscription and publishing and callback function that adds landmark to a list
1 parent d5ee259 commit d8bdb7d

File tree

6 files changed

+342
-0
lines changed

6 files changed

+342
-0
lines changed

‎mission/landmark_server/CMakeLists.txt‎

Lines changed: 23 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,23 @@
1+
cmake_minimum_required(VERSION 3.8)
2+
project(landmark_server)
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(vortex_msgs REQUIRED)
13+
14+
add_executable(landmark_server src/Landmark_server.cpp)
15+
ament_target_dependencies(landmark_server rclcpp vortex_msgs)
16+
17+
install(TARGETS
18+
landmark_server
19+
DESTINATION lib/${PROJECT_NAME})
20+
21+
22+
23+
ament_package()

‎mission/landmark_server/include/landmark_server/Landmark_server.hpp‎

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,19 @@
1+
#include "rclcpp/rclcpp.hpp"
2+
3+
class LandmarkServer : public rclcpp::Node
4+
{
5+
public:
6+
LandmarkServer() : Node("landmark_server")
7+
{
8+
RCLCPP_INFO(this->get_logger(), "Landmark server node started!");
9+
}
10+
};
11+
12+
int main(int argc, char * argv[])
13+
{
14+
rclcpp::init(argc, argv);
15+
rclcpp::spin(std::make_shared<LandmarkServer>());
16+
rclcpp::shutdown();
17+
return 0;
18+
}
19+

‎mission/landmark_server/package.xml‎

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
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>landmark_server</name>
5+
<version>0.0.0</version>
6+
<description>TODO: Package description</description>
7+
<maintainer email="[email protected]">mathias</maintainer>
8+
<license>TODO: License declaration</license>
9+
10+
<buildtool_depend>ament_cmake</buildtool_depend>
11+
12+
<depend>rclcpp</depend>
13+
<depend>vortex_msgs</depend>
14+
15+
<test_depend>ament_lint_auto</test_depend>
16+
<test_depend>ament_lint_common</test_depend>
17+
18+
<export>
19+
<build_type>ament_cmake</build_type>
20+
</export>
21+
</package>

‎mission/landmark_server/src/Landmark_server.cpp‎

Lines changed: 78 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,78 @@
1+
#include "rclcpp/rclcpp.hpp"
2+
#include "vortex_msgs/msg/landmark.hpp"
3+
#include "landmark_server/landmark_server.hpp"
4+
5+
using std::placeholders::_1;
6+
7+
8+
LandmarkServerNode::LandmarkServerNode(const rclcpp::NodeOptions& options)
9+
: Node("landmark_server_node", options)
10+
{
11+
12+
landmark_sub_ = this->create_subscription<vortex_msgs::msg::LandmarkArray>(
13+
"/landmarks",
14+
10,
15+
std::bind(&LandmarkServerNode::landmarksRecievedCallback, this, _1)
16+
);
17+
18+
landmarkPublisher_ = this->create_publisher<vortex_msgs::msg::LandmarkArray>(
19+
"/stored_landmarks", 10);
20+
21+
22+
timer_ = this->create_wall_timer(
23+
std::chrono::milliseconds(100),
24+
std::bind(&LandmarkServerNode::publishStoredLandmarks, this));
25+
26+
27+
storedLandmarks_ = std::make_shared<vortex_msgs::msg::LandmarkArray>();
28+
29+
RCLCPP_INFO(this->get_logger(), "LandmarkServerNode started and subscribed to /landmarks");
30+
31+
}
32+
33+
void LandmarkServerNode::landmarksRecievedCallback(
34+
const vortex_msgs::msg::LandmarkArray::SharedPtr msg)
35+
{
36+
if (!storedLandmarks_) {
37+
storedLandmarks_ = std::make_shared<vortex_msgs::msg::LandmarkArray>();
38+
}
39+
40+
41+
for (const auto &new_landmark : msg->landmarks)
42+
{
43+
vortex_msgs::msg::Landmark landmark_with_id = new_landmark;
44+
45+
46+
landmark_with_id.header.frame_id = "landmark_" + std::to_string(next_id_);
47+
next_id_++;
48+
49+
50+
storedLandmarks_->landmarks.push_back(landmark_with_id);
51+
52+
53+
RCLCPP_INFO(this->get_logger(),
54+
"Stored new landmark #%u (type=%u, subtype=%u, frame_id=%s)",
55+
next_id_ - 1,
56+
new_landmark.type,
57+
new_landmark.subtype,
58+
landmark_with_id.header.frame_id.c_str());
59+
}
60+
61+
RCLCPP_INFO(this->get_logger(),
62+
"Total stored landmarks: %zu",
63+
storedLandmarks_->landmarks.size());
64+
}
65+
66+
67+
68+
void LandmarkServerNode::publishStoredLandmarks()
69+
{
70+
if (!storedLandmarks_ || storedLandmarks_->landmarks.empty()) {
71+
return;
72+
}
73+
74+
storedLandmarks_->header.stamp = this->now();
75+
storedLandmarks_->header.frame_id = "map";
76+
77+
landmarkPublisher_->publish(*storedLandmarks_);
78+
}

‎mission/landmark_server/src/Landmark_server.hpp‎

Lines changed: 185 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,185 @@
1+
#ifndef LANDMARK_SERVER_HPP
2+
#define LANDMARK_SERVER_HPP
3+
4+
#include <rclcpp/rclcpp.hpp>
5+
#include <rclcpp_action/rclcpp_action.hpp>
6+
#include <sstream>
7+
#include <thread>
8+
9+
#include <geometry_msgs/msg/pose_array.hpp>
10+
#include <vortex_msgs/action/filtered_landmarks.hpp>
11+
#include <vortex_msgs/msg/landmark.hpp>
12+
#include <vortex_msgs/msg/landmark_array.hpp>
13+
#include <vortex_msgs/msg/odometry_array.hpp>
14+
15+
#include <tf2_ros/buffer.h>
16+
#include <tf2_ros/transform_listener.h>
17+
18+
#include <tf2/transform_datatypes.h>
19+
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
20+
21+
namespace landmark_server {
22+
23+
/**
24+
* @class LandmarkServerNode
25+
* @brief A class representing a node for handling landmarks in a ROS 2 system.
26+
*
27+
* This class inherits from rclcpp::Node and provides functionality for
28+
* receiving landmark array messages, publishing the poses of the landmarks,
29+
* handling filtered landmarks using an action server, and calculating the
30+
* distance between the poses and the drone.
31+
*/
32+
class LandmarkServerNode : public rclcpp::Node {
33+
public:
34+
/**
35+
* @brief Constructor for the LandmarkServerNode class.
36+
*
37+
* @param options The options for configuring the node.
38+
*/
39+
explicit LandmarkServerNode(
40+
const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
41+
42+
/**
43+
* @brief Destructor for the LandmarkServerNode class.
44+
*/
45+
~LandmarkServerNode() {};
46+
47+
protected:
48+
/**
49+
* @brief A shared pointer to a subscription object for receiving
50+
* vortex_msgs::msg::LandmarkArray messages.
51+
*/
52+
rclcpp::Subscription<vortex_msgs::msg::LandmarkArray>::SharedPtr
53+
landmark_sub_;
54+
55+
/**
56+
* @brief Callback function for receiving landmark array messages.
57+
*
58+
* This function is called when a landmark array message of type
59+
* vortex_msgs::msg::LandmarkArray::SharedPtr is received.
60+
*
61+
* @param msg The shared pointer to the received landmark array message.
62+
*/
63+
void landmarksRecievedCallback(
64+
const vortex_msgs::msg::LandmarkArray::SharedPtr msg);
65+
66+
/**
67+
* @brief A shared pointer to a publisher for the LandmarkArray message
68+
* type. Publishes all landmarks currently stored in the server.
69+
*/
70+
rclcpp::Publisher<vortex_msgs::msg::LandmarkArray>::SharedPtr
71+
landmarkPublisher_;
72+
73+
/**
74+
* @brief A shared pointer to a publisher for geometry_msgs::msg::PoseArray.
75+
* Publishes the pose of all landmarks currently stored in the server.
76+
*/
77+
rclcpp::Publisher<geometry_msgs::msg::PoseArray>::SharedPtr posePublisher_;
78+
79+
/**
80+
* @brief A shared pointer to a LandmarkArray message.
81+
* The array contains all landmarks currently stored by the server.
82+
*/
83+
std::shared_ptr<vortex_msgs::msg::LandmarkArray> storedLandmarks_;
84+
85+
/**
86+
* @brief A shared pointer to an rclcpp_action server for handling filtered
87+
* landmarks.
88+
*/
89+
rclcpp_action::Server<vortex_msgs::action::FilteredLandmarks>::SharedPtr
90+
action_server_;
91+
92+
/**
93+
* @brief Handles the goal request for the `handle_goal` function.
94+
*
95+
* This function is responsible for processing the goal request for the
96+
* `handle_goal` action.
97+
*
98+
* @param uuid The unique identifier of the goal request.
99+
* @param goal A shared pointer to the goal message containing the requested
100+
* goal.
101+
* @return The response to the goal request.
102+
*/
103+
rclcpp_action::GoalResponse handle_goal(
104+
const rclcpp_action::GoalUUID& uuid,
105+
std::shared_ptr<const vortex_msgs::action::FilteredLandmarks::Goal>
106+
goal);
107+
108+
/**
109+
* @brief Handles the cancellation of a goal.
110+
*
111+
* This function is called when a goal is cancelled by the client.
112+
*
113+
* @param goal_handle The goal handle associated with the cancelled goal.
114+
* @return The response indicating the result of the cancellation.
115+
*/
116+
rclcpp_action::CancelResponse handle_cancel(
117+
const std::shared_ptr<rclcpp_action::ServerGoalHandle<
118+
vortex_msgs::action::FilteredLandmarks>> goal_handle);
119+
120+
/**
121+
* @brief Handles the accepted goal for the FilteredLandmarks action server.
122+
*
123+
* This function is called when a goal is accepted by the action server.
124+
*
125+
* @param goal_handle The goal handle for the accepted goal.
126+
*/
127+
void handle_accepted(
128+
const std::shared_ptr<rclcpp_action::ServerGoalHandle<
129+
vortex_msgs::action::FilteredLandmarks>> goal_handle);
130+
131+
/**
132+
* @brief Executes the action server goal handle for the FilteredLandmarks
133+
* action.
134+
*
135+
* @param goal_handle The goal handle for the FilteredLandmarks action.
136+
*/
137+
void execute(const std::shared_ptr<rclcpp_action::ServerGoalHandle<
138+
vortex_msgs::action::FilteredLandmarks>> goal_handle);
139+
140+
/**
141+
* @brief Logs messages based on request.
142+
*
143+
* This function is responsible for logging messages based on the request
144+
*
145+
* @param goal_handle A shared pointer to the goal handle.
146+
*/
147+
void requestLogger(
148+
const std::shared_ptr<rclcpp_action::ServerGoalHandle<
149+
vortex_msgs::action::FilteredLandmarks>> goal_handle);
150+
151+
/**
152+
* Calculates the distance between a landmark and the drone.
153+
*
154+
* @param point The point to calculate the distance from.
155+
* @param header The header to calculate the distance to.
156+
* @return The distance between the landmark and the drone.
157+
*/
158+
double calculateDistance(const geometry_msgs::msg::Point& point,
159+
const std_msgs::msg::Header& header);
160+
161+
/**
162+
* @brief Creates a pose array from a landmark array.
163+
* @param landmarks The landmark array.
164+
* @return The pose array.
165+
*/
166+
geometry_msgs::msg::PoseArray poseArrayCreater(
167+
vortex_msgs::msg::LandmarkArray landmarks);
168+
169+
// Declare tf_buffer_ and tf_listener_ as class members
170+
std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
171+
std::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
172+
173+
vortex_msgs::msg::OdometryArray filterLandmarks(
174+
const std::shared_ptr<rclcpp_action::ServerGoalHandle<
175+
vortex_msgs::action::FilteredLandmarks>> goal_handle);
176+
177+
private:
178+
rclcpp::TimerBase::SharedPtr timer_;
179+
180+
void publishStoredLandmarks();
181+
};
182+
183+
} // namespace landmark_server
184+
185+
#endif // LANDMARK_SERVER_HPP

‎mission/landmark_server/src/Main.cpp‎

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,16 @@
1+
#include "landmark_server/landmark_server.hpp"
2+
#include "rclcpp/rclcpp.hpp"
3+
#include "vortex_msg/msg/landmarkarray.hpp"
4+
5+
6+
7+
8+
int main(int argc, char** argv)
9+
{
10+
rclcpp::init(argc, argv);
11+
auto node = std::make_shared<landmark_server::LandmarkServerNode>();
12+
rclcpp::spin(node);
13+
rclcpp::shutdown();
14+
return 0;
15+
16+
}

0 commit comments

Comments
 (0)