Program Listing for File gt_component.hpp

↰ Return to documentation for file (/tmp/ws/src/mocap4r2/mocap4r2_robot_gt/mocap4r2_robot_gt/include/mocap4r2_robot_gt/gt_component.hpp)

// Copyright 2022 Intelligent Robotics Lab
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

// Author: Jose Miguel Guerrero Hernandez <josemiguel.guerrero@urjc.es>

#ifndef MOCAP4R2_ROBOT_GT__GTNODE_HPP_
#define MOCAP4R2_ROBOT_GT__GTNODE_HPP_


#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>

#include <vector>

#include "mocap4r2_msgs/msg/rigid_bodies.hpp"
#include "mocap4r2_robot_gt_msgs/srv/set_gt_origin.hpp"

#include "rclcpp/rclcpp.hpp"


namespace mocap4r2_robot_gt
{
class GTNode : public rclcpp::Node
{
public:
  explicit GTNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());

protected:
  void rigid_bodies_callback(const mocap4r2_msgs::msg::RigidBodies::SharedPtr msg);
  void set_gt_origin_callback(
    const std::shared_ptr<mocap4r2_robot_gt_msgs::srv::SetGTOrigin::Request> req,
    std::shared_ptr<mocap4r2_robot_gt_msgs::srv::SetGTOrigin::Response> resp);

  geometry_msgs::msg::Pose get_pose_from_vector(const std::vector<double> & init_pos);

  tf2::BufferCore tf_buffer_;
  tf2_ros::TransformListener tf_listener_;
  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;

  rclcpp::Subscription<mocap4r2_msgs::msg::RigidBodies>::SharedPtr rigid_body_sub_;
  rclcpp::Service<mocap4r2_robot_gt_msgs::srv::SetGTOrigin>::SharedPtr set_gt_origin_srv_;

  std::string root_frame_;
  std::string robot_frame_;
  std::string mocap_frame_;

  tf2::Transform offset_;
  tf2::Transform gtbody2robot_;
  tf2::Transform mocap2gtbody_;

  bool valid_gtbody2robot_{false};

};

}  // namespace mocap4r2_robot_gt

#endif  // MOCAP4R2_ROBOT_GT__GTNODE_HPP_