[RIS] 1/11 (Navigation Stack 구성 완료)

- 아래의 내용들을 토대로 Robot의 전체적인 Software 구성

• https://youngseong.tistory.com/102

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

 

 

• 사용중인 모터 드라이버의 ROS Package clone
• https://github.com/YoungSeong98/md

GitHub - YoungSeong98/md

 

• https://github.com/wookbin/iahrs_driver

GitHub - wookbin/iahrs_driver: iahrs ros package

 

• https://github.com/Slamtec/rplidar_ros

GitHub - Slamtec/rplidar_ros

 

 

 

- Sensor 전체의 USB Port 설정

• https://youngseong.tistory.com/121
• 위 내용을 토대로 각 sensor에 대한 symbolic link를 생성

[RIS] 1/2 (USB 장치의 Symbolic Link 생성법)

$ cd /etc/udev/rules.d
$ sudo gedit portset.rules

• rules.d directory로 이동하여 port setting file 생성 후 각 sensor에 대한 설정 입력

KERNEL=="ttyUSB[0-9]*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", 
ATTRS{serial}=="0011", SYMLINK+="ttyIMU", GROUP="ris", MODE="0666"

KERNEL=="ttyUSB[0-9]*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", 
ATTRS{serial}=="0010", SYMLINK+="ttyRPLidar", GROUP="ris", MODE="0666"

KERNEL=="ttyUSB[0-9]*", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6001", 
ATTRS{serial}=="A10LT0DR", SYMLINK+="ttyRS485", GROUP="ris", MODE="0666"

 

• 정상 동작 여부 확인

 

 

- Navigation Stack 구성

 

$ sudo apt-get install ros-noetic-navigation

• ROS Navigation Stack 설치

$ rospack find amcl

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

 

- Package 생성

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

• create the ris_bot folder and  fnavstack_pub라는 이름의 패키지 생성
• navstack_pub가 package의 이름이고 뒤의 단어들은 dependency들을 의미

 

$ cd navstack_pub
$ mkdir param
$ gedit CMakeLists.txt

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

 

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

• Package를 compile

 

- Launch file 구성

$ roscd navstack_pub
$ mkdir launch
$ cd launch
$ gedit ris_bot.launch

• 새로운 터미널 창을 열고 navstack_pub 패키지로 이동하여 launch directory 생성
• 해당 directory로 이동 후 launch 파일 생성
• 각 sensor들을 한번에 구동시키기 위해 각각의 launch file 내용을 입력

<?xml version="1.0"?>
<launch>
	<!-- Launch Iahrs -->
	<param name="m_bSingle_TF_option" type="bool" value="true" /> <!--false-->
  	<node pkg="iahrs_driver" type="iahrs_driver" name="iahrs_driver" output="screen" />

	<!-- Launch RPLidar -->
    	<node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
  	<param name="serial_port"         type="string" value="/dev/ttyRPLidar"/>
	<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>
    
    	<!-- Launch MD200T -->
    	<node pkg="md" type="md_robot_node" name="md_robot_node" output="screen">
        <param name = "use_MDUI"                    value = "0"/>       <!-- 0: not use MDUI, 1: use MDUI -->
        <param name = "wheel_radius"                value = "0.0935"/>  <!-- unit: meter -->
        <param name = "wheel_length"                value = "0.454"/>   <!-- unit: meter -->
        <param name = "motor_pole"                  value = "10"/> 
        <param name = "reduction"                   value = "30"/> 
        <param name = "reverse_direction"           value = "0"/>       <!-- 0: forward, 1: reverse -->
        <param name = "maxrpm"                      value = "1000"/>    <!-- unit: RPM -->
        <param name = "motor_posi"                  value = "0"/>       <!-- motor pisition 0: hall sensor, 1: encoder -->
        <param name = "encoder_PPR"                 value = "16384"/>   <!-- if use encoder position, encoder PPR -->
        <param name = "position_proportion_gain"    value = "20"/>      <!-- reference PID 203(PID_GAIN) -->
        <param name = "speed_proportion_gain"       value = "50"/>      <!-- reference PID 203(PID_GAIN) -->
        <param name = "integral_gain"               value = "1800"/>    <!-- reference PID 203(PID_GAIN) -->
        <param name = "slow_start"                  value = "300"/>     <!-- unit: RPM -->
        <param name = "slow_down"                   value = "300"/>     <!-- unit: RPM -->
    </node>
</launch>

 

 

- TF (Transformation) 구성

<!-- Transformation Configuration -->
	<!-- Use static transform publisher because each frames doesn`t change through time-->
	<!-- 			    static_transform_publisher, broadcaster name, x y z yaw pitch roll frame_id child_frame_id period_in_ms -->
	<node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="-0.15 0 0.13 0 0 0 base_link laser 30" />
	<!-- base_link to imu_link will be provided by the iahrs_driver -->
	<node pkg="tf" type="static_transform_publisher" name="base_link_broadcaster" args="0 0 0 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" />

• 각 sensor들의 상대 거리를 측정한 후 robot의 base_link를 기준으로 상대값 입력

 

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

• Package compile 

 

$ roslaunch navstack_pub ris_bot.launch

• 새로운 터미널 창을 연 후 launch file을 실행시킨다
• 정상 동작 확인

 

 

- Move base의 parameter 구성

 

1) 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 생성
  • 다른 params file들도 같은 directory에 작성
• 그 후 아래 내용들 입력

 

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

 

 

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

$ gedit global_costmap_params.yaml

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

 

 

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

$ gedit local_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}

 

 

4) 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 패키지에 사용되는 값들은 어떤 로봇을 사용하냐에 따라 달라짐

 

$ gedit 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

 

 

- Set up localization stack  

$ cd ~/catkin_ws/src/ris_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

• ris_bot 폴더에 localization_data_pub package 생성 후 compile

 

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

• Package 내부로 이동하여 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/ris_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

• 정상 동작 확인

 

<!-- Initial Pose and Goal Publisher -->
<!-- Publish: /initialpose, /move_base_simple/goal -->

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

• Navstack의 launch file에 해당 파일을 불러오는 내용 입력

 

 

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

 

• Wheel Encoder 값으로부터 Odometry 값을 계산한 후 Euler Angle과 Quaternion의 형태로 Publish 함
  • 상위 Algorithm인 Extended Kalam Filter node에 Robot의 Motion에 대한 정보를 제공하는 입력으로 들어감

 

$ 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

 

<!-- Wheel Odometry Publisher -->
<!-- Subscribe: /right_ticks, /left_ticks, /initial_2d -->
<!-- Publish: /odom_data_euler, /odom_data_quat -->
<node pkg="localization_data" type="ekf_odom_pub" name="ekf_odom_pub">
</node>

• Navstack의 launch file에 해당 내용 입력

 

 

- 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를 통합

 

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

 

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

• Navigation Stack의 launch file에 EKF를 사용하는 내용 입력

 

- AMCL 

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

• Navigation Stack의 launch file에 AMCL의 amcl_diff.launch 예제를 불러오는 내용 입력

 

 

 

- SSH 사용을 위한 openssh-server 설치

$ sudo apt-get install openssh-server

• 설치 후 'ifconfig' 명령어를 통해 Xavier의 IP주소를 확인하고 PC에서 그 주소값을 통해 원격으로 접속

ssh ris@192.168.0.58

 

 

 

• Navstack launch file의 전체 내용

<?xml version="1.0"?>
<launch>
    <!-- Transformation Configuration -->
    <!-- Use static transform publisher because each frames doesn`t change through time-->
    <!-- 			    static_transform_publisher, broadcaster name, x y z yaw pitch roll frame_id child_frame_id period_in_ms -->
    <node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="0.15 0 0.13 0 0 0 base_link laser 30" />
    <!-- base_link to imu_link will be provided by the iahrs_driver -->
    <node pkg="tf" type="static_transform_publisher" name="base_link_broadcaster" args="0 0 0 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" />

    <!-- Launch Iahrs -->
    <param name="m_bSingle_TF_option" type="bool" value="true" /> <!--false-->
    <node pkg="iahrs_driver" type="iahrs_driver" name="iahrs_driver" output="screen" />

    <!-- Launch RPLidar -->
    <node name="rplidarNode"          pkg="rplidar_ros"  type="rplidarNode" output="screen">
    <param name="serial_port"         type="string" value="/dev/ttyRPLidar"/>
    <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>
    
    <!-- Launch MD200T -->
    <node pkg="md" type="md_robot_node" name="md_robot_node" output="screen">
    <param name = "use_MDUI"                    value = "0"/>       <!-- 0: not use MDUI, 1: use MDUI -->
    <param name = "wheel_radius"                value = "0.0935"/>  <!-- unit: meter -->
    <param name = "wheel_length"                value = "0.454"/>   <!-- unit: meter -->
    <param name = "motor_pole"                  value = "10"/> 
    <param name = "reduction"                   value = "30"/> 
    <param name = "reverse_direction"           value = "0"/>       <!-- 0: forward, 1: reverse -->
    <param name = "maxrpm"                      value = "1000"/>    <!-- unit: RPM -->
    <param name = "motor_posi"                  value = "0"/>       <!-- motor pisition 0: hall sensor, 1: encoder -->
    <param name = "encoder_PPR"                 value = "16384"/>   <!-- if use encoder position, encoder PPR -->
    <param name = "position_proportion_gain"    value = "20"/>      <!-- reference PID 203(PID_GAIN) -->
    <param name = "speed_proportion_gain"       value = "50"/>      <!-- reference PID 203(PID_GAIN) -->
    <param name = "integral_gain"               value = "1800"/>    <!-- reference PID 203(PID_GAIN) -->
    <param name = "slow_start"                  value = "300"/>     <!-- unit: RPM -->
    <param name = "slow_down"                   value = "300"/>     <!-- unit: RPM -->
    </node>
    
    <!-- Initial Pose and Goal Publisher -->
    <!-- Publish: /initialpose, /move_base_simple/goal -->

    <!-- Subscribe: /initialpose, /move_base_simple/goal -->
    <!-- Publish: /initial_2d, /goal_2d --> 
    <!-- Launch rviz_click_to_2d node -->
    <node pkg="localization_data" type="rviz_click_to_2d" name="rviz_click_to_2d">
    </node>
    
    <!-- Wheel Odometry Publisher -->
    <!-- Subscribe: /right_ticks, /left_ticks, /initial_2d -->
    <!-- Publish: /odom_data_euler, /odom_data_quat -->
    <node pkg="localization_data" type="ekf_odom_pub" name="ekf_odom_pub">
    </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>
    
    <!-- Map File -->
    <arg name="map_file" default="/home/ris/catkin_ws/maps/test.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)/param/costmap_common_params.yaml" command="load" ns="global_costmap"/>
    <rosparam file="$(find navstack)/param/costmap_common_params.yaml" command="load" ns="local_costmap"/>
    <rosparam file="$(find navstack)/param/local_costmap_params.yaml" command="load" ns="local_costmap"/>
    <rosparam file="$(find navstack)/param/global_costmap_params.yaml" command="load" ns="global_costmap"/>
    <rosparam file="$(find navstack)/param/base_local_planner_params.yaml" command="load"/>
    </node>
 
</launch>