ubuntu运行ego_planner问题

Question

在ubuntu20.04系统上运行ego_planner算法时，总是等待7次后报错
[FSM]: state: WAIT_TARGET
wait for goal or trigger.
Vins-fusion以及环境都已经部署完成，是不是有什么地方没设置，或者什么问题导致的呢？求佬解答

roslaunch ego_planner single_run_in_exp.launch 
... logging to /home/fastpx4/.ros/log/34ca6214-f716-11ee-b28b-df39fb9a9c74/roslaunch-fastpx4-10908.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://192.168.3.90:39137/

SUMMARY
========

PARAMETERS
 * /drone_0_ego_planner_node/bspline/limit_acc: 6.0
 * /drone_0_ego_planner_node/bspline/limit_ratio: 1.1
 * /drone_0_ego_planner_node/bspline/limit_vel: 0.5
 * /drone_0_ego_planner_node/fsm/emergency_time: 1.0
 * /drone_0_ego_planner_node/fsm/fail_safe: True
 * /drone_0_ego_planner_node/fsm/flight_type: 1
 * /drone_0_ego_planner_node/fsm/planning_horizen_time: 3.0
 * /drone_0_ego_planner_node/fsm/planning_horizon: 6.0
 * /drone_0_ego_planner_node/fsm/realworld_experiment: True
 * /drone_0_ego_planner_node/fsm/thresh_no_replan_meter: 1.0
 * /drone_0_ego_planner_node/fsm/thresh_replan_time: 1.0
 * /drone_0_ego_planner_node/fsm/waypoint0_x: 15.0
 * /drone_0_ego_planner_node/fsm/waypoint0_y: 0.0
 * /drone_0_ego_planner_node/fsm/waypoint0_z: 1.0
 * /drone_0_ego_planner_node/fsm/waypoint1_x: 0.0
 * /drone_0_ego_planner_node/fsm/waypoint1_y: 0.0
 * /drone_0_ego_planner_node/fsm/waypoint1_z: 1.0
 * /drone_0_ego_planner_node/fsm/waypoint2_x: 15.0
 * /drone_0_ego_planner_node/fsm/waypoint2_y: 0.0
 * /drone_0_ego_planner_node/fsm/waypoint2_z: 1.0
 * /drone_0_ego_planner_node/fsm/waypoint3_x: 0.0
 * /drone_0_ego_planner_node/fsm/waypoint3_y: 0.0
 * /drone_0_ego_planner_node/fsm/waypoint3_z: 1.0
 * /drone_0_ego_planner_node/fsm/waypoint4_x: 15.0
 * /drone_0_ego_planner_node/fsm/waypoint4_y: 0.0
 * /drone_0_ego_planner_node/fsm/waypoint4_z: 1.0
 * /drone_0_ego_planner_node/fsm/waypoint_num: 1
 * /drone_0_ego_planner_node/grid_map/cx: 654.2191772460938
 * /drone_0_ego_planner_node/grid_map/cy: 359.2149658203125
 * /drone_0_ego_planner_node/grid_map/depth_filter_margin: 2
 * /drone_0_ego_planner_node/grid_map/depth_filter_maxdist: 5.0
 * /drone_0_ego_planner_node/grid_map/depth_filter_mindist: 0.2
 * /drone_0_ego_planner_node/grid_map/depth_filter_tolerance: 0.15
 * /drone_0_ego_planner_node/grid_map/frame_id: world
 * /drone_0_ego_planner_node/grid_map/fx: 799.2300415039062
 * /drone_0_ego_planner_node/grid_map/fy: 799.2300415039062
 * /drone_0_ego_planner_node/grid_map/ground_height: -0.01
 * /drone_0_ego_planner_node/grid_map/k_depth_scaling_factor: 1000.0
 * /drone_0_ego_planner_node/grid_map/local_map_margin: 10
 * /drone_0_ego_planner_node/grid_map/local_update_range_x: 5.5
 * /drone_0_ego_planner_node/grid_map/local_update_range_y: 5.5
 * /drone_0_ego_planner_node/grid_map/local_update_range_z: 4.5
 * /drone_0_ego_planner_node/grid_map/map_size_x: 20
 * /drone_0_ego_planner_node/grid_map/map_size_y: 20
 * /drone_0_ego_planner_node/grid_map/map_size_z: 3.0
 * /drone_0_ego_planner_node/grid_map/max_ray_length: 5.0
 * /drone_0_ego_planner_node/grid_map/min_ray_length: 0.3
 * /drone_0_ego_planner_node/grid_map/obstacles_inflation: 0.2
 * /drone_0_ego_planner_node/grid_map/p_hit: 0.65
 * /drone_0_ego_planner_node/grid_map/p_max: 0.9
 * /drone_0_ego_planner_node/grid_map/p_min: 0.12
 * /drone_0_ego_planner_node/grid_map/p_miss: 0.35
 * /drone_0_ego_planner_node/grid_map/p_occ: 0.8
 * /drone_0_ego_planner_node/grid_map/pose_type: 2
 * /drone_0_ego_planner_node/grid_map/resolution: 0.1
 * /drone_0_ego_planner_node/grid_map/show_occ_time: False
 * /drone_0_ego_planner_node/grid_map/skip_pixel: 2
 * /drone_0_ego_planner_node/grid_map/use_depth_filter: True
 * /drone_0_ego_planner_node/grid_map/visualization_truncate_height: 1.8
 * /drone_0_ego_planner_node/manager/control_points_distance: 0.4
 * /drone_0_ego_planner_node/manager/drone_id: 0
 * /drone_0_ego_planner_node/manager/feasibility_tolerance: 0.05
 * /drone_0_ego_planner_node/manager/max_acc: 6.0
 * /drone_0_ego_planner_node/manager/max_jerk: 4.0
 * /drone_0_ego_planner_node/manager/max_vel: 0.5
 * /drone_0_ego_planner_node/manager/planning_horizon: 6.0
 * /drone_0_ego_planner_node/manager/use_distinctive_trajs: False
 * /drone_0_ego_planner_node/optimization/dist0: 0.5
 * /drone_0_ego_planner_node/optimization/lambda_collision: 0.5
 * /drone_0_ego_planner_node/optimization/lambda_feasibility: 0.1
 * /drone_0_ego_planner_node/optimization/lambda_fitness: 1.0
 * /drone_0_ego_planner_node/optimization/lambda_smooth: 1.0
 * /drone_0_ego_planner_node/optimization/max_acc: 6.0
 * /drone_0_ego_planner_node/optimization/max_vel: 0.5
 * /drone_0_ego_planner_node/optimization/swarm_clearance: 0.5
 * /drone_0_ego_planner_node/prediction/lambda: 1.0
 * /drone_0_ego_planner_node/prediction/obj_num: 10
 * /drone_0_ego_planner_node/prediction/predict_rate: 1.0
 * /drone_0_traj_server/traj_server/time_forward: 1.0
 * /rosdistro: noetic
 * /rosversion: 1.16.0

NODES
  /
    drone_0_ego_planner_node (ego_planner/ego_planner_node)
    drone_0_traj_server (ego_planner/traj_server)

ROS_MASTER_URI=http://192.168.3.90:11311

process[drone_0_ego_planner_node-1]: started with pid [10934]
process[drone_0_traj_server-2]: started with pid [10935]
hit: 0.619039
miss: -0.619039
min log: -1.99243
max: 2.19722
thresh log: 1.38629
[FSM]: from INIT to WAIT_TARGET
[ WARN] [1712738732.652992070]: [Traj server]: ready.
[FSM]: state: WAIT_TARGET
wait for goal or trigger.
[FSM]: state: WAIT_TARGET
wait for goal or trigger.
[FSM]: state: WAIT_TARGET
wait for goal or trigger.
[FSM]: state: WAIT_TARGET
wait for goal or trigger.
[FSM]: state: WAIT_TARGET
wait for goal or trigger.
[FSM]: state: WAIT_TARGET
wait for goal or trigger.
[FSM]: state: WAIT_TARGET
wait for goal or trigger.
[drone_0_ego_planner_node-1] process has died [pid 10934, exit code -11, cmd /home/fastpx4/abc/Fast-Drone-250-master/Fast-Drone-250/devel/lib/ego_planner/ego_planner_node ~odom_world:=/vins_fusion/imu_propagate ~planning/bspline:=/drone_0_planning/bspline ~planning/data_display:=/drone_0_planning/data_display ~planning/broadcast_bspline_from_planner:=/broadcast_bspline ~planning/broadcast_bspline_to_planner:=/broadcast_bspline ~grid_map/odom:=/vins_fusion/imu_propagate ~grid_map/cloud:=nouse2 ~grid_map/pose:=nouse1 ~grid_map/depth:=/stereo_inertial_publisher/stereo/image __name:=drone_0_ego_planner_node __log:=/home/fastpx4/.ros/log/34ca6214-f716-11ee-b28b-df39fb9a9c74/drone_0_ego_planner_node-1.log].
log file: /home/fastpx4/.ros/log/34ca6214-f716-11ee-b28b-df39fb9a9c74/drone_0_ego_planner_node-1*.log
^C[drone_0_traj_server-2] killing on exit
shutting down processing monitor...
... shutting down processing monitor complete
done

log文件中的报错：

[rosmaster.threadpool][ERROR] 2024-04-10 16:45:15,989: Traceback (most recent call last):
  File "/opt/ros/noetic/lib/python3/dist-packages/rosmaster/threadpool.py", line 218, in run
    result = cmd(*args)
  File "/opt/ros/noetic/lib/python3/dist-packages/rosmaster/master_api.py", line 210, in publisher_update_task
    ret = xmlrpcapi(api).publisherUpdate('/master', topic, pub_uris)
  File "/usr/lib/python3.8/xmlrpc/client.py", line 1109, in __call__
    return self.__send(self.__name, args)
  File "/usr/lib/python3.8/xmlrpc/client.py", line 1450, in __request
    response = self.__transport.request(
  File "/usr/lib/python3.8/xmlrpc/client.py", line 1153, in request
    return self.single_request(host, handler, request_body, verbose)
  File "/usr/lib/python3.8/xmlrpc/client.py", line 1165, in single_request
    http_conn = self.send_request(host, handler, request_body, verbose)
  File "/usr/lib/python3.8/xmlrpc/client.py", line 1278, in send_request
    self.send_content(connection, request_body)
  File "/usr/lib/python3.8/xmlrpc/client.py", line 1308, in send_content
    connection.endheaders(request_body)
  File "/usr/lib/python3.8/http/client.py", line 1251, in endheaders
    self._send_output(message_body, encode_chunked=encode_chunked)
  File "/usr/lib/python3.8/http/client.py", line 1011, in _send_output
    self.send(msg)
  File "/usr/lib/python3.8/http/client.py", line 951, in send
    self.connect()
  File "/usr/lib/python3.8/http/client.py", line 922, in connect
    self.sock = self._create_connection(
  File "/usr/lib/python3.8/socket.py", line 808, in create_connection
    raise err
  File "/usr/lib/python3.8/socket.py", line 796, in create_connection
    sock.connect(sa)
ConnectionRefusedError: [Errno 111] Connection refused

process[drone_0_ego_planner_node-1]: args[['/home/fastpx4/abc/Fast-Drone-250-master/Fast-Drone-250/devel/lib/ego_planner/ego_planner_node', '~odom_world:=/vins_fusion/imu_propagate', '~planning/bspline:=/drone_0_planning/bspline', '~planning/data_display:=/drone_0_planning/data_display', '~planning/broadcast_bspline_from_planner:=/broadcast_bspline', '~planning/broadcast_bspline_to_planner:=/broadcast_bspline', '~grid_map/odom:=/vins_fusion/imu_propagate', '~grid_map/cloud:=nouse2', '~grid_map/pose:=nouse1', '~grid_map/depth:=/stereo_inertial_publisher/stereo/image', '__name:=drone_0_ego_planner_node']]

Answer 1

引自免费微信小程序：皆我百晓生

首先，你需要确保ego_planner_node节点已经在drone_0 Traj Server节点上正确配置。这通常可以通过修改以下参数实现：/drone_0_ego_planner_node/bspline/limit_acc, /drone_0_ego_planner_node/bspline/limit_ratio, 和 /drone_0_ego_planner_node/bspline/limit_vel。

在Ubuntu20.04系统上运行这个模块时遇到错误Waiting for target or trigger, 你可能想要调整这些限制参数以便满足目标检测或者触发条件。这可以通过增加它们的值或减少它们的值来实现。

此外，你可能还需要确保目标传感器数据已经获取到。可以使用以下代码来检查速度信息：

from ego_planner import vehicle_info

if vehicle_info.is_acc_vdict_valid():
    # get current speed information
    vehicle_info.get_speed_info()
else:
    print("Missing sensor data.")

如果你怀疑可能的原因是传感器位置，例如地面、障碍物或其他物体的位置变化，可以使用以下代码进行分析：

import cv2

def get_shapes_and_fields(image_path):
    image = cv2.imread(image_path)
    
    x = np.array(image.shape[:2])[0]
    y = np.array(image.shape[:2])[1]
    
    shape_list = [(x, y), (x + 50, y)]
    field_list = [(x, y, "dom"), (x + 50, y, "cloud")]
    
    return shape_list, field_list

shape_list, field_list = get_shapes_and_fields("/home/fastpx4/abc/Fast-Drone-250-master/Fast-Drone-250/devel/lib/ego_planner/ego_planner_node/image")
print(shape_list)
print(field_list)

如果传感器没有数据，或者错误信息表明可能的问题导致了Wait_for_target，那么你可能需要重新训练或者更新ego_planner_node模块。

如果以上都无法解决问题，你可能需要进一步查看错误日志以确定具体问题所在的模块。在这个例子中，有几个关键点需要注意：

• /drone_0_ego_planner_node/bspline/limit_acc: 这是一个约束性的参数，用于限制避障矩阵的最小速度。
• /drone_0_ego_planner_node/bspline/limit_ratio: 这是一个约束性的参数，用于限制避障矩阵的最大速度。
• /drone_0_ego_planner_node/bspline/limit_vel: 这也是一个约束性的参数，用于限制避障矩阵的加速度。

尝试增加这些值可能有助于解决这个问题。你也可以尝试将这些值更改为一个可调范围内的特定值，这样可能会更加灵活，适应不同的场景。

请注意，在任何情况下，始终谨慎操作，并确保正确理解错误消息以了解需要做什么。如果可能的话，您可以将错误日志保存为文本文件，以便将来参考。