[RIS] 1/9 (initial pose/goal, odometry publisher, ekf)

- ROS를 통한 Robot의 Initial Pose / Goal에 대한 Publisher 생성법

$ 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

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

 

rviz 화면

• 2D Post Estimate Button
• Published Topic : initialpose
• Type : geometry_msgs/PoseWithCovarianceStamped
• Navigation stack의 localization system을 통해 실제 로봇의 pose를 설정할 수 있게 함
• 2D Nav Goal Button
• Published Topic : move_base_simple/goal
• Type : geometry_msgs/PoseStamped
• Robot의 목표 pose를 설정하여 navigation에 goal을 보낼 수 있게 함

 

• ROS에 작성하는 program들은 특정 format의 initial pose와 goal을 요구함
• 위의 두 topic들을 subscribe하고 그 data들을 ROS의 program에서 사용할 수 있게 해주는 rviz_click_to_2d.cpp 라는 프로그램을 생성하고자 함
• 즉, 아래의 두 topic들을 publish하는 node를 생성하고자 함
  • /initial_2d : odometry publisher가 subscribe하는 topic (geometry_msgs/PoseStamped)
  • /goal_2d : 후에 생성할 path planner node가 subscribe할 topic (geometry_msgs/PoseStamped)
• 위 node를 실행시키고 rivz에서 2D Pose Estimate, 2D Nav Goal 버튼을 누르면 해당 node가 data를 /initial_2d, /goal_2d topic을 publish 하기 적절한 format으로 변경해줌

 

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

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

/*
 * Automatic Addison
 * Website: https://automaticaddison.com
 *   ROS node that converts the user's desired initial pose and goal location
 *   into a usable format.
 * Subscribe:
 *   initialpose : The initial position and orientation of the robot using 
 *                 quaternions. (geometry_msgs/PoseWithCovarianceStamped)
 *   move_base_simple/goal : Goal position and 
 *                           orientation (geometry_msgs::PoseStamped)
 * Publish: This node publishes to the following topics:   
 *   goal_2d : Goal position and orientation (geometry_msgs::PoseStamped)
 *   initial_2d : The initial position and orientation of the robot using 
 *                Euler angles. (geometry_msgs/PoseStamped)
 * From Practical Robotics in C++ book (ISBN-10 : 9389423465)
 *   by Lloyd Brombach
 */

// Include statements 
#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

• ROS Master 실행 후 새 terminal 창을 열어 rviz_click_to_2d node 실행
• 또 다른 창에서는 rviz 실행

 

$ rostopic echo /initial_2d
$ rostopic echo /goal_2d

• 각각의 터미널에서 두 topic을 monitor
• rviz에서 '2D Pose Estimate' 버튼을 눌러 pose 설정

 

각 topic에 대한 정보

 

rviz를 실행시킨 terminal에서 나오는 정보

위 수행 결과

 

• Navigation Stack의 launch file에 아래 내용 추가

<!-- Initial Pose and Goal Publisher -->
  <!-- Publish: /initialpose, /move_base_simple/goal -->
  <!-- Launch Rviz --!>
  <node pkg="rviz" type="rviz" name="rviz" args="-d /home/lim/catkin_ws/src/ris_bot/navigation_data_pub/maps/floorplan4.rviz">
  </node> 

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

 

 

 

- ROS를 통한 Odometry 정보를 Publish 하는 방법 (Without IMU)

• odometry는 바퀴의 encoder로부터의 data를 통해 world-fixed point (ex)x = 0, y = 0, z= 0)와 연관된 robot의 위치와 방향을 시간이 지남에 따라 estimate한다
• 각 시간간격에 따른 wheel encoder로부터의 data를 trigonometry를 사용하여 robot의 위치를 estimate한다
• 아래 링크에서 estimate 과정에 대한 자세한 설명을 볼 수 있다
• https://automaticaddison.com/how-to-describe-the-rotation-of-a-robot-in-2d/

How to Describe the Rotation of a Robot in 2D – Automatic Addison

 

- Wheel encoder data를 토대로 하는 odometry 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 파일 생성 후 아래 코드 입력

/*
 * Automatic Addison
 * Date: May 20, 2021
 * ROS Version: ROS 1 - Melodic
 * Website: https://automaticaddison.com
 * Publishes odometry information for use with robot_pose_ekf package.
 *   This odometry information is based on wheel encoder tick counts.
 * Subscribe: ROS node that subscribes to the following topics:
 *  right_ticks : Tick counts from the right motor encoder (std_msgs/Int16)
 * 
 *  left_ticks : Tick counts from the left motor encoder  (std_msgs/Int16)
 * 
 *  initial_2d : The initial position and orientation of the robot.
 *               (geometry_msgs/PoseStamped)
 *
 * Publish: This node will publish to the following topics:
 *  odom_data_euler : Position and velocity estimate. The orientation.z 
 *                    variable is an Euler angle representing the yaw angle.
 *                    (nav_msgs/Odometry)
 *  odom_data_quat : Position and velocity estimate. The orientation is 
 *                   in quaternion format.
 *                   (nav_msgs/Odometry)
 * Modified from Practical Robotics in C++ book (ISBN-10 : 9389423465)
 *   by Lloyd Brombach
 */

// Include various libraries
#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;
}

 

• 위에서 작성한 C++ program을 CMakeLists.txt 파일에 추가해야 함

$ gedit CMakeLists.txt

• 파일의 끝부분에 아래 내용 입력

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

 

$ roscore
$ rosrun localization_data_pub ekf_odom_pub
$ rosrun rosserial_python serial_node.py _port:=/dev/ttyACM0 _baud:=115200

• 각각 다른 terminal 창에서 위 명령어들 입력
• serial_node.py는 tick count publisher

 

$ rosrun localization_data_pub rviz_click_to_2d
$ rviz

• 새로운 terminal 창에서 initial pose, goal publisher launch
• rviz 화면에서 robot의 initial pose와 goal destination 지정

 

$ rostopic echo /odom_data_quat

• 정상 동작하면 위 명령어를 통해 결과를 확인

 

• Navigation Stack의 launch file에 아래 내용 추가

 <!-- 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>

 

 

- ekf(extended kalman filter) package를 통해 sensor fusion을 하는 방법 (IMU + Encoder)

• 보다 정확한 estimate를 위해 Extended Kalman Filter를 사용 : IMU, Encoder를 따로 사용하는 것보다 더 정확한 robot의 pose estimate를 가능하게 함 (robot_pose_ekf package 사용)
• Robot이 움직임에 따른 위치와 방향을 보다 정확히 estimate하기 위해 필요한 improved된 odometry data를 생성하기 위해 odometry data (wheel Encoder의 tick counts)와 IMU sensor의 data를 통합
• Robot의 적절한 navigation과 더 정확한 mapping을 위해 필수적

 

$ 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해야 함

 

• 전체 launch file

<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 -->
 
  <!-- 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">
  </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>   
 
</launch>

 

 

Extended Kalman Filter란

sensor 값에는 noise가 섞여있고 각 시간 간격마다의 변화폭이 크다. 따라서 센서값들을 통해 robot의 위치와 방향을 완벽하게 알 수 없다

 

 

 

 

 

 

---

참고 자료 :

https://automaticaddison.com/how-to-create-an-initial-pose-and-goal-publisher-in-ros/

How to Create an Initial Pose and Goal Publisher in ROS – Automatic Addison

https://automaticaddison.com/how-to-publish-wheel-odometry-information-over-ros/

How to Publish Wheel Odometry Information Over ROS – Automatic Addison

http://wiki.ros.org/roslaunch/XML/remap

roslaunch/XML/remap - ROS Wiki