ubuntu20.04运行putn

参考文章：

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一片空白，如下图

可以参考：There is no map showing in the rviz window when launching the simulation.launch file. · Issue #3 · jianzhuozhuTHU/putn · GitHub

解决方式为：

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下运行了。