安裝最新版cmake & cmake
https://blog.csdn.net/u011291667/article/details/100022585
安裝/升級 cmake & cmake_gui 為最新版
首先需要安裝：
sudo apt install qt4*
sudo apt install openssl
init_workspace需要在 根目錄/src/ 下進行
創建工作空間workspace & 功能包package
- 選定工作空間根文件夾
- 在根文件夾下創建src文件夾
- 在src文件夾下catkin_init_workspce
- 在工作空間根文件夾下catkin_make
- 在工作空間根文件夾下使能環境變量source devel/nsetup.bash
- 在src文件夾下catkin_create_pkg <功能包名稱> [<依賴包名稱> <...>]
- 在src文件夾中編寫cpp文件。
- 修改CMakefile.txt
- 在工作根文件夾中catkin_make
- 使能環境變量
- 打開新的terminal設置環境變量后就可以運行設計好的功能包。
幾個名字說明
- 在命令行中調用的名字是CMakefile中add_executable()中出現的名字，而在rqt_graph中展示的名字是ros::init()中初始化的名字。
創建用戶定義的功能包
- 在功能包的文件夾目錄中創建msg文件夾
- 在msg文件夾中創建<name>.msg文件
- 在<name>.msg內部定義數據類型和備選結構(其中數據類型的關鍵詞都是在std_msgs中定義的)
- 在package.xml中添加依賴項（需要用的包在build_depend，執行時所需要的運行文件在exec_depend）
- 在CMakefile.txt中添加依賴項（需要用的包需要先查找find_package，執行時所需要的運行文件在exec_depend）
- CMakefile中需要注意為動態生成的文件添加依賴關系。
一些小細節
catkin_create_pkg <pkg_name> [<depend_pkg> ...]
如果創建包<depend_pkg>中什么都不添加，創建出的package中僅有一個CMakefile.txt & package.xml。
若添加額外依賴的功能包，生成的內容中才會包括
roscore在每次運行一個新的程序之前需要重新啟動一下以防止之前運行的程序參數干擾本次執行
生成的頭文件在devel下的include中。
在ubuntu系統中雙擊F2可以直接對文件的全名進行修改。
定義service時，注意引用兩部分內容:
- 發送的topic內容
- std_srvs中的觸發結構
transformer(tf)學習
tf 中的廣播用于通知其他節點當前節點的位置
tf 中的監聽用于獲得其他各個節點的位置并根據節點位置做出海龜的運動控制
查看message結構體的數據類型
rosmsg show geometry_msgs/Twist
ROS的代碼順序
注意ROS課程18中的這兩行代碼的順序導致報錯
正確的代碼&代用方式是：
先初始化的時候可以先使用__name更改初始化功能包的名字，而先判斷的話就一直報錯
ros::init(argc, argv, "transform_broadcaster");
if(argc != 2){
ROS_ERROR("need turtle name as an input argument");
return -1;
}
//調用方式
rosrun <pkg_name> <func_name> __name:=<changed_name> /turtle1
但是如果運行的是下面的代碼：
if(argc != 2){
ROS_ERROR("need turtle name as an input argument");
return -1;
}
ros::init(argc, argv, "transform_broadcaster");
//調用方式：
rosrun <pkg_name> <func_name> __name:=<changed_name> /turtle1
// 這種調用方式會出錯，報錯內容為ROS_ERROR 中定義的內容
load.manifest(<pkg_name>)
講<pkg_name>復制到python的依賴路徑中。
rospy & roscpp 對應關系
ROS roslib
運行python腳本撰寫的turtle_tf_broadcast.py中的第6行時候報錯，syntax error near unexpected
token 'learning_tf'
https://answers.ros.org/question/115346/what-does-roslibload_manifest-do/
ROS運行的python腳本頭部必須添加下述兩行代碼
#!/usr/bin/env python
#-*- coding: utf-8 -*-
否則python無法正確運行
ROS19講中的python執行方式
代碼中通過rospy.getparam來獲得實際的參數。
實際調用方式：
修改內容：
將ros.getparam('~turtle')修改為ros.getparam('/turtle')
//新終端
roscore
//新終端
rosrun turtlesim turtlesim_node
//新終端
rosparam set /turtle turtle1
rosrun learning_tf turtle_tf_broadcaster.py __name:=turtle1_tf_broadcast
//新終端
rosparam set /turtle turtle2
rosrun learning_tf turtle_tf_broadcaster.py __name:=turtle2_tf_broadcast
//新終端
rosrun learning_tf turtle_tf_listener.py
//新終端
rosrun turtlesim turtlesim_teleop_key
注意：使用launch運行turtlesim時
設定參數列表要更改為:
ros.get_param('~turtle')
因為launch下系統運行時定義的參數需使用~turtle來訪問。
// 對應于get_param('~turtle')的launch文件撰寫
<node pkg="learning_tf" type="turtle_tf_broatcaster.py" name="broadcast1" output="screen">
<param name="turtle" value="turtle1" >
</node>
<node pkg="learning_tf" type="turtle_tf_broatcaster.py" name="broadcast2" output="screen">
<param name="turtle" value="turtle2" >
</node>
<node pkg="learning_tf" type="turtle_tf_listener.py" name="listener" output="screen" />
運行18講的C代碼和python代碼可以直觀的感受到python運行速度的慢。
Launch
ROS中減少我們頻繁打開終端的啟動方式：
xml文件
實現多個節點的配置&啟動
使用roslaunch文件
launch文件可以自動啟動ROS Master
必選屬性：
標簽說明 http://wiki.ros.org/roslaunch/XML
pkg: 節點所在功能包的名字
type: cpp文件編譯生成的可執行文件名
name:節點運行的名稱
output, respawn, required, ns, args
<param name="output_frame" value="odom" />用來加載參數
<rosparam file="params.yaml" command="load" ns="params" /> 從文件中load參數
<arg name="arg_name" default="arg_value" />
arg調用方式
<param name="foo" value="$(arg arg_name)" />
<node name="node" pkg="package" type="type" args="$(arg arg_name)" />
<remap from="/turtlebot/cmd_vel" to="/cmd_vel" />
<include file="$(dirname)/other.launch" />
<launch>
<node pkg="learning_topic"
</launch>
上述內容中type就是CMakelist.txt中add_executable(<name> user_defined.cpp)中的<name>
roslaunch按照古月居上面的教程講解中
output="screen" 指定了信息打印的輸出位置為屏幕。
既然launch文件多teminal操作問題？
啟動后所有的rosrun命令都在同一個terminal中實現，那么如果一個launch文件中調用了很多個需要命令行操作的rosrun功能包，那么如何實現同一個terminal中操作多個需要操作的命令呢？
rqt是一個綜合工具，可以作為調試機器人的上位機
可視化工具＆使用方法
A. rqt_graph
rqt_console來打開rqt操作臺，默認收集系統的輸出信息（顯示日志），
rqt_plot繪制數據曲線，可以訂閱具體的某個數據，比如Topic中的xxx
輸入左斜線，自動匹配話題，顯示海龜的pose位置，不斷刷新數據。
rqt_image_view查看攝像頭圖像。RGB color image
rviz顯示模型，點云，障礙物，圖像（均已經預先定義好了）。也可以用戶定義數據結構。
- 使用xml進行數據傳遞
- 工具欄
Gazebo三維仿真平臺
本來沒有數據，用來創建數據，仿真機器人&傳感器&環境。
可以去gazebo上下載離線模型下載到本地，免去打開時在線訪問。
ROS應用
- moveit機械臂運動規劃，碰撞檢測，避障
- 機器人仿真平臺：gazebo + ROS + ros_control
- slam:: wiki.ros.org/gmapping. wiki.ros.org/hector_slam
運行別人的代碼的問題：建圖的精度不高，運行速度慢。
機器人學：
機器人基本理論
- CS223A - Introduction to Robot：https://see.stanford.edu/Course/CS223A
- Underactuated Robotics - Algorithms for Walking, Running, Swimming, Flying, and Manipulation：http://underactuated.csail.mit.edu/Spring2019/，中文翻譯：https://blog.csdn.net/ZhangRelay/article/details/99677487
- Modern Robotics：http://hades.mech.northwestern.edu/index.php/Modern_Robotics，中文翻譯：https://blog.csdn.net/ZhangRelay/article/details/88699314
- Robot Dynamics：https://rsl.ethz.ch/education-students/lectures/robotdynamics.html，中文翻譯：https://blog.csdn.net/ZhangRelay/article/details/69382096
- 15-494/694 - Cognitive Robotics：http://www.cs.cmu.edu/afs/cs/academic/class/15494-s19/，中文翻譯：https://blog.csdn.net/ZhangRelay/article/details/86736743
- Programming for Robotics - ROS：https://rsl.ethz.ch/education-students/lectures/ros.html，中文翻譯：https://blog.csdn.net/ZhangRelay/article/details/79463689
臺灣 交通大學.png
eth機器人系統課程：
古月居
如果我們想要開源&解耦合為什么一定要采用ros，為什么不能獨立開發一個我們的ROS？
IMU輸出線加速度&角加速度， ->積分得到速度 ->積分得到位置
里程計：碼盤->單位時間內的圈數->速度。
robot pose
EKF 使用卡爾曼濾波得到算法。
機器人本地：TK1&本地的
完成傳感器數據接口。
遠程通過PC連接機器人
處理器raspherry Pi -> 2. Odroid XU4 -> Nvidia Jetson TK1 -> Mini pc
外部傳感器：攝像頭，
攝像頭驅動
ros-kinertic-usb-cam
roslaunch usb_cam usb_cam-test.launch
rqt_image_view
Kinect
外部傳感器：激光雷達rplidar
ros-kinetic-rplidar-ros
rosrun rplidar_ros rplidarNode
機器人開發案例
move_base 路徑規劃，用于導航的功能包。
sudo apt-get install ros-kinetic-navigation
自帶
可選
針對所需機器人而專門設計的相關功能
功能包的輸入和輸出。
wiki會描述輸入&輸出，并不會描述功能包的各種細節，當所有功能都實現之后才需要關注功能包的實現。
機器人的輸入&輸出，傳感器數據，里程計信息，是否有IMU，base controller，機器人應當運行的速度。給你1m/s的速度來落實到電機上面。PWM波生成，從控制信號到電信號的轉換。ros功能包用到機器人上面。接口問題。
TurtleBot 資源最為豐富的機器人，上位機。相關書也是以這TurtleBot為例進行講解的。
ros中帶有turtlebot3相關的功能包。
應用：
- 云機器人，借助全局云完成數據存儲&共享。
ROS DSLAM安裝及其debug
OpenCV nonfree.hpp需要安裝OpenCV-contrib
cmake -DOPENCV_EXTRA_MODULES_PATH=<opencv_contrib>/modules -DOPENCV_ENABLE_NONFREE=ON <opencv_main>
上面兩個<>指定的內容要更改為對應的路徑
安裝opencv-contrib方法：
https://blog.csdn.net/echoamor/article/details/83022352
查看opencv版本
sift已經被申請專利了，所以，在opencv3.4.3.16 版本后，這個功能就不能用了。
采用gtsam-4.0.3進行cpython安裝后可以使用。
解決了OpenCV的問題之后catkin_make可以跑通，報錯
Client [/gen_tf] wants topic /visual_odometry/transform_relative to have datatype/md5sum [dslam_sp/TransformStamped_with_image/26e8057a7a61d2d334ba0ad6445993c3], but our version has [geometry_msgs/TransformStamped/b5764a33bfeb3588febc2682852579b0]. Dropping connection.
issue cannot find octomap_ros
sudo apt-get install ros-melodic-octomap ros-melodic-octomap-mapping ros-melodic-octomap-msgs ros-melodic-octomap-ros ros-melodic-octomap-rviz-plugins ros-melodic-octomap-server
問題：安裝g2o之后沒有G2OConfig.cmake
在對應功能包的CMakeLists.txt中添加
SET(G2O_CMAKE_MODULES /home/jimmy/bin/Package/cmake_modules/)
fatal error: Eigen/Core: No such file or directory
當調用 eigen 庫時，會報錯：fatal error: Eigen/Core: No such file or directory
這是因為 eigen 庫默認安裝在了 /usr/include/eigen3/Eigen 路徑下，需使用下面命令映射到 /usr/include 路徑下
sudo ln -s /usr/include/eigen3/Eigen /usr/include/Eigen
下一個任務time profiling。
由于ROS并不是實時系統，因此測得的時間并不精確，解決方法：設計一個client-server的service，在灰調函數中設計定時改變，標記為1時開始計時，標記為0時停止計時并打印程序所采用的時間。
PowerVR SDK使用
計時工具使用
#include<ros/time.h>
#include<ros/duration.h>
ros::Time begin=ros::Time::now();
//下面這5行不是實際可運行代碼，而是函數的聲明
//寫在這里用于說明函數的使用方式.
// _sec表示以秒為單位的時間
// _nsec表示以納秒為單位的時間
//_sec和_nsec時間之和為定義的時間
//double t用來直接定義絕對時間。
// duration數據類型=絕對時間之差
ros::Time::Time(uint32_t _sec, uint32_t _nsec)
ros::Time::Time(double t)
ros::Duration::Duration(uint32_t _sec, uint32_t _nsec)
ros::Duration::Duration(double t)
//下面是實際的運行代碼
ros::Time at_some_time1(5, 2000_0000);
ros::Time at_some_time2(5.2);
ros::Duration one_hour(60*60, 0);
//講時間轉換為以“秒”為單位
double secs1=at_some_time1.toSec();
double secs2=one_hour.toSec();
ros::Duration dur = at_some_time2 - at_some_t
</launch>ime1;
ros::Time now_delay3s = ros::Time::now() + ros::Duration(3);
ros::Duration d1=t2-t1;
ros::Duration d2=d1-ros::Duration(0, 300);
// 用于制定休眠的代碼
bool ros::Duration::sleep()
ros::Duration(0.5).sleep() 休眠 0.5s
// 定時觸發回調函數
ros::Timer timer1 = n.createTimer(ros::Duration(1), callback);
cpp傳入參數學習。
#include<iostream>
#include<unistd.h>
using namespace std;
int main(int argc, char** argv){
int o;
const char* optString = "a:s:d:";
while((o=getopt(argc, argv, optString)) != -1){
switch(o){
case 'a':
cout << " I am in a" << optarg <<endl;
break;
case 's':
cout << " I am in b" << optarg <<endl;
break;
case 'd':
cout << " I am in c" <<+ optarg <<endl;
break;
default:
break;
}
}
return 0;
}
使用方式
cmake_minimum_required(VERSION 2.8)
project(test_cpp_version)
set(CMAKE_BUILD_TYPE Release)
set(CMAKE_CXX_FLAGS "-std=c++14 -O3")
# OpenCV
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
add_executable(testcpp_option test_option.cpp)
target_link_libraries(testcpp_option ${OpenCV_LIBS})
運行方式
./testcpp_option -a sd
當同時制定了多個選項時，優先率先出現的選項
c++ 析構函數：函數名稱前面加上~
CVBridge
功能：由于ros內部信息發布publish時傳遞的圖片和OpenCV中讀取的圖片格式不同，因此在ros內部發布或者OpenCV處理時的圖片需要進行相應的轉換。
轉換使用方式：
OpenCV->ROS
cv::Mat image = imread(<path2img>, -1)
// -1 means read_image_unchanged
sensor_msgs::ImagePtr ptr = cv_bridge::CvImage(std_msgs::Header(), "mono8", image).toImageMsg();
ros::NodeHandle n;
ros::Publisher pub = n.advertise<sensor_msgs::Image>("/type", fifo_depth);
pub.publish(ptr);
注：上述ROS類型指sensor_msgs/Image.msg
// sensor_msgs/Image.msg數據結構
Header header # Header timestamp should be acquisition time of image
# Header frame_id should be optical frame of camera
# origin of frame should be optical center of camera
# +x should point to the right in the image
# +y should point down in the image
# +z should point into to plane of the image
# If the frame_id here and the frame_id of the CameraInfo
# message associated with the image conflict
# the behavior is undefined
uint32 height # image height, that is, number of rows
uint32 width # image width, that is, number of columns
# The legal values for encoding are in file src/image_encodings.cpp
# If you want to standardize a new string format, join
# ros-users@lists.sourceforge.net and send an email proposing a new encoding.
string encoding # Encoding of pixels -- channel meaning, ordering, size
# taken from the list of strings in include/sensor_msgs/image_encodings.h
uint8 is_bigendian # is this data bigendian?
uint32 step # Full row length in bytes
uint8[] data # actual matrix data, size is (step * rows
其中Header header內容如下:
uint32 seq
time stamp
string frame_id
其中seq是連續增長的ID
其中stamp包括兩個整數，stamp.sec & stamp.nsec
http://wiki.ros.org/cv_bridge/Tutorials/UsingCvBridgeToConvertBetweenROSImagesAndOpenCVImages
ROS->OpenCV
主要有兩種
cv_bridge::CvImagePtr cv_ptr;
sensor_msgs::ImagePtr dep_ptr = boost::make_shared<::sensor_msgs::Image>(msg->depth);
// cout << "PRE1" << endl;
cv_ptr=cv_bridge::toCvCopy(dep_ptr, dep_ptr->encoding);
cv_ptr->image.copyTo(depth_curr);
rso::init(argc, argv, <name>, init_option)
option的選項：
- NoSigintHandler: Don't install a SIGINT handler. You should install your own SIGINT handler in this case, to ensure that the node gets shutdown correctly when it exits.
- AnonymousName: Anonymize the node name. Adds a random number to the end of your node's name, to make it unique.
- NoRosout: Don't broadcast resconsole output to the /rosout topic.
share_ptr是智能指針，不需要額外處理指針的釋放。而且因為ros中所有的數據類型都采用的是share_ptr的指針定義，因此在對ros內部sensor_msgs/Image 進行數據分配時需要額外對輸入數據進行boost::make_shared<>使其變為動態指針。
visual_odometry_orb_node_orbh.cpp
這個文件讀取了keypoint & descriptor并且篩選K個top score的特征點。
送入orb進行匹配。
返回得到的R和t
下一步，閱讀ORB.hpp看其中的實現。
形參 v.s. 實參
- 比如你定義一個函數void add（int a, int b），這里的a和b就是形參。
- 當你進行函數調用的時候，add（1, 2），這里的1和2就是實參。
形式參數僅僅代表一個記號。
e.g.
// 函數定義時
int add(int a, int b);
實際參數
// 函數調用時
add(1, 2);
幾種函數傳遞類型：
常量引用 vs 變量引用
常量引用：
int add(const int& a, const int& b){
}
一般調用函數的思想就是：將內存中已有的數據傳入函數中進行處理得到返回數值。在函數中a &b就是用戶傳入的數值且在函數中不會創新新的變量&不會修改傳入的變量數值。
變量引用
int add(int& a, int & b){
}
這部分函數傳遞時會傳入定義的變量地址，在函數內部可以任意修改函數的數值
應用：
c++類定義時的構造函數定義：
class Foo{
private:
int num;
public:
Foo(const int& num_value): num(num_value){}
~Foo(){std::cout << "Destruct Foo with Name =" << num<< std::endl;}
}
其中類Foo的構造函數Foo，傳入參數就是常量引用類型，并將其賦值給內部私有變量num。
boost類型指針使用:
#include<boost/shared_ptr>;
boost::shared_ptr<Foo> FooPtr;
int main(int argc, char ** argv){
FooPtr ptr1(new Foo(1)); // 創建一個Foo類的shared_ptr類型的智能指針。
// new Foo(1) 創建了一個Foo類，初始化內部數值為1。
// FooPtr創建了指向新創建Foo類的指針，名稱為ptr1。
ptr1
FooPtr ptr2 = ptr1; //創建新指針，與ptr1指向相同的對象(上次創建的new Foo(1))，從而將ptr內部的引用計數+1并且同時ptr2的
引用計數也+1;
assert(ptr2.usr_count == 2);
ptr2.reset(); // 將ptr2的引用計數清空為NULL。同時ptr1內部的引用計數恢復為1，因為ptr2已經清空。
assert(ptr2.get() == NULL);
assert(ptr2.usr_count == 0);
FooPtr ptr3(new Foo(2)); // 創建了新的Foo類，初始化值為2，并且創建新的指向這個Foo類的指針ptr3.
ptr3.reset(new Foo(3)); // 首先清空ptr3原來的引用，并釋放ptr3對于Foo(2)類的指向&&同時創建了新的Foo(3)，將ptr3的指針轉移到Foo(3)上。
}
循環引用問題：
Parent&Child分別定義兩個指針，而且兩者相互引用。
class Parent;
class Child;
class Parent{
public:
~Parent(){std::cout << "call deconstruction function" << std::endl;
public:
boost::shared_ptr<Child> child_ptr;
}
int main(int argc, char** argv){
}
class Child{
public:
~Child(){
std::cout << "調用child的析構函數\n";
}
public:
ParentPtr parent;
};
int main(int argc, char** argv){
ParentPtr father(new Parent());
ChildPtr son(new Child());
father->child = son;
son->parent = father;
}
采用上述類定義下，若指針之間存在相互引用則在程序結束后指針無法析構。
而如果內部采用weak_ptr指針，則整個系統可以正常析構。
class Parent;
class Child;
class Parent{
public:
~Parent(){std::cout << "call deconstruction function" << std::endl;
public:
boost::weak_ptr<Child> child_ptr;
}
class Child{
public:
~Child(){std::cout << "deconstruct the child" << std::endl;
public:
boost::weak_ptr<Parent> parent_ptr;
修改使其成為weak_ptr后便可以正常進行析構。
但weak_ptr沒有對資源的操作權（weak ptr獲得的是資源的觀察權）所以無法使用son->parent對parent進行操作。
boost考慮到了這一點，因此額外增加一個操作使weak_ptr 的son也可以對parent進行操作：
方法為：
std::cout << son->parent.lock().use_count() << std::endl;
注意：
- weak_ptr獲得的是對象的觀察權，其不會引起被指向對象引用計數的增減。
- weak_ptr可以作為容器的元素。比如std::vector<boost::weak_ptr<Node> > children;
shared_ptr與容器
- boost::shared_ptr<std::vector<> >
- std::vector< boost::shared_ptr<> >
技巧，使用boost封裝fileptr自動實現內存管理
void FileClose(FILE* file){
int result = fclose(file);
printf("invoke fclose result = %d\n", result)
}
FilePtr FileOpen(char const* path, char const* mode){
FilePtr fptr(fopen(path, mode), fclose); // fclose就是f的刪除器，因為對于全局或者靜態函數而言，函數名稱就是他的首地址。
return fptr;
}
int main(int argc, char** argv){
FilePtr ptr = FileOpen("memory.log", "r");
FilePtr ptr2 = ptr;
char data[50];
FileRead(ptr, data, 50);
data[50] = "\0";
printf("%d", data[50]);
return 0;
}
//在程序執行結束后會自動調用fclose從而不必手動管理fclose。
技巧：使用c++橋接模式來封裝現有的C函數，隱藏實現細節。
在c++的.h文件中聲明如下類：
class FIleSharedPtr
{
private:
class impl;
boost::shared_ptr<impl> pimp;
public:
FileSharedPtr(char const * name, char const* mode);
指針定義
- 從右向左讀
char ** p1;
// pointer to pointer to char
const char **p2;
// pointer to pointer to const char
char * const * p3;
// pointer to const pointer to char
const char * const * p4;
// pointer to const pointer to const char
char ** const p5;
// const pointer to pointer to char
const char ** const p6;
// const pointer to pointer to const char
char * const * const p7;
// const pointer to const pointer to char
const char * const * const p8;
// const pointer to const pointer to const char
- 等效
const int *p; //same as below const (int) * p
int const *q; // (int) const *p
boost 技巧3
用c++設計模式來封裝現有的c++函數，隱藏實現細節。
// 頭文件
class FileSharedPtr
{
private:
class impl;
boost::shared_ptr<impl> pimpl;
public:
FIleSharedPtr(char const* name, char const* mode);
void read(void * data, size_t size);
};
// cpp 文件
class FIleSharedPtr::impl
{
private:
impl(impl const &){}
impl& operator = (impl const &){}
FILE* f;
public:
impl(char const* name, char const* mode){ f= fopen(name, mode);}
~impl()
{
int result = fclose(f);
printf("invoke FIleSharedPtr::impl析構函數result");
}
void read(void* data, size_t size){ fread(data, 1, size, f)}
}
FileSharedPtr::FileSharedPtr(const char* name, const char * mode): pimpl (new FIleSharedPtr::impl(name, mode)){}
void FileSharedPtr::Read(void* data, size_tr size){pimpl->read(data, size);}
void Test_CPP_File_Ptr(){
FileSharedPtr ptr("memory.log", "r");
FileSharedPtr ptr2 = ptr;
char data[100];
ptr.Read(data, 100);
printf("%s\n", data);
}
rospython如果報類似如下的錯誤
import-im6.q16: not authorized `rospy' @ error/constitute.c/WriteImage/1037.
import-im6.q16: not authorized `time' @ error/constitute.c/WriteImage/1037.
/home/jimmy/work/ROS_work/test/src/time_profiling/scripts/time_profiling_node.py: line 12: syntax error near unexpected token `'rgbd_to_pointcloud''
/home/jimmy/work/ROS_work/test/src/time_profiling/scripts/time_profiling_node.py: line 12: ` rospy.init_node('rgbd_to_pointcloud')'
請在python開頭加上如下內容：
#!/usr/bin/env python
#-*- coding: utf-8 -*-
python閱讀文本內容：
文件打開模式 描述
r 以只讀模式打開文件，并將文件指針指向文件頭；如果文件不存在會報錯
w 以只寫模式打開文件，并將文件指針指向文件頭；如果文件存在則將其內容清空，如果文件不存在則創建
a 以只追加可寫模式打開文件，并將文件指針指向文件尾部；如果文件不存在則創建
r+ 在r的基礎上增加了可寫功能
w+ 在w的基礎上增加了可讀功能
a+ 在a的基礎上增加了可讀功能
b 讀寫二進制文件（默認是t，表示文本），需要與上面幾種模式搭配使用，如ab，wb, ab, ab+（POSIX系統，包括Linux都會忽略該字符）
- 查看rgb2point_cloud的c++ profiling。
- 查找xzl寫的octomap的時間print函數。
rgb2point_cloud的python實現在秒級別
Gazebo
按照下面的方式解決gazebo打開黑屏的問題。
https://blog.csdn.net/weixin_41536025/article/details/85783092
rviz中無法顯示gazebo仿真環境下的機器人
在terminal中運行
roslaunch rrt_exploration_tutorials single_simulated_house.launch
報錯Timed out waiting for transform from robot_1/base_link to robot_1/map to bec
余老板說ros-kinetic-kuboki包在melodic中沒有直接的對應包。因此需要源碼編譯。
按照此教程操作：
https://www.ncnynl.com/archives/201903/2884.html
操作中遇到cmake版本較低(不知道我安裝了什么東西改了原先cmake-3.18.2的版本為3.3.2）
解決方法，找到cmake-3.18.2源碼編譯安裝的目錄將目標/bin文件夾添加到路徑中即可。
cmake報有找不到efl的包
pkg_check_modules(BFL REQUIRED orocos-bfl)
安裝ros-melodic-efl解決。
sudo apt install ros-melodic-bfl
按照上面的教程運行時報下列錯誤：
[ WARN] [1598951602.664929798]: Kobuki : device does not (yet) available, is the usb connected?.
[ WARN] [1598951602.915293883]: Kobuki : no data stream, is kobuki turned on?
[WARN] [1598951602.983775]: Battery : unable to check laptop battery info [/sys/class/power_supply/BAT0/charge_full_design || /sys/class/power_supply/BAT0/energy_full_design does not exist]
似乎仍然沒有安裝上kobuki。
要求按照下面的代碼安裝melodic版本的rocon
mkdir ~/rocon
cd ~/rocon
wstool init -j5 src https://raw.github.com/robotics-in-concert/rocon/indigo/rocon.rosinstall
source /opt/ros/indigo/setup.bash
rosdep install --from-paths src -i -y
catkin_make
但是這個url 404了，所以上github上直接找rocon項目對應的rocon.rosinstall源碼然后查看對應的raw文件。
rosdep install
報錯
首先，ros wiki上說明這個功能包是用于安裝catkin workspace中缺失依賴的http://wiki.ros.org/rosdep
而且上面那個代碼似乎無法安裝依賴，直接運行下面這一行代碼使其可以運行。
rosdep install --from-paths src --ignore-src -r -y
最終運行的代碼為：
因為我的個人PC已經安裝ros，所以沒有運行激活ros的環境變量代碼。
mkdir ~/Package/rocon
cd ~/Package/rocon
wstool init -j5 src https://raw.githubusercontent.com/robotics-in-concert/rocon/devel/rocon.rosinstall
rosdep install --from-paths src --ignore-src -r -y
catkin_make
sudo apt install ros-melodic-rocon-*
這個方法安裝過的內容似乎無法被所需函數使用。
https://blog.csdn.net/zhuquan945/article/details/79694599
編譯安裝kobuki時候遇到問題：找不到gzmath.hh。兩種辦法：
- 重新編譯安裝gazebo9：現在正在下載gazebo9，通過cmakelist中的版本號能夠判斷具體的型號、
- 直接只安裝gazebo/math的數據庫：這條路可能行不通。
- 是否應該安裝更高版本的gazebo？
kobuki能夠下載的版本和gazebo接口bug
因為kobuki的gazebo接口是采用舊版本gazebo而非新版本的gazebo，新版本去掉了原有的math而改用了ignition::math，而且還有physics和sensor兩個大類中存在很多函數區別，因此整體有很多依賴bug。下面是修改的一些bug。
/home/jimmy/Package/kobuki_wstool/src/kobuki_desktop/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_updates.cpp:52:50: error: ‘class gazebo::physics::Joint’ has no member named ‘GetAngle’; did you mean ‘GetForce’?
joint_state_.position[RIGHT] = joints_[RIGHT]->GetAngle(0).Radian();
// GetAngle(0).Radian() -> Position(0)
/home/jimmy/Package/kobuki_wstool/src/kobuki_desktop/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_updates.cpp:92:24: error: ‘using element_type = class gazebo::sensors::ImuSensor {aka class gazebo::sensors::ImuSensor}’ has no member named ‘GetAngularVelocity’; did you mean ‘AngularVelocity’?
vel_angular_ = imu_->GetAngularVelocity();
/home/jimmy/Package/kobuki_wstool/src/kobuki_desktop/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_loads.cpp: In member function ‘bool gazebo::GazeboRosKobuki::prepareIMU()’:
/home/jimmy/Package/kobuki_wstool/src/kobuki_desktop/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_loads.cpp:298:41: error: ‘class gazebo::physics::World’ has no member named ‘GetName’; did you mean ‘Name’?
sensors::get_sensor(world_->GetName()+"::"+node_name_+"::base_footprint::"+imu_name));
// GetName() -> Name()
In file included from /home/jimmy/Package/kobuki_wstool/src/kobuki_desktop/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_updates.cpp:36:0:
/home/jimmy/Package/kobuki_wstool/src/kobuki_desktop/kobuki_gazebo_plugins/include/kobuki_gazebo_plugins/gazebo_ros_kobuki.h:233:3: error: ‘math’ does not name a type; did you mean ‘tanh’?
math::Vector3 vel_angular_;
// math::Vector3 -> ignition::math::Vector3
// #include<gazebo/math/gzmath.hh> -> #include<ignition/math/Vector3.hh>
如果有需要就在gazebo的repo里面搜math，找到對應的頭文件include。
/home/jimmy/Package/kobuki_wstool/src/kobuki_desktop/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_updates.cpp:92:24: error: ‘using element_type = class gazebo::sensors::ImuSensor {aka class gazebo::sensors::ImuSensor}’ has no member named ‘GetAngularVelocity’; did you mean ‘AngularVelocity’?
vel_angular_ = imu_->GetAngularVelocity();
^~~~~~~~~~~~~~~~~~
// GetAngularVelocity() -> AngularVelocity();
除此之外還有很多其他由于版本不兼容而導致的bug，所以不進行kobuki的debug
新問題：
gazebo沒有發出odom庫，需要安裝的方式為：
分別下載以下兩個repo到兩個獨立的catkin_workspace中分別編譯
在安裝下面兩個功能包之前需要先初始化rosdep，并且更新（如果機器之前已經初始化過則無需再次初始化）
rosdep init
rosdep update
若上述rosdep init失敗，則運行下面代碼替換rosdep init
sudo mkdir -p /etc/ros/rosdep/source.list.d
cd /etc/ros/rosdep/sources.list.d
sudo gedit 20-default.list
# copy the following contents into 20-default.list
#################
# os-specific listings first
yaml https://raw.githubusercontent.com/ros/rosdistro/master/rosdep/osx-homebrew.yaml osx
# generic
yaml https://raw.githubusercontent.com/ros/rosdistro/master/rosdep/base.yaml
yaml https://raw.githubusercontent.com/ros/rosdistro/master/rosdep/python.yaml
yaml https://raw.githubusercontent.com/ros/rosdistro/master/rosdep/ruby.yaml
gbpdistro https://raw.githubusercontent.com/ros/rosdistro/master/releases/fuerte.yaml fuerte
#################
下面安裝下面的kobuki包
第一個repo
kobuki_desktop
mkdir kobuki_desktop_ws && cd kobuki_desktop_ws
mkdir src && cd src
git clone https://github.com/yujinrobot/kobuki_desktop
cd ../
rosdep install --from-paths src --ignore-src -r -y
第二個repo
mkdir kobuki_msgs_ws && cd kobuki_msgs_ws
mkdir src && cd src
git clone https://github.com/yujinrobot/kobuki_msgs
cd ../
rosdep install --from-paths src --ignore-src -r -y
```擇日哦通過rosdep安裝可以自己安裝相應的依賴關系。
到此為止
------
回到rrt debug增加一個 / 的問題debug
[ERROR] [1599011298.911594610, 2434.510000000]: TF Exception that should never happen for sensor frame: /robot_1/base_laser_link, cloud frame: robot_1/base_laser_link, Invalid argument "/robot_1/base_laser_link" passed to lookupTransform argument source_frame in tf2 frame_ids cannot start with a '/' like:
在debug的同時需要了解系統是如何工作的，方法：
學習tf tree
rosrun tf view_frames
可以看到frame之間的關系，生成pdf(tf tree)
morphology
rosrun rqt_tf_tree rqt_tf_tree
也可以展示tf tree并且可以刷新看。
當tf停止發送時，rqt顯示的tf tree是最后一次運行時的tf tree
通過下面的參數n來制定顯示多少個tf關系。
rostopic echo -n1 /tf
直接查看turle1 到turtle2的tf
rosrun tf view_frames
rosrun tf tf_echo turtle1 turtle2
最后一種可視化方式，采用rviz
rosrun rviz rviz
在其中add 直接添加tf就可以顯示frame之間的相互關系
tf中的publisher就是broadcaster
tf中的subscriber就是listener
rossrv show <service_name>
rosservice info <service_name>
rosservice call <service_name>
查看ros環境變量
env | grep ROS_
unset <path_name>
// set path using source devel/setup.bash
roscd <package_name> // change the directory to the root path of the package
使用相同的topic的ros模塊會產生多個robot pub相同的topic，導致無法區分topic產生方到底是哪一個。
解決方法：采用namespace
rosrun <pkg_name> <scripts_name> __ns:=robot1
rosbag可以記錄機器人的移動位置
//start a new terminal
rosbag -a
// after record the path of the robot
rosbag play xxx.bag
rqt_plot <topic_name>可以用于繪制某個topic的名字
rosrun rqt_console rqt_ console
load debug info
rospack list 列出所有功能包的名字
roscd后面的功能包名字就是package.xml中的定義的<name>
gazebo通過.world文件來創建仿真環境
rostest 用于測
costmap 是障礙物的inflation
單機代碼學習&理解地圖中所有信息。
有問題主要看wiki
rrt_exploration的邊界設置
測試的時候centroid record[]等等都是空的是什么意思？
地圖Map的topic的含義。
1. 單機：rviz上點，小車走，
2. 探索速度，走的路徑更短，達到多少覆蓋率時的探索時間短。
3. 啟動修改
3. 多機新指標：一致性？
- 一致性？自己定義：盡可能使兩個車探索時走同一個路徑。強行。（新問題需要定義：定義新問題然后提出解決方案）
- 攝像頭數據
?? rospack
視覺里程計-VO測試
因為沒有多機地圖融合所以沒有 global merge map，所以收不到地圖。
# launch 文件中定義的param在參數服務器中的名稱
<node pkg="rrt_exploration" type="local_rrt_detector" name="robot2_rrt_detector" output="screen">
<param name="eta" value="$(arg eta)"/>
<param name="map_topic" value="robot_2/map"/>
<param name="robot_frame" value="robot_2/base_link"/>
</node>
定義出的param實際名稱為：
robot2_rrt_detector/robot_frame
# 自定義msg列表中在c++和python中使用的不同
An array of pointstamped
geometry_msgs/PointStamped[] pointstamped
這個msg在c++中是一個vector
而在python中是一個list
懵逼內容：filter.py中 24行中args[1]是global_frame到哪個機器人的tf
得到了tf之后經過24行后所有的坐標點規范到了哪個機器人坐標系下。
歸根結底：
1. 搞懂transformPoint的含義
2. 弄清waitTransform的含義。
核心目標：每個機器人sendgoal時得到的是自己坐標系下的坐標。
# 對于transform的理解
1. 重點要搞清楚是哪個坐標向哪個坐標的坐標變換（由誰指向誰）
2. 坐標系的朝向，平移正方向：x朝前，y朝左，z朝上，選擇正方向是右手握成“你真棒”，拇指指向z軸正方向，四指指向為z旋轉正方向。
3.
StampedTransform (const tf::Transform &input, const ros::Time ×tamp, const std::string &frame_id, const std::string &child_frame_id)
上述函數理解為：
1. 目前tf樹中已經有frame_id定義的frame，我想定義一個新的坐標系（叫做child_frame_id）。從frame_id -> child_frame_id的坐標變換（transform)為input。
tf::TransformBroadcaster br;
br.sendTransform(tf::StrampedTransform(transform, ros::Time::now(), "world", turtle_name))
下面的sendTransform即將定義好的新坐標系child_frame_id發布，即插入tf樹中。
胡春旭寫的ROS實踐一書中，從frame_id -> child_frame_id的坐標變換（transform)寫成了“child_frame_id相對于frame_id的坐標變換，字符記為T_{frameId_childFrameId}"
以烏龜車追蹤為例理解transform的相對性。
每個車都是一個frame，都是一個坐標系。
tf::StampedTransform transform;
tf::TransformListener listener;
listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));
// listener接收tf樹中在ros::Time(0)時刻由"/turtle1" -> "/turtle2"的坐標變換，接收后緩存3s。
listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
// listener查找由"/turtle1" -> "/turtle2"的坐標變換，并將該變換寫入transform中。
//（We want the transform from frame /turtle1 to frame /turtle2.）。
調用的函數接口
waitForTransform(const std::string &traget_frame, const std::string & source_frame, const ros::Time &time, const ros::Duration &timeout);
//等待tf 樹中出現由source_frame指向target_frame的tf
最后，烏龜車turtle2追蹤turtle1應該獲得turtle2 - turtle1的坐標變換，但是transform獲得的是turtle1 -> turtle2的變換，這為什么仍然能夠正常work呢？因為turtle2 - turtle1移動是通過調整角度 & 向調整角度后的方向移動x、y的平方和的距離來實現的。由于turtle2 & turtle1之間坐標角度變換只涉及xOy平面的變換，其實turtle1->turtle2變換的角度就是turtle2->turtle1變換的角度（手動畫張圖就能夠理解）。而x,y只是用來平方和距離的，所以正負都不重要，所以該函數能夠實現功能。
附錄：
1. 其中atan2和atan的功能是不同的。
atan2(y, x)是4象限反正切，它的取值不僅取決于正切值y/x，還取決于點 (x, y) 落入哪個象限：
當點(x, y) 落入第一象限時，atan2(y, x)的范圍是 0 ~ pi/2;
當點(x, y) 落入第二象限時，atan2(y, x)的范圍是 pi/2 ~ pi;
當點(x, y) 落入第三象限時，atan2(y, x)的范圍是 －pi～－pi/2;
當點(x, y) 落入第四象限時，atan2(y, x)的范圍是 -pi/2～0.
而 atan(y/x) 僅僅根據正切值為y/x求出對應的角度 （可以看作僅僅是2象限反正切）：
當 y/x > 0 時，atan(y/x)取值范圍是 0 ~ pi/2；
當 y/x < 0 時，atan(y/x)取值范圍是 -pi/2～0.
2. ros::Time(0)
You can also see we specified a time equal to 0. For tf, time 0 means "the latest available" transform in the buffer.
說白了就是本地緩存中最新的tf。
那這個最新的tf是不是現在的呢？
No。
那現在是的是什么？怎么調用？
Now, change this line to get the transform at the current time, "now()":
ros教程
[http://wiki.ros.org/ROS/Tutorials](http://wiki.ros.org/ROS/Tutorials)
actionlib
[http://wiki.ros.org/ROS/Tutorials](http://wiki.ros.org/ROS/Tutorials)
namespace或frame增加/為絕對路徑，不加/為相對路徑。
如何查看frame_id中有沒有斜杠？
Solution: rostopic echo <topic_name> > /tmp/log之后可以直接看/tmp/log開始的頭文件中的frame_id中有沒有
# 地圖合并出問題
能夠rosrun啟動的節點無法通過roslaunch 啟動。
通過修改機器人的movebase中讀取的參數數據能夠修改機器人運行時的costmap inflation 大小。
----
#
先修改move_base的參數使其存儲局部地圖 或者 直接采用全局地圖進行導航。
此bug解決之后搞人工勢場。
[http://wiki.ros.org/move_slow_and_clear](http://wiki.ros.org/move_slow_and_clear)
move_base_params_robot1.yaml中的move_slow_and_clear;
報錯：
[ INFO] [1600327283.341323192]: No matching device found.... waiting for devices. Reason: std::__cxx11::string astra_wrapper::AstraDriver::resolveDeviceURI(const string&) @ /home/xtark/ros_ws/src/third_packages/ros_astra_camera/src/astra_driver.cpp @ 972 : Invalid device number 1, there are 0 devices connected.
[ INFO] [1600327283.822143753]: Recovery behavior will clear layer 'obstacle_layer'
通過直接看costmap的數值發現，探索過的地方存在的障礙物被自動清理為0了。
從static map更改為rolling window之后全局的global costmap中的障礙物不再消失，但是所有的障礙物會累加使精度降低。
move_base實際不發送costmap，costmap是通過costmap_2d_ros啟動發送，在costmap中創建LayeredCostmap發送對應消息。LayeredCostmap初始化中有rolling_window參數，而沒有static_map參數。
全部與costmap相關的初始化，修改都在costmap_2d_ros.cpp中完成。
costmap_2d_ros中定義了
改掉了動態清除障礙物的問題之后出現了障礙物檢測飄的情況。
## launch文件中group標簽的作用
group標簽含有ns(namespace)參數，指定參數后，其中全部的topic/service都會增加ns的前綴。
而frame_id前面不會增加ns作為前綴，若想在frame_id前面增加前綴，有以下方案：
1. 手動修改所有的frame_id字符串，有的寫在程序中，有的寫在參數配置文件中。
2. 在launch中寫一個arg文件，并在所有的程序.py/cpp文件中增加讀取arg并定義。
比如我自己寫的turtle_tracking.launch文件，若不增加namespace的rqt_graph如下所示
由此可見,turtle1/pose topic采用參數傳遞的方式定義，如下所示：
in launch file
<group ns="ns1">
<node pkg="learning_tf" type="turtle_tf_broadcaster.py" name="broadcast1" output="screen">
<param name="turtle" type="string" value="$(arg ns)/turtle1" />
</node>
</group>
in py file
turtlename = rospy.get_param('~turtle')
rospy.Subscriber('/%s/pose' % turtlename,
turtlesim.msg.Pose,
handle_turtle_pose,
turtlename)
'/%s/pose' % turtlename沒有在前面加上ns的前綴，因此無法正常工作。
另外，請注意：
turtlename = rospy.get_param('~turtle')
采用~<參數名稱>讀取的參數就是實際launch文件中指定的數值，因此
若publish內部定義的topic名稱前面沒有/，代表相對路徑，則實際發布的topic名稱前會增加如下：
turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist, queue_size = 1)
則實際發布的topic會將相對路徑名稱的topic前增加ns，而frameid就只能手動指定。
最終版代碼如下，修改group標簽的ns和arg標簽namespace的value可以直接修改全局的topic和frame id。
###############################
############# wrapper launch file
###############################
<launch>
<group ns="nsw">
<include file="/home/jimmy/work/ROS_work/test/src/learning_launch/launch/turtle_tracking.launch" >
<arg name="namespace" value="nsw" />
</include>
</group >
</launch>
###############################
############# launch file
###############################
<launch>
<arg name="namespace" default="" />
<node pkg="turtlesim" type="turtlesim_node" name="node" />
<node pkg="turtlesim" type="turtle_teleop_key" name="teleop_key" output="screen" />
<node pkg="learning_tf" type="turtle_tf_broadcaster.py" name="broadcast1" output="screen">
<param name="turtle" type="string" value="turtle1" />
<param name="namespace" type="string" value="$(arg namespace)" />
</node>
<node pkg="learning_tf" type="turtle_tf_broadcaster.py" name="broadcast2" output="screen">
<param name="turtle" type="string" value="turtle2" />
<param name="namespace" type="string" value="$(arg namespace)" />
</node>
<node pkg="learning_tf" type="turtle_tf_listener.py" name="listener"
output="screen" >
<param name="namespace" type="string" value="$(arg namespace)" />
</node>
</launch>
#################################
############# turtle_tf_broadcaster.py
#################################
!/usr/bin/env python
-- coding: utf-8 --
import roslib
roslib.load_manifest('learning_tf')
import rospy
import argparse #引入模塊
import tf
import turtlesim.msg
###################
added by jianming
import time
from time_profiling_node_prevar import TIME_PROFILING
from time_profiling_node_prevar import TIME_PROFILING_PATH
###################
def handle_turtle_pose(msg, framename):
br = tf.TransformBroadcaster()
br.sendTransform((msg.x, msg.y, 0),
tf.transformations.quaternion_from_euler(0, 0, msg.theta),
rospy.Time.now(),
framename,
"world")
if TIME_PROFILING:
ofile = open(TIME_PROFILING_PATH + "turtle_tf_broadcaster.txt", 'a')
end = time.time()
ofile.write('time_profiling_node time: %s Seconds'%(end-start))
ofile.write('\n')
if name == 'main':
if TIME_PROFILING:
start =time.time()
rospy.init_node('turtle_tf_broadcaster')
parser = argparse.ArgumentParser()
parser.add_argument("turtle_name", type=str, help="input turtle's name")
turtlename = parser.parse_args().turtle_name
namespace = rospy.get_param('~namespace')
turtlename = rospy.get_param('~turtle')
rospy.Subscriber('%s/pose' % (turtlename),
turtlesim.msg.Pose,
handle_turtle_pose,
namespace +"/"+ turtlename)
for i in range(10):
rospy.loginfo("%s\n", turtlename)
rospy.spin()
#################################
############# turtle_tf_listener.py
#################################
!/usr/bin/env python
-- coding: utf-8 --
import roslib
roslib.load_manifest('learning_tf')
import rospy
import math
import tf
import geometry_msgs.msg
import turtlesim.srv
if name == "main":
rospy.init_node('turtle_tf_listener')
namespace = rospy.get_param('~namespace')
listener = tf.TransformListener()
rospy.wait_for_service('spawn')
spawner = rospy.ServiceProxy('spawn', turtlesim.srv.Spawn)
spawner(4, 2, 0, 'turtle2')
turtle_vel = rospy.Publisher('turtle2/cmd_vel', geometry_msgs.msg.Twist, queue_size = 1)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
try:
(trans, rot) = listener.lookupTransform(namespace+'/turtle2', namespace+'/turtle1', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
angular = 4 * math.atan2(trans[1], trans[0])
linear = 0.5 * math.sqrt( trans[0] ** 2 + trans[1] ** 2)
cmd = geometry_msgs.msg.Twist()
cmd.linear.x = linear
cmd.angular.z = angular
turtle_vel.publish(cmd)
rate.sleep()
為了找到為什么之前更改的車存在bug，而xtark原廠的程序直接連通cartographer不出現bug，逐步比對之前余老板/天翔哥定義的文件的內容和原有之間的區分。
1. 余老板殺出了EKF融合定位節點。xtark_bringup_robot1.launch中沒有
其中所有的topic均使用相對路徑，即名稱前不加"/"，而所有的frame_id均使用arg傳入的參數作為前綴。
## 修改xtark內部launch文件使其適配robot1前綴的修改流程：
1. 新建xtark_mapping_cartographer_robot<num>_jianming.launch (<num>替換為robot號碼）
2. 將xtark_mapping_cartographer_robot1_jianming.launch 內容復制到1新建文件中，修改第二行ns為robot<num>，修改第三行robot1為指定編號。
3. xtark_bringup_robot1_jianming.launch文件。其內部包含一下模塊
- xtark_ros_wrapper_node.py：修改:base_footprint, odom, base_imu_link三個frame_id前面增加了robot1. 另外yaml文件中需要手動修改base_frame。
- robot_pose_ekf.cpp：修改:odom, base_footprint兩個frame_id前面增加了robot1.
- odom_ekf_jianming.py：修改child_frame_id該frame前面增加robot1.
- rplidarNode.cpp 由于該車的雷達型號為XAS直接找到對應的調用文件復制到此處。修改laser frame前面增加robot1
- scan_to_scan_filter_chain的參數配置文件中需要修改yaml文件中的box_frame
- 靜態tf樹發布為兩個frame_id增加robot1前綴。
- 由于找不到laser所以發布laser和base_footprint之間的tf關系。前面添加robot1名稱。
4. xtark_camera_robot1_jianming.launch文件。里面根據不同的分辨率啟動了不同分辨率的launch文件，無frameid需要修改。
5. cartographer_node文件，需要修改xtark_robot1_jianming.lua內部的frameid，包括：map_frame, tracking_frame, published_frame & odom_frame.
6. teb_move_base_omni_jianming.launch 文件內需要修改6個yaml文件，不僅要修改frameid還需要修改topic。
- teb_local_planner_params_robot1.yaml: map_frame: robot1/odom
- global_costmap_params_robot1.yaml: global_frame: robot1/map, robot_base_frame: robot1/base_footprint
- local_costmap_params_robot1.yaml ( global_frame: robot1/odom）（ robot_base_frame: robot1/base_footprint)
注意：一下的topic不能修改，因為把movebase放在group內其會自動增加相應的索引。
- costmap_common_params_robot1.yaml: （ laser_scan_sensor: topic: robot1/scan ）（ laser_scan_sensor_2: topic: robot1/scan ）（ sonar_scan_sensor: topic: robot1/sonar_cloudpoint ） (static_layer: "robot1/map")
其中9,10,11,17,18,30,40,56行均手動加入了$(arg namespace)前綴：即手動為下列frame增加了namespace前綴 laser。另有7行xtark_params.yaml需要修改內部的frame名稱(8行）
7. cartographer2discrete.cpp：內部topic定義采用相對路徑，內部無frameid，因此無需額外修改。
8. cartographer_occupancy_grid_node：內部topic定義采用相對路徑，內部無frameid，因此無需額外修改。
## 如何將robot1更改為robot2？
1. xtark_mapping_cartographer_robot2jianming.launch中namespace/robotname取值都修改為robot2。
2. 參數修改：
- cartographer的參數修改：xtark_robot1_jianming.lua----所有frame前綴都加入robot1
- teb_move_base_onmi_jianming.launch中include的6個參數文件中的robot1全部更改為robot2.
- 創建"$(find xtark_ros_wrapper)/config/xtark_params_robot<num>_jianming.yaml"中并在frame_id前增加robot<num>
- 創建"$(find xtark_ros_wrapper)/config/xtark_laserfilter_robot1_jianming.yaml"中并在frame_id前增加robot<num>
- 創建xtark_ros_wrapper/script/odom_ekf_jianming.py，將backup中的內容復制到該文件中并增加**可執行權限**。
下面的操作，逐個修改全部的frameid&topic.
Solution: 在xtark_bringup.launch文件中
<node pkg="xtark_ros_wrapper" type="odom_ekf.py" name="odom_ekf_node" args="-c base_footprint" output="screen">
默認參數選擇-c base_footprint。
另外一個warning
Map update loop missed its desired rate of 5.0000Hz... the loop actually took 0.3952 seconds
網上搜索到的一個解釋：
This means that your map cannot be updated as fast as you've parameterized it. You can choose to use a smaller or lower resolution map or provide more computational power.
使用低精度地圖或者增加更高的算力。
最簡單的方法就是降低地圖的更新頻率。實際地圖為0.39s那就 2 Hz每秒。
另外方法：釋放算力。
與樹莓派相比，樹莓派是A72四核心，jetson nano b01是a57四核心。 The Cortex-A72 is a direct successor to the Cortex-A57 - taking the predecessor as a baseline in order to iterate and improve it.
直接將cartographer和move_base直接連接在圖片中會額外多出很多數值為0的點。
對應的costmap數據會錯誤。
發現laser scan偶爾會錯位找不到tf。
如果解決laser_scan錯位？
已經測試：使用MECX1沒有出現該問題。
使用MECX1的雷達&MECX2的move_base出現了該問題。
使用MECX2的雷達&MECX1的move_base沒有出現該問題，因此認為MECX2的move_base有問題，目前正在重新安裝MECX2的move_base模塊。
其中
sudo apt-get purge ros-*
sudo rm -rf /etc/ros
gedit ~/.bashrc
找到：帶有melodic的那一行刪除，保存，然后：
source ~/.bashrc
按照ros官網上下載key并且安裝完整桌面版本。
sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
curl -sSL 'http://keyserver.ubuntu.com/pks/lookup?op=get&search=0xC1CF6E31E6BADE8868B172B4F42ED6FBAB17C654' | sudo apt-key add -
安裝之后啟動在安裝以下模塊：
sudo apt install ros-melodic-uvc-camera
sudo apt install ros-melodic-bfl
sudo apt install ros-melodic-navigation
sudo apt-get install ros-melodic-web-video-server
sudo apt-get install ros-melodic-teb-local-planner
rosdep install teb_local_planner
sudo apt-get install ros-melodic-teb-local-planner-tutorials
sudo apt install ros-melodic-uuid-msgs
sudo apt install ros-melodic-depthimage*
sudo apt install ros-melodic-unique-id*
[robot_pose_ekf](https://github.com/ros-planning/robot_pose_ekf)
安裝
git clone https://github.com/ros-planning/robot_pose_ekf.git
catkin_make
rosdep install robot_pose_ekf
rosmake
# 從本身增加robot1 namespace需要修改以下文件：
- /home/xtark/ros_ws/src/xtark_nav/launch/xtark_mapping_cartographer_robot1_jianming.launch
- /home/xtark/ros_ws/src/xtark_ros_wrapper/config/xtark_params_robot1_jianming.yaml
- /home/xtark/ros_ws/src/xtark_ros_wrapper/launch/xtark_bringup_robot1_jianming.launch
還有其他的，此處不列舉。
rqt_console可以查看warn的報錯節點。
hypre-param 就是至yaml文件中說明的內容。
