00001 // Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC. 00002 // Author 00003 // All rights reserved. 00004 // 00005 // This file is part of iri-ros-pkg 00006 // iri-ros-pkg is free software: you can redistribute it and/or modify 00007 // it under the terms of the GNU Lesser General Public License as published by 00008 // the Free Software Foundation, either version 3 of the License, or 00009 // at your option) any later version. 00010 // 00011 // This program is distributed in the hope that it will be useful, 00012 // but WITHOUT ANY WARRANTY; without even the implied warranty of 00013 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00014 // GNU Lesser General Public License for more details. 00015 // 00016 // You should have received a copy of the GNU Lesser General Public License 00017 // along with this program. If not, see <http://www.gnu.org/licenses/>. 00018 // 00019 // IMPORTANT NOTE: This code has been generated through a script from the 00020 // iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness 00021 // of the scripts. ROS topics can be easly add by using those scripts. Please 00022 // refer to the IRI wiki page for more information: 00023 // http://wikiri.upc.es/index.php/Robotics_Lab 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 #include <Eigen/Geometry> 00039 00040 #ifndef PI 00041 #define PI 3.141592 00042 #endif 00043 00044 // [publisher subscriber headers] 00045 #include <nav_msgs/Odometry.h> 00046 #include <iri_segway_rmp_msgs/SegwayRMP400Status.h> 00047 00048 // [service client headers] 00049 00050 // [action server client headers] 00051 00056 class SegwayRmp400OdomAlgNode : public algorithm_base::IriBaseAlgorithm<SegwayRmp400OdomAlgorithm> 00057 { 00058 private: 00059 // [publisher attributes] 00060 ros::Publisher odom_publisher_; 00061 nav_msgs::Odometry Odometry_msg_; 00062 00063 // [subscriber attributes] 00064 ros::Subscriber segway_status_subscriber_; 00065 void segway_status_callback(const iri_segway_rmp_msgs::SegwayRMP400Status::ConstPtr& msg); 00066 CMutex segway_status_mutex_; 00067 00068 // [service attributes] 00069 00070 // [client attributes] 00071 00072 // [action server attributes] 00073 00074 // [action client attributes] 00075 00076 // odometry variables 00077 double left_wheels_velocity_; 00078 double right_wheels_velocity_; 00079 double yaw_rate_; 00080 double pitch_rate_; 00081 double roll_rate_; 00082 geometry_msgs::Transform accum_; 00083 Eigen::Matrix4f H; 00084 Eigen::Vector3f w; 00085 Eigen::Vector3f wt; 00086 Eigen::Vector3f ww; 00087 Eigen::Vector3f v; 00088 Eigen::Matrix3f wt_hat; 00089 double wt_norm; 00090 double epsilon; 00091 Eigen::Matrix3f R; 00092 Eigen::Matrix3f I; 00093 Eigen::Vector3f t; 00094 Eigen::Vector3f ta, tb; 00095 Eigen::Matrix4f g; 00096 Eigen::Vector4f d; 00097 Eigen::Vector4f q; // quaternion vector in q(0)-q(2), scalar part in q(3) 00098 geometry_msgs::PoseWithCovariance pose_; 00099 geometry_msgs::TwistWithCovariance twist_; 00100 geometry_msgs::Transform transform_; 00101 00102 // parameter variables 00103 std::string tf_prefix_; 00104 std::string odom_id_; 00105 std::string base_link_id_; 00106 bool publish_tf_; 00107 00108 public: 00115 SegwayRmp400OdomAlgNode(void); 00116 00123 ~SegwayRmp400OdomAlgNode(void); 00124 00125 protected: 00126 00127 tf::TransformBroadcaster odom_broadcaster_; 00128 00141 void mainNodeThread(void); 00142 00155 void node_config_update(Config &config, uint32_t level); 00156 00163 void addNodeDiagnostics(void); 00164 00165 // [diagnostic functions] 00166 00167 // [test functions] 00168 }; 00169 00170 #endif