How to use chrono to publish posestamped ROS 2

I’m using a drone package I took off github but it only publishes the pose

I’m trying to convert the message to pose stamped but I’m struggling with figuring out how to use chrono to publish the time stamp

Could anyone please help?

void DroneSimpleController::UpdateDynamics(double dt){
  ignition::math::v6::Vector3<double> force, torque;
   
  // Get Pose/Orientation from Gazebo (if no state subscriber is active)
  //  if (imu_subscriber_.getTopic()=="")
    {
      pose = link->WorldPose();
      angular_velocity = link->WorldAngularVel();
      euler = pose.Rot().Euler();
    }
   // if (state_topic_.empty())
    {
      acceleration = (link->WorldLinearVel() - velocity) / dt;
      velocity = link->WorldLinearVel();
    }
    

    //publish the ground truth pose of the drone to the ROS topic
    geometry_msgs::msg::PoseStamped gt_pose;
    auto start = std::chrono::high_resolution_clock::now();
    gt_pose.header.frame_id = "base_link";
    gt_pose.header.stamp = start;
    gt_pose.pose.position.x = pose.Pos().X();
    gt_pose.pose.position.y = pose.Pos().Y();
    gt_pose.pose.position.z = pose.Pos().Z();
    
    gt_pose.pose.orientation.w = pose.Rot().W();
    gt_pose.pose.orientation.x = pose.Rot().X();
    gt_pose.pose.orientation.y = pose.Rot().Y();
    gt_pose.pose.orientation.z = pose.Rot().Z();
    pub_gt_pose_->publish(gt_pose);

This topic was automatically closed 10 days after the last reply. New replies are no longer allowed.