How can I get Z axis on rviz?

[UcanYusuf]

I am trying to get z position of hector_quadrotor in simulation. I can get X and Y axis coordinates but I couldn't get Z coordinate. I tried to get it by using GPS but the values are not correct. So I want to get Z coordinate by using barometer or another sensor.

Here the a part of pose_estimation_node.cpp:

void PoseEstimationNode::gpsCallback(const sensor_msgs::NavSatFixConstPtr& gps, const geometry_msgs::Vector3StampedConstPtr& gps_velocity) { boost::shared_ptr m = boost::static_pointer_cast(poseestimation->getMeasurement("gps"));

if (gps->status.status == sensor_msgs::NavSatStatus::STATUS_NO_FIX) {
  if (m->getStatusFlags() > 0) m->reset(pose_estimation_->state());
  return;
}

GPS::Update update;
update.latitude = gps->latitude * M_PI/180.0;
update.longitude = gps->longitude * M_PI/180.0;
update.velocity_north =  gps_velocity->vector.x;
update.velocity_east  = -gps_velocity->vector.y;
m->add(update);

if (gps_pose_publisher_ || sensor_pose_publisher_) {
  geometry_msgs::PoseStamped gps_pose;
  pose_estimation_->getHeader(gps_pose.header);
  gps_pose.header.stamp = gps->header.stamp;
  GPSModel::MeasurementVector y = m->getVector(update, pose_estimation_->state());

  if (gps_pose_publisher_) {
    gps_pose.pose.position.x = y(0);
    gps_pose.pose.position.y = y(1);
    gps_pose.pose.position.z = gps->altitude - pose_estimation_->globalReference()- >position().altitude;
    double track = atan2(gps_velocity->vector.y, gps_velocity->vector.x);
    gps_pose.pose.orientation.w = cos(track/2);
    gps_pose.pose.orientation.z = sin(track/2);
    gps_pose_publisher_.publish(gps_pose);
  }

  sensor_pose_.pose.position.x = y(0);
  sensor_pose_.pose.position.y = y(1);
  """I add it here""" 
}

If I add -----> sensorpose.pose.position.z = gps->altitude,

I can get a Z coordinate on RVIZ simulation or gnome-terminal. But as I said, the values are very meanless (negative values).

Also ------> gps_pose.pose.position.z = gps->altitude - poseestimation->globalReference()- >position().altitude;

It is not working because position().altitude return NAN. There are another measurement method in pose_estimation_node.cpp like barometer. How can I use barometer value.

Here the another part of pose_estimation_node.cpp:

#if defined(USE_HECTOR_UAV_MSGS)
void PoseEstimationNode::baroCallback(const hector_uav_msgs::AltimeterConstPtr& 
altimeter) {
  boost::shared_ptr<Baro> m = boost::static_pointer_cast<Baro>(pose_estimation_->getMeasurement("baro"));
  m->add(Baro::Update(altimeter->pressure, altimeter->qnh));
}

#else
void PoseEstimationNode::heightCallback(const geometry_msgs::PointStampedConstPtr& 
height) {
boost::shared_ptr<Height> m = boost::static_pointer_cast<Height>(pose_estimation_->getMeasurement("height"));

Height::MeasurementVector update;
update(0) = height->point.z;
m->add(Height::Update(update));

if (sensor_pose_publisher_) {
sensor_pose_.pose.position.z = height->point.z - m->getElevation();
  }
}
#endif

void PoseEstimationNode::magneticCallback(const geometry_msgs::Vector3StampedConstPtr& magnetic) {
boost::shared_ptr<Magnetic> m = boost::static_pointer_cast<Magnetic>(pose_estimation_->getMeasurement("magnetic"));

Magnetic::MeasurementVector update;
update.x() = magnetic->vector.x;
update.y() = magnetic->vector.y;
update.z() = magnetic->vector.z;
m->add(Magnetic::Update(update));

if (sensor_pose_publisher_) {
sensor_pose_yaw_ = -(m->getModel()->getTrueHeading(pose_estimation_->state(), update) - pose_estimation_->globalReference()->heading());
}
}

[RAFALAMAO]

Let me check it bro! Thanks for your feedback

[RAFALAMAO]

I am trying to get z position of hector_quadrotor in simulation. I can get X and Y axis coordinates but I couldn't get Z coordinate. I tried to get it by using GPS but the values are not correct. So I want to get Z coordinate by using barometer or another sensor.

Here the a part of pose_estimation_node.cpp:

void PoseEstimationNode::gpsCallback(const sensor_msgs::NavSatFixConstPtr& gps, const geometry_msgs::Vector3StampedConstPtr& gps_velocity) { boost::shared_ptr m = boost::static_pointer_cast(poseestimation->getMeasurement("gps"));

if (gps->status.status == sensor_msgs::NavSatStatus::STATUS_NO_FIX) {
  if (m->getStatusFlags() > 0) m->reset(pose_estimation_->state());
  return;
}

GPS::Update update;
update.latitude = gps->latitude * M_PI/180.0;
update.longitude = gps->longitude * M_PI/180.0;
update.velocity_north =  gps_velocity->vector.x;
update.velocity_east  = -gps_velocity->vector.y;
m->add(update);

if (gps_pose_publisher_ || sensor_pose_publisher_) {
  geometry_msgs::PoseStamped gps_pose;
  pose_estimation_->getHeader(gps_pose.header);
  gps_pose.header.stamp = gps->header.stamp;
  GPSModel::MeasurementVector y = m->getVector(update, pose_estimation_->state());

  if (gps_pose_publisher_) {
    gps_pose.pose.position.x = y(0);
    gps_pose.pose.position.y = y(1);
    gps_pose.pose.position.z = gps->altitude - pose_estimation_->globalReference()- >position().altitude;
    double track = atan2(gps_velocity->vector.y, gps_velocity->vector.x);
    gps_pose.pose.orientation.w = cos(track/2);
    gps_pose.pose.orientation.z = sin(track/2);
    gps_pose_publisher_.publish(gps_pose);
  }

  sensor_pose_.pose.position.x = y(0);
  sensor_pose_.pose.position.y = y(1);
  """I add it here""" 
}

If I add -----> sensorpose.pose.position.z = gps->altitude,

I can get a Z coordinate on RVIZ simulation or gnome-terminal. But as I said, the values are very meanless (negative values).

Also ------> gps_pose.pose.position.z = gps->altitude - poseestimation->globalReference()- >position().altitude;

It is not working because position().altitude return NAN. There are another measurement method in pose_estimation_node.cpp like barometer. How can I use barometer value.

Here the another part of pose_estimation_node.cpp:

#if defined(USE_HECTOR_UAV_MSGS)
void PoseEstimationNode::baroCallback(const hector_uav_msgs::AltimeterConstPtr& 
altimeter) {
  boost::shared_ptr<Baro> m = boost::static_pointer_cast<Baro>(pose_estimation_->getMeasurement("baro"));
  m->add(Baro::Update(altimeter->pressure, altimeter->qnh));
}

#else
void PoseEstimationNode::heightCallback(const geometry_msgs::PointStampedConstPtr& 
height) {
boost::shared_ptr<Height> m = boost::static_pointer_cast<Height>(pose_estimation_->getMeasurement("height"));

Height::MeasurementVector update;
update(0) = height->point.z;
m->add(Height::Update(update));

if (sensor_pose_publisher_) {
sensor_pose_.pose.position.z = height->point.z - m->getElevation();
  }
}
#endif

void PoseEstimationNode::magneticCallback(const geometry_msgs::Vector3StampedConstPtr& magnetic) {
boost::shared_ptr<Magnetic> m = boost::static_pointer_cast<Magnetic>(pose_estimation_->getMeasurement("magnetic"));

Magnetic::MeasurementVector update;
update.x() = magnetic->vector.x;
update.y() = magnetic->vector.y;
update.z() = magnetic->vector.z;
m->add(Magnetic::Update(update));

if (sensor_pose_publisher_) {
sensor_pose_yaw_ = -(m->getModel()->getTrueHeading(pose_estimation_->state(), update) - pose_estimation_->globalReference()->heading());
}
}

Bro, but you can get pose (x,y,z) from topic "/ground_truth_to_tf/pose" and if you want in RVIZ, you can suscribe to mentioned topic...

Or, what do you want to do?

[UcanYusuf]

I am trying to get z position of hector_quadrotor in simulation. I can get X and Y axis coordinates but I couldn't get Z coordinate. I tried to get it by using GPS but the values are not correct. So I want to get Z coordinate by using barometer or another sensor. Here the a part of pose_estimation_node.cpp: void PoseEstimationNode::gpsCallback(const sensor_msgs::NavSatFixConstPtr& gps, const geometry_msgs::Vector3StampedConstPtr& gps_velocity) { boost::shared_ptr m = boost::static_pointer_cast(poseestimation->getMeasurement("gps"));

if (gps->status.status == sensor_msgs::NavSatStatus::STATUS_NO_FIX) {
  if (m->getStatusFlags() > 0) m->reset(pose_estimation_->state());
  return;
}

GPS::Update update;
update.latitude = gps->latitude * M_PI/180.0;
update.longitude = gps->longitude * M_PI/180.0;
update.velocity_north =  gps_velocity->vector.x;
update.velocity_east  = -gps_velocity->vector.y;
m->add(update);

if (gps_pose_publisher_ || sensor_pose_publisher_) {
  geometry_msgs::PoseStamped gps_pose;
  pose_estimation_->getHeader(gps_pose.header);
  gps_pose.header.stamp = gps->header.stamp;
  GPSModel::MeasurementVector y = m->getVector(update, pose_estimation_->state());

  if (gps_pose_publisher_) {
    gps_pose.pose.position.x = y(0);
    gps_pose.pose.position.y = y(1);
    gps_pose.pose.position.z = gps->altitude - pose_estimation_->globalReference()- >position().altitude;
    double track = atan2(gps_velocity->vector.y, gps_velocity->vector.x);
    gps_pose.pose.orientation.w = cos(track/2);
    gps_pose.pose.orientation.z = sin(track/2);
    gps_pose_publisher_.publish(gps_pose);
  }

  sensor_pose_.pose.position.x = y(0);
  sensor_pose_.pose.position.y = y(1);
  """I add it here""" 
}

If I add -----> sensorpose.pose.position.z = gps->altitude, I can get a Z coordinate on RVIZ simulation or gnome-terminal. But as I said, the values are very meanless (negative values). Also ------> gps_pose.pose.position.z = gps->altitude - poseestimation->globalReference()- >position().altitude; It is not working because position().altitude return NAN. There are another measurement method in pose_estimation_node.cpp like barometer. How can I use barometer value. Here the another part of pose_estimation_node.cpp:

#if defined(USE_HECTOR_UAV_MSGS)
void PoseEstimationNode::baroCallback(const hector_uav_msgs::AltimeterConstPtr& 
altimeter) {
  boost::shared_ptr<Baro> m = boost::static_pointer_cast<Baro>(pose_estimation_->getMeasurement("baro"));
  m->add(Baro::Update(altimeter->pressure, altimeter->qnh));
}

#else
void PoseEstimationNode::heightCallback(const geometry_msgs::PointStampedConstPtr& 
height) {
boost::shared_ptr<Height> m = boost::static_pointer_cast<Height>(pose_estimation_->getMeasurement("height"));

Height::MeasurementVector update;
update(0) = height->point.z;
m->add(Height::Update(update));

if (sensor_pose_publisher_) {
sensor_pose_.pose.position.z = height->point.z - m->getElevation();
  }
}
#endif

void PoseEstimationNode::magneticCallback(const geometry_msgs::Vector3StampedConstPtr& magnetic) {
boost::shared_ptr<Magnetic> m = boost::static_pointer_cast<Magnetic>(pose_estimation_->getMeasurement("magnetic"));

Magnetic::MeasurementVector update;
update.x() = magnetic->vector.x;
update.y() = magnetic->vector.y;
update.z() = magnetic->vector.z;
m->add(Magnetic::Update(update));

if (sensor_pose_publisher_) {
sensor_pose_yaw_ = -(m->getModel()->getTrueHeading(pose_estimation_->state(), update) - pose_estimation_->globalReference()->heading());
}
}

Bro, but you can get pose (x,y,z) from topic "/ground_truth_to_tf/pose" and if you want in RVIZ, you can suscribe to mentioned topic...

Or, what do you want to do?

Oh thank you for this. Actually I didn't realize that topic. I will try it.