Added camera component
This commit is contained in:
parent
1177140a0c
commit
209e99a4b3
9 changed files with 168 additions and 8 deletions
42
env_components/camera_component/src/ign_camera_component.cpp
Normal file
42
env_components/camera_component/src/ign_camera_component.cpp
Normal file
|
@ -0,0 +1,42 @@
|
|||
#include <component_manager/publisher_component.hpp>
|
||||
|
||||
#include <iostream>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <ignition/transport/Node.hh>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <image_transport/image_transport.hpp>
|
||||
#include <ros_gz_bridge/convert.hpp>
|
||||
|
||||
#include <sensor_msgs/msg/image.hpp>
|
||||
|
||||
namespace camera_component
|
||||
{
|
||||
|
||||
class IgnCameraComponent: public env_manager::component_manager::PublisherComponent<sensor_msgs::msg::Image>
|
||||
{
|
||||
public:
|
||||
explicit IgnCameraComponent(const rclcpp::NodeOptions &options)
|
||||
: env_manager::component_manager::PublisherComponent<sensor_msgs::msg::Image>(options)
|
||||
{
|
||||
auto topic_name = "/camera";
|
||||
_ign_node = std::make_shared<ignition::transport::Node>();
|
||||
_ign_node->Subscribe(topic_name, &IgnCameraComponent::on_image, this);
|
||||
}
|
||||
|
||||
private:
|
||||
void on_image(const ignition::msgs::Image& img)
|
||||
{
|
||||
sensor_msgs::msg::Image rimg;
|
||||
ros_gz_bridge::convert_gz_to_ros(img, rimg);
|
||||
populate_publication(rimg);
|
||||
}
|
||||
|
||||
std::shared_ptr<ignition::transport::Node> _ign_node;
|
||||
};
|
||||
} // namespace pub_component
|
||||
#include "rclcpp_components/register_node_macro.hpp"
|
||||
|
||||
RCLCPP_COMPONENTS_REGISTER_NODE(camera_component::IgnCameraComponent)
|
Loading…
Add table
Add a link
Reference in a new issue