nav2_driver.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <std_msgs/String.h>
00003 #include <geometry_msgs/Twist.h>
00004 #include <nav_msgs/Odometry.h>
00005 #include <tf/transform_broadcaster.h>
00006 
00007 #include <nav2_driver/nav2remote.h>
00008 
00009 #include <boost/interprocess/smart_ptr/shared_ptr.hpp>
00010 
00011 namespace nav2_driver{
00012 
00016 class Nav2Driver{
00017 
00018 public:
00019 
00024     Nav2Driver(std::string name) :
00025         nh_(),
00026         private_nh_("~"),
00027         robot_prefix_()
00028     {
00029 
00030         //get robot address and port
00031         private_nh_.param<std::string>("robot_address", robot_address_, "");
00032         if(robot_address_.empty()){
00033             std::string message = "Please provide address for Nav2";
00034             ROS_ERROR_STREAM(message);
00035             throw std::runtime_error(message);
00036         }
00037         private_nh_.param<int>("robot_port", robot_port_, 5010);
00038 
00039         //get parameter for unique tf names
00040         std::string robot_name;
00041         private_nh_.param<std::string>("robot_name", robot_name, "");
00042         if (!robot_name.empty()) { robot_prefix_ = robot_name + "_"; }
00043 
00044         //get parameter for inverted odometry (for use with robot_pose_ekf)
00045         private_nh_.param<bool>("invert_odom", invert_odom_, false);
00046 
00047         connect(robot_address_, robot_port_);
00048 
00049         odom_pub_ = nh_.advertise<nav_msgs::Odometry>("odom", 10);
00050         odom_loop_ = nh_.createTimer(ros::Duration(0.1), boost::bind(&Nav2Driver::publishOdometry, this, invert_odom_, robot_prefix_));
00051         cmd_sub_ = nh_.subscribe("cmd_vel", 5, &Nav2Driver::setVelocity, this);
00052     }
00053 
00054 protected:
00055 
00060     void connect(std::string robot_address, int robot_port){
00061 
00062         //check if already connected
00063         if(remote_){
00064             ROS_INFO("Resetting connection to Nav2 base");
00065             remote_.reset();
00066 
00067             //save odometry offset for new base connection odometry
00068             Pose2D offset = base_odom_.getPose();
00069             base_odom_ = BaseOdometry(offset);
00070         }
00071 
00072         //attempt to connect several times, if failed then die
00073         int retry = 5;
00074         while(retry-- > 0){
00075             try{
00076                 //leave address:port validation to Nav2Remote. Must use shared_ptr since constructor can throw expception
00077                 remote_ = boost::shared_ptr<Nav2Remote>(new Nav2Remote(robot_address.c_str(),robot_port));
00078                 ROS_INFO_STREAM("Connected to Nav2 base on " << robot_address <<":"<< robot_port);
00079                 return;
00080             }catch(std::exception& e){
00081                 ROS_WARN_STREAM(e.what());
00082                 ros::Duration(0.2).sleep();
00083             }
00084         }
00085 
00086         std::string message = "Failed to connect to Nav2 base";
00087         ROS_ERROR_STREAM(message << " on " << robot_address_ <<":"<< robot_port_);
00088         throw std::runtime_error(message);
00089 
00090     }
00091 
00097     void publishOdometry(bool invert_odom, std::string robot_prefix){
00098 
00099         //get odometry from Nav2, reconnect on error
00100         Pose2D data;
00101         while(!remote_ || remote_->estimatePosition(data.x, data.y, data.th) < 0){
00102             connect(robot_address_, robot_port_);
00103         }
00104 
00105         //update internal state, and publish required message/tf information
00106         base_odom_.updateWithAbsolute(data);
00107         tf_broadcaster_.sendTransform(base_odom_.getTransform(invert_odom,robot_prefix));
00108         odom_pub_.publish(base_odom_.getMessage(robot_prefix));
00109 
00110     }
00111 
00116     void setVelocity(const geometry_msgs::TwistConstPtr& twist){
00117 
00118         //convert from vector velocity to relative angular velocity
00119         double vs = sqrt(pow(twist->linear.x,2) + pow(twist->linear.y,2));
00120         double vn = atan2(twist->linear.y,twist->linear.x) * (180 / M_PI);
00121 
00122         //send velocity command to Nav2, reconnect on error
00123         while(!remote_ || remote_->setRelativeVelocity(vn, vs, twist->angular.z) < 0){
00124             connect(robot_address_, robot_port_);
00125         }
00126     }
00127 
00128 private:
00129 
00133     struct Pose2D{
00134 
00135         Pose2D() : x(0), y(0), th(0) {}
00136         Pose2D(double x, double y, double th) : x(x), y(y), th(th) {}
00137         double x, y, th;
00138         Pose2D& operator +=(Pose2D const& other) {
00139             x += other.x;
00140             y += other.y;
00141             th += other.th;
00142             return *this;
00143         }
00144         Pose2D& operator -=(Pose2D const& other) {
00145             x -= other.x;
00146             y -= other.y;
00147 
00148             //detect rollover during velocity calculation
00149             if(abs(th - other.th) > M_PI){
00150                 if(other.th>0){
00151                     th += 2*M_PI;
00152                 }else{
00153                     th -= 2*M_PI;
00154                 }
00155             }
00156             th -= other.th;
00157             return *this;
00158         }
00159         Pose2D& operator /=(double const& other) {
00160             x /= other;
00161             y /= other;
00162             th /= other;
00163             return *this;
00164         }
00165         inline Pose2D& operator +(const Pose2D& other) { return *this += other; }
00166         inline Pose2D& operator -(const Pose2D& other) { return *this -= other; }
00167         inline Pose2D& operator /(const double& other) { return *this /= other; }
00168 
00169     };
00170 
00174     class BaseOdometry{
00175 
00176     public:
00177 
00181         BaseOdometry(): last_time_(ros::Time::now()) {}
00182 
00187         BaseOdometry(Pose2D offset): offset_(offset), last_time_(ros::Time::now()) {}
00188 
00189         void updateWithAbsolute(Pose2D abs){
00190             updateWithRelative(abs - prev_);
00191             prev_ = abs;
00192         }
00193 
00194         void updateWithRelative(Pose2D delta){
00195             double elapsed = (ros::Time::now() - last_time_).toSec();
00196             last_time_ = ros::Time::now();
00197 
00198             pose_ += delta;
00199             vel_ = delta / elapsed;
00200 
00201         }
00202 
00209         geometry_msgs::TransformStamped getTransform(bool invert_odom, std::string robot_prefix){
00210 
00211             //build transform message
00212             geometry_msgs::TransformStamped transform;
00213             transform.header.stamp = last_time_;
00214 
00215             //invert odometry if necessary
00216             tf::Transform temp = tf::Transform(tf::createQuaternionFromYaw(pose_.th + offset_.th), tf::Vector3(pose_.x + offset_.x, pose_.y + offset_.y, 0));
00217             if(invert_odom){
00218                 temp = temp.inverse();
00219                 transform.header.frame_id = robot_prefix + "base_footprint";
00220                 transform.child_frame_id = robot_prefix + "odom";
00221             }else{
00222                 transform.header.frame_id = robot_prefix + "odom";
00223                 transform.child_frame_id = robot_prefix + "base_footprint";
00224             }
00225             tf::transformTFToMsg(temp, transform.transform);
00226 
00227             return transform;
00228 
00229         }
00230 
00236         nav_msgs::Odometry getMessage(std::string robot_prefix){
00237 
00238             //build odometry message
00239             nav_msgs::Odometry message;
00240 
00241             message.header.stamp = last_time_;
00242             message.header.frame_id = robot_prefix + "odom";
00243             message.child_frame_id = robot_prefix + "base_link";
00244             message.pose.pose.position.x = pose_.x + offset_.x;
00245             message.pose.pose.position.y = pose_.y + offset_.y;
00246             message.pose.pose.position.z = 0.0;
00247             message.pose.pose.orientation = tf::createQuaternionMsgFromYaw(pose_.th + offset_.th);
00248             message.pose.covariance[0] = 10^-3;
00249             message.pose.covariance[7] = 10^-3;
00250             message.pose.covariance[14] = 10^6;
00251             message.pose.covariance[21] = 10^6;
00252             message.pose.covariance[28] = 10^6;
00253             message.pose.covariance[35] = 10^3;
00254 
00255             message.twist.twist.linear.x = vel_.x;
00256             message.twist.twist.linear.y = vel_.y;
00257             message.twist.twist.angular.z = vel_.th;
00258             message.twist.covariance[0] = 10^-3;
00259             message.twist.covariance[7] = 10^-3;
00260             message.twist.covariance[14] = 10^6;
00261             message.twist.covariance[21] = 10^6;
00262             message.twist.covariance[28] = 10^6;
00263             message.twist.covariance[35] = 10^3;
00264 
00265             return message;
00266 
00267         }
00268 
00273         Pose2D getPose(){
00274             return pose_;
00275         }
00276 
00277     private:
00278 
00279         Pose2D pose_, vel_, prev_, offset_;
00280         ros::Time last_time_;
00281 
00282     };
00283 
00284     ros::NodeHandle nh_;
00285     ros::NodeHandle private_nh_;
00286     tf::TransformBroadcaster tf_broadcaster_;
00287 
00288     boost::shared_ptr<Nav2Remote> remote_;
00289 
00290     ros::Publisher odom_pub_;
00291     ros::Subscriber cmd_sub_;
00292     ros::Timer odom_loop_;
00293     BaseOdometry base_odom_;
00294 
00295     std::string robot_address_, robot_prefix_;
00296     int robot_port_;
00297     bool invert_odom_;
00298 
00299 };
00300 
00301 }
00302 
00303 int main(int argc, char **argv) {
00304 
00305     ros::init(argc, argv, "nav2_driver");
00306     nav2_driver::Nav2Driver nav2_driver(ros::this_node::getName());
00307     ros::spin();
00308 
00309     return 0;
00310 }
00311 
00312 
00313 


nav2_driver
Author(s): Paul Bovbel
autogenerated on Sat Jun 8 2019 19:22:46