[KUSMO] 11/4 (Navigation Stack 구성법)

 

로봇의 ROS Navigaion Stack 구성

• Navigation stack은 로봇을 시작점에서부터 목적지까지 안전하게 움직일 수 있게 하는 소프트웨어의 집합
• http://wiki.ros.org/navigation/Tutorials/RobotSetup 

navigation/Tutorials/RobotSetup - ROS Wiki

 

 

흰색은 반드시 미리 실행중이어야 하는 node, 회색은 선택적으로 미리 실행되는 node, 파란색은 각 robot platform마다 형성되어야 하는 node

- Prerequisite : Robot이 각 Navigation Stack에서 사용할 각 센서들로부터의 data ( ex) sensor_msgs/LaserScan)를 받아 TF를 통해 coordinate frame에 대한 정보를 publish해야 하며, TF, 그리고 base로 전송할 velocity command를 받으며  nav_msgs/Odometry message를 통해 odometry 정보를 publish해야 한다.

 

 

$ sudo apt install ros-noetic-navigation

• ROS Navigation Stack 설치

$ rospack find amcl

• 제대로 설치되었는지 확인
• '/opt/ros/noetic/share/amcl' 가 출력되면 올바르게 설치가 된 것

 

- Package 생성

$ cd ~/catkin_ws/src
$ mkdir kusmo
$ cd /kusmo
$ catkin_create_pkg navstack rospy roscpp std_msgs tf tf2_ros geometry_msgs sensor_msgs nav_msgs move_base

• catkin folder 로 이동 후 'kusmo_bot' 라는 이름의 폴더 생성
• kusmo는 우리 팀의 이름
• 생성한 폴더로 이동하여 navstack_pub라는 이름의 패키지 생성
• navstack_pub가 package의 이름이고 뒤의 단어들은 dependency들을 의미

 

$ cd navstack
$ mkdir param
$ gedit CMakeLists.txt

• 해당 패키지로 이동 후 param이라는 이름의 폴더 생성
• CMakeLists.txt 파일을 열고 C+11 사용을 위해 5번 줄의 '#' 제거

 

$ cd ~/catkin_ws/
$ catkin_make --only-pkg-with-deps navstack

• Package를 compile

$ cd src/kusmo/navstack
$ mkdir launch
$ cd launch
$ gedit kusmo.launch

• 새로운 터미널 창을 열고 navstack_pub 패키지로 이동하여 launch directory 생성
• 해당 directory로 이동 후 launch 파일 생성

<launch>
</launch>

 

• Navigation에 필요한 각종 정보들을 사용하기 위해 아래의 코드 입력

<launch>

  <!-- Transformation Configuration ... Setting Up the Relationships Between Coordinate Frames --> 
  <node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="0.06 0 0.08 0 0 0 base_link laser 30" />
  <node pkg="tf" type="static_transform_publisher" name="imu_broadcaster" args="0 0.06 0.02 0 0 0 base_link imu 30" />
  <node pkg="tf" type="static_transform_publisher" name="base_link_broadcaster" args="0 0 0.09 0 0 0 base_footprint base_link 30" />
  <!-- odom to base_footprint transform will be provided by the robot_pose_ekf node -->
  <!-- map to odom will be provided by the AMCL -->
  <node pkg="tf" type="static_transform_publisher" name="map_to_odom" args="0 0 0 0 0 0 map odom 30" />

  <!-- Wheel Encoder Tick Publisher and Base Controller Using Arduino -->  
  <!-- motor_controller_diff_drive_2.ino is the Arduino sketch -->
  <!-- Subscribe: /cmd_vel -->
  <!-- Publish: /right_ticks, /left_ticks -->
  <node pkg="rosserial_python" type="serial_node.py" name="serial_node">
    <param name="port" value="/dev/ttyACM0"/>
    <param name="baud" value="115200"/>
  </node>

  <!-- Wheel Odometry Publisher -->
  <!-- Subscribe: /right_ticks, /left_ticks, /initial_2d -->
  <!-- Publish: /odom_data_euler, /odom_data_quat -->
  <node pkg="localization_data_pub" type="ekf_odom_pub" name="ekf_odom_pub">
  </node> 
	
  <!-- IMU Data Publisher Using the BNO055 IMU Sensor -->
  <!-- Publish: /imu/data -->
  <node ns="imu" name="imu_node" pkg="imu_bno055" type="bno055_i2c_node" respawn="true" respawn_delay="2">
    <param name="device" type="string" value="/dev/i2c-1"/>
    <param name="address" type="int" value="40"/> <!-- 0x28 == 40 is the default for BNO055 -->
    <param name="frame_id" type="string" value="imu"/>
  </node>
	
  <!-- Extended Kalman Filter from robot_pose_ekf Node-->
  <!-- Subscribe: /odom, /imu_data, /vo -->
  <!-- Publish: /robot_pose_ekf/odom_combined -->
  <remap from="odom" to="odom_data_quat" />
  <remap from="imu_data" to="imu/data" />
  <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
    <param name="output_frame" value="odom"/>
    <param name="base_footprint_frame" value="base_footprint"/>
    <param name="freq" value="30.0"/>
    <param name="sensor_timeout" value="1.0"/>
    <param name="odom_used" value="true"/>
    <param name="imu_used" value="true"/>
    <param name="vo_used" value="false"/>
    <param name="gps_used" value="false"/>
    <param name="debug" value="false"/>
    <param name="self_diagnose" value="false"/>
  </node>
	
  <!-- Initial Pose and Goal Publisher -->
  <!-- Publish: /initialpose, /move_base_simple/goal -->
  <node pkg="rviz" type="rviz" name="rviz" args="-d /home/automaticaddison/catkin_ws/src/jetson_nano_bot/navigation_data_pub/maps/floorplan4.rviz">
  </node> 

  <!-- Subscribe: /initialpose, /move_base_simple/goal -->
  <!-- Publish: /initial_2d, /goal_2d --> 
  <node pkg="localization_data_pub" type="rviz_click_to_2d" name="rviz_click_to_2d">
  </node>   

  <!-- Lidar Data Publisher Using RPLIDAR from Slamtec -->
  <!-- Used for obstacle avoidance and can be used for mapping --> 
  <!-- Publish: /scan -->
  <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
    <param name="serial_port"         type="string" value="/dev/ttyUSB0"/>
    <param name="serial_baudrate"     type="int"    value="115200"/><!--A1/A2 -->
    <!--param name="serial_baudrate"  type="int"    value="256000"--><!--A3 -->
    <param name="frame_id"            type="string" value="laser"/>
    <param name="inverted"            type="bool"   value="false"/>
    <param name="angle_compensate"    type="bool"   value="true"/>
  </node>  

  <!-- Map File -->
  <arg name="map_file" default="$(find navigation_data_pub)/maps/floorplan4.yaml"/>
	
  <!-- Map Server -->
  <!-- Publish: /map, /map_metadata -->
  <node pkg="map_server" name="map_server" type="map_server" args="$(arg map_file)" />
	
  <!-- Add AMCL example for differential drive robots for Localization -->
  <!-- Subscribe: /scan, /tf, /initialpose, /map -->
  <!-- Publish: /amcl_pose, /particlecloud, /tf -->
  <include file="$(find amcl)/examples/amcl_diff.launch"/>
	
  <!-- Move Base Node -->
  <!-- Subscribe: /move_base_simple/goal -->
  <!-- Publish: /cmd_vel -->
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <rosparam file="$(find navstack_pub)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find navstack_pub)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find navstack_pub)/param/local_costmap_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find navstack_pub)/param/global_costmap_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find navstack_pub)/param/base_local_planner_params.yaml" command="load" />
  </node>

</launch>

 

- Transformation / Costmap(Global and Local Costmaps) / Common(Global and Local Costmap) / Global / Local / Base Local Planner / AMCL Configuration,

Sensor / Lidar / Odometry / Mapping Information,

Base Controller, Move Base Node 설정

• Transformation Configuration : Navigation stack을 사용하려면 로봇은 ROS의 tf package를 통해 coordinate frame간 관계에 대한 정보를 지속적으로 publish 해야 함
  • Transformation Tree는 서로 다른 coordinate frame들 사이의 회전과 평행 이동(x, y, z축)에 대한 offset들을 정의한다
  • Laser 정보는 Lidar를 통해서 받는데 Lidar와 로봇의 중심간에 거리차이가 있을 경우 tf를 통해 해당 거리차이만큼의 offset을 정의한다

• Costmap Configuration (Global and Local Costmaps) : ROS Navigation Stack은 현실 세계에 있는 장애물에 대한 정보를 아래의 두 costmap들에 저장하며, 이 costmap들은 .yaml 파일에 구성되어야 한다
  
  • Global costmap: 사용자가 생성한 static map(SLAM을 통해 생성된 지도)으로부터 만들어지며 Global Planner가 경로를 계산하기 위해 사용한다. Static map에 의해 제공된 너비, 높이, 장애물의 정보에 맞게 초기화되어 amcl등의 localization system과 함께 조합된다
    
    • Global costmap은 새로운 장애물이 나타난다거나 하는 등의 환경이 바뀌더라도 바뀌지 않음
  • Local costmap: 로봇의 센서 데이터로부터 만들어지며 Local Planner가 local plan을 계산하기 위해 사용한다. 로봇이 움직임에 따라 같이 움직이며 실시간으로 장애물에 대한 정보를 얻는다
    • Local planner는 Global planner가 계산한 경로를 분할한 후 실행하며, Global planner 와 달리 odometry와 laser data를 계속 확인하고, 충돌이 없는 local plan을 선택함
    • 즉 Local planner는 로봇의 충돌을 피하기 위해 경로를 즉시 수정할 수 있다. Local plan은 /local_plan topic으로 publish 됨
    • Local plan은 Global plan의 일부분
•  Costmap은 각 cell들이 특정한 값(cost)를 부여받는 grid map으로, 값이 클수록 로봇과 장애물 사이의 거리가 가까움을 의미한다. 일반적으로 각 cell들은 0~255의 binary값을 가지며, 그 중 자주 사용되는 값들이 있다
  • 255 = NO_INFORMATION : 충분한 정보가 없어 보류된 cell
  • 254 = LETHAL_OBSTACLE: 충돌이 일어나는 장애물이 있는 cell
  • 253 = INSCRIBED_INFLATED_OBSTACLE: 장애물은 없으나 해당 cell이 로봇의 중심에 가면 충돌이 일어날 수 있는 cell
  • 128 ~ 252 = 충돌이 일어날 가능성이 있는 cell
  • 0 = FREE_SPACE: 장애물이 없는 cell

거리에 따라 감소하는, cell들이 갖는 cost값의 전파 과정인 inflation에 대한 그림. 출처 : http://wiki.ros.org/costmap_2d

 

• Common Configuration : global / local costmap이 모두 다 사용하는 parameter들 구성

$ cd ~/catkin_ws
$ roscd navstack_pub
$ cd param
$ gedit costmap_common_params.yaml

• navstack_pub 패키지로 이동하여 costmap_common_params.yaml 생성
• 그 후 아래 내용들 입력

obstacle_range: 0.5
raytrace_range: 0.5
footprint: [[-0.14, -0.14], [-0.14, 0.14], [0.14, 0.14], [0.14, -0.14]]
map_topic: /map
subscribe_to_updates: true
global_frame: odom		# costmap이 작동할 때의 global frame. /map이 기본값
robot_base_frame: base_link
update_frequency: 30.0
publish_frequency: 30.0
rolling_window: false	# costmap의 rolling window version 사용 여부를 결정	
# static_map parameter가 true이면 이 parameter는 false이어야 함
# rolling window 사용시 주행 환경을 costmap을 통해 전체적으로 나타내지 않고 
# 로봇 주변의 반경 일부만 나타냄

plugins:	# costmap_2d layer에 사용되는 plugin의 parameter namespace 정의를 위해 사용
# ROS는 layer를 통해 parameter들을 분리함
  - {name: static_layer, type: "costmap_2d::StaticLayer"}
  - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
  - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

static_layer:
  map_topic: /map
  subscribe_to_updates: false

obstacle_layer:
    observation_sources: laser_scan_sensor
    laser_scan_sensor: {sensor_frame: laser, data_type: LaserScan, topic: scan, marking: true, clearing: true}

inflation_layer:
  inflation_radius: 0.2

 

• Global Configuration(Global Costmap) : Global costmap이 사용하는 parameter들 구성

$ cd ~/catkin_ws
$ roscd navstack_pub
$ cd param
$ gedit global_costmap_params.yaml

• navstack_pub 패키지로 이동하여 global_costmap_params.yaml 생성
• 그 후 아래 내용들 입력

global_costmap:
  global_frame: odom
  update_frequency: 30.0
  publish_frequency: 30.0
  transform_tolerance: 0.2
  resolution: 0.1

 

• Local Configuration(Local Costmap) : Local costmap이 사용하는 parameter들 구성

$ cd ~/catkin_ws
$ roscd navstack_pub
$ cd param
$ gedit local_costmap_params.yaml

• navstack_pub 패키지로 이동하여 global_costmap_params.yaml 생성
• 그 후 아래 내용들 입력

local_costmap:
  update_frequency: 30.0
  publish_frequency: 30.0
  transform_tolerance: 0.2
  static_map: false		# costmap을 초기화 할 때 static map 사용 여부를 결정
  rolling_window: true
  resolution: 0.1
  inflation_radius: 0.1
  width: 1.0
  height: 1.0

  plugins:
    - {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}

  obstacle_layer:
    observation_sources: laser_scan_sensor
    laser_scan_sensor: {sensor_frame: laser, data_type: LaserScan, topic: scan, marking: true, clearing: true}

 

• Base Local Planner Configuration : Base local planner는 robot base controller로 보내진 velocity commands를 계산하는 패키지로써, Global plan을 계산하고 실행하기 위해 Trajectory Rollout과 DWA(Dynamic Window Approaches) 알고리즘을 제공하는 패키지이다. 
  • Trajectory Rollout은 로봇의 가속도 제한 내에서 주어진 모든 시뮬레이션에 걸친 속도의 집합을 sampling 하는 반면, DWA는 한 스텝의 시뮬레이션만을 사용함
• base_local_planner 패키지에 사용되는 값들은 어떤 로봇을 사용하냐에 따라 달라짐

$ cd ~/catkin_ws
$ roscd navstack_pub
$ cd param
$ gedit base_local_planner_params.yaml

• navstack_pub 패키지로 이동하여 base_local_planner_params.yaml 생성
• 그 후 아래 내용들 입력

TrajectoryPlannerROS:
  max_vel_x: 0.12
  min_vel_x: 0.11
  #max_vel_theta: 0.05
  #min_vel_theta: -0.05
  #min_in_place_vel_theta: 0.05

  #acc_lim_theta: 0.07
  #acc_lim_x: 0.25
  #acc_lim_Y: 0.25

  holonomic_robot: false

  meter_scoring: true

  xy_goal_tolerance: 0.15
  yaw_goal_tolerance: 0.25

 

• AMCL Configuration : ROS Navigation Stack은 AMCL (Adaptive Monte Carlo Localization)이라는 probabilistic localization system을 사용해야 함.
  • AMCL : Particle Filter를 통해 localization을 수행하는 알고리즘 중 하나로써, 주어진 지도에 대해 로봇의 자세를 추적한다. Monte Carlo Localization의 변형이다
  • Monte Carlo Algorithm : 랜덤으로 점들을 무수히 만들고 그 점들의 갯수를 통해 면적을 구하는 알고리즘
• Map, Lidar 스캔값, transformation messages를 입력으로 받고 위치 추종을 결과로 내보낸다
• amcl node는 아래 topic들을 subscribes 한다 
  • /scan : Lidar로부터의 laser scan messages  (sensor_msgs / LaserScan) 
  • /tf : 좌표계 변환 (tf / tfMessage) 
  • /initialpose : Quaternions을 사용하여 로봇의 초기 위치와 회전값 표현 (geometry_msgs / PoseWithCovarianceStamped) - Rviz initial pose button이 이 topic에 publish를 함
  • /map :  Gmapping, Hector SLAM 혹은 이미지를 수동으로 사용하여 생성된 occupancy grid map (nav_msgs / OccupancyGrid)amcl node는 아래 topic들을 publish 한다 
    
    • /amcl_pose : Map에서의 로봇의 추종된 자세 (geometry_msgs / PoseWithCovarianceStamped)
    • /particlecloud : 필터에 의해 유지되는 자세 추종의 집합 (geometry_msgs / PoseArray)
    • /tf (tf / tfMessage) : Publishes the transform from odom (which can be remapped via the ~odom_frame_id_parameter) to map

• Sensor Information : Navigation Stack은 장애물을 피하기 위한 정보를 sensor가 ROS를 통해 publish하는 sensor_msgs/LaserScan 또는 sensor_msgs/PointCloud message를 통해 얻는다
• Lidar Information :
• Odometry Information : Navigation stack을 이용하려면 odometry 정보를 tf나 nav_msgs/Odometry meesage를 통해 publish 해야 한다
• 이를 위해선 아래의 정보들이 필요하다
  • 바퀴 Encoder로부터의 data
  • IMU sensor로부터의 data
  • 바퀴 Encoder와 IMU sensor로부터의 data를 융합시키기 위해 확장된 Kalman Filter를 사용하는 robot_pose_ekf package
• Mapping Information : Navigation Stack을 구동하기 위해 필수적인 요소는 아니나 있으면 로봇의 초기 자세와 목적지를 설정할 수 있다.
• Lidar 혹은 image를 통해 map을 만들 수 있다

We need to delete this arguments

 

• Base Controller : Navigation stack을 사용하기 위해선 로봇의 기본 coordinate frame을 기준으로 한 velocity command를 geometry_msgs/Twist message를 통해  "cmd_vel(velocity command)" topic을 subscribe하고 모터에 적합한 command로 변환시키는 node가 필요하다
• Move Base Node : 위의 과정들을 통해 configuration file들을 생성하였다. 이제 그 file들을 launch file에 추가해야 한다. 그 file들은 Navigation Stack의 move_base node에 의해 쓰여질 것이다
  
  • move_base package는 action의 구현을 제공한다 
  • move_base node는 로봇을 현재의 위치에서 목표 위치로 움직이는 기능을 하는, Navigation 과정에서 발생하는 모든 요소들을 이어주는 ROS Navigtion Stack에서 매우 중요한 요소 중 하나이다. 충돌위험이 없는 경로를 계획하는데 중요한 역할을 한다
• move_base node는 아래의 topic들을 subscribe한다 :
  • /move_base_simple/goal : 목표 지점과 방향  (geometry_msgs::PoseStamped). 이 topic의 message들은 Rviz의 goal button으로 생성된다 Publisher는 아래의  topic들을 publish 한다 :
    • /cmd_vel : Linear and angular velocity command (geometry_msgs/Twist Message)위의 과정들을 완료했으면, 거의 다 온 것이다

$ cd ~/catkin_ws
$ catkin_make --only-pkg-with-deps navstack_pub

• Package compile 
• 모터를 킨다

$ roslaunch navstack_pub jetson_nano_bot.launch

• 새로운 터미널 창을 연 후 launch file을 실행시킨다
• 필요한 경우 각 Rviz plugin들을 설정하여 지도와 costmap에서 로봇의 축이 보일 수 있게 한다
• 2D Pose Estimate button을 눌러 로봇의 초기 자세를 설정한다
• 2D Nav Goal button을 눌러 로봇에게 목적지를 부여한다
• Rviz 화면에 아래와 같은 경로가 표시되고, 로봇이 경로를 따라가기 시작한다

planned path automatically drawn on the map

 

$ rosrun tf view_frames
$ evince frames.pdf

• 새로운 터미널을 열어 tf tree를 보여주는 명령어를 입력한다
• 결과는 아래와 같다

 

 

ROS Navigation Stack을 사용하는 mobile 로봇의 standard coordinate frames

 

 

$ rqt_graph

• node graph 표시 명령어

• 위의 모든 과정이 완료됐으면, ROS Navigation Stack 설정과 구동이 끝난 것이다

 

 

로봇의 ROS Navigaion Stack 구성

$ cd ~/catkin_ws/src/kusmo_bot
$ catkin_create_pkg localization_data_pub rospy roscpp std_msgs tf tf2_ros geometry_msgs sensor_msgs nav_msgs
$ cd ~/catkin_ws
$ catkin_make --only-pkg-with-deps localization_data_pub

• kusmo_bot 폴더에 localization_data_pub package 생성 후 compile

 

$ cd /src/kusmo_bot/localization_data_pub
$ geit CMakeLists.txt

• CMakeLists.txt 실행 후 5번째 줄의 해시태그를 제거하여 C++11 사용을 가능하게 함

 

아래의 두 topic들을 publish하는 node를 생성

• /initial_2d : odometry publisher가 subscribe하는 topic (geometry_msgs/PoseStamped)
• Navigation stack의 localization system을 통해 실제 로봇의 pose를 설정할 수 있게 함
• /goal_2d : 후에 생성할 path planner node가 subscribe할 topic (geometry_msgs/PoseStamped)
• Robot의 목표 pose를 설정하여 navigation에 goal을 보낼 수 있게 함

 

$ cd ~/catkin_ws/src/kusmo_bot/localization_data_pub/src
$ gedit rviz_click_to_2d.cpp

• localization package의 src 폴더로 이동하여 rviz_click_to_2d.cpp 파일 생성 후 아래 코드 입력

#include "ros/ros.h"
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include <tf/transform_broadcaster.h>
#include <iostream>

using namespace std;

// Initialize ROS publishers
ros::Publisher pub;
ros::Publisher pub2;

// Take move_base_simple/goal as input and publish goal_2d
void handle_goal(const geometry_msgs::PoseStamped &goal) {
geometry_msgs::PoseStamped rpyGoal;
rpyGoal.header.frame_id = "map";
rpyGoal.header.stamp = goal.header.stamp;
rpyGoal.pose.position.x = goal.pose.position.x;
rpyGoal.pose.position.y = goal.pose.position.y;
rpyGoal.pose.position.z = 0;
tf::Quaternion q(0, 0, goal.pose.orientation.z, goal.pose.orientation.w);
tf::Matrix3x3 m(q);
double roll, pitch, yaw;
m.getRPY(roll, pitch, yaw);
rpyGoal.pose.orientation.x = 0;
rpyGoal.pose.orientation.y = 0;
rpyGoal.pose.orientation.z = yaw;
rpyGoal.pose.orientation.w = 0;
pub.publish(rpyGoal);
}

// Take initialpose as input and publish initial_2d
void handle_initial_pose(const geometry_msgs::PoseWithCovarianceStamped &pose) {
geometry_msgs::PoseStamped rpyPose;
rpyPose.header.frame_id = "map";
rpyPose.header.stamp = pose.header.stamp;
rpyPose.pose.position.x = pose.pose.pose.position.x;
rpyPose.pose.position.y = pose.pose.pose.position.y;
rpyPose.pose.position.z = 0;
tf::Quaternion q(0, 0, pose.pose.pose.orientation.z, pose.pose.pose.orientation.w);
tf::Matrix3x3 m(q);
double roll, pitch, yaw;
m.getRPY(roll, pitch, yaw);
rpyPose.pose.orientation.x = 0;
rpyPose.pose.orientation.y = 0;
rpyPose.pose.orientation.z = yaw;
rpyPose.pose.orientation.w = 0;
pub2.publish(rpyPose);
}

int main(int argc, char **argv) {
ros::init(argc, argv, "rviz_click_to_2d");
ros::NodeHandle node;
pub = node.advertise<geometry_msgs::PoseStamped>("goal_2d", 0);
pub2 = node.advertise<geometry_msgs::PoseStamped>("initial_2d", 0);
ros::Subscriber sub = node.subscribe("move_base_simple/goal", 0, handle_goal);
ros::Subscriber sub2 = node.subscribe("initialpose", 0, handle_initial_pose);
ros::Rate loop_rate(10);
while (ros::ok()) {
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

 

$ cd ..
$ gedit CMakeLists.txt

• 이전directory로 이동하여  CMakeLists.txt 파일을 열고 아래 내용을 마지막 부분에 입력

 

INCLUDE_DIRECTORIES(/usr/local/lib)
LINK_DIRECTORIES(/usr/local/lib)

add_executable(rviz_click_to_2d src/rviz_click_to_2d.cpp)
target_link_libraries(rviz_click_to_2d ${catkin_LIBRARIES})

 

$ cd ~/catkin_ws
$ catkin_make --only-pkg-with-deps localization_data_pub

• Package Compile

 

$ roscore
$ rosrun localization_data_pub rviz_click_to_2d
$ rviz
$ rostopic echo /initial_2d
$ rostopic echo /goal_2d

• 정상 동작 확인

 

- Wheel Encoder Data를 통해 Odometry를 Publish하는 Publisher 생성

• 해당 node는 아래의 topic들을 subscribe함
• /right_ticks : 오른쪽 motor의 encoder로부터의 tick counts (std_msgs / Int16)
• /left_ticks : 왼쪽 motor의 encoder로부터의 tick counts (std_msgs / Int16)
• /initial_2d : Robot의 2D initial position과 방향
• Publisher는 아래의 topic에 odometry data를 publish함
• /odom_data_euler : 위치, 속도 estimate. orientation.z 변수는 yaw 각을 나타내는 Euler angle (nav_msgs/Odometry)
• /odom_data_quat : 위치, 속도 estimate. Orientation은 quaternion format으로 표현된다 (nav_msgs/Odometry)

 

$ cd ~/catkin_ws/src/kusmo_bot/localization_data_pub/src
$ gedit ekf_odom_pub.cpp

• localization package로 이동하여 ekf_odom_pub.cpp 파일 생성 후 아래 코드 입력

#include "ros/ros.h"
#include "std_msgs/Int16.h"
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <cmath>

// Create odometry data publishers
ros::Publisher odom_data_pub;
ros::Publisher odom_data_pub_quat;
nav_msgs::Odometry odomNew;
nav_msgs::Odometry odomOld;

// Initial pose
const double initialX = 0.0;
const double initialY = 0.0;
const double initialTheta = 0.00000000001;
const double PI = 3.141592;

// Robot physical constants
const double TICKS_PER_REVOLUTION = 620; // For reference purposes.
const double WHEEL_RADIUS = 0.033; // Wheel radius in meters
const double WHEEL_BASE = 0.17; // Center of left tire to center of right tire
const double TICKS_PER_METER = 3100; // Original was 2800

// Distance both wheels have traveled
double distanceLeft = 0;
double distanceRight = 0;

// Flag to see if initial pose has been received
bool initialPoseRecieved = false;

using namespace std;

// Get initial_2d message from either Rviz clicks or a manual pose publisher
void set_initial_2d(const geometry_msgs::PoseStamped &rvizClick) {

odomOld.pose.pose.position.x = rvizClick.pose.position.x;
odomOld.pose.pose.position.y = rvizClick.pose.position.y;
odomOld.pose.pose.orientation.z = rvizClick.pose.orientation.z;
initialPoseRecieved = true;
}

// Calculate the distance the left wheel has traveled since the last cycle
void Calc_Left(const std_msgs::Int16& leftCount) {

static int lastCountL = 0;
if(leftCount.data != 0 && lastCountL != 0) {

int leftTicks = (leftCount.data - lastCountL);
if (leftTicks > 10000) {
leftTicks = 0 - (65535 - leftTicks);
}
else if (leftTicks < -10000) {
leftTicks = 65535-leftTicks;
}
else{}
distanceLeft = leftTicks/TICKS_PER_METER;
}
lastCountL = leftCount.data;
}

// Calculate the distance the right wheel has traveled since the last cycle
void Calc_Right(const std_msgs::Int16& rightCount) {

static int lastCountR = 0;
if(rightCount.data != 0 && lastCountR != 0) {

int rightTicks = rightCount.data - lastCountR;

if (rightTicks > 10000) {
distanceRight = (0 - (65535 - distanceRight))/TICKS_PER_METER;
}
else if (rightTicks < -10000) {
rightTicks = 65535 - rightTicks;
}
else{}
distanceRight = rightTicks/TICKS_PER_METER;
}
lastCountR = rightCount.data;
}

// Publish a nav_msgs::Odometry message in quaternion format
void publish_quat() {

tf2::Quaternion q;

q.setRPY(0, 0, odomNew.pose.pose.orientation.z);

nav_msgs::Odometry quatOdom;
quatOdom.header.stamp = odomNew.header.stamp;
quatOdom.header.frame_id = "odom";
quatOdom.child_frame_id = "base_link";
quatOdom.pose.pose.position.x = odomNew.pose.pose.position.x;
quatOdom.pose.pose.position.y = odomNew.pose.pose.position.y;
quatOdom.pose.pose.position.z = odomNew.pose.pose.position.z;
quatOdom.pose.pose.orientation.x = q.x();
quatOdom.pose.pose.orientation.y = q.y();
quatOdom.pose.pose.orientation.z = q.z();
quatOdom.pose.pose.orientation.w = q.w();
quatOdom.twist.twist.linear.x = odomNew.twist.twist.linear.x;
quatOdom.twist.twist.linear.y = odomNew.twist.twist.linear.y;
quatOdom.twist.twist.linear.z = odomNew.twist.twist.linear.z;
quatOdom.twist.twist.angular.x = odomNew.twist.twist.angular.x;
quatOdom.twist.twist.angular.y = odomNew.twist.twist.angular.y;
quatOdom.twist.twist.angular.z = odomNew.twist.twist.angular.z;

for(int i = 0; i<36; i++) {
if(i == 0 || i == 7 || i == 14) {
quatOdom.pose.covariance[i] = .01;
}
else if (i == 21 || i == 28 || i== 35) {
quatOdom.pose.covariance[i] += 0.1;
}
else {
quatOdom.pose.covariance[i] = 0;
}
}

odom_data_pub_quat.publish(quatOdom);
}

// Update odometry information
void update_odom() {

// Calculate the average distance
double cycleDistance = (distanceRight + distanceLeft) / 2;

// Calculate the number of radians the robot has turned since the last cycle
double cycleAngle = asin((distanceRight-distanceLeft)/WHEEL_BASE);

// Average angle during the last cycle
double avgAngle = cycleAngle/2 + odomOld.pose.pose.orientation.z;

if (avgAngle > PI) {
avgAngle -= 2*PI;
}
else if (avgAngle < -PI) {
avgAngle += 2*PI;
}
else{}

// Calculate the new pose (x, y, and theta)
odomNew.pose.pose.position.x = odomOld.pose.pose.position.x + cos(avgAngle)*cycleDistance;
odomNew.pose.pose.position.y = odomOld.pose.pose.position.y + sin(avgAngle)*cycleDistance;
odomNew.pose.pose.orientation.z = cycleAngle + odomOld.pose.pose.orientation.z;

// Prevent lockup from a single bad cycle
if (isnan(odomNew.pose.pose.position.x) || isnan(odomNew.pose.pose.position.y)
|| isnan(odomNew.pose.pose.position.z)) {
odomNew.pose.pose.position.x = odomOld.pose.pose.position.x;
odomNew.pose.pose.position.y = odomOld.pose.pose.position.y;
odomNew.pose.pose.orientation.z = odomOld.pose.pose.orientation.z;
}

// Make sure theta stays in the correct range
if (odomNew.pose.pose.orientation.z > PI) {
odomNew.pose.pose.orientation.z -= 2 * PI;
}
else if (odomNew.pose.pose.orientation.z < -PI) {
odomNew.pose.pose.orientation.z += 2 * PI;
}
else{}

// Compute the velocity
odomNew.header.stamp = ros::Time::now();
odomNew.twist.twist.linear.x = cycleDistance/(odomNew.header.stamp.toSec() - odomOld.header.stamp.toSec());
odomNew.twist.twist.angular.z = cycleAngle/(odomNew.header.stamp.toSec() - odomOld.header.stamp.toSec());

// Save the pose data for the next cycle
odomOld.pose.pose.position.x = odomNew.pose.pose.position.x;
odomOld.pose.pose.position.y = odomNew.pose.pose.position.y;
odomOld.pose.pose.orientation.z = odomNew.pose.pose.orientation.z;
odomOld.header.stamp = odomNew.header.stamp;

// Publish the odometry message
odom_data_pub.publish(odomNew);
}

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

// Set the data fields of the odometry message
odomNew.header.frame_id = "odom";
odomNew.pose.pose.position.z = 0;
odomNew.pose.pose.orientation.x = 0;
odomNew.pose.pose.orientation.y = 0;
odomNew.twist.twist.linear.x = 0;
odomNew.twist.twist.linear.y = 0;
odomNew.twist.twist.linear.z = 0;
odomNew.twist.twist.angular.x = 0;
odomNew.twist.twist.angular.y = 0;
odomNew.twist.twist.angular.z = 0;
odomOld.pose.pose.position.x = initialX;
odomOld.pose.pose.position.y = initialY;
odomOld.pose.pose.orientation.z = initialTheta;

// Launch ROS and create a node
ros::init(argc, argv, "ekf_odom_pub");
ros::NodeHandle node;

// Subscribe to ROS topics
ros::Subscriber subForRightCounts = node.subscribe("right_ticks", 100, Calc_Right, ros::TransportHints().tcpNoDelay());
ros::Subscriber subForLeftCounts = node.subscribe("left_ticks", 100, Calc_Left, ros::TransportHints().tcpNoDelay());
ros::Subscriber subInitialPose = node.subscribe("initial_2d", 1, set_initial_2d);

// Publisher of simple odom message where orientation.z is an euler angle
odom_data_pub = node.advertise<nav_msgs::Odometry>("odom_data_euler", 100);

// Publisher of full odom message where orientation is quaternion
odom_data_pub_quat = node.advertise<nav_msgs::Odometry>("odom_data_quat", 100);

ros::Rate loop_rate(30); 

while(ros::ok()) {

if(initialPoseRecieved) {
update_odom();
publish_quat();
}
ros::spinOnce();
loop_rate.sleep();
}

return 0;
}

 

$ cd..
$ gedit CMakeLists.txt

• CMakeLists를 열고 파일의 끝부분에 아래 내용 입력

add_executable(ekf_odom_pub src/ekf_odom_pub.cpp)
target_link_libraries(ekf_odom_pub ${catkin_LIBRARIES})

 

$ cd ~/catkin_ws
$ catkin_make --only-pkg-with-deps localization_data_pub

• Package Compile

 

- ekf(Entended Kalman Filter) node 작성

• 보다 정확한 estimate를 위해 Extended Kalman Filter를 사용 (robot_pose_ekf package 사용)
• Robot이 움직임에 따른 위치와 방향을 보다 정확히 estimate하기 위해 필요한 improved된 odometry data를 생성하기 위해 odometry data (wheel Encoder의 tick counts + IMU sensor의 data)에 ekf를 적용

$ sudo apt-get install ros-noetic-robot-pose-ekf
$ cd ~/catkin_ws
$ catkin_make

• robot_pose_ekf package 설치 후 compile

 

• robot_pose_ekf node는 아래의 topic들을 subscribe함
• /odom : wheel encoder의 tick count 정보로부터 얻은 위치와 속도 estimate .Orientation은 quaternion format임 (nav_msgs/Odometry)
• /imu_data : IMU(Inertial Measurement Unit) sensor로부터의 data
• robot_pose_ekf node는 아래의 topic들을 publish 함
• /robot_pose_ekf/odom_combined : Filter의 output, robot의 3D estimated pose (geometry_msgs / PoseWIthCovarianceStamped)
• robot_pose_ekf node가 /odom, /imu_data이름으로 된 topic을 필요로 하므로 launch file에서 /odom_data_quat와 /imu/data topic에서 오는 data를 remap해야 함

 

• Navigation Stack의 launch file에 아래 내용 입력

<!-- Extended Kalman Filter from robot_pose_ekf Node-->
  <!-- Subscribe: /odom, /imu_data, /vo -->
  <!-- Publish: /robot_pose_ekf/odom_combined -->
  <remap from="odom" to="odom_data_quat" />
  <remap from="imu_data" to="imu/data" />
  <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
    <param name="output_frame" value="odom"/>
    <param name="base_footprint_frame" value="base_footprint"/>
    <param name="freq" value="30.0"/>
    <param name="sensor_timeout" value="1.0"/>
    <param name="odom_used" value="true"/>
    <param name="imu_used" value="true"/>
    <param name="vo_used" value="false"/>
    <param name="gps_used" value="false"/>
    <param name="debug" value="false"/>
    <param name="self_diagnose" value="false"/>
  </node>

 

 

 

 

 

 

 

---

참고 자료 : 

https://automaticaddison.com/how-to-set-up-the-ros-navigation-stack-on-a-robot/

How to Set Up the ROS Navigation Stack on a Robot – Automatic Addison

https://velog.io/@richard7714/ROS-Navigation-Navigation-Stack-%EC%84%A4%EC%A0%95%ED%95%98%EA%B8%B0

ROS Navigation - Navigation Stack 설정하기

https://soohwan-justin.tistory.com/39

ROS Navigation(4. Path Planning 1)

https://soohwan-justin.tistory.com/40

ROS Navigation(4. Path Planning 2)

https://answers.ros.org/question/223880/what-is-rolling-window-used-for/

What is "Rolling Window" used for? - ROS Answers: Open Source Q&A Forum