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);