Subt virtual publish ground truth

ground_truth_publisher.cpp

#include <ignition/transport.hh>
#include <ignition/msgs/pose_v.pb.h>

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/static_transform_broadcaster.h>

class GroundTruthPublisher
{
  public: GroundTruthPublisher() : privateRosNode("~") {
    // set by subt_ros/competition_init.launch
    this->publicRosNode.getParam("/world_name", this->worldName);
    this->robotName = this->privateRosNode.param("robot_name", ros::this_node::getNamespace().substr(1));
    this->frameName = this->privateRosNode.param("frame_name", this->robotName + "_ground_truth");

    this->odomPublisher = this->publicRosNode.advertise<nav_msgs::Odometry>("ground_truth_odom", 10);

    this->ignNode.Subscribe("/world/" + this->worldName + "/pose/info", &GroundTruthPublisher::OnPoseInfo, this);
  }

  protected: void OnPoseInfo(const ignition::msgs::Pose_V& msg)
  {
    const auto stamp = msg.header().stamp();

    for (const auto& pose : msg.pose())
    {
      if (pose.name() == this->robotName)
      {
        geometry_msgs::TransformStamped tf;
        tf.header.stamp.sec = stamp.sec();
        tf.header.stamp.nsec = stamp.nsec();
        tf.header.frame_id = "world";
        tf.child_frame_id = this->frameName;
        tf.transform.translation.x = pose.position().x();
        tf.transform.translation.y = pose.position().y();
        tf.transform.translation.z = pose.position().z();
        tf.transform.rotation.x = pose.orientation().x();
        tf.transform.rotation.y = pose.orientation().y();
        tf.transform.rotation.z = pose.orientation().z();
        tf.transform.rotation.w = pose.orientation().w();
        this->tfBroadcaster.sendTransform(tf);

        nav_msgs::Odometry odom;
        odom.header = tf.header;
        odom.child_frame_id = tf.child_frame_id;
        odom.pose.pose.position.x = tf.transform.translation.x;
        odom.pose.pose.position.y = tf.transform.translation.y;
        odom.pose.pose.position.z = tf.transform.translation.z;
        odom.pose.pose.orientation.x = tf.transform.rotation.x;
        odom.pose.pose.orientation.y = tf.transform.rotation.y;
        odom.pose.pose.orientation.z = tf.transform.rotation.z;
        odom.pose.pose.orientation.w = tf.transform.rotation.w;
        this->odomPublisher.publish(odom);
        if (this->artifactOriginPublished)
          break;
      } else if (!this->artifactOriginPublished && pose.name() == "artifact_origin") {
        geometry_msgs::TransformStamped tf;
        tf.header.stamp.sec = stamp.sec();
        tf.header.stamp.nsec = stamp.nsec();
        tf.header.frame_id = "world";
        tf.child_frame_id = "subt";
        tf.transform.translation.x = pose.position().x();
        tf.transform.translation.y = pose.position().y();
        tf.transform.translation.z = pose.position().z();
        tf.transform.rotation.x = pose.orientation().x();
        tf.transform.rotation.y = pose.orientation().y();
        tf.transform.rotation.z = pose.orientation().z();
        tf.transform.rotation.w = pose.orientation().w();
        this->tfStaticBroadcaster.sendTransform(tf);
        this->artifactOriginPublished = true;
      }
    }
  }

  private: ignition::transport::Node ignNode;
  private: ros::NodeHandle publicRosNode;
  private: ros::NodeHandle privateRosNode;
  private: std::string worldName;
  private: std::string robotName;
  private: std::string frameName;
  private: tf2_ros::TransformBroadcaster tfBroadcaster;
  private: tf2_ros::StaticTransformBroadcaster tfStaticBroadcaster;
  private: ros::Publisher odomPublisher;
  private: bool artifactOriginPublished {false};
};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "ground_truth_publisher");

  GroundTruthPublisher pub;

  ros::spin();
}

Relevant part of CMakeLists.txt:

find_package(catkin REQUIRED COMPONENTS nav_msgs roscpp tf2_msgs tf2_ros)

find_package(ignition-transport7 REQUIRED)
find_package(ignition-msgs4 REQUIRED)

catkin_package()

add_executable(ground_truth_publisher nodes/ground_truth_publisher.cpp)
target_link_libraries(ground_truth_publisher ${catkin_LIBRARIES} ignition-transport7::core ignition-msgs4::core)
target_include_directories(ground_truth_publisher PUBLIC ${catkin_INCLUDE_DIRS})
add_dependencies(ground_truth_publisher ${catkin_EXPORTED_TARGETS})