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
00038 #include <nav_msgs/Odometry.h>
00039 #include <sensor_msgs/Imu.h>
00040 #include <iri_segway_rmp_msgs/SegwayRMP400Status.h>
00041
00042
00043
00044
00045
00050 class SegwayRmp400OdomAlgNode : public algorithm_base::IriBaseAlgorithm<SegwayRmp400OdomAlgorithm>
00051 {
00052 private:
00053
00054 ros::Publisher odom_publisher_;
00055 nav_msgs::Odometry Odometry_msg_;
00056
00057
00058 ros::Subscriber segway_status_subscriber_;
00059 void segway_status_callback(const iri_segway_rmp_msgs::SegwayRMP400Status::ConstPtr& msg);
00060 CMutex segway_status_mutex_;
00061
00062 ros::Subscriber imu_subscriber_;
00063 void imu_callback(const sensor_msgs::Imu::ConstPtr& msg);
00064 CMutex imu_mutex_;
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074 double pseudokalman3(std::vector<double> v, std::vector<double> c);
00075
00076
00077
00078 std::vector<double> rk_,pk_,vyk_;
00079 std::vector<double> rkc_,pkc_,vykc_;
00080
00081
00082
00083 double left_wheels_velocity_;
00084 double right_wheels_velocity_;
00085 double yaw_rate_;
00086 double r_,vrimu_,rimu_;
00087 double p_,vpimu_,pimu_;
00088 double y_,vyimu_,yimu_;
00089 bool six_d_;
00090 ros::Time last_time_;
00091 ros::Time current_time_;
00092
00093 geometry_msgs::PoseWithCovariance pose_;
00094 geometry_msgs::TwistWithCovariance twist_;
00095 geometry_msgs::Transform transform_;
00096
00097
00098 std::string tf_prefix_;
00099 std::string odom_id_;
00100 std::string base_link_id_;
00101 bool publish_tf_;
00102
00103 public:
00110 SegwayRmp400OdomAlgNode(void);
00111
00118 ~SegwayRmp400OdomAlgNode(void);
00119
00120 protected:
00121
00122 tf::TransformBroadcaster odom_broadcaster_;
00123
00136 void mainNodeThread(void);
00137
00150 void node_config_update(Config &config, uint32_t level);
00151
00158 void addNodeDiagnostics(void);
00159
00160
00161
00162
00163 };
00164
00165 #endif