109 lines
3.6 KiB
C++
109 lines
3.6 KiB
C++
#include "component_manager/service_component.hpp"
|
|
#include "component_interfaces/srv/detect_object.hpp"
|
|
|
|
#include <rclcpp/rclcpp.hpp>
|
|
#include <tf2_ros/transform_broadcaster.h>
|
|
#include <tf2_msgs/msg/tf_message.hpp>
|
|
#include <geometry_msgs/msg/transform_stamped.hpp>
|
|
|
|
#include <ignition/transport/Node.hh>
|
|
#include <ros_gz_bridge/convert.hpp>
|
|
|
|
#define IGN_DEFAULT_BASE_FRAME "world"
|
|
#define IGN_DEFAULT_WORLD_NAME "mir"
|
|
|
|
namespace scene_component
|
|
{
|
|
|
|
class IgnDetectObjectService: public
|
|
env_manager::component_manager::ServiceComponent<component_interfaces::srv::DetectObject>
|
|
{
|
|
public:
|
|
IgnDetectObjectService(const rclcpp::NodeOptions &options)
|
|
: env_manager::component_manager::ServiceComponent<component_interfaces::srv::DetectObject>(options)
|
|
{
|
|
std::string ign_topic_name = "/world/" + std::string(IGN_DEFAULT_WORLD_NAME) + "/dynamic_pose/info";
|
|
_tf2_broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>(this);
|
|
_ign_node = std::make_shared<ignition::transport::Node>();
|
|
_ign_node->Subscribe(ign_topic_name, &IgnDetectObjectService::on_pose, this);
|
|
}
|
|
|
|
void callback(
|
|
std::shared_ptr<component_interfaces::srv::DetectObject::Request> request,
|
|
std::shared_ptr<component_interfaces::srv::DetectObject::Response> response)
|
|
{
|
|
auto kind = static_cast<DetectObjectReqKind>(request->req_kind);
|
|
bool status = false;
|
|
switch(kind)
|
|
{
|
|
case DETECT: {
|
|
(void) kind;
|
|
break;
|
|
} case FOLLOW: {
|
|
auto ok = std::find(_follow_frames.begin(), _follow_frames.end(),
|
|
request->object_name) == _follow_frames.end();
|
|
if (ok)
|
|
_follow_frames.push_back(request->object_name);
|
|
status = ok;
|
|
break;
|
|
} case CANCEL: {
|
|
auto it = std::find(_follow_frames.begin(), _follow_frames.end(),
|
|
request->object_name);
|
|
auto ok = it != _follow_frames.end();
|
|
if (ok)
|
|
_follow_frames.erase(it);
|
|
status = ok;
|
|
break;
|
|
}
|
|
}
|
|
response->call_status = status;
|
|
}
|
|
|
|
private:
|
|
enum DetectObjectReqKind
|
|
{
|
|
DETECT=0,
|
|
FOLLOW=1,
|
|
CANCEL=2
|
|
};
|
|
|
|
void on_pose(const ignition::msgs::Pose_V &pose_v)
|
|
{
|
|
auto poses = pose_v.pose();
|
|
for (const auto& obj: _follow_frames)
|
|
{
|
|
auto it = std::find_if(poses.begin(), poses.end(),
|
|
[&obj](const auto& pose){ return pose.name() == obj;});
|
|
if (it == poses.end())
|
|
continue;
|
|
|
|
geometry_msgs::msg::PoseStamped rpose;
|
|
ros_gz_bridge::convert_gz_to_ros(*it, rpose);
|
|
geometry_msgs::msg::TransformStamped t;
|
|
|
|
t.header.stamp = this->get_clock()->now();
|
|
t.header.frame_id = IGN_DEFAULT_BASE_FRAME;
|
|
t.child_frame_id = it->name();
|
|
t.transform.translation.x = rpose.pose.position.x;
|
|
t.transform.translation.y = rpose.pose.position.y;
|
|
t.transform.translation.z = rpose.pose.position.z;
|
|
t.transform.rotation.x = rpose.pose.orientation.x;
|
|
t.transform.rotation.y = rpose.pose.orientation.y;
|
|
t.transform.rotation.z = rpose.pose.orientation.z;
|
|
t.transform.rotation.w = rpose.pose.orientation.w;
|
|
|
|
_tf2_broadcaster->sendTransform(t);
|
|
}
|
|
}
|
|
|
|
std::vector<std::string> _follow_frames;
|
|
std::unique_ptr<tf2_ros::TransformBroadcaster> _tf2_broadcaster;
|
|
std::shared_ptr<ignition::transport::Node> _ign_node;
|
|
|
|
};
|
|
|
|
} // namespace scene_component
|
|
|
|
#include "rclcpp_components/register_node_macro.hpp"
|
|
|
|
RCLCPP_COMPONENTS_REGISTER_NODE(scene_component::IgnDetectObjectService)
|