ROS基礎知識學習筆記（10）—Robot_Localization代碼

Boost用法詳解原博客鏈接：https://blog.csdn.net/Windgs_YF/article/details/81201456
https://github.com/Kapernikov/ros_robot_localization_tutorial

odometry_node.cpp

#include <boost/program_options.hpp>
#include <ros/ros.h>
#include <robot_localization_demo/odometry.hpp>


int main(int argc, char ** argv) {

  double frequency;
  double error_vx_systematic;
  double error_vx_random;
  double error_wz_systematic;
  double error_wz_random;

//命令行或配置文件讀取參數選項，設置四個參數X，x，T，t，v。這個例子的參數在launch文件中設置
  namespace po = boost::program_options;
  po::options_description description("Recognised options");
  description.add_options()
      ("help,h", "print help message")
      ("frequency,f", po::value<double>(&frequency)->default_value(1.), "set measurement frequency (Hz)")
      ("error-vx-systematic,X", po::value<double>(&error_vx_systematic)->default_value(0.), "set systematic error on X velocity")
      ("error-vx-random,x", po::value<double>(&error_vx_random)->default_value(0.), "set random error on X velocity")
      ("error-wz-systematic,T", po::value<double>(&error_wz_systematic)->default_value(0.), "set systematic error on angular velocity")
      ("error-wz-random,t", po::value<double>(&error_wz_random)->default_value(0.), "set random error on angular velocity")
      ("visualize,v", "visualize positioning system measurement");
  po::variables_map variables_map;
  po::store(po::parse_command_line(argc, argv, description), variables_map);
  po::notify(variables_map);

  if (variables_map.count("help")) {
    std::cout << description << std::endl;
    return EXIT_FAILURE;
  }
//初始化節點，創建句柄
  ros::init(argc, argv, "turtle_odometry");
  ros::NodeHandle node_handle;
//有參構造函數
  robot_localization_demo::TurtleOdometry turtle_odometry{node_handle, frequency,
      error_vx_systematic, error_vx_random, error_wz_systematic, error_wz_random,
      (variables_map.count("visualize")? true: false)};
  turtle_odometry.spin();

  return EXIT_SUCCESS;
}

odometry.cpp

#include <geometry_msgs/TwistWithCovarianceStamped.h>
#include <tf/transform_datatypes.h>
#include <turtlesim/Spawn.h>
#include <turtlesim/SetPen.h>
#include <turtlesim/TeleportRelative.h>
#include <robot_localization_demo/odometry.hpp>


namespace robot_localization_demo {

  TurtleOdometry::TurtleOdometry(ros::NodeHandle node_handle, double frequency,
      double error_vx_systematic, double error_vx_random, double error_wz_systematic, double error_wz_random, bool visualize):
    node_handle_{node_handle},
    turtle_pose_subscriber_{node_handle_.subscribe("turtle1/pose", 16, &TurtleOdometry::turtlePoseCallback, this)},
    turtle_twist_publisher_{node_handle_.advertise<geometry_msgs::TwistWithCovarianceStamped>("turtle1/sensors/twist", 16)},
    frequency_{frequency},
    random_generator_{},
    random_distribution_vx_{error_vx_systematic, error_vx_random},
    random_distribution_wz_{error_wz_systematic, error_wz_random},
    frame_sequence_{0},
    visualize_{visualize},
    visualization_turtle_name_{""}
  {
    ;
  }


  TurtleOdometry::~TurtleOdometry() {
    ;
  }


  void TurtleOdometry::spin() {
    ros::Rate rate(frequency_);
    while(node_handle_.ok()) {
      ros::spinOnce();
      // Distort real twist to get a 'measurement'.
      auto measurement = cached_pose_;
      measurement.linear_velocity *= (1. + random_distribution_vx_(random_generator_));
      measurement.angular_velocity += measurement.linear_velocity * random_distribution_wz_(random_generator_);
      // Publish measurement.
      geometry_msgs::TwistWithCovarianceStamped current_twist;
      current_twist.header.seq = ++ frame_sequence_;
      current_twist.header.stamp = ros::Time::now();
      current_twist.header.frame_id = "base_link";
      current_twist.twist.twist.linear.x = measurement.linear_velocity;
      current_twist.twist.twist.linear.y = 0.;
      current_twist.twist.twist.linear.z = 0.;
      current_twist.twist.twist.angular.x = 0.;
      current_twist.twist.twist.angular.y = 0.;
      current_twist.twist.twist.angular.z = measurement.angular_velocity;
      current_twist.twist.covariance = boost::array<double, 36>({
          std::pow(random_distribution_vx_.mean() + random_distribution_vx_.stddev(), 2), 0., 0., 0., 0., 0.,
          0., 0., 0., 0., 0., 0.,
          0., 0., 0., 0., 0., 0.,
          0., 0., 0., 0., 0., 0.,
          0., 0., 0., 0., 0., 0.,
          0., 0., 0., 0., 0., std::pow(measurement.linear_velocity * (random_distribution_wz_.mean() + random_distribution_wz_.stddev()), 2)});
      turtle_twist_publisher_.publish(current_twist);
      if(isVisualizationRequested() && isVisualizationTurtleAvailable()) {
        moveVisualizationTurtle(measurement);
      }
      // Sleep until we need to publish a new measurement.
      rate.sleep();
    }
  }


  void TurtleOdometry::turtlePoseCallback(const turtlesim::PoseConstPtr & message) {
    cached_pose_timestamp_ = ros::Time::now();
    cached_pose_ = *message;
    // If this is the first message, initialize the visualization turtle.
    if(isVisualizationRequested() && !isVisualizationTurtleAvailable()) {
      spawnAndConfigureVisualizationTurtle(*message);
    }
  }


  void TurtleOdometry::spawnAndConfigureVisualizationTurtle(const turtlesim::Pose & initial_pose) {
    if(isVisualizationRequested() && !isVisualizationTurtleAvailable()) {
      // Spawn a new turtle and store its name.
      ros::service::waitForService("spawn");
      turtlesim::Spawn spawn_visualization_turtle;
      spawn_visualization_turtle.request.x = initial_pose.x;
      spawn_visualization_turtle.request.y = initial_pose.y;
      spawn_visualization_turtle.request.theta = initial_pose.theta;
      auto client = node_handle_.serviceClient<decltype(spawn_visualization_turtle)>("spawn");
      client.call(spawn_visualization_turtle);
      visualization_turtle_name_ = spawn_visualization_turtle.response.name;
      // Set pen color to blue.
      turtlesim::SetPen configure_visualization_turtle;
      configure_visualization_turtle.request.r = 255;
      configure_visualization_turtle.request.g = 0;
      configure_visualization_turtle.request.b = 0;
      configure_visualization_turtle.request.width = 1;
      configure_visualization_turtle.request.off = 0;
      auto client_configure = node_handle_.serviceClient<decltype(configure_visualization_turtle)>(
          visualization_turtle_name_ + "/set_pen");
      client_configure.call(configure_visualization_turtle);
      // Log message.
      ROS_INFO("Relative position measurement (odometry) visualized by '%s' with a red pen.", visualization_turtle_name_.c_str());
    }
  }


  void TurtleOdometry::moveVisualizationTurtle(const turtlesim::Pose & measurement) {
    if(isVisualizationRequested() && isVisualizationTurtleAvailable()) {
      // Move visualization turtle to the 'measured' position.
      turtlesim::TeleportRelative visualize_current_twist;
      visualize_current_twist.request.linear = measurement.linear_velocity / frequency_;
      visualize_current_twist.request.angular = measurement.angular_velocity / frequency_;
      auto client = node_handle_.serviceClient<decltype(visualize_current_twist)>(
          visualization_turtle_name_ + "/teleport_relative");
      client.call(visualize_current_twist);
    }
  }

}

odometry.hpp

#ifndef __robot_localization_demo__odometry__
#define __robot_localization_demo__odometry__

#include <random>
#include <ros/ros.h>
#include <turtlesim/Pose.h>

namespace robot_localization_demo {
//定義一個類，成員函數spin（），有參構造函數和析構函數
  class TurtleOdometry {
    public:
      TurtleOdometry(ros::NodeHandle node_handle, double frequency, double error_vx_systematic, double error_vx_random,
          double error_wz_systematic, double error_wz_random, bool visualize=false);
      ~TurtleOdometry();
      void spin();
    private:
      ros::NodeHandle node_handle_;
      ros::Subscriber turtle_pose_subscriber_;
      ros::Publisher turtle_twist_publisher_;
      double frequency_;
      std::default_random_engine random_generator_;
      std::normal_distribution<double> random_distribution_vx_;
      std::normal_distribution<double> random_distribution_wz_;
      bool visualize_;
      std::string visualization_turtle_name_;
      unsigned frame_sequence_;
      ros::Time cached_pose_timestamp_;
      turtlesim::Pose cached_pose_;
      void turtlePoseCallback(const turtlesim::PoseConstPtr & message);
      inline bool isVisualizationRequested() { return visualize_; };
      inline bool isVisualizationTurtleAvailable() { return visualization_turtle_name_ != ""; };
      void spawnAndConfigureVisualizationTurtle(const turtlesim::Pose & initial_pose);
      void moveVisualizationTurtle(const turtlesim::Pose & measurement);
  };

}
#endif

launch文件

<launch>

  <!-- visualization node to show the real turtle, the measurements and the estimated position -->
  <node pkg="turtlesim" type="turtlesim_node" name="turtlesim" />
  <!-- keyboard control for the real turtle -->
  <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen" />

  <!-- 'turtle positioning system', transforming the position of the real turtle to a noisy measurement at a given frequency -->
  <node pkg="robot_localization_demo" type="positioning_system_node" name="turtle1_positioning_system_node"
        args="-f 1. -x 0.2 -y 0.2 -t 0.2 -v" output="screen" />
  <!-- 'turtle odometry node', transforming the movements of the real turtle to a noisy measurement at a given frequency -->
  <node pkg="robot_localization_demo" type="odometry_node" name="turtle1_odometry_node"
        args="-f 20. -x 0.05 -X 0. -t 0. -T 0.02 -v" output="screen" />

  <!-- robot_localization EKF node for the odom frame -->
  <node pkg="robot_localization" type="ekf_localization_node" name="robot_localization_ekf_node_odom" clear_params="true">
    <param name="frequency" value="10." />  
    <param name="sensor_timeout" value="0.2" />  
    //確保機器人在X-Y平面上
    <param name="two_d_mode" value="true" />
    <param name="publish_tf" value="true" />
    <param name="map_frame" value="map" />
    <param name="odom_frame" value="odom" />
    <param name="base_link_frame" value="base_link" />
    <param name="world_frame" value="odom" />
    <param name="print_diagnostics" value="true" />
    <remap from="odometry/filtered" to="odometry/filtered_twist" />
    <param name="twist0" value="turtle1/sensors/twist" />
    <param name="twist0_differential" value="false"/>
    <rosparam param="twist0_config">[false, false, false, false, false, false,
                                     true, true, false, false, false, true,
                                     false, false, false]</rosparam>
  </node>

  <!-- robot_localization EKF node for the map frame -->
  <node pkg="robot_localization" type="ekf_localization_node" name="robot_localization_ekf_node_map"  clear_params="true">
    <param name="frequency" value="10" />  
    <param name="sensor_timeout" value="0.2" />  
    <param name="two_d_mode" value="true" />
    <param name="publish_tf" value="true" />
    <param name="map_frame" value="map" />
    <param name="odom_frame" value="odom" />
    <param name="base_link_frame" value="base_link" />
    <param name="world_frame" value="map" />
    <param name="twist0" value="turtle1/sensors/twist" />
    <rosparam param="twist0_config">[false, false, false, false, false, false,
                                     true, true, false, false, false, true,
                                     false, false, false]</rosparam>
    <param name="pose0" value="turtle1/sensors/pose" />
    <rosparam param="pose0_config">[true, true, false, false, false, true,
                                    false, false, false, false, false, false,
                                    false, false, false]</rosparam>
    <remap from="odometry/filtered" to="odometry/filtered_map"/>
  </node>

  <!-- transformation visualization node, visualizing the estimated position of the turtle in the map frame -->
  <node pkg="robot_localization_demo" type="transformation_visualization_node" name="transformation_visualization_node" />

</launch>