Localization pure

最新推荐文章于 2023-06-16 09:42:03 发布

老薛之猫

最新推荐文章于 2023-06-16 09:42:03 发布

阅读量296

点赞数

分类专栏： ROS平台搭建文章标签： ubuntu

本文链接：https://blog.csdn.net/qq_42276066/article/details/103496641

版权

ROS平台搭建专栏收录该内容

6 篇文章 0 订阅

订阅专栏

https://blog.csdn.net/weixin_43259286/article/details/105143605

4-25 导航定位测试（盛）

发现问题

1、导航过程中地图漂移，说明地图还在更新。
2、全局规划，穿越障碍物、和未知区域，以及反复更新方向完全不同的全局规划。
无障碍物时，全局规划不成功。
3、全局规划成功，但车辆不运动。
4、导航过程中，回原点，偏差很大（发现导航运行时，位置已经不准，也不能自动校准恢复）
改进：导航过程中开启ODOM，是否会有改善。
5、掉网问题。
6、压脚问题（下部无传感器）。
7、陀螺仪损坏一个，导致单片机看门狗动作，反复复位。
8、激光雷达在特定角度，发现大片不存在的障碍物。

改进测试方案

1、增加手推建图模式，平板操作车辆不方便。
2、查看ctrl_buff 模块CPU占用率，如果过大，删除模块。
3、增加导航时，ODOM 模块信息，看导航定位效果是否提升。
4、改进纯定位模式，修改cartographer源程序，进行测试，是否在导航时，地图不再更新。
5、建立完整地图，并录制数据，进行回放分析。
6、导航时也录制数据，进行回放仿真分析。
7、查找充电模块的BUG，避免因通讯丢包无法退出充电模式的问题。
8、搭载M1M1 进行建图对比测试。
9、修改配置文件，雷达最远距离。将回环检测由90,改为20，提高建图精度。

改为amcl

1、cpu下降至20-30左右，定位比较迅速，定位精度也不差，感觉socket_node通信也变好了。
2、修改cartographer 的pure 源文件，不发布SUB子图了，但导航地图也不发了。
与原文代码版本不一样。

另一篇文章，解决纯定位问题

<launch>
  <!-- <param name="/use_sim_time" value="true" /> -->
  <param name="/use_sim_time" value="false" />  <!--//1.仿真为true-->
 
  <param name="robot_description"
    textfile="$(find cartographer_ros)/urdf/my_backpack_2d.urdf" />  <!--//2.修改机器人模型-->
 
  <node name="robot_state_publisher" pkg="robot_state_publisher"
    type="robot_state_publisher" />
 
  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename my_backpack_2d_localization.lua  //3.纯定位接口参数设置 
          <!-- -load_state_filename $(arg load_state_filename)" -->
          -load_state_filename /home/yourname/Map/map.pbstream" <!--//4.纯定位使用的地图，我是直接用固定路径,这里按照自己的路径修改即可-->
      output="screen">
    <!-- <remap from="echoes" to="horizontal_laser_2d" /> --> <!--//这个好像没啥用，我们一般用的都是 scan-->
  </node>
 
  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.05
                                                    -pure_localization 1" /> <!--//5.修改文件occupancy_node_main.cc  增加是否纯定位模式参数pure_localization（如果选择直接注释就不需要增加这个参数了）-->
 
  <node name="rviz" pkg="rviz" type="rviz" required="true"
      args="-d $(find cartographer_ros)/configuration_files/demo_2d.rviz" />
   <!--<node name="playbag" pkg="rosbag" type="play"
      args="--clock $(arg bag_filename)" /> --> <!--//6.不需要跑数据集，直接打开自己的雷达-->
</launch>
————————————————
版权声明：本文为CSDN博主「梦凝小筑」的原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/weixin_36976685/article/details/103834608在这里插入代码片

Cartographer纯定位接口：

文件：backpack_2d_localization.lua:

TRAJECTORY_BUILDER.pure_localization_trimmer = {
  max_submaps_to_keep = 3, //最大保存子图数，存定位模式通过子图进行定位，但只需要当前和上一个子图即可，我这里设置的是2
}
POSE_GRAPH.optimize_every_n_nodes = 20 //每20个有效帧一个子图，子图构建完成要闭环检测一次，这个数越小，闭环检测越频繁，当然CPU爆炸

修改说明：

仿真为true，实际测试false

2.修改机器人模型这里建议新建一个 my_backpack_2d.urdf

里面设置自己的机器人模型，下面是我的

<robot name="cartographer_backpack_2d">
  <material name="orange">
    <color rgba="1.0 0.5 0.2 1" />
  </material>
  <material name="gray">
    <color rgba="0.2 0.2 0.2 1" />
  </material>
 
  <link name="imu_link">
    <visual>
      <origin xyz="0 0 0" />
      <geometry>
        <box size="0.06 0.04 0.02" />
      </geometry>
      <material name="orange" />
    </visual>
  </link>
 
  <link name="base_scan">
    <visual>
      <origin xyz="0 0 0" />
      <geometry>
        <cylinder length="0.05" radius="0.03" />
      </geometry>
      <material name="gray" />
    </visual>
  </link>
  
  <link name="base_footprint" />
 
  <joint name="imu_link_joint" type="fixed">
    <parent link="base_footprint" />
    <child link="imu_link" />
    <origin xyz="0 0 0" />
  </joint>
 
  <joint name="base_scan_joint" type="fixed">
    <parent link="base_footprint" />
    <child link="base_scan" />
    <origin xyz="0.160 0 0.0558" />
  </joint>
 
</robot>

3.纯定位接口参数设置同样建议新建一个 my_backpack_2d_localization.lua
设置如下：
include “my_backpack_2d.lua”

TRAJECTORY_BUILDER.pure_localization_trimmer = {
  max_submaps_to_keep = 3, //最大保存子图数，存定位模式通过子图进行定位，但只需要当前和上一个子图即可，我这里设置的是2
}
POSE_GRAPH.optimize_every_n_nodes = 20 //每20个有效帧一个子图，子图构建完成要闭环检测一次，这个数越小，闭环检测越频繁，当然CPU爆炸

上面第一行引用的文件是 my_backpack_2d.lua，这个文件是backpack_2d.lua修改过来的，配置Cartographer在定位过程中的其它参数的，这里也建议新建一个my_backpack_2d.lua，用于区分建图时用的参数。

4.纯定位使用的地图，我是直接用固定路径,这里按照自己的路径修改即可

5.修改文件occupancy_node_main.cc
这个文件在路径cartographer_ros-master\cartographer_ros\cartographer_ros\occupancy_grid_node_main.cc
第171行或者直接搜索下面这行代码：
//occupancy_grid_publisher_.publish(*msg_ptr);
注释掉这行代码，不然你纯定位过程建立的子图会覆盖在之前建好的地图上。
但是这行代码在建图的时候又肯定的是需要的，我的解决方法是引入外部参数进行切换。
修改文件在https://github.com/lllray/file/blob/master/occupancy_grid_node_main.cc

6.不需要跑数据集，直接打开自己的雷达
因为是用在自己的机器人上，所以不需要数据集
一般的步骤是：
先开雷达驱动
再开机器人驱动
最后打开我们修改的demo_backpack_2d_localization.launch文件

补充说明：
如果上述步骤失败的，检查下自己的TF变换是否正确，TOPIC是否对上了。
ROS用在机器人导航上就这些问题，只要TOPIC对上了，TF变换正确，基本都没啥问题，这也正是ROS的强大之处。

三、定位结果
我觉的很好，比AMCL好多了，哈哈哈，你们可以试试。
有一个很强大之处，由于Cartographer定位是是当前帧和子图进行匹配，所以对于机器人周围被新物体遮挡的情况完全没有影响。
唯一影响就是没法重定位了，因为地图上没有这些新物体，如下面那一圈障碍。
但是就算长时间被遮挡，存在累积定位误差，只要机器人离开了遮挡区域，回到了之前的环境，就可以重定位回来。
因此Cartographer定位十分稳定，特别是一大堆领导来检查的时候（一般都会把可怜的机器人团团围住），完全没怕的，哈哈哈。

结语：好久没更新了，多了好多粉丝，为了感谢大家的关注，写一篇粉丝可见的文章。
————————————————
版权声明：本文为CSDN博主「梦凝小筑」的原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/weixin_36976685/article/details/103834608

/*
 * Copyright 2016 The Cartographer Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <cmath>
#include <string>
#include <vector>

#include "Eigen/Core"
#include "Eigen/Geometry"
#include "absl/synchronization/mutex.h"
#include "cairo/cairo.h"
#include "cartographer/common/port.h"
#include "cartographer/io/image.h"
#include "cartographer/io/submap_painter.h"
#include "cartographer/mapping/id.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer_ros/msg_conversion.h"
#include "cartographer_ros/node_constants.h"
#include "cartographer_ros/ros_log_sink.h"
#include "cartographer_ros/submap.h"
#include "cartographer_ros_msgs/SubmapList.h"
#include "cartographer_ros_msgs/SubmapQuery.h"
#include "gflags/gflags.h"
#include "nav_msgs/OccupancyGrid.h"
#include "ros/ros.h"

//LX ADD 0412
DEFINE_int32(pure_localization,0,"Pure localization !");
//END 0412
DEFINE_double(resolution, 0.05,
              "Resolution of a grid cell in the published occupancy grid.");
DEFINE_double(publish_period_sec, 1.0, "OccupancyGrid publishing period.");
DEFINE_bool(include_frozen_submaps, true,
            "Include frozen submaps in the occupancy grid.");
DEFINE_bool(include_unfrozen_submaps, true,
            "Include unfrozen submaps in the occupancy grid.");
DEFINE_string(occupancy_grid_topic, cartographer_ros::kOccupancyGridTopic,
              "Name of the topic on which the occupancy grid is published.");

namespace cartographer_ros {
namespace {

using ::cartographer::io::PaintSubmapSlicesResult;
using ::cartographer::io::SubmapSlice;
using ::cartographer::mapping::SubmapId;

class Node {
 public:
  //LX CHANGE 0412
  //explicit Node(double resolution, double publish_period_sec);
  explicit Node(int pure_localization, double resolution, double publish_period_sec);
  //END 0412

  ~Node() {}

  Node(const Node&) = delete;
  Node& operator=(const Node&) = delete;

 private:
  void HandleSubmapList(const cartographer_ros_msgs::SubmapList::ConstPtr& msg);
  void DrawAndPublish(const ::ros::WallTimerEvent& timer_event);

  ::ros::NodeHandle node_handle_;
  const double resolution_;

  //LX ADD 0412
  const int pure_localization_;
  //END 0412

  absl::Mutex mutex_;
  ::ros::ServiceClient client_ GUARDED_BY(mutex_);
  ::ros::Subscriber submap_list_subscriber_ GUARDED_BY(mutex_);
  ::ros::Publisher occupancy_grid_publisher_ GUARDED_BY(mutex_);
  std::map<SubmapId, SubmapSlice> submap_slices_ GUARDED_BY(mutex_);
  ::ros::WallTimer occupancy_grid_publisher_timer_;
  std::string last_frame_id_;
  ros::Time last_timestamp_;
};

    //LX CHANGE 0412
    //Node::Node(const double resolution, const double publish_period_sec)
 Node::Node(const int pure_localization, const double resolution, const double publish_period_sec)
 //: pure_localization_(pure_localization), //END 0412
  :  resolution_(resolution),
     pure_localization_(pure_localization),
      client_(node_handle_.serviceClient<::cartographer_ros_msgs::SubmapQuery>(
          kSubmapQueryServiceName)),
      submap_list_subscriber_(node_handle_.subscribe(
          kSubmapListTopic, kLatestOnlyPublisherQueueSize,
          boost::function<void(
              const cartographer_ros_msgs::SubmapList::ConstPtr&)>(
              [this](const cartographer_ros_msgs::SubmapList::ConstPtr& msg) {
                HandleSubmapList(msg);
              }))),
      occupancy_grid_publisher_(
          node_handle_.advertise<::nav_msgs::OccupancyGrid>(
              FLAGS_occupancy_grid_topic, kLatestOnlyPublisherQueueSize,
              true /* latched */)),
      occupancy_grid_publisher_timer_(
          node_handle_.createWallTimer(::ros::WallDuration(publish_period_sec),
                                       &Node::DrawAndPublish, this)) {}

void Node::HandleSubmapList(
    const cartographer_ros_msgs::SubmapList::ConstPtr& msg) {
  absl::MutexLock locker(&mutex_);

  // We do not do any work if nobody listens.
  if (occupancy_grid_publisher_.getNumSubscribers() == 0) {
    return;
  }

  // Keep track of submap IDs that don't appear in the message anymore.
  std::set<SubmapId> submap_ids_to_delete;
  for (const auto& pair : submap_slices_) {
    submap_ids_to_delete.insert(pair.first);
  }

  for (const auto& submap_msg : msg->submap) {
    const SubmapId id{submap_msg.trajectory_id, submap_msg.submap_index};
    submap_ids_to_delete.erase(id);
    if ((submap_msg.is_frozen && !FLAGS_include_frozen_submaps) ||
        (!submap_msg.is_frozen && !FLAGS_include_unfrozen_submaps)) {
      continue;
    }
    SubmapSlice& submap_slice = submap_slices_[id];
    submap_slice.pose = ToRigid3d(submap_msg.pose);
    submap_slice.metadata_version = submap_msg.submap_version;
    if (submap_slice.surface != nullptr &&
        submap_slice.version == submap_msg.submap_version) {
      continue;
    }

    auto fetched_textures =
        ::cartographer_ros::FetchSubmapTextures(id, &client_);
    if (fetched_textures == nullptr) {
      continue;
    }
    CHECK(!fetched_textures->textures.empty());
    submap_slice.version = fetched_textures->version;

    // We use the first texture only. By convention this is the highest
    // resolution texture and that is the one we want to use to construct the
    // map for ROS.
    const auto fetched_texture = fetched_textures->textures.begin();
    submap_slice.width = fetched_texture->width;
    submap_slice.height = fetched_texture->height;
    submap_slice.slice_pose = fetched_texture->slice_pose;
    submap_slice.resolution = fetched_texture->resolution;
    submap_slice.cairo_data.clear();
    submap_slice.surface = ::cartographer::io::DrawTexture(
        fetched_texture->pixels.intensity, fetched_texture->pixels.alpha,
        fetched_texture->width, fetched_texture->height,
        &submap_slice.cairo_data);
  }

  // Delete all submaps that didn't appear in the message.
  for (const auto& id : submap_ids_to_delete) {
    submap_slices_.erase(id);
  }

  last_timestamp_ = msg->header.stamp;
  last_frame_id_ = msg->header.frame_id;
}

void Node::DrawAndPublish(const ::ros::WallTimerEvent& unused_timer_event) {
  absl::MutexLock locker(&mutex_);
  if (submap_slices_.empty() || last_frame_id_.empty()) {
    return;
  }
  auto painted_slices = PaintSubmapSlices(submap_slices_, resolution_);
  std::unique_ptr<nav_msgs::OccupancyGrid> msg_ptr = CreateOccupancyGridMsg(
      painted_slices, resolution_, last_frame_id_, last_timestamp_);
  //LX CHANGE 0412
  //occupancy_grid_publisher_.publish(*msg_ptr);
  if(pure_localization_ != 1 )occupancy_grid_publisher_.publish(*msg_ptr);
  //END 0412
}

}  // namespace
}  // namespace cartographer_ros

int main(int argc, char** argv) {
  google::InitGoogleLogging(argv[0]);
  google::ParseCommandLineFlags(&argc, &argv, true);

  CHECK(FLAGS_include_frozen_submaps || FLAGS_include_unfrozen_submaps)
      << "Ignoring both frozen and unfrozen submaps makes no sense.";

  ::ros::init(argc, argv, "cartographer_occupancy_grid_node");
  ::ros::start();

  cartographer_ros::ScopedRosLogSink ros_log_sink;
  ::cartographer_ros::Node node(FLAGS_pure_localization, FLAGS_resolution, FLAGS_publish_period_sec);

  ::ros::spin();
  ::ros::shutdown();
}

更早的文章1

上车测试：
测试：发现车辆位置轻微快速漂移，后关闭了提供ODOM帧，TF树上无ODOM了，并将MOVE_BASE的
参数都改为 /map 和 /base_link , 别的参数不用改，主要改地代价地图，将ODOM 改为MAP 后，定位
很准，无漂移。

后将车子凌空搬动1米，还是出现位置漂移，未达到预期效果，关闭use_odometry，单独使用IMU还是
不正常，原因待查，怀疑是IMU 数据不对。
观察到车辆轻微打滑，定位几乎不受影响。

1）修改了SCAN 的 num_accumulated_range_data 参数后，在单独SCAN/ IMU+SCAN /IMU+ODOM+SCAN
感觉定位效果都可以，CPU 占用率差不不大。平均30%-40%，偶尔到120%。i3 - 6100U。

1、Localization launch

 <param name="/use_sim_time" value="true" />

  <param name="robot_description"
    textfile="$(find cartographer_ros)/urdf/backpack_2d.urdf" />

  <node name="robot_state_publisher" pkg="robot_state_publisher"
    type="robot_state_publisher" />

  <node name="cartographer_node" pkg="cartographer_ros"
      type="cartographer_node" args="
          -configuration_directory $(find cartographer_ros)/configuration_files
          -configuration_basename backpack_2d_localization.lua
          -load_state_filename $(arg load_state_filename)"
      output="screen">
    <remap from="echoes" to="horizontal_laser_2d" />
  </node>

  <node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
      type="cartographer_occupancy_grid_node" args="-resolution 0.05" />

  <node name="playbag" pkg="rosbag" type="play"
      args="--clock $(arg bag_filename)" />

2、仿真启动命令
roslaunch t3_l.launch load_state_filename:= ${HOME}/maps/mymap.pbstream bag_filename:=$ {HOME}/bag/rdata_1.bag

3、backpack_2d_localization.lua 文件内容
在这里插入图片描述
4、屏幕信息截图

5、TF树
提供：

不提供ODOM 帧：

在这里插入图片描述

老薛之猫

关注

0
点赞
踩
1

收藏

觉得还不错? 一键收藏
0
评论
Localization pure

1、Localization launch <param name="/use_sim_time" value="true" /> <param name="robot_description" textfile="$(find cartographer_ros)/urdf/backpack_2d.urdf" /> <node name="rob...
复制链接

扫一扫