参考文章:
PUTN在Ubuntu20.04 noetic下的编译 - 知乎
Jetson AGX Orin+Ubuntu20.04+ROS noetic上安装运行A-LOAM和PUTN_jetson多线程运行loam-CSDN博客
【PUTN-针对复杂地形的自主导航系统】:快速上手篇 - 知乎
1.下载编译a-loam,由于putn的建图与定位依赖与a-loam,因此首先下载编译a-loam
(1)首先创建工作空间
mkdir putn_ws
cd putn_ws
mkdir src
catkin_make
(2)克隆a-loam代码
cd putn_ws/src
git clone https://github.com/HKUST-Aerial-Robotics/A-LOAM.git
(3)下载并编译 ceres库,这里不要用最新版本,用ceres-solver-refs_tags_1.14.0版本
下载ceres-solver-refs_tags_1.14.0并解压缩
tar zxf ceres-solver-refs_tags_1.14.0.tar.gz
mkdir ceres-bin
cd ceres-bin
cmake ../ceres-solver-refs_tags_1.14.0
make -j3
make test
sudo make install
(4)修改a-loam代码,参考文章
Ubuntu20.04安装和运行A-LOAM_a-loam 20.04-CSDN博客
ubuntu20.04+ros-noetic运行A-LOAM记录_ubuntu20.04编译aloam-CSDN博客
Ubuntu20.04下运行LOAM系列:A-LOAM、LeGO-LOAM、SC-LeGO-LOAM、LIO-SAM 和 LVI-SAM_loam运行-CSDN博客
Ubuntu20.04下A-LOAM配置安装及测试教程(包含报错问题踩坑)_aloam安装-CSDN博客
2.下载并运行putn
(1)首先修改CMakeLists.txt,和前面一样,将c++11修改为c++14即可
(2)编译报错
/usr/bin/ld: CMakeFiles/global_planning_node.dir/src/global_planning_node.cpp.o: in function `main.cold':
global_planning_node.cpp:(.text.unlikely+0x273): undefined reference to `tf::TransformListener::~TransformListener()'
/usr/bin/ld: CMakeFiles/global_planning_node.dir/src/global_planning_node.cpp.o: in function `main':
global_planning_node.cpp:(.text.startup+0xc64): undefined reference to `tf::Transformer::DEFAULT_CACHE_TIME'
/usr/bin/ld: global_planning_node.cpp:(.text.startup+0xc92): undefined reference to `tf::TransformListener::TransformListener(ros::Duration, bool)'
/usr/bin/ld: global_planning_node.cpp:(.text.startup+0xd7a): undefined reference to `tf::Transformer::lookupTransform(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&, tf::StampedTransform&) const'
/usr/bin/ld: global_planning_node.cpp:(.text.startup+0xe74): undefined reference to `tf::TransformListener::~TransformListener()'
collect2: error: ld returned 1 exit status
make[2]: *** [putn/src/putn/putn_planning/CMakeFiles/global_planning_node.dir/build.make:246:/home/lk/ROS/putn_ws/devel/lib/putn/global_planning_node] 错误 1
make[1]: *** [CMakeFiles/Makefile2:3402:putn/src/putn/putn_planning/CMakeFiles/global_planning_node.dir/all] 错误 2
make[1]: *** 正在等待未完成的任务....
/usr/bin/ld: CMakeFiles/local_obs_node.dir/src/local_obs.cpp.o: in function `rcvVelodyneCallBack(sensor_msgs::PointCloud2_<std::allocator<void> > const&)':
local_obs.cpp:(.text+0x998): undefined reference to `tf::Transformer::waitForTransform(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, ros::Time const&, ros::Duration const&, ros::Duration const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >*) const'
/usr/bin/ld: local_obs.cpp:(.text+0xbd1): undefined reference to `tf::TransformListener::transformPoint(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, geometry_msgs::PointStamped_<std::allocator<void> > const&, geometry_msgs::PointStamped_<std::allocator<void> >&) const'
/usr/bin/ld: CMakeFiles/local_obs_node.dir/src/local_obs.cpp.o: in function `main.cold':
local_obs.cpp:(.text.unlikely+0x3ef): undefined reference to `tf::TransformListener::~TransformListener()'
/usr/bin/ld: CMakeFiles/local_obs_node.dir/src/local_obs.cpp.o: in function `main':
local_obs.cpp:(.text.startup+0x62a): undefined reference to `tf::Transformer::DEFAULT_CACHE_TIME'
/usr/bin/ld: local_obs.cpp:(.text.startup+0x64d): undefined reference to `tf::TransformListener::TransformListener(ros::Duration, bool)'
/usr/bin/ld: local_obs.cpp:(.text.startup+0x6dc): undefined reference to `tf::TransformListener::~TransformListener()'
collect2: error: ld returned 1 exit status
make[2]: *** [putn/src/putn/putn_planning/CMakeFiles/local_obs_node.dir/build.make:246:/home/lk/ROS/putn_ws/devel/lib/putn/local_obs_node] 错误 1
make[1]: *** [CMakeFiles/Makefile2:3375:putn/src/putn/putn_planning/CMakeFiles/local_obs_node.dir/all] 错误 2
make: *** [Makefile:141:all] 错误 2
Invoking "make -j16 -l16" failed
解决办法:修改putn_planning功能包的CMakeLists.txt,进行如下修改
cmake_minimum_required(VERSION 3.0.2)
project(putn)
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
nav_msgs
roscpp
rviz_visual_tools
std_msgs
tf
)
find_package(PCL REQUIRED)
catkin_package(
INCLUDE_DIRS include
)
include_directories(
include
${catkin_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
)
set(CMAKE_CXX_FLAGS "-std=c++14 ${CMAKE_CXX_FLAGS} -O3 -Wall") # -Wextra -Werror
add_executable(global_planning_node
src/global_planning_node.cpp
src/PUTN_planner.cpp
src/PUTN_classes.cpp
src/PUTN_vis.cpp)
target_link_libraries(global_planning_node
${catkin_LIBRARIES}
${PCL_LIBRARIES}
${tf_LIBRARIES}
)
add_executable(local_obs_node
src/local_obs.cpp
src/PUTN_planner.cpp
src/PUTN_classes.cpp
src/PUTN_vis.cpp)
target_link_libraries(local_obs_node
${catkin_LIBRARIES}
${PCL_LIBRARIES}
${tf_LIBRARIES}
)
此时编译成功
(3)运行roslaunch putn_launch simulation.launch,出现错误。
错误1:
[ERROR] [1714782395.298807154, 0.112000000]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/front_right_wheel
[ERROR] [1714782395.299242297, 0.112000000]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/front_left_wheel
[ERROR] [1714782395.299950433, 0.112000000]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/rear_left_wheel
[ERROR] [1714782395.300355268, 0.112000000]: No p gain specified for pid. Namespace: /gazebo_ros_control/pid_gains/rear_right_wheel
这个错误本人并没有解决,但是不影响后面的运行
错误2:rviz一片空白,如下图
解决方式为:
num1:scout_description功能包scout_v2.xacro文件第12行,修改为
<xacro:HDL-32E parent="base link" name="velodyne" topic="/velodyne points" hz="10"
samples="440" gpu="false" lasers="32" max range="100"
<origin xyz=o.3" rpy="o o o" />
</xacro:HDL-32E>
num2:putn_planning功能包PUTN_vis.cpp第52行,修改为
map_vis.header.frame_id = "world";
num3:运行
sudo apt-get install ros-noetic-velodyne-gazebo-plugins
(4)此时rviz正常运行,接下来手动运行机器人,移动到rrt树出现的时候,然后停止机器人,点击3D Goal,点击i键可是机器人无法移动,需要对putn_mpc功能包下的local_planner.py文件进行修改,具体修改内容如下
#!/usr/bin/env python3
import rospy
from std_msgs.msg import Bool, Float64, Float32MultiArray
from geometry_msgs.msg import Pose, PoseArray, PoseStamped, Point, Twist
from nav_msgs.msg import Path, Odometry, OccupancyGrid
import numpy as np
import tf
from MPC import MPC
from sensor_msgs.msg import LaserScan
from visualization_msgs.msg import Marker,MarkerArray
from std_srvs.srv import SetBool
class Local_Planner():
def __init__(self):
self.replan_period = rospy.get_param('/local_planner/replan_period', 0.01)
self.curr_state = np.zeros(5)
self.z = 0
self.N = 10
self.goal_state = np.zeros([self.N,4])
self.ref_path_close_set = False
self.target_state = np.array([-1,4,np.pi/2])
self.target_state_close = np.zeros(3)
self.desired_global_path = [ np.zeros([300,4]) , 0]
self.have_plan = False
self.is_close = False
self.is_get = False
self.is_grasp = False
self.is_all_task_down = False
self.robot_state_set = False
self.ref_path_set = False
self.ob=[]
self.is_end=0
self.ob_total = []
self.__timer_replan = rospy.Timer(rospy.Duration(self.replan_period), self.__replan_cb)
self.__sub_curr_state = rospy.Subscriber('/curr_state', Float32MultiArray, self.__curr_pose_cb, queue_size=10)
self.__sub_obs = rospy.Subscriber('/obs', Float32MultiArray, self.__obs_cb, queue_size=10)
self.__sub_goal_state = rospy.Subscriber('/surf_predict_pub', Float32MultiArray, self._global_path_callback2, queue_size=10)
self.__pub_local_path = rospy.Publisher('/local_path', Path, queue_size=10)
self.__pub_local_plan = rospy.Publisher('/local_plan', Float32MultiArray, queue_size=10)
self.control_cmd = Twist()
self.listener = tf.TransformListener()
self.times = 0
self.obstacle_markerarray = MarkerArray()
self.ob_pub = rospy.Publisher('/ob_draw', MarkerArray, queue_size=10)
def distance_sqaure(self,c1,c2):
distance = (c1[0]-c2[0])*(c1[0]-c2[0])+(c1[1]-c2[1])*(c1[1]-c2[1])
return distance
def draw_ob(self):
self.obstacle_markerarray.markers=[]
num = 0
for i in range(len(self.ob)):
t_ob = Marker()
t_ob.header.frame_id = "world"
t_ob.id = num
t_ob.type = t_ob.CYLINDER
t_ob.action = t_ob.ADD
t_ob.pose.position.x = self.ob[i][0]
t_ob.pose.position.y = self.ob[i][1]
t_ob.pose.position.z=0.2
t_ob.scale.x = 0.1
t_ob.scale.y = 0.1
t_ob.scale.z = 0.4
t_ob.color.a= 1
t_ob.color.r = 0
t_ob.color.g = 1
t_ob.color.b = 0
self.obstacle_markerarray.markers.append(t_ob)
num = num +1
self.ob_pub.publish(self.obstacle_markerarray)
def _scan_callback(self, data):
self.ob = []
phi = data.angle_min
point_last = np.array([100, 100])
for r in data.ranges:
point = np.array([self.curr_state[0]+r*np.cos(phi+self.curr_state[2]),self.curr_state[1]+r*np.sin(phi+self.curr_state[2])])
if (r >= data.range_min and r <= data.range_max and r<=1.0 and self.distance_sqaure(point,point_last) > 0.04 ):
self.ob.append( point )
point_last = point
phi += data.angle_increment
self.draw_ob()
def __obs_cb(self, data):
self.ob = []
if(len(data.data)!=0):
size = len(data.data)/3
for i in range(int(size)):
self.ob.append(( (data.data[3*i]//0.3)*0.3, (data.data[3*i+1]//0.3)*0.3) )
dic = list(set([tuple(t) for t in self.ob]))
self.ob = [list(v) for v in dic]
self.draw_ob()
def __replan_cb(self, event):
if self.robot_state_set and self.ref_path_set:
target = []
self.choose_goal_state() ## gobal planning
dist = 1
goal = np.array([self.target_state[0], self.target_state[1], self.target_state[2]])
start_time = rospy.Time.now()
states_sol, input_sol = MPC(np.expand_dims(self.curr_state, axis=0),self.goal_state,self.ob) ## gobal planning
end_time = rospy.Time.now()
rospy.loginfo('[pHRI Planner] phri so[lved in {} sec'.format((end_time-start_time).to_sec()))
if(self.is_end == 0):
self.__publish_local_plan(input_sol,states_sol)
self.have_plan = True
elif self.robot_state_set==False and self.ref_path_set==True:
print("no pose")
elif self.robot_state_set==True and self.ref_path_set==False:
print("no path")
else:
print("no path and no pose")
def __publish_local_plan(self,input_sol,state_sol):
local_path = Path()
local_plan = Float32MultiArray()
sequ = 0
local_path.header.stamp = rospy.Time.now()
local_path.header.frame_id = "world"
for i in range(self.N):
this_pose_stamped = PoseStamped()
this_pose_stamped.pose.position.x = state_sol[i,0]
this_pose_stamped.pose.position.y = state_sol[i,1]
this_pose_stamped.pose.position.z = self.z+0.5 #self.desired_global_path[0][0,2]
this_pose_stamped.header.seq = sequ
sequ += 1
this_pose_stamped.header.stamp = rospy.Time.now()
this_pose_stamped.header.frame_id="world"
local_path.poses.append(this_pose_stamped)
for j in range(2):
local_plan.data.append(input_sol[i][j])
self.__pub_local_path.publish(local_path)
self.__pub_local_plan.publish(local_plan)
def distance_global(self,c1,c2):
distance = np.sqrt((c1[0]-c2[0])*(c1[0]-c2[0])+(c1[1]-c2[1])*(c1[1]-c2[1]))
return distance
def find_min_distance(self,c1):
# number = np.argmin( np.array([self.distance_global(c1,self.desired_global_path[0][i]) for i in range(self.desired_global_path[1])]) )
number = np.argmin(np.array([self.distance_global(c1, self.desired_global_path[0][i]) for i in range(int(self.desired_global_path[1]))]))
return number
def choose_goal_state(self):
num = self.find_min_distance(self.curr_state)
scale = 1
num_list = []
for i in range(self.N):
num_path = min(self.desired_global_path[1]-1,int(num+i*scale))
num_list.append(num_path)
if(num >= self.desired_global_path[1]):
self.is_end = 1
for k in range(self.N):
# self.goal_state[k] = self.desired_global_path[0][num_list[k]]
self.goal_state[k] = self.desired_global_path[0][int(num_list[k])]
print(self.goal_state)
def __curr_pose_cb(self, data):
self.robot_state_set = True
self.curr_state[0] = data.data[0]
self.curr_state[1] = data.data[1]
self.curr_state[2] = data.data[3]
self.curr_state[3] = data.data[4]
self.curr_state[4] = data.data[5]
self.z = data.data[2]
def _global_path_callback(self, data):
if(len(data.data)!=0):
self.ref_path_set = True
size = len(data.data)/3
self.desired_global_path[1]=size
for i in range(int(size)):
self.desired_global_path[0][i,0]=data.data[3*(int(size)-i)-3]
self.desired_global_path[0][i,1]=data.data[3*(int(size)-i)-2]
self.desired_global_path[0][i,2]=data.data[3*(int(size)-i)-1]
def _global_path_callback2(self, data):
if(len(data.data)!=0):
self.ref_path_set = True
size = len(data.data)/5
self.desired_global_path[1]=size
for i in range(int(size)):
self.desired_global_path[0][i,0]=data.data[5*(int(size)-i)-5]
self.desired_global_path[0][i,1]=data.data[5*(int(size)-i)-4]
self.desired_global_path[0][i,2]=data.data[5*(int(size)-i)-2]
self.desired_global_path[0][i,3]=data.data[5*(int(size)-i)-1]
def cmd(self, data):
self.control_cmd.linear.x = data[0]
self.control_cmd.angular.z = data[1]
self.__pub_rtc_cmd.publish(self.control_cmd)
if __name__ == '__main__':
rospy.init_node("local_planner")
phri_planner = Local_Planner()
rospy.spin()
主要修改为_global_path_callback和_global_path_callback2函数的size变量强制转化为int类型,__obs_cb函数的size变量修改为int类型,find_min_distance函数的self.desired_global_path[1]强制转化为int类型,choose_goal_state函数的num_list[k]变量修改为int类型。
至此putn算法已经可以在ubuntu20.04下运行了。