Closed UcanYusuf closed 2 years ago
Let me check it bro! Thanks for your feedback
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?
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.
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 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: