odometer_base.h
Go to the documentation of this file.
00001 
00002 #ifndef ODOMETER_BASE_H_
00003 #define ODOMETER_BASE_H_
00004 
00005 #include <ros/ros.h>
00006 #include <nav_msgs/Odometry.h>
00007 #include <std_srvs/Empty.h>
00008 
00009 #include <tf/transform_listener.h>
00010 #include <tf/transform_broadcaster.h>
00011 
00012 namespace viso2_ros
00013 {
00014 
00020 class OdometerBase
00021 {
00022 
00023 private:
00024 
00025   // publisher
00026   ros::Publisher odom_pub_;
00027   ros::Publisher pose_pub_;
00028 
00029   ros::ServiceServer reset_service_;
00030 
00031   // tf related
00032   std::string sensor_frame_id_;
00033   std::string odom_frame_id_;
00034   std::string base_link_frame_id_;
00035   tf::TransformListener tf_listener_;
00036   tf::TransformBroadcaster tf_broadcaster_;
00037   bool publish_tf_;
00038 
00039   // the current integrated camera pose
00040   tf::Transform integrated_pose_;
00041   // timestamp of the last update
00042   ros::Time last_update_time_;
00043 
00044   // covariances
00045   boost::array<double, 36> pose_covariance_;
00046   boost::array<double, 36> twist_covariance_;
00047 
00048 public:
00049 
00050   OdometerBase()
00051   {
00052     // Read local parameters
00053     ros::NodeHandle local_nh("~");
00054 
00055     local_nh.param("odom_frame_id", odom_frame_id_, std::string("/odom"));
00056     local_nh.param("base_link_frame_id", base_link_frame_id_, std::string("/base_link"));
00057     local_nh.param("sensor_frame_id", sensor_frame_id_, std::string("/camera"));
00058     local_nh.param("publish_tf", publish_tf_, true);
00059 
00060     ROS_INFO_STREAM("Basic Odometer Settings:" << std::endl <<
00061                     "  odom_frame_id      = " << odom_frame_id_ << std::endl <<
00062                     "  base_link_frame_id = " << base_link_frame_id_ << std::endl <<
00063                     "  publish_tf         = " << (publish_tf_?"true":"false"));
00064     
00065     // advertise
00066     odom_pub_ = local_nh.advertise<nav_msgs::Odometry>("odometry", 1);
00067     pose_pub_ = local_nh.advertise<geometry_msgs::PoseStamped>("pose", 1);
00068 
00069     reset_service_ = local_nh.advertiseService("reset_pose", &OdometerBase::resetPose, this);
00070 
00071     integrated_pose_.setIdentity();
00072 
00073     pose_covariance_.assign(0.0);
00074     twist_covariance_.assign(0.0);
00075   }
00076 
00077 protected:
00078 
00079   void setSensorFrameId(const std::string& frame_id)
00080   {
00081     sensor_frame_id_ = frame_id;
00082   }
00083 
00084   std::string getSensorFrameId() const
00085   {
00086     return sensor_frame_id_;
00087   }
00088 
00089   void setPoseCovariance(const boost::array<double, 36>& pose_covariance)
00090   {
00091     pose_covariance_ = pose_covariance;
00092   }
00093 
00094   void setTwistCovariance(const boost::array<double, 36>& twist_covariance)
00095   {
00096     twist_covariance_ = twist_covariance;
00097   }
00098 
00099   void integrateAndPublish(const tf::Transform& delta_transform, const ros::Time& timestamp)
00100   {
00101     if (sensor_frame_id_.empty())
00102     {
00103       ROS_ERROR("[odometer] update called with unknown sensor frame id!");
00104       return;
00105     }
00106     if (timestamp < last_update_time_)
00107     {
00108       ROS_WARN("[odometer] saw negative time change in incoming sensor data, resetting pose.");
00109       integrated_pose_.setIdentity();
00110       tf_listener_.clear();
00111     }
00112     integrated_pose_ *= delta_transform;
00113 
00114     // transform integrated pose to base frame
00115     tf::StampedTransform base_to_sensor;
00116     std::string error_msg;
00117     if (tf_listener_.canTransform(base_link_frame_id_, sensor_frame_id_, timestamp, &error_msg))
00118     {
00119       tf_listener_.lookupTransform(
00120           base_link_frame_id_,
00121           sensor_frame_id_,
00122           timestamp, base_to_sensor);
00123     }
00124     else
00125     {
00126       ROS_WARN_THROTTLE(10.0, "The tf from '%s' to '%s' does not seem to be available, "
00127                               "will assume it as identity!", 
00128                               base_link_frame_id_.c_str(),
00129                               sensor_frame_id_.c_str());
00130       ROS_DEBUG("Transform error: %s", error_msg.c_str());
00131       base_to_sensor.setIdentity();
00132     }
00133 
00134     tf::Transform base_transform = base_to_sensor * integrated_pose_ * base_to_sensor.inverse();
00135 
00136     nav_msgs::Odometry odometry_msg;
00137     odometry_msg.header.stamp = timestamp;
00138     odometry_msg.header.frame_id = odom_frame_id_;
00139     odometry_msg.child_frame_id = base_link_frame_id_;
00140     tf::poseTFToMsg(base_transform, odometry_msg.pose.pose);
00141 
00142     // calculate twist (not possible for first run as no delta_t can be computed)
00143     tf::Transform delta_base_transform = base_to_sensor * delta_transform * base_to_sensor.inverse();
00144     if (!last_update_time_.isZero())
00145     {
00146       double delta_t = (timestamp - last_update_time_).toSec();
00147       if (delta_t)
00148       {
00149         odometry_msg.twist.twist.linear.x = delta_base_transform.getOrigin().getX() / delta_t;
00150         odometry_msg.twist.twist.linear.y = delta_base_transform.getOrigin().getY() / delta_t;
00151         odometry_msg.twist.twist.linear.z = delta_base_transform.getOrigin().getZ() / delta_t;
00152         tf::Quaternion delta_rot = delta_base_transform.getRotation();
00153         tfScalar angle = delta_rot.getAngle();
00154         tf::Vector3 axis = delta_rot.getAxis();
00155         tf::Vector3 angular_twist = axis * angle / delta_t;
00156         odometry_msg.twist.twist.angular.x = angular_twist.x();
00157         odometry_msg.twist.twist.angular.y = angular_twist.y();
00158         odometry_msg.twist.twist.angular.z = angular_twist.z();
00159       }
00160     }
00161 
00162     odometry_msg.pose.covariance = pose_covariance_;
00163     odometry_msg.twist.covariance = twist_covariance_;
00164     odom_pub_.publish(odometry_msg);
00165     
00166     geometry_msgs::PoseStamped pose_msg;
00167     pose_msg.header.stamp = odometry_msg.header.stamp;
00168     pose_msg.header.frame_id = odometry_msg.header.frame_id;
00169     pose_msg.pose = odometry_msg.pose.pose;
00170 
00171     pose_pub_.publish(pose_msg);
00172 
00173     if (publish_tf_)
00174     {
00175       tf_broadcaster_.sendTransform(
00176           tf::StampedTransform(base_transform, timestamp,
00177           odom_frame_id_, base_link_frame_id_));
00178     }
00179 
00180     last_update_time_ = timestamp;
00181   }
00182 
00183 
00184   bool resetPose(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
00185   {
00186     integrated_pose_.setIdentity();
00187     return true;
00188   }
00189 
00190 };
00191 
00192 } // end of namespace
00193 
00194 #endif
00195 


viso2_ros
Author(s): Stephan Wirth
autogenerated on Fri Aug 28 2015 13:37:51