Comments (3)
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(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()); } }
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?
from hector-quadrotor-noetic.
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(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()); } }
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.
from hector-quadrotor-noetic.
Let me check it bro! Thanks for your feedback
from hector-quadrotor-noetic.
Related Issues (13)
- I couldn't use the Hector Quadrotor to control the UAV? HOT 2
- hector_quadrotor hovering while using teleop_twist_keyboard HOT 3
- Undefined Tag in kinect_camera.urdf.xacro HOT 2
- Error in building HOT 3
- Invalid Param error while roslaunch hector_quadrotor_gazebo quadrotor_empty_world.launch HOT 8
- Roslaunch is not working HOT 2
- Error loading some assets HOT 1
- Protocol Buffer Error in Arch Linux HOT 1
- Can hector_ui work for Ros Melodic (Ubuntu 18.04) HOT 1
- ERROR: invalid command 'unique_id' HOT 1
- How to control the angular velocity of the UAV HOT 1
- How to restart motor after flipping over HOT 3
Recommend Projects
-
React
A declarative, efficient, and flexible JavaScript library for building user interfaces.
-
Vue.js
🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
-
Typescript
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
-
TensorFlow
An Open Source Machine Learning Framework for Everyone
-
Django
The Web framework for perfectionists with deadlines.
-
Laravel
A PHP framework for web artisans
-
D3
Bring data to life with SVG, Canvas and HTML. 📊📈🎉
-
Recommend Topics
-
javascript
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
-
web
Some thing interesting about web. New door for the world.
-
server
A server is a program made to process requests and deliver data to clients.
-
Machine learning
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
-
Visualization
Some thing interesting about visualization, use data art
-
Game
Some thing interesting about game, make everyone happy.
Recommend Org
-
Facebook
We are working to build community through open source technology. NB: members must have two-factor auth.
-
Microsoft
Open source projects and samples from Microsoft.
-
Google
Google ❤️ Open Source for everyone.
-
Alibaba
Alibaba Open Source for everyone
-
D3
Data-Driven Documents codes.
-
Tencent
China tencent open source team.
from hector-quadrotor-noetic.