Skip to content

Component (Draft) #132

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 12 commits into
base: humble-devel
Choose a base branch
from
Open

Component (Draft) #132

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
12 commits
Select commit Hold shift + click to select a range
f2d0080
marker_publisher: component implementation
mhubii Apr 4, 2024
0b175a0
register node macro
mhubii Apr 4, 2024
7d76b45
simple single component
mhubii Apr 8, 2024
aff4aa7
alphabetic order
mhubii Apr 8, 2024
46c45eb
consistent member names: '*_'
mhubii Apr 8, 2024
fd791a7
bump version 5.0.4 -> 5.1.0
mhubii Apr 8, 2024
7c43636
parameter callback handle
mhubii Apr 8, 2024
7f93e0b
improved logging and parameters
mhubii Apr 8, 2024
85d8696
revert to default
mhubii Apr 8, 2024
cbaa6fb
formatting variables
mhubii Apr 8, 2024
1fd796f
add camera info sub
mhubii Apr 8, 2024
33e99da
consistent member names: '*_'
mhubii Apr 9, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
32 changes: 24 additions & 8 deletions aruco_ros/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,12 @@ project(aruco_ros)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# compile release by default
if(NOT CMAKE_BUILD_TYPE)
Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

default Release compile

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Nice 👍
Good one

set(CMAKE_BUILD_TYPE Release CACHE STRING "Build type." FORCE)
endif()

set(CMAKE_CXX_STANDARD 11) # C++11...
set(CMAKE_CXX_STANDARD_REQUIRED ON) #...is required...
set(CMAKE_CXX_EXTENSIONS ON) #...with compiler extensions like gnu++11
Expand All @@ -14,6 +20,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
geometry_msgs
image_transport
rclcpp
rclcpp_components
rclpy
tf2
tf2_ros
Expand Down Expand Up @@ -71,18 +78,25 @@ target_include_directories(double
ament_target_dependencies(double ${THIS_PACKAGE_INCLUDE_DEPENDS})
target_link_libraries(double ${OpenCV_LIBRARIES})

add_executable(marker_publisher src/marker_publish.cpp
src/aruco_ros_utils.cpp)
target_include_directories(marker_publisher
add_library(marker_publisher_component
Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

marker_publisher_component

SHARED
src/marker_publish.cpp
src/aruco_ros_utils.cpp)

target_include_directories(marker_publisher_component
PUBLIC
include)

target_include_directories(marker_publisher
target_include_directories(marker_publisher_component
SYSTEM PUBLIC
${OpenCV_INCLUDE_DIRS}
)
ament_target_dependencies(marker_publisher ${THIS_PACKAGE_INCLUDE_DEPENDS})
target_link_libraries(marker_publisher ${OpenCV_LIBRARIES})
ament_target_dependencies(marker_publisher_component ${THIS_PACKAGE_INCLUDE_DEPENDS})
target_link_libraries(marker_publisher_component ${OpenCV_LIBRARIES})

rclcpp_components_register_node(marker_publisher_component
PLUGIN aruco_ros::ArucoMarkerPublisher
EXECUTABLE marker_publisher)
Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

executable to maintain current behavior

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes, better to maintain current behaviour


if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
Expand All @@ -102,8 +116,10 @@ install(TARGETS aruco_ros_utils
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin)

install(TARGETS marker_publisher single double
DESTINATION lib/${PROJECT_NAME})
install(TARGETS marker_publisher_component single double
LIBRARY DESTINATION lib
Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

library installation

RUNTIME DESTINATION lib/${PROJECT_NAME})


install(DIRECTORY include/
DESTINATION include
Expand Down
7 changes: 4 additions & 3 deletions aruco_ros/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,13 +20,14 @@
<depend>geometry_msgs</depend>
<depend>image_transport</depend>
<depend>rclcpp</depend>
<depend>rclcpp_component</depend>
<depend>sensor_msgs</depend>
<depend>tf2</depend>
<depend>tf2_ros</depend>
<depend>tf2_geometry_msgs</depend>
<depend>tf2_ros</depend>
<depend>visualization_msgs</depend>
<depend>aruco</depend>
<depend>aruco_msgs</depend>
<depend>sensor_msgs</depend>
<depend>visualization_msgs</depend>

<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
Expand Down
89 changes: 47 additions & 42 deletions aruco_ros/src/marker_publish.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,10 +34,6 @@
*/

#include <iostream>
#include "aruco/aruco.h"
#include "aruco/cvdrawingutils.h"
#include "aruco_ros/aruco_ros_utils.hpp"
#include "aruco_msgs/msg/marker_array.hpp"

#include "cv_bridge/cv_bridge.h"
#include "image_transport/image_transport.hpp"
Expand All @@ -46,16 +42,24 @@
#include "rcpputils/asserts.hpp"
#include "sensor_msgs/image_encodings.hpp"
#include "std_msgs/msg/u_int32_multi_array.hpp"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2_ros/buffer.h"
Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

alphabetic sort

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

👍

#include "tf2_ros/transform_listener.h"

#include "aruco/aruco.h"
#include "aruco/cvdrawingutils.h"
#include "aruco_msgs/msg/marker_array.hpp"
#include "aruco_ros/aruco_ros_utils.hpp"

using namespace std::chrono_literals;

class ArucoMarkerPublisher : public rclcpp::Node
namespace aruco_ros
{
class ArucoMarkerPublisher
{
private:
rclcpp::Node::SharedPtr subNode;
rclcpp::Node::SharedPtr node_;
rclcpp::Node::SharedPtr subNode_;
// ArUco stuff
aruco::MarkerDetector mDetector_;
aruco::CameraParameters camParam_;
Expand Down Expand Up @@ -85,40 +89,46 @@ class ArucoMarkerPublisher : public rclcpp::Node
std_msgs::msg::UInt32MultiArray marker_list_msg_;

public:
ArucoMarkerPublisher()
: Node("marker_publisher"), useCamInfo_(true)
ArucoMarkerPublisher(const rclcpp::NodeOptions & options)
: node_(rclcpp::Node::make_shared("marker_publisher", options)), useCamInfo_(true)
{
setup();
}

rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() const
{
return node_->get_node_base_interface();
}

bool setup()
{
subNode = this->create_sub_node(this->get_name());
subNode_ = node_->create_sub_node(node_->get_name());
// Declare node parameters
this->declare_parameter<double>("marker_size", 0.05);
this->declare_parameter<std::string>("reference_frame", "");
this->declare_parameter<std::string>("camera_frame", "");
this->declare_parameter<bool>("image_is_rectified", true);
this->declare_parameter<bool>("use_camera_info", true);
node_->declare_parameter<double>("marker_size", 0.05);
node_->declare_parameter<std::string>("reference_frame", "");
node_->declare_parameter<std::string>("camera_frame", "");
node_->declare_parameter<bool>("image_is_rectified", true);
node_->declare_parameter<bool>("use_camera_info", true);

tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
tf_buffer_ = std::make_unique<tf2_ros::Buffer>(node_->get_clock());
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);

it_ = std::make_unique<image_transport::ImageTransport>(shared_from_this());
it_ = std::make_unique<image_transport::ImageTransport>(node_);
image_sub_ = it_->subscribe("/image", 1, &ArucoMarkerPublisher::image_callback, this);

this->get_parameter_or<bool>("use_camera_info", useCamInfo_, true);
node_->get_parameter_or<bool>("use_camera_info", useCamInfo_, true);
if (useCamInfo_) {
RCLCPP_INFO(this->get_logger(), "Waiting for the camera info...");
RCLCPP_INFO(node_->get_logger(), "Waiting for the camera info...");
sensor_msgs::msg::CameraInfo camera_info;
rclcpp::wait_for_message<sensor_msgs::msg::CameraInfo>(
camera_info,
shared_from_this(), "/camera_info");
RCLCPP_INFO(this->get_logger(), "Successfully obtained the camera info!");
node_, "/camera_info");
RCLCPP_INFO(node_->get_logger(), "Successfully obtained the camera info!");

this->get_parameter_or<double>("marker_size", marker_size_, 0.05);
this->get_parameter_or<bool>("image_is_rectified", useRectifiedImages_, true);
this->get_parameter_or<std::string>("reference_frame", reference_frame_, "");
this->get_parameter_or<std::string>("camera_frame", camera_frame_, "");
node_->get_parameter_or<double>("marker_size", marker_size_, 0.05);
node_->get_parameter_or<bool>("image_is_rectified", useRectifiedImages_, true);
node_->get_parameter_or<std::string>("reference_frame", reference_frame_, "");
node_->get_parameter_or<std::string>("camera_frame", camera_frame_, "");
camParam_ = aruco_ros::rosCameraInfo2ArucoCamParams(camera_info, useRectifiedImages_);
rcpputils::assert_true(
!(camera_frame_.empty() && !reference_frame_.empty()),
Expand All @@ -130,15 +140,15 @@ class ArucoMarkerPublisher : public rclcpp::Node
camParam_ = aruco::CameraParameters();
}

image_pub_ = it_->advertise(this->get_name() + std::string("/result"), 1);
debug_pub_ = it_->advertise(this->get_name() + std::string("/debug"), 1);
marker_pub_ = subNode->create_publisher<aruco_msgs::msg::MarkerArray>("markers", 100);
image_pub_ = it_->advertise(node_->get_name() + std::string("/result"), 1);
debug_pub_ = it_->advertise(node_->get_name() + std::string("/debug"), 1);
marker_pub_ = subNode_->create_publisher<aruco_msgs::msg::MarkerArray>("markers", 100);
marker_list_pub_ =
subNode->create_publisher<std_msgs::msg::UInt32MultiArray>("markers_list", 10);
subNode_->create_publisher<std_msgs::msg::UInt32MultiArray>("markers_list", 10);

marker_msg_ = aruco_msgs::msg::MarkerArray::Ptr(new aruco_msgs::msg::MarkerArray());
marker_msg_->header.frame_id = reference_frame_;
RCLCPP_INFO(this->get_logger(), "Successfully setup the marker publisher!");
RCLCPP_INFO(node_->get_logger(), "Successfully setup the marker publisher!");

return true;
}
Expand All @@ -153,7 +163,7 @@ class ArucoMarkerPublisher : public rclcpp::Node
refFrame, childFrame, tf2::TimePointZero,
tf2::durationFromSec(0.5), &errMsg))
{
RCLCPP_ERROR_STREAM(this->get_logger(), "Unable to get pose from TF : " << errMsg.c_str());
RCLCPP_ERROR_STREAM(node_->get_logger(), "Unable to get pose from TF : " << errMsg.c_str());
return false;
} else {
try {
Expand All @@ -162,7 +172,7 @@ class ArucoMarkerPublisher : public rclcpp::Node
0.5));
} catch (const tf2::TransformException & e) {
RCLCPP_ERROR_STREAM(
this->get_logger(),
node_->get_logger(),
"Error in lookupTransform of " << childFrame << " in " << refFrame << " : " << e.what());
return false;
}
Expand Down Expand Up @@ -275,16 +285,11 @@ class ArucoMarkerPublisher : public rclcpp::Node
debug_pub_.publish(debug_msg.toImageMsg());
}
} catch (cv_bridge::Exception & e) {
RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what());
RCLCPP_ERROR(node_->get_logger(), "cv_bridge exception: %s", e.what());
}
}
};
} // namespace aruco_ros

int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
std::shared_ptr<ArucoMarkerPublisher> marker_pub = std::make_shared<ArucoMarkerPublisher>();
marker_pub->setup();
rclcpp::spin(marker_pub);
rclcpp::shutdown();
}
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(aruco_ros::ArucoMarkerPublisher)