Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #ifndef _segway_rmp400_odom_alg_node_h_
00026 #define _segway_rmp400_odom_alg_node_h_
00027
00028 #include <iri_base_algorithm/iri_base_algorithm.h>
00029 #include "segway_rmp400_odom_alg.h"
00030
00031 #include <geometry_msgs/PoseWithCovariance.h>
00032 #include <geometry_msgs/TwistWithCovariance.h>
00033 #include <geometry_msgs/Transform.h>
00034
00035 #include <tf/transform_broadcaster.h>
00036
00037 #include <Eigen/Core>
00038
00039
00040 #include <nav_msgs/Odometry.h>
00041 #include <sensor_msgs/Imu.h>
00042 #include <iri_segway_rmp_msgs/SegwayRMP400Status.h>
00043
00044
00045
00046
00047
00052 class SegwayRmp400OdomAlgNode : public algorithm_base::IriBaseAlgorithm<SegwayRmp400OdomAlgorithm>
00053 {
00054 private:
00055
00056 ros::Publisher odom_publisher_;
00057 nav_msgs::Odometry Odometry_msg_;
00058 ros::Publisher odom_rel_publisher_;
00059 nav_msgs::Odometry Odometry_rel_msg_;
00060
00061
00062 ros::Subscriber segway_status_subscriber_;
00063 void segway_status_callback(const iri_segway_rmp_msgs::SegwayRMP400Status::ConstPtr& msg);
00064 CMutex segway_status_mutex_;
00065
00066 ros::Subscriber imu_subscriber_;
00067 void imu_callback(const sensor_msgs::Imu::ConstPtr& msg);
00068 CMutex imu_mutex_;
00069
00070 geometry_msgs::PoseWithCovariance pose_;
00071 geometry_msgs::TwistWithCovariance twist_;
00072 geometry_msgs::Transform transform_;
00073
00074
00075 double vT_;
00076 double r_,vrimu_,rimu_;
00077 double p_,vpimu_,pimu_;
00078 double th_,dth_,vthimu_,thimu_;
00079 double dx_local_accum_;
00080
00081 bool six_d_;
00082 ros::Time last_time_;
00083 ros::Time last_time_imu_;
00084
00085 double sigma_dx_,sigma2_dth_,sigma_thimu_,sigma_dx_fwd_,sigma_dx_turn_;
00086 double offset_fwd_, offset_turn_;
00087 bool first_segway_status_, first_imu_;
00088
00089 Eigen::Matrix3f Jp_,Jd_,Q_,cov_;
00090
00091
00092 std::string tf_prefix_;
00093 std::string odom_id_;
00094 std::string base_link_id_;
00095 bool publish_tf_;
00096 bool publish_odom_rel_;
00097
00098 public:
00105 SegwayRmp400OdomAlgNode(void);
00106
00113 ~SegwayRmp400OdomAlgNode(void);
00114
00115 protected:
00116
00117 tf::TransformBroadcaster odom_broadcaster_;
00118
00131 void mainNodeThread(void);
00132
00145 void node_config_update(Config &config, uint32_t level);
00146
00153 void addNodeDiagnostics(void);
00154
00155
00156
00157
00158 };
00159
00160 #endif