30 #include <hector_pose_estimation/ros/parameters.h> 41 #ifdef USE_HECTOR_TIMING 42 #include <hector_diagnostics/timing.h> 50 , transform_listener_(0)
51 , world_nav_transform_updated_(true)
52 , world_nav_transform_valid_(false)
53 , sensor_pose_roll_(0), sensor_pose_pitch_(0), sensor_pose_yaw_(0)
59 #if defined(USE_HECTOR_UAV_MSGS) 79 ROS_WARN(
"Parameter 'publish_world_map_transform' is deprecated. Use 'publish_world_other_transform' and 'other_frame' instead.");
83 ROS_WARN(
"Parameter 'map_frame' is deprecated. Use 'other_frame' instead.");
94 ROS_ERROR(
"Intitialization of pose estimation failed!");
101 #if defined(USE_HECTOR_UAV_MSGS) 130 #ifdef USE_HECTOR_TIMING 177 tf::Vector3 linear_acceleration_body(imu->linear_acceleration.x, imu->linear_acceleration.y, imu->linear_acceleration.z);
200 #if defined(USE_HECTOR_UAV_MSGS) 201 void PoseEstimationNode::baroCallback(
const hector_uav_msgs::AltimeterConstPtr& altimeter) {
203 m->add(
Baro::Update(altimeter->pressure, altimeter->qnh));
211 update(0) = height->point.z;
215 sensor_pose_.pose.position.z = height->point.z - m->getElevation();
224 update.x() = magnetic->vector.x;
225 update.y() = magnetic->vector.y;
226 update.z() = magnetic->vector.z;
237 if (gps->status.status == sensor_msgs::NavSatStatus::STATUS_NO_FIX) {
243 update.latitude = gps->latitude * M_PI/180.0;
244 update.longitude = gps->longitude * M_PI/180.0;
245 update.velocity_north = gps_velocity->vector.x;
246 update.velocity_east = -gps_velocity->vector.y;
250 geometry_msgs::PoseStamped gps_pose;
252 gps_pose.header.stamp = gps->header.stamp;
256 gps_pose.pose.position.x = y(0);
257 gps_pose.pose.position.y = y(1);
259 double track = atan2(gps_velocity->vector.y, gps_velocity->vector.x);
260 gps_pose.pose.orientation.w = cos(track/2);
261 gps_pose.pose.orientation.z = sin(track/2);
274 if (pose->pose.covariance[0] > 0)
sensor_pose_.pose.position.x = pose->pose.pose.position.x;
275 if (pose->pose.covariance[7] > 0)
sensor_pose_.pose.position.y = pose->pose.pose.position.y;
276 if (pose->pose.covariance[14] > 0)
sensor_pose_.pose.position.z = pose->pose.pose.position.z;
278 double yaw, pitch, roll;
292 if (syscommand->data ==
"reset") {
293 ROS_INFO(
"Resetting pose_estimation");
300 geographic_msgs::GeoPose geopose;
322 nav_msgs::Odometry state;
328 geometry_msgs::PoseStamped pose_msg;
334 sensor_msgs::Imu imu_msg;
342 geometry_msgs::Vector3Stamped velocity_msg;
348 geographic_msgs::GeoPose geopose_msg;
354 sensor_msgs::NavSatFix global_msg;
360 geometry_msgs::Vector3Stamped euler_msg;
368 geometry_msgs::Vector3Stamped angular_velocity_msg, linear_acceleration_msg;
397 ROS_WARN(
"Could not find a transformation from %s to %s to publish the world transformation", nav_frame.c_str(),
other_frame_.c_str());
410 #ifdef USE_HECTOR_TIMING message_filters::TimeSynchronizer< sensor_msgs::NavSatFix, geometry_msgs::Vector3Stamped > * gps_synchronizer_
ros::Publisher global_fix_publisher_
const SystemPtr & addSystem(const SystemPtr &system, const std::string &name="system")
PoseEstimation * pose_estimation_
bool world_nav_transform_updated_
void twistupdateCallback(const geometry_msgs::TwistWithCovarianceStampedConstPtr &twist)
ros::Publisher geopose_publisher_
void publish(const boost::shared_ptr< M > &message) const
bool publish_covariances_
std::string getPrefixParam(ros::NodeHandle &nh)
virtual const GlobalReferencePtr & globalReference()
ros::Duration publish_world_nav_transform_period_
ros::Publisher pose_publisher_
ros::Subscriber imu_subscriber_
void setOrientation(const Quaternion &orientation)
virtual ParameterList & parameters()
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
virtual const State & state() const
void getEulerYPR(tfScalar &yaw, tfScalar &pitch, tfScalar &roll, unsigned int solution_number=1) const
ros::Subscriber rollpitch_subscriber_
T & getAs(const std::string &key) const
virtual void getPose(tf::Pose &pose)
tf::TransformListener * getTransformListener()
virtual void updateWorldToOtherTransform(tf::StampedTransform &world_to_other_transform)
MeasurementPtr getMeasurement(const std::string &name) const
void syscommandCallback(const std_msgs::StringConstPtr &syscommand)
ros::Publisher velocity_publisher_
ros::Publisher imu_publisher_
virtual ros::NodeHandle & getPrivateNodeHandle()
void gpsCallback(const sensor_msgs::NavSatFixConstPtr &gps, const geometry_msgs::Vector3StampedConstPtr &gps_velocity)
tf::TransformBroadcaster * getTransformBroadcaster()
geometry_msgs::TransformStamped world_nav_transform_
void imuCallback(const sensor_msgs::ImuConstPtr &imu)
TFSIMD_FORCE_INLINE const tfScalar & y() const
virtual ros::NodeHandle & getNodeHandle()
ros::Publisher angular_velocity_bias_publisher_
InputPtr setInput(const Input &input, std::string name=std::string())
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
std::string resolve(const std::string &prefix, const std::string &frame_name)
message_filters::Subscriber< sensor_msgs::NavSatFix > gps_subscriber_
TFSIMD_FORCE_INLINE const tfScalar & x() const
Eigen::Quaternion< ScalarType > Quaternion
PoseEstimationNode(const SystemPtr &system=SystemPtr(), const StatePtr &state=StatePtr())
virtual void getImuWithBiases(geometry_msgs::Vector3 &linear_acceleration, geometry_msgs::Vector3 &angular_velocity)
virtual void getTransforms(std::vector< tf::StampedTransform > &transforms)
Connection registerCallback(C &callback)
ros::Publisher linear_acceleration_bias_publisher_
TFSIMD_FORCE_INLINE const tfScalar & z() const
ros::Timer publish_world_nav_transform_timer_
void setRPY(const tfScalar &roll, const tfScalar &pitch, const tfScalar &yaw)
static void quaternionMsgToTF(const geometry_msgs::Quaternion &msg, Quaternion &bt)
virtual void getHeader(std_msgs::Header &header)
ros::Publisher sensor_pose_publisher_
tf::TransformListener * transform_listener_
static void quaternionTFToMsg(const Quaternion &bt, geometry_msgs::Quaternion &msg)
void update(ros::Time timestamp)
Model::MeasurementVector MeasurementVector
TFSIMD_FORCE_INLINE const tfScalar & y() const
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
std::vector< tf::StampedTransform > transforms_
ros::Subscriber poseupdate_subscriber_
ros::Subscriber magnetic_subscriber_
void poseupdateCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr &pose)
ros::Subscriber syscommand_subscriber_
bool publish_world_nav_transform_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
const MeasurementPtr & addMeasurement(const MeasurementPtr &measurement, const std::string &name=std::string())
ros::Publisher euler_publisher_
void ahrsCallback(const sensor_msgs::ImuConstPtr &ahrs)
ros::Publisher timing_publisher_
virtual void getState(nav_msgs::Odometry &state, bool with_covariances=true)
boost::shared_ptr< InputType > addInput(const std::string &name=std::string())
TFSIMD_FORCE_INLINE Vector3 & normalize()
traits::Update< BaroModel >::type Update
ros::Subscriber height_subscriber_
uint32_t getNumSubscribers() const
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
virtual bool getWorldToNavTransform(geometry_msgs::TransformStamped &transform)
void publishWorldNavTransform(const ros::TimerEvent &=ros::TimerEvent())
bool getParam(const std::string &key, std::string &s) const
ros::Subscriber ahrs_subscriber_
void magneticCallback(const geometry_msgs::Vector3StampedConstPtr &magnetic)
virtual void getVelocity(tf::Vector3 &vector)
geometry_msgs::PoseStamped sensor_pose_
void setRollPitch(const Quaternion &orientation)
void globalReferenceUpdated()
void heightCallback(const geometry_msgs::PointStampedConstPtr &height)
message_filters::Subscriber< geometry_msgs::Vector3Stamped > gps_velocity_subscriber_
virtual void getBias(geometry_msgs::Vector3 &angular_velocity, geometry_msgs::Vector3 &linear_acceleration)
ros::Publisher state_publisher_
ros::Subscriber twistupdate_subscriber_
double sensor_pose_pitch_
virtual ~PoseEstimationNode()
tf::TransformBroadcaster transform_broadcaster_
virtual void getGlobal(double &latitude, double &longitude, double &altitude)
void rollpitchCallback(const sensor_msgs::ImuConstPtr &attitude)
bool world_nav_transform_valid_
virtual void getOrientation(tf::Quaternion &quaternion)
ros::Publisher gps_pose_publisher_
bool publish_world_other_transform_
void initialize(ParameterRegisterFunc func) const
ros::Publisher global_reference_publisher_