ROS2 robot_localization包的使用教程

已于 2023-03-30 15:30:37 修改
阅读量2.6k 收藏 22
点赞数 3
文章标签： c++ 开发语言
于 2022-12-22 16:45:27 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/weixin_44813425/article/details/127866936
版权

1、引言

Robot_localization是一个ROS的包，基于卡尔曼滤波，对多种传感器进行数据融合，进而完成机器人的定位。如果你有相关应用场景，那么该包可以大大降低工作量与工作难度。该包的使用不是那么复杂，但网上相关的资料却较少。如下链接，是一个基于ROS1的教程，较为清晰直接的介绍了该包的具体作用与如何使用。然而在我的工作中，采用的是ROS2，因此我将该教程的代码移植到了ROS2中，同时希望可以帮助更多的人。因此该文章主要是移植后的ROS2代码，具体的教程请参考链接。

教程链接

2、准备工作

Github源码链接
在Github上，根据使用的ROS版本，下载相应的源码，并放在自己工作目录的src文件夹下，并进行编译。
第一次大概率由于没有安装相关的包，而编译报错，根据相应错误提示，安装相关的包即可。
完成编译后，即可以进行教程代码的编写。

3、移植ROS2后的教程代码

代码目录如下，源码按顺序给出
在这里插入图片描述

//odometry.hpp
#ifndef __robot_localization_demo__odometry__
#define __robot_localization_demo__odometry__

#include <random>
#include "rclcpp/rclcpp.hpp"
#include "turtlesim/msg/pose.hpp"
#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>

class TurtleOdometry: public rclcpp::Node  {
public:
    TurtleOdometry( 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:
    double frequency_;
    std::default_random_engine random_generator_;
    std::normal_distribution<double> random_distribution_vx_;
    std::normal_distribution<double> random_distribution_wz_;
    unsigned frame_sequence_;
    bool visualize_;
    std::string visualization_turtle_name_;

    rclcpp::callback_group::CallbackGroup::SharedPtr client_cb_group_;
    rclcpp::callback_group::CallbackGroup::SharedPtr other_cb_group_;

    rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr turtle_pose_subscriber_;
    rclcpp::Publisher<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr turtle_twist_publisher_;
    rclcpp::Time cached_pose_timestamp_;
    turtlesim::msg::Pose cached_pose_;
    void turtlePoseCallback(const turtlesim::msg::Pose::SharedPtr message);
    inline bool isVisualizationRequested() { return visualize_; };
    inline bool isVisualizationTurtleAvailable() { return visualization_turtle_name_ != ""; };
    void spawnAndConfigureVisualizationTurtle(const turtlesim::msg::Pose & initial_pose);
    void moveVisualizationTurtle(const turtlesim::msg::Pose & measurement);
    rclcpp::TimerBase::SharedPtr timer_;
};

#endif

//positioning_system.hpp
#ifndef __robot_localization_demo__positioning_system__
#define __robot_localization_demo__positioning_system__


#include <random>
#include "rclcpp/rclcpp.hpp"
#include "turtlesim/msg/pose.hpp"
#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"



class TurtlePositioningSystem: public rclcpp::Node {
public:
    TurtlePositioningSystem(double frequency, double error_x_systematic,
          double error_x_random, double error_y_systematic, double error_y_random, double error_yaw_systematic,
          double error_yaw_random, bool visualize=false);
    ~TurtlePositioningSystem();
    void spin();
private:
    double frequency_;
    std::default_random_engine random_generator_;
    std::normal_distribution<double> random_distribution_x_;
    std::normal_distribution<double> random_distribution_y_;
    std::normal_distribution<double> random_distribution_yaw_;
    unsigned frame_sequence_;
    bool visualize_;
    std::string visualization_turtle_name_;

    rclcpp::callback_group::CallbackGroup::SharedPtr client_cb_group_;
    rclcpp::callback_group::CallbackGroup::SharedPtr other_cb_group_;

    rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr turtle_pose_subscriber_;
    rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr turtle_pose_publisher_;
    rclcpp::Time cached_pose_timestamp_;
    turtlesim::msg::Pose cached_pose_;
    void turtlePoseCallback(const turtlesim::msg::Pose::SharedPtr message);
    inline bool isVisualizationRequested() { return visualize_; };
    inline bool isVisualizationTurtleAvailable() { return visualization_turtle_name_ != ""; };
    void spawnAndConfigureVisualizationTurtle(const turtlesim::msg::Pose & initial_pose);
    void moveVisualizationTurtle(const turtlesim::msg::Pose & measurement);


    rclcpp::TimerBase::SharedPtr timer_;
};

#endif

#robot_localization_demo.launch.py
from launch import LaunchDescription
from ament_index_python.packages import get_package_share_directory
import os
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
        Node(
            package         = 'turtlesim',
            node_executable = 'turtlesim_node',
            node_name       = 'turtlesim',
        ),
        # Node(
        #     package         = 'turtlesim',
        #     node_executable = 'turtle_teleop_key',
        #     node_name       = 'teleop',
        #     output='screen',
        # ),
        Node(
            package         = 'robot_localization_demo',
            node_executable = 'positioning_system_node',
            node_name       = 'turtle1_positioning_system_node',
            output='screen',
            arguments=["-f","1.","-x","0.2","-y","0.2","-t","0.2","-v"],
            #prefix = ['gdbserver localhost:3000'],
        ),
        Node(
            package         = 'robot_localization_demo',
            node_executable = 'odometry_node',
            node_name       = 'turtle1_odometry_node',
            output='screen',
            arguments=["-f","20.","-x","0.05","-X","0.","-t","0.","-T","0.02","-v"],
            #prefix = ['gdbserver localhost:3000'],
        ),
        Node(
            package         = 'robot_localization',
            node_executable = 'ekf_node',
            name       = 'robot_localization_ekf_node_odom',
            output='screen',
            parameters=[os.path.join(get_package_share_directory("robot_localization_demo"), 'params', 'node_odom.yaml')],
            remappings=[
               ('/odometry/filtered', '/odometry/filtered_twist'),
            ]
        ),
        Node(
            package         = 'robot_localization',
            node_executable = 'ekf_node',
            name       = 'robot_localization_ekf_node_map',
            output='screen',
            parameters=[os.path.join(get_package_share_directory("robot_localization_demo"), 'params', 'node_map.yaml')],
            remappings=[
                ('/odometry/filtered', 'odometry/filtered_map'),
            ]
        ),
        Node(
            package         = 'robot_localization_demo',
            node_executable = 'transformation_visualization_node',
            output='screen',
        ),       

    ])

#node_map.yaml
### ekf config file ###
ekf_filter_node:
    ros__parameters:
        frequency: 30.0
        sensor_timeout: 0.1
        two_d_mode: true
        transform_time_offset: 0.0
        transform_timeout: 0.0
        print_diagnostics: true
        debug: false
        debug_out_file: /path/to/debug/file.txt
        publish_tf: true
        publish_acceleration: false
        map_frame: map              
        odom_frame: odom            
        base_link_frame: base_link  
        world_frame: map           

        twist0: /turtle1/sensors/twist
        twist0_config: [false, false, false,
                       false, false, false,
                       true, true, false,
                       false, false, true,
                       false, false, false]

        pose0: /turtle1/sensors/pose
        pose0_config: [true, true, false,
                false, false, true,
                false, false, false,
                false, false, true,
                false, false, false]

#node_odom.yaml
### ekf config file ###
ekf_filter_node:
    ros__parameters:
        frequency: 30.0
        sensor_timeout: 0.1
        two_d_mode: true
        transform_time_offset: 0.0
        transform_timeout: 0.0
        print_diagnostics: true
        debug: false
        publish_tf: true
        publish_acceleration: false
        map_frame: map              
        odom_frame: odom            
        base_link_frame: base_link  
        world_frame: odom           
        twist0: /turtle1/sensors/twist
        twist0_config: [false, false, false,
                       false, false, false,
                       true, true, false,
                       false, false, true,
                       false, false, false]
        twist0_differential: false

//odometry.cpp

#include "turtlesim/srv/spawn.hpp"
#include "turtlesim/srv/set_pen.hpp"
#include "turtlesim/srv/teleport_relative.hpp"
#include "odometry.hpp"
#include "rclcpp/rclcpp.hpp"
#include <chrono>

using std::placeholders::_1;
using namespace std::chrono_literals;

TurtleOdometry::TurtleOdometry( double frequency,
    double error_vx_systematic, double error_vx_random, double error_wz_systematic, double error_wz_random, bool visualize):
  Node("odometry_node"),
  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_{""}
{
  client_cb_group_ = this->create_callback_group(rclcpp::callback_group::CallbackGroupType::MutuallyExclusive);
  other_cb_group_ = this->create_callback_group(rclcpp::callback_group::CallbackGroupType::MutuallyExclusive);
  other_cb_group_ = this->create_callback_group(rclcpp::callback_group::CallbackGroupType::MutuallyExclusive);
  auto sub_opt = rclcpp::SubscriptionOptions();
  sub_opt.callback_group = other_cb_group_;
  turtle_pose_subscriber_ = this->create_subscription<turtlesim::msg::Pose>(
      "turtle1/pose", 16, std::bind(&TurtleOdometry::turtlePoseCallback, this, _1),sub_opt);
  turtle_twist_publisher_ = this->create_publisher<geometry_msgs::msg::TwistWithCovarianceStamped>("turtle1/sensors/twist", 16);
  timer_ = this->create_wall_timer(
  std::chrono::milliseconds((int)(1000/frequency_)), std::bind(&TurtleOdometry::spin, this),other_cb_group_);
}


TurtleOdometry::~TurtleOdometry() {
  ;
}


void TurtleOdometry::spin() {
    // 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::msg::TwistWithCovarianceStamped current_twist;
    //current_twist.header.seq = ++ frame_sequence_;
    current_twist.header.stamp = this->get_clock()->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 = std::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);
    }
}


void TurtleOdometry::turtlePoseCallback(const turtlesim::msg::Pose::SharedPtr message) {
  cached_pose_timestamp_ = this->get_clock()->now();
  cached_pose_ = *message;
  // If this is the first message, initialize the visualization turtle.
  if(isVisualizationRequested() && !isVisualizationTurtleAvailable()) {
    spawnAndConfigureVisualizationTurtle(*message);
  }
}


void TurtleOdometry::spawnAndConfigureVisualizationTurtle(const turtlesim::msg::Pose & initial_pose) {
  if(isVisualizationRequested() && !isVisualizationTurtleAvailable()) {
    // Spawn a new turtle and store its name.
    auto client = this->create_client<turtlesim::srv::Spawn>("spawn",rmw_qos_profile_services_default,client_cb_group_);
    client->wait_for_service();
    turtlesim::srv::Spawn::Request::SharedPtr spawn_visualization_turtle = std::make_shared<turtlesim::srv::Spawn::Request>();
    spawn_visualization_turtle->x = initial_pose.x;
    spawn_visualization_turtle->y = initial_pose.y;
    spawn_visualization_turtle->theta = initial_pose.theta;
    auto result_future = client->async_send_request(spawn_visualization_turtle);
    result_future.wait();
    auto result = result_future.get();
    visualization_turtle_name_=result->name;
    // Set pen color to red.
    turtlesim::srv::SetPen::Request::SharedPtr configure_visualization_turtle  = std::make_shared<turtlesim::srv::SetPen::Request>();
    configure_visualization_turtle->r = 255;
    configure_visualization_turtle->g = 0;
    configure_visualization_turtle->b = 0;
    configure_visualization_turtle->width = 1;
    configure_visualization_turtle->off = 0;
    auto client_configure = this->create_client<turtlesim::srv::SetPen>(visualization_turtle_name_ + "/set_pen",rmw_qos_profile_services_default,client_cb_group_);
    auto set_pen_result_future = client_configure->async_send_request(configure_visualization_turtle);
    set_pen_result_future.wait();
    // Log message.
    RCLCPP_INFO(this->get_logger(),"Relative position measurement (odometry) visualized by '%s' with a red pen.", visualization_turtle_name_.c_str());
  }
}


void TurtleOdometry::moveVisualizationTurtle(const turtlesim::msg::Pose & measurement) {
  if(isVisualizationRequested() && isVisualizationTurtleAvailable()) {
    // Move visualization turtle to the 'measured' position.
    turtlesim::srv::TeleportRelative::Request::SharedPtr visualize_current_twist =std::make_shared<turtlesim::srv::TeleportRelative::Request>();
    visualize_current_twist->linear = measurement.linear_velocity / frequency_;
    visualize_current_twist->angular = measurement.angular_velocity / frequency_;
    auto client = this->create_client<turtlesim::srv::TeleportRelative>(
        visualization_turtle_name_ + "/teleport_relative",rmw_qos_profile_services_default,client_cb_group_);
    client->async_send_request(visualize_current_twist);
  }
}


//odometry_node.cpp
#include <boost/program_options.hpp>
#include "rclcpp/rclcpp.hpp"
#include "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;

  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;
  }

  rclcpp::init(argc, argv);

  auto node = std::make_shared<TurtleOdometry>(frequency,
      error_vx_systematic, error_vx_random, error_wz_systematic, error_wz_random,
      (variables_map.count("visualize")? true: false));
  rclcpp::executors::MultiThreadedExecutor executor;
  executor.add_node(node);
  executor.spin();
  rclcpp::shutdown();

  return EXIT_SUCCESS;
}


//positioning_system.cpp
#include "positioning_system.hpp"
#include "turtlesim/srv/spawn.hpp"
#include "turtlesim/srv/set_pen.hpp"
#include "turtlesim/srv/teleport_absolute.hpp"
#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
#include <chrono>
#include <tf2/LinearMath/Quaternion.h>
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"

using std::placeholders::_1;
using namespace std::chrono_literals;


TurtlePositioningSystem::TurtlePositioningSystem(double frequency, double error_x_systematic,
        double error_x_random, double error_y_systematic, double error_y_random, double error_yaw_systematic,
        double error_yaw_random, bool visualize):
  Node("positioning_node"),
  frequency_{frequency},
  random_generator_{},
  random_distribution_x_{error_x_systematic, error_x_random},
  random_distribution_y_{error_y_systematic, error_y_random},
  random_distribution_yaw_{error_yaw_systematic, error_yaw_random},
  frame_sequence_{0},
  visualize_{visualize},
  visualization_turtle_name_{""}
{
  client_cb_group_ = this->create_callback_group(rclcpp::callback_group::CallbackGroupType::MutuallyExclusive);
  other_cb_group_ = this->create_callback_group(rclcpp::callback_group::CallbackGroupType::MutuallyExclusive);
  auto sub_opt = rclcpp::SubscriptionOptions();
  sub_opt.callback_group = other_cb_group_;
  turtle_pose_subscriber_ = this->create_subscription<turtlesim::msg::Pose>(
  "turtle1/pose", 16, std::bind(&TurtlePositioningSystem::turtlePoseCallback, this, _1),sub_opt);
  turtle_pose_publisher_ = this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("turtle1/sensors/pose", 16);
  timer_ = this->create_wall_timer(
  std::chrono::milliseconds((int)(1000/frequency_)), std::bind(&TurtlePositioningSystem::spin, this),other_cb_group_);
}


  TurtlePositioningSystem::~TurtlePositioningSystem() {
    ;
  }


  void TurtlePositioningSystem::spin() {
      // Distort real pose to get a 'measurement'.
      auto measurement = cached_pose_;
      measurement.x += random_distribution_x_(random_generator_);
      measurement.y += random_distribution_y_(random_generator_);
      measurement.theta += random_distribution_yaw_(random_generator_);
      // Publish measurement.
      geometry_msgs::msg::PoseWithCovarianceStamped current_pose;
      //current_pose.header.seq = ++ frame_sequence_;
      current_pose.header.stamp = this->get_clock()->now();
      current_pose.header.frame_id = "map";
      current_pose.pose.pose.position.x = measurement.x;
      current_pose.pose.pose.position.y = measurement.y;
      current_pose.pose.pose.position.z = 0.;
      tf2::Quaternion quat;
      quat.setRPY(0., 0., measurement.theta);
      current_pose.pose.pose.orientation = tf2::toMsg(quat);
      current_pose.pose.covariance = std::array<double, 36>({
          std::pow(random_distribution_x_.mean() + random_distribution_x_.stddev(), 2), 0., 0., 0., 0., 0.,
          0., std::pow(random_distribution_y_.mean() + random_distribution_y_.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., std::pow(random_distribution_yaw_.mean() + random_distribution_yaw_.stddev(), 2)});
      turtle_pose_publisher_->publish(current_pose);
      if(isVisualizationRequested() && isVisualizationTurtleAvailable()) {
        moveVisualizationTurtle(measurement);
      }
    }


  void TurtlePositioningSystem::turtlePoseCallback(const turtlesim::msg::Pose::SharedPtr message) {
    cached_pose_timestamp_ =this->get_clock()->now();
    cached_pose_ = *message;
    // If this is the first message, initialize the visualization turtle.
    if(isVisualizationRequested() && !isVisualizationTurtleAvailable()) {
      spawnAndConfigureVisualizationTurtle(*message);
    }
  }


  void TurtlePositioningSystem::spawnAndConfigureVisualizationTurtle(const turtlesim::msg::Pose & initial_pose) {
    if(isVisualizationRequested() && !isVisualizationTurtleAvailable()) {
      // Spawn a new turtle and store its name.
      auto client = this->create_client<turtlesim::srv::Spawn>("spawn",rmw_qos_profile_services_default,client_cb_group_);
      client->wait_for_service();
      turtlesim::srv::Spawn::Request::SharedPtr spawn_visualization_turtle = std::make_shared<turtlesim::srv::Spawn::Request>();

      spawn_visualization_turtle->x = initial_pose.x;
      spawn_visualization_turtle->y = initial_pose.y;
      spawn_visualization_turtle->theta = initial_pose.theta;
      // auto set_turtle_name = [&](rclcpp::Client<turtlesim::srv::Spawn>::SharedFuture inner_future){
      //   auto result = inner_future.get();
      //   visualization_turtle_name_=result->name;
      auto result_future = client->async_send_request(spawn_visualization_turtle);
      result_future.wait();
      auto result = result_future.get();
      visualization_turtle_name_=result->name;
      // Set pen color to blue.
      turtlesim::srv::SetPen::Request::SharedPtr configure_visualization_turtle = std::make_shared<turtlesim::srv::SetPen::Request>();
      configure_visualization_turtle->r = 0;
      configure_visualization_turtle->g = 0;
      configure_visualization_turtle->b = 255;
      configure_visualization_turtle->width = 1;
      configure_visualization_turtle->off = 0;
      auto client_configure = this->create_client<turtlesim::srv::SetPen>(visualization_turtle_name_ + "/set_pen",rmw_qos_profile_services_default,client_cb_group_);
      auto set_pen_result_future = client_configure->async_send_request(configure_visualization_turtle);
      set_pen_result_future.wait();
      // Log message.
      RCLCPP_INFO(this->get_logger(),"Relative position measurement (odometry) visualized by '%s' with a blue pen.", visualization_turtle_name_.c_str()); 
    }
  }


  void TurtlePositioningSystem::moveVisualizationTurtle(const turtlesim::msg::Pose & measurement) {
    if(isVisualizationRequested() && isVisualizationTurtleAvailable()) {
      // Move visualization turtle to the 'measured' position.
      turtlesim::srv::TeleportAbsolute::Request::SharedPtr visualize_current_pose = std::make_shared<turtlesim::srv::TeleportAbsolute::Request>();
      visualize_current_pose->x = measurement.x;
      visualize_current_pose->y = measurement.y;
      visualize_current_pose->theta = measurement.theta;
      auto client = this->create_client<turtlesim::srv::TeleportAbsolute>(
        visualization_turtle_name_ + "/teleport_absolute",rmw_qos_profile_services_default,client_cb_group_);
      client->async_send_request(visualize_current_pose);
    }
  }



//positioning_system_node.cpp
#include <boost/program_options.hpp>
#include "rclcpp/rclcpp.hpp"
#include "positioning_system.hpp"

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

    double frequency;
    double error_x_systematic;
    double error_x_random;
    double error_y_systematic;
    double error_y_random;
    double error_yaw_systematic;
    double error_yaw_random;

    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-x-systematic,X", po::value<double>(&error_x_systematic)->default_value(0.), "set systematic error on X")
        ("error-x-random,x", po::value<double>(&error_x_random)->default_value(0.), "set random error on X")
        ("error-y-systematic,Y", po::value<double>(&error_y_systematic)->default_value(0.), "set systematic error on Y")
        ("error-y-random,y", po::value<double>(&error_y_random)->default_value(0.), "set random error on Y")
        ("error-yaw-systematic,T", po::value<double>(&error_yaw_systematic)->default_value(0.), "set systematic error on yaw")
        ("error-yaw-random,t", po::value<double>(&error_yaw_random)->default_value(0.), "set random error on yaw")
        ("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;
    }

    rclcpp::init(argc, argv);
    auto node = std::make_shared<TurtlePositioningSystem> (frequency,
    error_x_systematic, error_x_random, error_y_systematic, error_y_random,
    error_yaw_systematic, error_yaw_random, (variables_map.count("visualize")? true: false));
    rclcpp::executors::MultiThreadedExecutor executor;
    executor.add_node(node);
    executor.spin();
    rclcpp::shutdown();

    return EXIT_SUCCESS;
}

//transformation_visualization_node.cpp
#include "rclcpp/rclcpp.hpp"
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "turtlesim/srv/spawn.hpp"
#include "turtlesim/srv/set_pen.hpp"
#include "turtlesim/srv/teleport_absolute.hpp"
#include <chrono>
using namespace std::chrono_literals;

int main(int argc, char* argv[]) {
    rclcpp::init(argc, argv);

  std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("transformation_visualization_node");

  tf2_ros::Buffer tf_buffer(node->get_clock());
  tf2_ros::TransformListener tf_listener(tf_buffer);

  // Spawn a new turtle and store its name.
    auto client = node->create_client<turtlesim::srv::Spawn>("spawn");
    while (!client->wait_for_service(std::chrono::seconds(1))) {
    if (!rclcpp::ok()) {
        RCLCPP_ERROR(node->get_logger(), "client interrupted while waiting for service to appear.");
        return 1;
    }
    RCLCPP_INFO(node->get_logger(), "waiting for service to appear...");
    }

  turtlesim::srv::Spawn::Request::SharedPtr spawn_visualization_turtle = std::make_shared<turtlesim::srv::Spawn::Request>();
  spawn_visualization_turtle->x = 0;
  spawn_visualization_turtle->y = 0;
  spawn_visualization_turtle->theta = 0;
  auto result_future = client->async_send_request(spawn_visualization_turtle);
    if (rclcpp::spin_until_future_complete(node, result_future) !=
    rclcpp::executor::FutureReturnCode::SUCCESS)
  {
    RCLCPP_ERROR(node->get_logger(), "service call failed :(");
    //client->remove_pending_request(result_future);
    return 1;
  }
  auto result = result_future.get();
  auto visualization_turtle_name = result->name;
  // Set pen color to light blue.
  turtlesim::srv::SetPen::Request::SharedPtr configure_visualization_turtle = std::make_shared<turtlesim::srv::SetPen::Request>();
  configure_visualization_turtle->r = 0;
  configure_visualization_turtle->g = 255;
  configure_visualization_turtle->b = 0;
  configure_visualization_turtle->width = 3;
  configure_visualization_turtle->off = 0;
  auto client_configure = node->create_client<turtlesim::srv::SetPen>( visualization_turtle_name + "/set_pen");
  auto result_future_setpen = client_configure->async_send_request(configure_visualization_turtle);
    if (rclcpp::spin_until_future_complete(node, result_future_setpen) !=
    rclcpp::executor::FutureReturnCode::SUCCESS)
  {
    RCLCPP_ERROR(node->get_logger(), "service call failed :(");
    //client->remove_pending_request(result_future);
    return 1;
  }
  // Log message.
  RCLCPP_INFO(node->get_logger(),"Absolute position estimate visualized by '%s' using a green pen.", visualization_turtle_name.c_str());

  // Visualize the estimated position of the turtle in the map frame.
  rclcpp::Rate a(10);
  while(rclcpp::ok()) {
    // Get base_link to map transformation.
    geometry_msgs::msg::TransformStamped base_link_to_map_transform;
    try {
      base_link_to_map_transform = tf_buffer.lookupTransform("map", "base_link", tf2::TimePointZero);
    }
    catch (tf2::TransformException &ex) {
      RCLCPP_INFO(node->get_logger(),"tf2_ros::Buffer::lookupTransform failed: %s", ex.what());
      rclcpp::sleep_for(1s);
      continue;
    }
    // Move visualization turtle to the estimated position.
    geometry_msgs::msg::PoseStamped pose_base_link, pose_map;
    pose_base_link.header.stamp = node->get_clock()->now();
    pose_base_link.header.frame_id = "base_link";
    pose_base_link.pose.position.x = 0.;
    pose_base_link.pose.position.y = 0.;
    pose_base_link.pose.position.z = 0.;
    pose_base_link.pose.orientation.x = 0.;
    pose_base_link.pose.orientation.y = 0.;
    pose_base_link.pose.orientation.z = 0.;
    pose_base_link.pose.orientation.w = 1.;
    tf2::doTransform(pose_base_link, pose_map, base_link_to_map_transform);
    turtlesim::srv::TeleportAbsolute::Request::SharedPtr visualize_current_pose = std::make_shared<turtlesim::srv::TeleportAbsolute::Request>();
    visualize_current_pose->x = pose_map.pose.position.x;
    visualize_current_pose->y = pose_map.pose.position.y;
    tf2::Quaternion quaternion(pose_map.pose.orientation.x, pose_map.pose.orientation.y, pose_map.pose.orientation.z, pose_map.pose.orientation.w);
    visualize_current_pose->theta = quaternion.getAngle();
    auto client_visual =node->create_client<turtlesim::srv::TeleportAbsolute>(visualization_turtle_name + "/teleport_absolute");
    auto result_future_visual =client_visual->async_send_request(visualize_current_pose);
        if (rclcpp::spin_until_future_complete(node, result_future_visual) !=
    rclcpp::executor::FutureReturnCode::SUCCESS)
  {
    RCLCPP_ERROR(node->get_logger(), "service call failed :(");
    //client->remove_pending_request(result_future);
    return 1;
  }
    // Sleep until the next update.
    a.sleep();
  }

  return EXIT_SUCCESS;
}


#CMakeLists.txt
cmake_minimum_required(VERSION 3.5)
project(robot_localization_demo)

# Default to C99
if(NOT CMAKE_C_STANDARD)
  set(CMAKE_C_STANDARD 99)
endif()

# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

set(CMAKE_PREFIX_PATH ../../install)
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(tf2_ros REQUIRED) 
find_package(geometry_msgs REQUIRED)
find_package(turtlesim REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(robot_localization REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)

find_package(Boost REQUIRED COMPONENTS program_options)
include_directories(${Boost_INCLUDE_DIRS})

add_executable(odometry_node src/odometry_node.cpp src/odometry.cpp)
target_include_directories(odometry_node PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>)

  ament_target_dependencies(
    odometry_node   
    rclcpp
    geometry_msgs
    turtlesim
    tf2
    tf2_ros
  )  
  target_link_libraries(odometry_node
  Boost::program_options
)   

add_executable(positioning_system_node src/positioning_system_node.cpp src/positioning_system.cpp)
target_include_directories(positioning_system_node PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>)

  ament_target_dependencies(
    positioning_system_node   
    rclcpp
    geometry_msgs
    turtlesim
    tf2
    tf2_ros
  )    
  target_link_libraries(positioning_system_node
  Boost::program_options
)   
add_executable(transformation_visualization_node src/transformation_visualization_node.cpp)
target_include_directories(transformation_visualization_node PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>)

  ament_target_dependencies(
    transformation_visualization_node   
    rclcpp
    geometry_msgs
    turtlesim
    tf2
    tf2_ros
    tf2_geometry_msgs
  )    

install(TARGETS odometry_node positioning_system_node transformation_visualization_node
  DESTINATION lib/${PROJECT_NAME})

install(DIRECTORY
  params
  launch
  DESTINATION share/${PROJECT_NAME}
  USE_SOURCE_PERMISSIONS
)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # uncomment the line when a copyright and license is not present in all source files
  #set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # uncomment the line when this package is not in a git repo
  #set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()


///package.xml
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>robot_localization_demo</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="a@todo.todo">a</maintainer>
  <license>TODO: License declaration</license>

  <depend>geometry_msgs</depend>
  <depend>robot_localization</depend>
  <depend>rclcpp</depend>
  <depend>tf2</depend>
  <depend>tf2_ros</depend>
  <depend>turtlesim</depend>
  <depend>RL</depend>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

4、编译使用

将教程代码进行编译，随后launch运行，并在单独的窗口运行小乌龟控制节点，即可开始测试。

幽哈
关注
  • 3
    点赞
  • 踩
  • 22
    收藏
    觉得还不错? 一键收藏
  • 8
    评论
robot_localization
04-13
robot_localization robot_localization是非线性状态估计节点的程序包。 该软件包是由Charles River Analytics，Inc.开发的。 请在此处查看文档： :
ROS基础知识学习笔记（9）—Robot_Localization
01-20
Robot_Localization 链接：https://kapernikov.com/the-ros-robot_localization-package/ github源代码链接：https://github.com/Kapernikov/ros_robot_localization_tutorial 虚拟传感器 This tutorial uses the turtlesim package as a virtual robot. We will add a virtual odometer and a virtual (LiDAR) positioning system (both
探索ROS中的`robot_localization`：精准定位与导航的利器
gitblog_00069的博客
03-26 466
探索ROS中的robot_localization：精准定位与导航的利器 项目地址:https://gitcode.com/cra-ros-pkg/robot_localization 在机器人领域，精确的定位和导航是所有自主移动机器人的基础。robot_localization是一个强大的开源ROS（Robot Operating System）包，专为解决这一问题而设计。本文将深入介绍该项目...
一文讲清楚robot localization如何在自动驾驶小车上实现ROS机器人定位
渡众机器人
02-01 1096
关键参数包括传感器的噪声协方差、初始估计的状态协方差、过程噪声协方差等。正确设置这些参数对于确保定位精度至关重要。例如，过小的噪声协方差可能导致过分依赖某些不准确的传感器数据，而过大的噪声协方差可能导致滤波器对传感器数据不够敏感
使用robot_localization 实现传感器融合 的深入分步教程_设计_文档_相关文件_下载
07-13
一个深入的分步教程，用于使用来自robot_localization 的扩展卡尔曼滤波器节点实现传感器融合！这里解释了协方差和卡尔曼滤波器等基本概念！ 本教程特别有用，因为还没有一个完整的端到端实现教程，用于使用 robots_localization 包进行传感器融合。 您可以在示例实现文件夹中找到实现！ 为什么要融合传感器数据 很多时候，机器人应用程序中的单个导航堆栈组件可能会经常失败，但它们一起形成了一个更强大的整体。 一种方法是使用robots_localization包中的扩展卡尔曼滤波器。该软件包具有一个相对简单的 ROS 界面，可帮助您融合和配置传感器，这就是我们将要使用的！ 更多详情、使用方法，请下载后阅读README.md文件
LidarSLAM（四）：robot_localization
qq_37662375的博客
08-01 1550
X i= [1.1, 1.9, 3] Yi = [5.0, 10.4, 14.6] E(X) = (1.1 + 1.9 + 3) / 3 = 2 E(Y) = (5.0 + 10.4 + 14.6) / 3 = 10 E(XY) = (1.1 × 5.0 + 1.9 × 10.4 + 3 × 14.6) / 3 = 23.02 Cov(X,Y) = E(XY) - E(X)E(Y) = 23.02 - 2 × 10 = 3.02 此外：还可以计算：D(X) = E(X^2) - E^2(X) =
【ROS2定位】Robot_localization教程
gongdiwudu的专栏
05-15 4202
为了对robot_localization这个包进行介绍，本篇特意用乌龟演示程序为例，尽可能多地进行虚拟场景传感器演示。虚拟演示通过了，更换实际传感器也无更多区别。
ROS学习（一）robot_localization功能包
刘小浪
11-13 5588
内容参考：ROS_wiki robot_localization功能包是状态估计节点的集合，每个状态估计节点都是对机器人在三维空间中移动的非线性状态估计的实现。包含两个节点ekf_localization_node and ukf_localization_node.此外还提供了 navsat_transform_node节点用于整合GPS数据 Features特点 Fusion of an ...
robot_localization：使用
viphl的专栏
05-11 3061
转自：https://blog.csdn.net/viphl/article/details/116651921 robot_localization包没有限制传感器的数据输入。 支持的状态估计节点数据类型： • nav_msgs/Odometry • geometry_msgs/PoseWithCovarianceStamped • geometry_msgs/TwistWithCovarianceStamped • sensor_msgs/Imu 状态向量：[x y z α β γ x˙ y˙
ros导航【02】：robot_localization基本概念
gongdiwudu的专栏
04-16 6321
robot_localization 是状态估计节点的集合，每个节点都是用于在 3D 空间中移动的机器人的非线性状态估计器的实现。它包含两个状态估计节点，ekf_localization_node 和 ukf_localization_node。此外，robot_localization 提供了 navsat_transform_node，它有助于 GPS 数据的集成。 robots_localization 中的所有状态估计节点都有共同的特征，即： 融合任意数量的传感器。 节点不限制输入源的数量。例如
ros robot_localization
weixin_35536487的博客
01-21 820
robot_localization是状态估计节点的集合，每个节点都是针对在3D空间中移动的机器人的非线性状态估计器的实现。 它包含两个状态估计节点ekf_localization_node和ukf_localization_node。 另外，robot_localization提供了navsat_transform_node，它有助于集成GPS数据。 robot_localization中的所有状态估计节点都具有共同的特征，即： 融合任意数量的传感器。 节点不限制输入源的数量。 例如，如果您的机器人具
robot_localization_localization_雷达定位ekf_环境建图_ROS_ros小车建图_
10-04
在ros环境下，通过单线雷达实现小车室内建图并定位
Robot Localization and Map Building
02-23
Robot Localization and Map Building;Robot Localization and Map Building;Robot Localization and Map Building;
ＲＯＳ定位功能包：ｒｏｂｏｔ＿ｌｏｃａｌｉｚａｔｉｏｎ
最新发布
04-16
ＲＯＳ定位功能包：ｒｏｂｏｔ＿ｌｏｃａｌｉｚａｔｉｏｎ
robot_localization:robot_localization是非线性状态估计节点的程序包。 该程序包是由Charles River Analytics，Inc.开发的。请在Answers.ros.org上提问。
04-29
robot_localization robot_localization是非线性状态估计节点的程序包。 该软件包是由Charles River Analytics，Inc.开发的。 请在此处查看文档： :
机器人状态估计：robot_localization 功能包使用方法
月照银海似蛟龙的博客
08-06 1264
移动机器人的状态估计需要用到很多传感器，因为对单一的传感器来讲，都存在各自的优缺点，所以需要一种多传感器融合技术，将机器人的状态估计出来。惯导轮速里程计激光里程计视觉里程计gps等如何利用各传感器的优点，将所有数据结合起来，取长补短，就用到了本篇博客介绍的内容，一个移动机器人状态估计功能包。是状态估计节点的集合，每个节点都是非线性状态估计器的一种实现，用于在3D空间中移动的机器人。它包括两个状态估计节点和。另外，提供，它有助于集成GPS数据。功能包特点融合任意数量的传感器。
robot_localization(1)：介绍，安装和状态估计节点
热门推荐
W_Tortoise的博客
12-02 1万+
1 robot_localization robot_localization是状态估计节点的集合，每个节点都是非线性状态估计器的一种实现，用于在3D空间中移动的机器人。它包括两个状态估计节点ekf_localization_node和ukf_localization_node。另外，robot_localization提供navsat_transform_node，它有助于集成GPS数据。 Gi...
ROS察微【51】：如何将里程计和 IMU 与 robots_localization 融合
gongdiwudu的专栏
05-20 7303
笔记 是 ROS 开发人员 Live Class n.51 的附加材料，由 The Construct 的 Alberto Ezquerro 和 Ricardo Tellez 免费创建和提供。 只要您提供本段的副本，您就可以分发此笔记本。 在今天的直播课中，我们将学习以下内容： 为什么需要融合传感器数据进行导航 什么是 robots_localization 包 如何使用robot_localization 包进行传感器融合 此直播课程的先决条件是： ROS 概念的基础知识，如主题、发布和
数据融合-机器人定位
kkkmmmjjjj的博客
04-21 3713
来源：https://www.jianshu.com/p/415b288c8f3d什么是机器人定位robot_localizationrobot_localization是一系列的机器人状态估计节点集合，其中每一个都是用于三维平面的机器人非线性状态估计，它包括两个机器人状态估计节点ekf_localization_node和ukf_localization_node。此外也提供了 navsat_t...
ros功能包robot_localization如何使用
06-09
要使用ROS功能包robot_localization，您需要按照以下步骤操作： 1. 安装robot_localization功能包 在终端中执行以下命令，安装robot_localization功能包： ``` sudo apt-get install ros-<distro>-robot-localization ``` 其中，`<distro>`是您正在使用的ROS发行版的名称。例如，如果您使用的是ROS Kinetic，那么命令应该是： ``` sudo apt-get install ros-kinetic-robot-localization ``` 2. 配置参数文件 在使用robot_localization功能包之前，您需要配置一个参数文件来定义您的传感器和估计器的设置。您可以使用以下命令将默认参数文件复制到您的工作空间中： ``` cp /opt/ros/<distro>/share/robot_localization/params/ekf_template.yaml <workspace>/src/<package>/config/ekf.yaml ``` 请注意将`<distro>`替换为您正在使用的ROS发行版的名称，并将`<workspace>`替换为您的工作空间的路径，`<package>`替换为您要使用robot_localization功能包的ROS软件包的名称。 然后，您需要编辑`<workspace>/src/<package>/config/ekf.yaml`文件，根据您的传感器和估计器的设置进行配置。 3. 启动节点 在终端中执行以下命令，启动robot_localization节点： ``` roslaunch <package> <launch_file>.launch ``` 请注意将`<package>`替换为您要使用robot_localization功能包的ROS软件包的名称，`<launch_file>`替换为您要启动的launch文件的名称。 4. 查看输出 启动节点后，robot_localization将开始接收传感器数据，并使用您的配置进行状态估计。您可以使用以下命令查看robot_localization节点的输出： ``` rostopic echo /odometry/filtered ``` 请注意将`/odometry/filtered`替换为您在参数文件中配置的输出话题。 这些是使用robot_localization功能包的基本步骤。您可以在ROS Wiki上找到更多详细的信息和示例。

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 8
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值