PosePublisherItem.h

Go to the documentation of this file.

/* Author: Masaki Murooka */
 
#pragma once
 
#include <cnoid/Archive>
#include <cnoid/Item>
#include <cnoid/SimulatorItem>
#include <cnoid/WorldItem>
 
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <tf2_eigen/tf2_eigen.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <rclcpp/rclcpp.hpp>
 
namespace CnoidRosUtils
{
class PosePublisherItem : public cnoid::Item
{
public:
  static void initialize(cnoid::ExtensionManager * ext);
 
  inline static bool initialized_ = false;
 
public:
  PosePublisherItem();
 
  PosePublisherItem(const PosePublisherItem & org);
 
  ~PosePublisherItem();
 
protected:
  virtual cnoid::Item * doDuplicate() const override;
 
  virtual void doPutProperties(cnoid::PutPropertyFunction & putProperty) override;
 
  virtual bool store(cnoid::Archive & archive) override;
 
  virtual bool restore(const cnoid::Archive & archive) override;
 
  void setup();
 
  bool start();
 
  void stop();
 
  void onPostDynamics();
 
  int getPubSkip() const;
 
protected:
  cnoid::WorldItemPtr world_;
  cnoid::SimulatorItemPtr sim_;
 
  std::set<std::string> hooked_sims_;
 
  int post_dynamics_func_id_ = -1;
 
  cnoid::BodyItem * body_item_;
 
  std::string link_name_ = "";
 
  bool output_tf_ = false;
 
  rclcpp::Node::SharedPtr nh_;
  std::string frame_id_ = "";
  std::string pose_topic_name_ = "";
  std::string vel_topic_name_ = "";
  std::string tf_child_frame_id_ = "";
  double pub_rate_ = 30.0;
  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_pub_;
  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr vel_pub_;
  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_br_;
 
  int sim_cnt_ = 0;
  int pub_skip_ = 0;
};
} // namespace CnoidRosUtils