How can I get z position of hector_quadrotor in ROS?

Question

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 (You can find full version on github source):

void PoseEstimationNode::gpsCallback(const sensor_msgs::NavSatFixConstPtr& gps, const 
geometry_msgs::Vector3StampedConstPtr& gps_velocity) {
  boost::shared_ptr<GPS> m = boost::static_pointer_cast<GPS>(pose_estimation_->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 -----> sensor_pose_.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 - pose_estimation_->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());
}
}

score 0 · Accepted Answer · answered Oct 13 '21 at 14:09

To use a barometer you need to actually add that sensor to your robot. What you're looking at is only the callback meaning such messages are supported. If you want to add a new sensor to your robot I'd suggest looking at this tutorial.

All that being said, if you're getting incorrect altitude values you need to go back and recheck your transforms because the localization built into hector should work fine.

How can I get z position of hector_quadrotor in ROS?

1 Answers1