#include <pose_estimation.h>
Public Member Functions | |
| void | doFlatTrim () |
| calls flattrim from ardrone_autonomy | |
| void | doReset () |
| performs the reset | |
| PoseEstimator () | |
| Constructor. | |
| void | publish_end_reset_pose () |
| sends message at the end of the reset time | |
| void | publish_pose () |
| sends message with the fused pose | |
| ~PoseEstimator () | |
| Destructor. | |
Public Attributes | |
| bool | odometry_publishing |
| true after the first Odometry message is received | |
| bool | pending_reset |
| true during the reset phase | |
| bool | visual_pose_available |
| true when the last visual pose message is not yet processed | |
Private Member Functions | |
| void | navdataCb (const ardrone_autonomy::Navdata::ConstPtr navdataPtr) |
| Callback: do a copy of last Navdata message. | |
| void | odometryCb (const nav_msgs::Odometry::ConstPtr odometryPtr) |
| Callback: do a copy of last Odometry message. | |
| bool | poseCopy (ucl_drone::Pose3D &pose_msg) |
| Simple copy of Odometry. | |
| bool | poseFusion (ucl_drone::Pose3D &pose_msg) |
| Copy of Odometry behalve for (x,y) position, the integration of velocities is recomputed. | |
| void | poseVisualCb (const ucl_drone::Pose3D::ConstPtr poseVisualPtr) |
| Callback: do a copy of last visual pose message. | |
| void | processQueue (std::queue< std::vector< double > > &myqueue, double &result) |
| Sum all the values contained in myqueue (used in queuePoseFusion) | |
| void | pushQueue (std::queue< std::vector< double > > &myqueue, double item) |
| Add item to myqueue (used in queuePoseFusion) | |
| bool | queuePoseFusion (ucl_drone::Pose3D &pose_msg) |
| Simple fusion between Odometry and visual pose estimation. | |
| void | resetCb (const std_msgs::Empty msg) |
| Callback: Listen to reset message and switch reset on. | |
Private Attributes | |
| std::string | end_reset_pose_channel |
| ros::Publisher | end_reset_pose_pub |
| Publisher of signal for the reset ending. | |
| std::string | imu_channel |
| ardrone_autonomy::Navdata | lastNavdataReceived |
| Copy of the last Navdata message received from ardrone_autonomy. | |
| nav_msgs::Odometry | lastOdometryReceived |
| Copy of the last Odometry message received from ardrone_autonomy. | |
| ucl_drone::Pose3D | lastposeVisualReceived |
| Copy of the last visual pose computed in map node. | |
| double | lastRotX |
| Copy of the last rotX received in Odometry. | |
| double | lastRotY |
| Copy of the last rotY received in Odometry. | |
| double | lastRotZ |
| Copy of the last rotZ received in Odometry. | |
| std::string | navdata_channel |
| ros::Subscriber | navdata_sub |
| Subscriber to Navdata messages from ardrone_autonomy. | |
| ros::NodeHandle | nh |
| ros::Time | odom_time |
| std::string | odometry_channel |
| double | odometry_rotX |
| Accumulator for the rotX angle. | |
| double | odometry_rotY |
| double | odometry_rotZ |
| ros::Subscriber | odometry_sub |
| Subscriber to Odometry messages from ardrone_autonomy. | |
| double | odometry_x |
| Accumulator for the x direction. | |
| double | odometry_y |
| Accumulator for the y direction. | |
| std::string | pose_channel |
| ros::Publisher | pose_pub |
| Publisher of fused pose estimation. | |
| std::string | pose_visual_channel |
| std::string | pose_visual_correction_channel |
| ros::Subscriber | pose_visual_sub |
| Subscriber to visual pose messages from mapping node. | |
| double | previous_rotX |
| Copy of the previous rotX received in Odometry. | |
| double | previous_rotY |
| Copy of the previous rotY received in Odometry. | |
| double | previous_rotZ |
| Copy of the previous rotZ received in Odometry. | |
| std::queue< std::vector< double > > | queue_drotZ |
| std::queue< std::vector< double > > | queue_dx |
| std::queue< std::vector< double > > | queue_dy |
| std::string | reset_channel |
| ros::Subscriber | reset_sub |
| Subscriber to Reset messages (e.g. from gui) | |
| double | rot_Z_offset |
| Offset of rotZ from ardrone_autonomy at reset. | |
| bool | use_visual_pose |
| if false only information from ardrone_autonomy is used | |
This class defines an object which wraps all method to perform the pose estimation based on fusion between sensors and visual information
Definition at line 42 of file pose_estimation.h.
Constructor.
This file is part of ucl_drone 2016. For more information, refer to the corresponding header file.
Definition at line 15 of file pose_estimation.cpp.
Destructor.
Definition at line 66 of file pose_estimation.cpp.
| void PoseEstimator::doFlatTrim | ( | ) |
calls flattrim from ardrone_autonomy
Definition at line 77 of file pose_estimation.cpp.
| void PoseEstimator::doReset | ( | ) |
performs the reset
Definition at line 85 of file pose_estimation.cpp.
| void PoseEstimator::navdataCb | ( | const ardrone_autonomy::Navdata::ConstPtr | navdataPtr | ) | [private] |
Callback: do a copy of last Navdata message.
Definition at line 100 of file pose_estimation.cpp.
| void PoseEstimator::odometryCb | ( | const nav_msgs::Odometry::ConstPtr | odometryPtr | ) | [private] |
Callback: do a copy of last Odometry message.
Definition at line 123 of file pose_estimation.cpp.
| bool PoseEstimator::poseCopy | ( | ucl_drone::Pose3D & | pose_msg | ) | [private] |
Simple copy of Odometry.
Definition at line 174 of file pose_estimation.cpp.
| bool PoseEstimator::poseFusion | ( | ucl_drone::Pose3D & | pose_msg | ) | [private] |
Copy of Odometry behalve for (x,y) position, the integration of velocities is recomputed.
Definition at line 199 of file pose_estimation.cpp.
| void PoseEstimator::poseVisualCb | ( | const ucl_drone::Pose3D::ConstPtr | poseVisualPtr | ) | [private] |
Callback: do a copy of last visual pose message.
Definition at line 107 of file pose_estimation.cpp.
| void PoseEstimator::processQueue | ( | std::queue< std::vector< double > > & | myqueue, |
| double & | result | ||
| ) | [private] |
Sum all the values contained in myqueue (used in queuePoseFusion)
Definition at line 375 of file pose_estimation.cpp.
sends message at the end of the reset time
Definition at line 147 of file pose_estimation.cpp.
| void PoseEstimator::publish_pose | ( | ) |
sends message with the fused pose
Definition at line 154 of file pose_estimation.cpp.
| void PoseEstimator::pushQueue | ( | std::queue< std::vector< double > > & | myqueue, |
| double | item | ||
| ) | [private] |
Add item to myqueue (used in queuePoseFusion)
Definition at line 363 of file pose_estimation.cpp.
| bool PoseEstimator::queuePoseFusion | ( | ucl_drone::Pose3D & | pose_msg | ) | [private] |
Simple fusion between Odometry and visual pose estimation.
Definition at line 268 of file pose_estimation.cpp.
| void PoseEstimator::resetCb | ( | const std_msgs::Empty | msg | ) | [private] |
Callback: Listen to reset message and switch reset on.
Definition at line 70 of file pose_estimation.cpp.
std::string PoseEstimator::end_reset_pose_channel [private] |
Definition at line 61 of file pose_estimation.h.
Publisher of signal for the reset ending.
Definition at line 55 of file pose_estimation.h.
std::string PoseEstimator::imu_channel [private] |
Definition at line 64 of file pose_estimation.h.
ardrone_autonomy::Navdata PoseEstimator::lastNavdataReceived [private] |
Copy of the last Navdata message received from ardrone_autonomy.
Definition at line 68 of file pose_estimation.h.
nav_msgs::Odometry PoseEstimator::lastOdometryReceived [private] |
Copy of the last Odometry message received from ardrone_autonomy.
Definition at line 71 of file pose_estimation.h.
ucl_drone::Pose3D PoseEstimator::lastposeVisualReceived [private] |
Copy of the last visual pose computed in map node.
Definition at line 74 of file pose_estimation.h.
double PoseEstimator::lastRotX [private] |
Copy of the last rotX received in Odometry.
Definition at line 86 of file pose_estimation.h.
double PoseEstimator::lastRotY [private] |
Copy of the last rotY received in Odometry.
Definition at line 87 of file pose_estimation.h.
double PoseEstimator::lastRotZ [private] |
Copy of the last rotZ received in Odometry.
Definition at line 88 of file pose_estimation.h.
std::string PoseEstimator::navdata_channel [private] |
Definition at line 58 of file pose_estimation.h.
ros::Subscriber PoseEstimator::navdata_sub [private] |
Subscriber to Navdata messages from ardrone_autonomy.
Definition at line 48 of file pose_estimation.h.
ros::NodeHandle PoseEstimator::nh [private] |
Definition at line 45 of file pose_estimation.h.
ros::Time PoseEstimator::odom_time [private] |
Definition at line 76 of file pose_estimation.h.
std::string PoseEstimator::odometry_channel [private] |
Definition at line 59 of file pose_estimation.h.
true after the first Odometry message is received
Definition at line 129 of file pose_estimation.h.
double PoseEstimator::odometry_rotX [private] |
Accumulator for the rotX angle.
Definition at line 81 of file pose_estimation.h.
double PoseEstimator::odometry_rotY [private] |
Definition at line 82 of file pose_estimation.h.
double PoseEstimator::odometry_rotZ [private] |
Definition at line 83 of file pose_estimation.h.
ros::Subscriber PoseEstimator::odometry_sub [private] |
Subscriber to Odometry messages from ardrone_autonomy.
Definition at line 49 of file pose_estimation.h.
double PoseEstimator::odometry_x [private] |
Accumulator for the x direction.
Definition at line 77 of file pose_estimation.h.
double PoseEstimator::odometry_y [private] |
Accumulator for the y direction.
Definition at line 79 of file pose_estimation.h.
true during the reset phase
Definition at line 131 of file pose_estimation.h.
std::string PoseEstimator::pose_channel [private] |
Definition at line 60 of file pose_estimation.h.
ros::Publisher PoseEstimator::pose_pub [private] |
Publisher of fused pose estimation.
Definition at line 54 of file pose_estimation.h.
std::string PoseEstimator::pose_visual_channel [private] |
Definition at line 62 of file pose_estimation.h.
std::string PoseEstimator::pose_visual_correction_channel [private] |
Definition at line 63 of file pose_estimation.h.
Subscriber to visual pose messages from mapping node.
Definition at line 50 of file pose_estimation.h.
double PoseEstimator::previous_rotX [private] |
Copy of the previous rotX received in Odometry.
Definition at line 89 of file pose_estimation.h.
double PoseEstimator::previous_rotY [private] |
Copy of the previous rotY received in Odometry.
Definition at line 90 of file pose_estimation.h.
double PoseEstimator::previous_rotZ [private] |
Copy of the previous rotZ received in Odometry.
Definition at line 91 of file pose_estimation.h.
std::queue< std::vector< double > > PoseEstimator::queue_drotZ [private] |
Definition at line 84 of file pose_estimation.h.
std::queue< std::vector< double > > PoseEstimator::queue_dx [private] |
Definition at line 78 of file pose_estimation.h.
std::queue< std::vector< double > > PoseEstimator::queue_dy [private] |
Definition at line 80 of file pose_estimation.h.
std::string PoseEstimator::reset_channel [private] |
Definition at line 65 of file pose_estimation.h.
ros::Subscriber PoseEstimator::reset_sub [private] |
Subscriber to Reset messages (e.g. from gui)
Definition at line 51 of file pose_estimation.h.
double PoseEstimator::rot_Z_offset [private] |
Offset of rotZ from ardrone_autonomy at reset.
Definition at line 85 of file pose_estimation.h.
bool PoseEstimator::use_visual_pose [private] |
if false only information from ardrone_autonomy is used
Definition at line 120 of file pose_estimation.h.
true when the last visual pose message is not yet processed
Definition at line 130 of file pose_estimation.h.