# ROS 工具
###### tags: `ROS`
[TOC]
## 昨天回顧
[昨天的講義](https://hackmd.io/icK5Y4f7RSq1bwdqgZEymQ)
[昨天練習的檔案](https://github.com/WangMahua/ros_tutorial.git)
1. 如果錯誤顯示找不到檔案，可以先試試source
```
$ cd ~/catkin_ws
$ source devel/setup.bash
```
或是直接開啟~/.bashrc將source 指令加入.bashrc檔
```
$ gedit ~/.bashrc
```
在最後一行加上```source ~/catkin_ws/devel/setup.bash```
2. 如何運行節點
```
$ rosrun <package_name> <node_name>
```
## Rviz
Rviz是用來將topic可視化的工具，像是點雲圖、相機圖片
1. 原始畫面
2. 可以從add by topic將想看的topic內容加入rviz
3. 就可以看到想看到的內容了～
#### 小嘗試：利用rviz看topic
1. 下載 usb_cam
```
$ cd ~/catkin_ws/src
$ git clone https://github.com/ros-drivers/usb_cam.git
$ cd ~/catkin_ws
$ catkin_make
```
2. rosrun 節點
```
$ roscore
$ rosrun usb_cam usb_cam_node
```
3. 開啟rviz按下add，選add by topic就可以看到多出image的部份，點開後就可以透過rviz檢查camera畫面了
## rqt_graph
用來查看topic的流向
## rqt_plot
右上角綠色+號可以新增plot
## rosbag
[ros wiki說明](http://wiki.ros.org/rosbag/Commandline)
rosbag用於紀錄被topic發布過的訊息，這樣的功能對於我們在debug也相當方便，可以透過回放檢查數據是否有哪邊出問題
- ```$ rosbag record -a```: 錄製所有topic資訊
- ```$ rosbag record <topic_name>``` : 錄製某個topic上的資訊
- ```$ rosbag info <topic_name>``` : 查看某個bag檔錄製了甚麼資訊
- ```rosbag play <bag_name>``` : 回放某個bag檔錄製的資訊
- ```rosbag play -r 0.5 <bag_name>``` : 以0.5倍速回放某個bag檔錄製的資訊，其中0.5這個參數可被替換為任意希望回放的倍速
- ```rosbag play -l <bag_name>``` : 循環播放bag內容
## tf
[ROS wiki](http://wiki.ros.org/navigation/Tutorials/RobotSetup/TF)
tf是用於座標轉換的工具，因為很多時候sensor讀取到的資訊並不能直接套用在world frame或機器人質心的frame上，所以必須使用更動座標系
要構成tf，就必須要有發布座標轉換的tf broadcaster，負責定義座標系間的轉換
##### 修改CMakelists.txt
```
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
message_generation
tf
)
```
##### tf_broadcaster.cpp
主要是透過這行來定義座標系的轉換
```cpp=
broadcaster.sendTransform(
tf::StampedTransform(
tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),
ros::Time::now(),"base_link", "base_laser"));
```
其中tf::Transform的第一個變數用於定義座標系旋轉、第二個用於座標系平移
```cpp=
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
int main(int argc, char** argv){
ros::init(argc, argv, "robot_tf_publisher");
ros::NodeHandle n;
ros::Rate r(100);
tf::TransformBroadcaster broadcaster;
while(n.ok()){
broadcaster.sendTransform(
tf::StampedTransform(
tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),
ros::Time::now(),"base_link", "base_laser"));
r.sleep();
}
}
```
##### tf_listener.cpp
最重要的是這兩行：
```cpp=
listener.waitForTransform("/base_laser","/base_link", ros::Time(), ros::Duration(5.0));
listener.transformPoint("/base_link", laser_point, base_point);
```
只要發布了tf轉換，用上面兩行就可以達成座標系轉換
```cpp=
#include <ros/ros.h>
#include <geometry_msgs/PointStamped.h>
#include <tf/transform_listener.h>
int main(int argc, char** argv){
ros::init(argc, argv, "robot_tf_listener");
ros::NodeHandle n;
tf::TransformListener listener(ros::Duration(10));
geometry_msgs::PointStamped laser_point;
laser_point.header.frame_id = "/base_laser";
//we'll just use the most recent transform available for our simple example
laser_point.header.stamp = ros::Time();
//just an arbitrary point in space
laser_point.point.x = 1.0;
laser_point.point.y = 0.2;
laser_point.point.z = 0.0;
try{
geometry_msgs::PointStamped base_point;
listener.waitForTransform("/base_laser","/base_link", ros::Time(), ros::Duration(5.0));
listener.transformPoint("/base_link", laser_point, base_point);
ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f",
laser_point.point.x, laser_point.point.y, laser_point.point.z,
base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec());
}catch(tf::TransformException& ex){
ROS_ERROR("Received an exception trying to transform a point from \"base_laser\" to \"base_link\": %s", ex.what());
}
}
```
#### tf debug
可以生成目前所有的座標系轉換
```
$ rosrun tf view_frames
$ evince frames.pdf
```
#### 20.04可能會遇到的bug
https://github.com/ros/geometry/pull/193
## roslaunch
我們在寫專案的時候往往都會需要使用多個檔案，如果每一個都用rosrun去跑就會變得相當麻煩，此時就可以使用launch檔來運行多個節點，其中launch檔會被放置於<package_name>/launch這個資料夾底下:
```bash=
cd <package_name>
mkdir launch
cd launch
gedit startup.launch
roslaunch <pckage_name> startup.launch
```
介紹一下基本launch檔內容，基本上launch node需要被包在<launch>與</launch>之間:
```
<launch>
<rosparam command="load" file="$(find beginner_tutorials)/config/param.yaml" />
<node pkg="beginner_tutorials" type="uav_pub" name="uav_pub" output="screen" >
</node>
<node pkg="beginner_tutorials" type="uav_sub" name ="uav_sub" output="screen" >
</node>
<include file="$(find openni_launch_marvin)/launch/kinect_left.launch" />
</launch>
```
1. node啟動
最常用的功能，有以下兩種表達方式：
```
<node pkg="beginner_tutorials" type="uav_pub" name="uav_pub" output="screen" >
</node>
```
or
```
<node pkg="beginner_tutorials" type="uav_pub" name="uav_pub" output="screen" />
```
2. roslaunch 其他 launch file
可以用來launch其他package中的launch檔
```
<include file="$(find openni_launch_marvin)/launch/kinect_left.launch" />
</launch>
```
3. remap
節點映射用於更換topic name，就不用進去修改檔案
```
<remap from="topic_1" to="topic_2" />
```
4. param 使用
如果param每次都用command line去給會過長，可以用下面的方法直接打在launch檔中，第一個方法是使用.yaml賦值：
```
<rosparam command="load" file="$(find beginner_tutorials)/config/param.yaml" />
```
or
```
<param name="param_name" type="int" value="10" />
```
5. tf轉換
不用自己開一個broadcaster直接用指令賦值就可以產生
```!
<node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="0.1 0.0 3.0 0.0 0.0 0.0 /base_link /base_laser 40" />
```
前三個數是xyz座標，後三個是yaw pitch roll 三個角度，兩個frame_id第一個是父座標系，後一個是子座標系
#### 小練習：
1. 試著launch不同package中的launch file，可以用昨天的檔案試試看
沒有下載過得直接git clone 下載過得請git pull把更新拉下來
https://github.com/WangMahua/ros_tutorial.git
2. 試著寫寫看.yaml檔或是利用launch檔直接修改ros param
3. 試著透過launch檔發布一個tf broadcater
## ROS 多台電腦連線
電腦需連至同個內網
```
$ gedit ~/.bashrc
```
在文件中加上
```
export ROS_IP=<自己的IP>
export ROS_MASTER_URI=http://<roscore機器的IP>:11311
```
查看自己IP的方法
```
$ ifconfig
```
https://github.com/ros/geometry/pull/193
## 練習
### odometry 積分
1.先下載bag檔
https://reurl.cc/GoeeOx
```
今天的練習為實作odometry sensor 的積分，
與昨天練習類似，
昨天是以速度向量來更新位置向量，
今天速度資訊從odometry sensor拿
```
```
1.請寫一個subscriber訂閱sensor的資料
2.寫兩個publisher，一個發布從sensor拿到的位置資訊當ground_truth
另一個發布速度積分後的結果
3.將兩個publisher發布的topic用rqt_plot繪出做比較
```
Output
```
虛線為groundtruth,實線為積分後的結果
```
```
註
Initial Point P0為
(0.627782718948, 0.0194202284956, 1.32679280569)
```
2.在CMakeLists.txt中添加eigen package
```
find_package(Eigen3)
include_directories(
${catkin_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIR}
)
```
```cpp=
#include "ros/ros.h"
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/Vector3Stamped.h"
#include <nav_msgs/Odometry.h>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Core>
#include <iostream>
#include <cmath>
double last_time = 0;
double now_time;
double dt = 0;
// define publisher
ros::Publisher pub_pose_ground_truth;
ros::Publisher pub_pose;
Eigen::Vector3d pos;
//geometry_msgs::PoseStamped rpyGoal;
geometry_msgs::Vector3Stamped pose_ground_truth;
geometry_msgs::Vector3Stamped pose;
geometry_msgs::PoseStamped pose_rviz;
void Predict(const nav_msgs::Odometry::ConstPtr& msg)
{
//Define now_time, last_time and dt
if(last_time != 0)
{
now_time = msg->header.stamp.toSec();
dt = now_time - ???;
??? = now_time;
}
else
{
last_time = msg->header.stamp.toSec();
}
// position groundtruth
Eigen::Vector3d pos_ground(msg->???, msg->pose.pose.position.y, msg->pose.pose.position.z);
// velocity groundtruth
Eigen::Vector3d vel_ground(msg->twist.twist.linear.x, msg->twist.twist.linear.y, msg->???);
// Integral equation
pos = ??? + ??? * dt;
pose_ground_truth.header.stamp=ros::Time::now();
pose_ground_truth.vector.x = ???.x();
pose_ground_truth.vector.y = ???.y();
pose_ground_truth.vector.z = ???.z();
pose.header.stamp=ros::Time::now();
pose.vector.x = ???.x();
pose.vector.y = ???.y();
pose.vector.z = ???.z();
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "integral_odometry");
pos << ???;
ros::NodeHandle nh;
ros::Subscriber odometry_sub;
odometry_sub = nh.subscribe("???", 100 ,Predict);
ros::Rate r(100);
while (ros::ok())
{
??? = nh.advertise<geometry_msgs::Vector3Stamped>("pose_ground_truth", 100);
??? = nh.advertise<geometry_msgs::Vector3Stamped>("position", 100);
???.publish(???);
???.publish(???);
ros::spinOnce();
r.sleep();
}
return 0;
}
```
## 解答
```cpp=
#include "ros/ros.h"
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/Vector3Stamped.h"
#include <nav_msgs/Odometry.h>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <iostream>
#include <cmath>
double last_time = 0;
double now_time;
double dt = 0;
ros::Publisher pub_pose_ground_truth;
ros::Publisher pub_pose;
Eigen::Vector3d pos;
//geometry_msgs::PoseStamped rpyGoal;
geometry_msgs::Vector3Stamped pose_ground_truth;
geometry_msgs::Vector3Stamped pose;
void Predict(const nav_msgs::Odometry::ConstPtr& msg)
{
//Time
if(last_time != 0)
{
now_time = msg->header.stamp.toSec();
dt = now_time - last_time;
last_time = now_time;
}
else
{
last_time = msg->header.stamp.toSec();
}
Eigen::Vector3d pos_sensor(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z);
Eigen::Vector3d vel(msg->twist.twist.linear.x, msg->twist.twist.linear.y, msg->twist.twist.linear.z);
pos = pos + vel * dt;
pose_ground_truth.header.stamp=ros::Time::now();
pose_ground_truth.vector.x = pos_sensor.x();
pose_ground_truth.vector.y = pos_sensor.y();
pose_ground_truth.vector.z = pos_sensor.z();
pose.header.stamp=ros::Time::now();
pose.vector.x = pos.x();
pose.vector.y = pos.y();
pose.vector.z = pos.z();
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "ekf");
pos << 0.627782718948, 0.0194202284956, 1.32679280569;
ros::NodeHandle nh;
ros::Subscriber odometry_sub;
odometry_sub = nh.subscribe("/iris1/ground_truth/odometry", 100 ,Predict);
ros::Rate r(100);
while (ros::ok())
{
pub_pose_ground_truth = nh.advertise<geometry_msgs::Vector3Stamped>("pose_ground_truth", 100);
pub_pose = nh.advertise<geometry_msgs::Vector3Stamped>("position", 100);
pub_pose_ground_truth.publish(pose_ground_truth);
pub_pose.publish(pose);
ros::spinOnce();
r.sleep();
}
return 0;
}
```
Sign in with Wallet
Connect another wallet