Skip to content

Commit

Permalink
Convert agents to lifecycle nodes
Browse files Browse the repository at this point in the history 
Signed-off-by: Alberto Tudela <[email protected]>
  • Loading branch information
@ajtudela
ajtudela committed Oct 10, 2024
1 parent 36fe1a1 commit 50e2619
Show file tree
Hide file tree
Showing 13 changed files with 587 additions and 52 deletions.
5 changes: 4 additions & 1 deletion dsr_agents/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,7 @@ find_package(nav2_util REQUIRED)
find_package(opennav_docking_msgs REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(rclcpp_lifecycle REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_msgs REQUIRED)
Expand All @@ -70,7 +71,7 @@ find_package(tf2_geometry_msgs REQUIRED)
set(dsr_LIBRARIES dsr_api dsr_core dsr_gui)
set(fastrtps_LIBRARIES fastrtps fastcdr)

# Docking agent executable node
# Docking agent library node
add_executable(docking_agent src/docking_agent.cpp)
target_include_directories(docking_agent PUBLIC
"$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/include>"
Expand Down Expand Up @@ -143,6 +144,7 @@ target_link_libraries(topic_agent
dsr_util::dsr_util
PRIVATE
${fastrtps_LIBRARIES}
rclcpp_lifecycle::rclcpp_lifecycle
${sensor_msgs_TARGETS}
)

Expand Down Expand Up @@ -189,6 +191,7 @@ ament_export_dependencies(
opennav_docking_msgs
rclcpp
rclcpp_action
rclcpp_lifecycle
sensor_msgs
tf2
tf2_msgs
Expand Down
1 change: 1 addition & 0 deletions dsr_agents/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
<depend>opennav_docking_msgs</depend>
<depend>rclcpp</depend>
<depend>rclcpp_action</depend>
<depend>rclcpp_lifecycle</depend>
<depend>sensor_msgs</depend>
<depend>tf2</depend>
<depend>tf2_msgs</depend>
Expand Down
2 changes: 1 addition & 1 deletion dsr_agents/src/docking_agent.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -227,7 +227,7 @@ int main(int argc, char ** argv)
auto node = std::make_shared<dsr_agents::DockingAgent>();

dsr_util::QtExecutor exe;
exe.add_node(node);
exe.add_node(node->get_node_base_interface());
exe.start();

auto res = app.exec();
Expand Down
2 changes: 1 addition & 1 deletion dsr_agents/src/nav_agent.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -255,7 +255,7 @@ int main(int argc, char ** argv)
auto node = std::make_shared<dsr_agents::NavigationAgent>();

dsr_util::QtExecutor exe;
exe.add_node(node);
exe.add_node(node->get_node_base_interface());
exe.start();

auto res = app.exec();
Expand Down
2 changes: 1 addition & 1 deletion dsr_agents/src/tf_agent.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -120,7 +120,7 @@ int main(int argc, char ** argv)
auto node = std::make_shared<dsr_agents::TFAgent>();

dsr_util::QtExecutor exe;
exe.add_node(node);
exe.add_node(node->get_node_base_interface());
exe.start();

auto res = app.exec();
Expand Down
7 changes: 4 additions & 3 deletions dsr_agents/src/topic_agent.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@
#include "sensor_msgs/msg/image.hpp"
#include "sensor_msgs/image_encodings.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"

// DSR
#include "dsr_util/qt_executor.hpp"
Expand All @@ -41,7 +42,7 @@ TopicAgent::TopicAgent()
std::this_thread::sleep_for(std::chrono::milliseconds(1000));

// Subscriber to the topic with a generic subscription
auto data = rclcpp::Node::get_topic_names_and_types();
auto data = rclcpp_lifecycle::LifecycleNode::get_topic_names_and_types();
for (auto type : data[ros_topic_]) {
generic_sub_ = create_generic_subscription(
ros_topic_, type, rclcpp::QoS(rclcpp::SystemDefaultsQoS()),
Expand Down Expand Up @@ -116,7 +117,7 @@ void TopicAgent::serial_callback(const std::shared_ptr<rclcpp::SerializedMessage
{
// In order to deserialize the message we have to manually create a ROS2
// message in which we want to convert the serialized data.
auto data = rclcpp::Node::get_topic_names_and_types();
auto data = rclcpp_lifecycle::LifecycleNode::get_topic_names_and_types();
const std::string topic_type = data[ros_topic_][0];
RCLCPP_INFO_ONCE(
this->get_logger(), "Subscribed to topic [%s] of type [%s]",
Expand Down Expand Up @@ -243,7 +244,7 @@ int main(int argc, char ** argv)
auto node = std::make_shared<dsr_agents::TopicAgent>();

dsr_util::QtExecutor exe;
exe.add_node(node);
exe.add_node(node->get_node_base_interface());
exe.start();

auto res = app.exec();
Expand Down
3 changes: 3 additions & 0 deletions dsr_util/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,7 @@ find_package(fastcdr REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(Qt5 REQUIRED COMPONENTS Core)
find_package(rclcpp REQUIRED)
find_package(rclcpp_lifecycle REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)

# ##########
Expand Down Expand Up @@ -96,6 +97,7 @@ target_link_libraries(${library_name}
${dsr_LIBRARIES}
${geometry_msgs_TARGETS}
rclcpp::rclcpp
rclcpp_lifecycle::rclcpp_lifecycle
${qt_LIBRARIES}
PRIVATE
Eigen3::Eigen
Expand Down Expand Up @@ -149,6 +151,7 @@ ament_export_dependencies(
geometry_msgs
Qt5Core
rclcpp
rclcpp_lifecycle
tf2_geometry_msgs
)

Expand Down
97 changes: 67 additions & 30 deletions dsr_util/include/dsr_util/agent_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,22 +30,24 @@
// ROS
#include "geometry_msgs/msg/transform.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"

// DSR
#include "dsr/api/dsr_api.h"

#include "dsr_msgs/srv/save_dsr.hpp"
#include "dsr_util/dsr_api_ext.hpp"

namespace dsr_util
{

using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;

/**
* @class dsr_util::AgentNode
* @brief Base class to connect the DSR graph with ROS 2. It contains common methods and attributes
* to send data from ROS 2 to the DSR graph and vice versa. All agents must inherit from this class.
*/
class AgentNode : public QObject, public rclcpp::Node
class AgentNode : public QObject, public rclcpp_lifecycle::LifecycleNode
{
Q_OBJECT

Expand All @@ -54,14 +56,56 @@ class AgentNode : public QObject, public rclcpp::Node
* @brief Construct a new Agent Node object.
*
* @param ros_node_name Name of the ROS node and the DSR agent.
* @param options Node options
*/
explicit AgentNode(
std::string ros_node_name, const rclcpp::NodeOptions & options = rclcpp::NodeOptions());

/**
* @brief Configure the node
*
* @param state State of the node
* @return CallbackReturn
*/
CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;

/**
* @brief Activate the node
*
* @param state State of the node
* @return CallbackReturn
*/
CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;

/**
* @brief Deactivate the node
*
* @param state State of the node
* @return CallbackReturn
*/
explicit AgentNode(std::string ros_node_name);
CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;

/**
* @brief Cleanup the node
*
* @param state State of the node
* @return CallbackReturn
*/
CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;

/**
* @brief Shutdown the node
*
* @param state State of the node
* @return CallbackReturn
*/
CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;

/**
* @brief Destroy the Agent Node object.
*
*/
virtual ~AgentNode();
virtual ~AgentNode() = default;

protected:
/**
Expand All @@ -88,8 +132,7 @@ class AgentNode : public QObject, public rclcpp::Node
"Inserted node [%s] successfully of type [%s]", name.c_str(), new_node.type().c_str());
} else {
RCLCPP_ERROR(
this->get_logger(),
"The node [%s] couldn't be inserted", name.c_str());
this->get_logger(), "The node [%s] couldn't be inserted", name.c_str());
}
return return_node;
}
Expand Down Expand Up @@ -165,15 +208,13 @@ class AgentNode : public QObject, public rclcpp::Node
RCLCPP_INFO(
this->get_logger(),
"Inserted edge [%s->%s] successfully of type [%s]",
parent_node.value().name().c_str(),
child_node.value().name().c_str(),
parent_node.value().name().c_str(), child_node.value().name().c_str(),
new_edge.type().c_str());
} else {
RCLCPP_ERROR(
this->get_logger(),
"The edge [%s->%s] of type [%s] couldn't be inserted",
parent_node.value().name().c_str(),
child_node.value().name().c_str(),
parent_node.value().name().c_str(), child_node.value().name().c_str(),
new_edge.type().c_str());
}
} else {
Expand Down Expand Up @@ -215,15 +256,13 @@ class AgentNode : public QObject, public rclcpp::Node
RCLCPP_INFO(
this->get_logger(),
"Inserted edge [%s->%s] successfully of type [%s]",
parent_node.value().name().c_str(),
child_node.value().name().c_str(),
parent_node.value().name().c_str(), child_node.value().name().c_str(),
new_edge.type().c_str());
} else {
RCLCPP_ERROR(
this->get_logger(),
"The edge [%s->%s] of type [%s] couldn't be inserted",
parent_node.value().name().c_str(),
child_node.value().name().c_str(),
parent_node.value().name().c_str(), child_node.value().name().c_str(),
new_edge.type().c_str());
}
}
Expand All @@ -241,12 +280,13 @@ class AgentNode : public QObject, public rclcpp::Node
bool delete_node(uint64_t id)
{
// Check if the node exists
bool success = false;
if (auto node = G_->get_node(id); node.has_value()) {
// Delete the node
if (G_->delete_node(id)) {
RCLCPP_INFO(
this->get_logger(), "Deleted node [%s] successfully", node.value().name().c_str());
return true;
success = true;
} else {
RCLCPP_ERROR(
this->get_logger(), "The node [%s] couldn't be deleted", node.value().name().c_str());
Expand All @@ -255,7 +295,7 @@ class AgentNode : public QObject, public rclcpp::Node
RCLCPP_WARN(
this->get_logger(), "The node [%lu] couldn't be deleted because it doesn't exists", id);
}
return false;
return success;
}

/**
Expand All @@ -282,7 +322,7 @@ class AgentNode : public QObject, public rclcpp::Node
*/
bool delete_edge(uint64_t from, uint64_t to, std::string edge_type)
{
// Check if the parent and child nodes exist and if the edge existsd
// Check if the parent and child nodes exist and if the edge exists
auto parent_node = G_->get_node(from);
auto child_node = G_->get_node(to);
if (parent_node.has_value() && child_node.has_value()) {
Expand All @@ -292,16 +332,14 @@ class AgentNode : public QObject, public rclcpp::Node
RCLCPP_INFO(
this->get_logger(),
"Deleted edge [%s->%s] successfully of type [%s]",
parent_node.value().name().c_str(),
child_node.value().name().c_str(),
parent_node.value().name().c_str(), child_node.value().name().c_str(),
edge_type.c_str());
return true;
} else {
RCLCPP_ERROR(
this->get_logger(),
"The edge [%s->%s] of type [%s] couldn't be deleted",
parent_node.value().name().c_str(),
child_node.value().name().c_str(),
parent_node.value().name().c_str(), child_node.value().name().c_str(),
edge_type.c_str());
}
} else {
Expand Down Expand Up @@ -373,26 +411,22 @@ class AgentNode : public QObject, public rclcpp::Node
RCLCPP_INFO(
this->get_logger(),
"Replaced edge [%s->%s] of type [%s] with type [%s]",
parent_node.value().name().c_str(),
child_node.value().name().c_str(),
old_edge.c_str(),
new_edge.type().c_str());
parent_node.value().name().c_str(), child_node.value().name().c_str(),
old_edge.c_str(), new_edge.type().c_str());
return true;
}
} else {
RCLCPP_ERROR(
this->get_logger(),
"The edge [%s->%s] of type [%s] couldn't be deleted",
parent_node.value().name().c_str(),
child_node.value().name().c_str(),
parent_node.value().name().c_str(), child_node.value().name().c_str(),
old_edge.c_str());
}
} else {
RCLCPP_WARN(
this->get_logger(),
"The edge [%lu->%lu] of type [%s] couldn't be deleted "
"because it doesn't exists",
from, to, old_edge.c_str());
"because it doesn't exists", from, to, old_edge.c_str());
}
} else {
RCLCPP_WARN(
Expand Down Expand Up @@ -478,12 +512,12 @@ class AgentNode : public QObject, public rclcpp::Node
}
}

private:
/**
* @brief Initialize ROS parameters.
*/
void get_common_params();

private:
/**
* @brief Save the DSR graph into a file.
*
Expand Down Expand Up @@ -555,6 +589,9 @@ class AgentNode : public QObject, public rclcpp::Node
// Id of the DSR agent.
int agent_id_;

// Name of the agent.
std::string agent_name_;

// Name of the input file to load the DSR graph from.
std::string dsr_input_file_;
};
Expand Down
1 change: 1 addition & 0 deletions dsr_util/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@
<depend>fastrtps</depend>
<depend>geometry_msgs</depend>
<depend>rclcpp</depend>
<depend>rclcpp_lifecycle</depend>
<depend>tf2_geometry_msgs</depend>

<exec_depend>libqt5-core</exec_depend>
Expand Down
Loading

0 comments on commit 50e2619

Please sign in to comment.