写python脚本订阅/amcl_pose坐标

源码链接:公众号(文章末尾二维码)内回复qita0002获取
一个简单的python小脚本用来订阅/amcl_pose的坐标,代码如下:

#! /usr/bin/env python
#coding=utf-8
import rospy
from geometry_msgs.msg import PoseWithCovarianceStamped,PoseStamped
def PoseCallBack(msg):
	data=""
	#订阅到的坐标信息
	x = msg.pose.pose.position.x
	y = msg.pose.pose.position.y
	#订阅到的四元数的信息,用来表示朝向
	orien_z = msg.pose.pose.orientation.z
	orien_w = msg.pose.pose.orientation.w
	
	data = str(x) + "," + str(y)+ "," + str(orien_z)+ "," + str(orien_w)
	rospy.loginfo(data)

def PoseSub():
	rospy.init_node('pose_sub',anonymous=False)
	#监控话题,并在回调函数中处理
	rospy.Subscriber('/amcl_pose',PoseWithCovarianceStamped,PoseCallBack)
	rospy.spin()
if __name__=='__main__':
	try:
		PoseSub()
	except:
		rospy.loginfo("出错,退出中...")

测试:启动roscore->启动amcl并调用一张地图->启动rviz->启动刚才写的脚本用来实时监控话题->点击rviz中的2D Pose Estimate。查看脚本监控窗口
在这里插入图片描述

EPRobot@EPRobot:~/robot_ws$ roscd usb_cam EPRobot@EPRobot:/opt/ros/melodic/share/usb_cam$ source/opt/ros/melodic/setup.bash bash: source/opt/ros/melodic/setup.bash: No such file or directory EPRobot@EPRobot:/opt/ros/melodic/share/usb_cam$ cd ~/robot_ws EPRobot@EPRobot:~/robot_ws$ source devel/setup.bash EPRobot@EPRobot:~/robot_ws$ roslaunch eprobot_start one_car_start.launch ... logging to /home/EPRobot/.ros/log/99f833d2-6236-11f0-a6b3-e45f0131f240/roslaunch-EPRobot-14068.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://EPRobot:33445/ SUMMARY ======== CLEAR PARAMETERS * /ekf_se/ PARAMETERS * /amcl/base_frame_id: base_footprint * /amcl/global_frame_id: map * /amcl/gui_publish_rate: 10.0 * /amcl/initial_pose_a: 0.0 * /amcl/initial_pose_x: 0.0 * /amcl/initial_pose_y: 0.0 * /amcl/kld_err: 0.05 * /amcl/kld_z: 0.99 * /amcl/laser_lambda_short: 0.1 * /amcl/laser_likelihood_max_dist: 5.0 * /amcl/laser_max_beams: 60 * /amcl/laser_max_range: 12.0 * /amcl/laser_model_type: likelihood_field * /amcl/laser_sigma_hit: 0.2 * /amcl/laser_z_hit: 0.7 * /amcl/laser_z_max: 0.1 * /amcl/laser_z_rand: 0.3 * /amcl/laser_z_short: 0.1 * /amcl/max_particles: 2000 * /amcl/min_particles: 600 * /amcl/odom_alpha1: 0.2 * /amcl/odom_alpha2: 0.2 * /amcl/odom_alpha3: 0.2 * /amcl/odom_alpha4: 0.2 * /amcl/odom_alpha5: 0.1 * /amcl/odom_frame_id: odom * /amcl/odom_model_type: diff * /amcl/recovery_alpha_fast: 0.2 * /amcl/recovery_alpha_slow: 0.005 * /amcl/resample_interval: 0.4 * /amcl/transform_tolerance: 0.5 * /amcl/update_min_a: 0.05 * /amcl/update_min_d: 0.05 * /amcl/use_map_topic: True * /base_control/base_kd: 0.0 * /base_control/base_ki: 100.0 * /base_control/base_kp: 1000.0 * /base_control/base_kv: 1.0 * /base_control/is_send_anger: false * /camera_uri: http://192.168.3.... * /ekf_se/base_link_frame: /base_footprint * /ekf_se/debug: False * /ekf_se/debug_out_file: /path/to/debug/fi... * /ekf_se/frequency: 30 * /ekf_se/imu0: /imu_data * /ekf_se/imu0_config: [False, False, Fa... * /ekf_se/imu0_differential: False * /ekf_se/imu0_nodelay: True * /ekf_se/imu0_pose_rejection_threshold: 0.8 * /ekf_se/imu0_queue_size: 8 * /ekf_se/imu0_relative: True * /ekf_se/imu0_remove_gravitational_acceleration: True * /ekf_se/initial_estimate_covariance: ['1e-9', 0, 0, 0,... * /ekf_se/map_frame: /map * /ekf_se/odom0: /odom * /ekf_se/odom0_config: [False, False, Fa... * /ekf_se/odom0_differential: False * /ekf_se/odom0_nodelay: True * /ekf_se/odom0_queue_size: 5 * /ekf_se/odom0_relative: True * /ekf_se/odom_frame: /odom * /ekf_se/print_diagnostics: True * /ekf_se/process_noise_covariance: [0.01, 0, 0, 0, 0... * /ekf_se/publish_acceleration: True * /ekf_se/publish_tf: True * /ekf_se/sensor_timeout: 0.025 * /ekf_se/transform_time_offset: 0.0001 * /ekf_se/transform_timeout: 0.025 * /ekf_se/two_d_mode: True * /ekf_se/world_frame: /odom * /image_view/autosize: True * /laser_filter/scan_filter_chain: [{'type': 'laser_... * /lslidar_driver_node/angle_disable_max: 0 * /lslidar_driver_node/angle_disable_min: 0 * /lslidar_driver_node/compensation: False * /lslidar_driver_node/frame_id: base_laser_link * /lslidar_driver_node/high_reflection: False * /lslidar_driver_node/interface_selection: serial * /lslidar_driver_node/lidar_name: M10 * /lslidar_driver_node/max_range: 100.0 * /lslidar_driver_node/min_range: 0.1 * /lslidar_driver_node/pubPointCloud2: False * /lslidar_driver_node/pubScan: True * /lslidar_driver_node/scan_topic: scan * /lslidar_driver_node/serial_port: /dev/EPRobot_laser * /lslidar_driver_node/use_gps_ts: False * /map_server/frame_id: map * /move_base/TebLocalPlannerROS/acc_lim_theta: 0.3 * /move_base/TebLocalPlannerROS/acc_lim_x: 0.2 * /move_base/TebLocalPlannerROS/allow_init_with_backwards_motion: True * /move_base/TebLocalPlannerROS/cmd_angle_instead_rotvel: False * /move_base/TebLocalPlannerROS/costmap_converter_plugin: * /move_base/TebLocalPlannerROS/costmap_converter_rate: 5 * /move_base/TebLocalPlannerROS/costmap_converter_spin_thread: True * /move_base/TebLocalPlannerROS/costmap_obstacles_behind_robot_dist: 1.0 * /move_base/TebLocalPlannerROS/dt_hysteresis: 0.1 * /move_base/TebLocalPlannerROS/dt_ref: 0.3 * /move_base/TebLocalPlannerROS/dynamic_obstacle_inflation_dist: 0.2 * /move_base/TebLocalPlannerROS/enable_homotopy_class_planning: False * /move_base/TebLocalPlannerROS/enable_multithreading: True * /move_base/TebLocalPlannerROS/exact_arc_length: False * /move_base/TebLocalPlannerROS/feasibility_check_no_poses: 3 * /move_base/TebLocalPlannerROS/footprint_model/line_end: [0.1, 0.0] * /move_base/TebLocalPlannerROS/footprint_model/line_start: [-0.1, 0.0] * /move_base/TebLocalPlannerROS/footprint_model/type: line * /move_base/TebLocalPlannerROS/force_reinit_new_goal_dist: 0.5 * /move_base/TebLocalPlannerROS/free_goal_vel: False * /move_base/TebLocalPlannerROS/global_plan_overwrite_orientation: True * /move_base/TebLocalPlannerROS/global_plan_viapoint_sep: 3.0 * /move_base/TebLocalPlannerROS/h_signature_prescaler: 0.5 * /move_base/TebLocalPlannerROS/h_signature_threshold: 0.1 * /move_base/TebLocalPlannerROS/include_costmap_obstacles: True * /move_base/TebLocalPlannerROS/include_dynamic_obstacles: True * /move_base/TebLocalPlannerROS/inflation_dist: 0.35 * /move_base/TebLocalPlannerROS/is_footprint_dynamic: False * /move_base/TebLocalPlannerROS/legacy_obstacle_association: False * /move_base/TebLocalPlannerROS/max_global_plan_lookahead_dist: 10.0 * /move_base/TebLocalPlannerROS/max_number_classes: 2 * /move_base/TebLocalPlannerROS/max_vel_theta: 1.0 * /move_base/TebLocalPlannerROS/max_vel_x: 0.6 * /move_base/TebLocalPlannerROS/max_vel_x_backwards: 0.3 * /move_base/TebLocalPlannerROS/max_vel_y: 0.0 * /move_base/TebLocalPlannerROS/min_obstacle_dist: 0.55 * /move_base/TebLocalPlannerROS/min_turning_radius: 0.6 * /move_base/TebLocalPlannerROS/no_inner_iterations: 5 * /move_base/TebLocalPlannerROS/no_outer_iterations: 4 * /move_base/TebLocalPlannerROS/obstacle_association_cutoff_factor: 5.0 * /move_base/TebLocalPlannerROS/obstacle_association_force_inclusion_factor: 1.5 * /move_base/TebLocalPlannerROS/obstacle_cost_exponent: 5.5 * /move_base/TebLocalPlannerROS/obstacle_heading_threshold: 0.45 * /move_base/TebLocalPlannerROS/obstacle_keypoint_offset: 0.1 * /move_base/TebLocalPlannerROS/obstacle_poses_affected: 15 * /move_base/TebLocalPlannerROS/odom_topic: odom * /move_base/TebLocalPlannerROS/optimization_activate: True * /move_base/TebLocalPlannerROS/optimization_verbose: False * /move_base/TebLocalPlannerROS/oscillation_recovery: True * /move_base/TebLocalPlannerROS/penalty_epsilon: 0.08 * /move_base/TebLocalPlannerROS/roadmap_graph_area_length_scale: 1.0 * /move_base/TebLocalPlannerROS/roadmap_graph_area_width: 5 * /move_base/TebLocalPlannerROS/roadmap_graph_no_samples: 15 * /move_base/TebLocalPlannerROS/selection_alternative_time_cost: False * /move_base/TebLocalPlannerROS/selection_cost_hysteresis: 1.0 * /move_base/TebLocalPlannerROS/selection_obst_cost_scale: 1.0 * /move_base/TebLocalPlannerROS/selection_viapoint_cost_scale: 1.0 * /move_base/TebLocalPlannerROS/shrink_horizon_backup: True * /move_base/TebLocalPlannerROS/simple_exploration: False * /move_base/TebLocalPlannerROS/teb_autosize: True * /move_base/TebLocalPlannerROS/via_points_ordered: False * /move_base/TebLocalPlannerROS/visualize_hc_graph: False * /move_base/TebLocalPlannerROS/weight_acc_lim_theta: 1 * /move_base/TebLocalPlannerROS/weight_acc_lim_x: 1 * /move_base/TebLocalPlannerROS/weight_adapt_factor: 1.0 * /move_base/TebLocalPlannerROS/weight_dynamic_obstacle: 100 * /move_base/TebLocalPlannerROS/weight_dynamic_obstacle_inflation: 0.6 * /move_base/TebLocalPlannerROS/weight_inflation: 1.0 * /move_base/TebLocalPlannerROS/weight_kinematics_forward_drive: 800 * /move_base/TebLocalPlannerROS/weight_kinematics_nh: 1000 * /move_base/TebLocalPlannerROS/weight_kinematics_turning_radius: 6.0 * /move_base/TebLocalPlannerROS/weight_max_vel_theta: 1 * /move_base/TebLocalPlannerROS/weight_max_vel_x: 2 * /move_base/TebLocalPlannerROS/weight_obstacle: 190 * /move_base/TebLocalPlannerROS/weight_optimaltime: 15 * /move_base/TebLocalPlannerROS/weight_shortest_path: 5.0 * /move_base/TebLocalPlannerROS/weight_viapoint: 10.0 * /move_base/TebLocalPlannerROS/wheelbase: 0.3 * /move_base/TebLocalPlannerROS/xy_goal_tolerance: 0.1 * /move_base/TebLocalPlannerROS/yaw_goal_tolerance: 0.1 * /move_base/base_local_planner: teb_local_planner... * /move_base/clearing_rotation_allowed: False * /move_base/conservative_reset_dist: 0.2 * /move_base/controller_frequency: 5.0 * /move_base/controller_patience: 15.0 * /move_base/global_costmap/footprint: [[-0.18, -0.18], ... * /move_base/global_costmap/global_frame: map * /move_base/global_costmap/inflation_layer/cost_scaling_factor: 10.0 * /move_base/global_costmap/inflation_layer/enabled: True * /move_base/global_costmap/inflation_layer/inflation_radius: 0.3 * /move_base/global_costmap/inflation_radius: 0.4 * /move_base/global_costmap/laser_scan_sensor/clearing: True * /move_base/global_costmap/laser_scan_sensor/data_type: LaserScan * /move_base/global_costmap/laser_scan_sensor/marking: True * /move_base/global_costmap/laser_scan_sensor/topic: scan * /move_base/global_costmap/map_type: costmap * /move_base/global_costmap/observation_sources: laser_scan_sensor * /move_base/global_costmap/obstacle_layer/combination_method: 1 * /move_base/global_costmap/obstacle_layer/enabled: True * /move_base/global_costmap/obstacle_layer/inflation_radius: 0.2 * /move_base/global_costmap/obstacle_layer/obstacle_range: 6.5 * /move_base/global_costmap/obstacle_layer/raytrace_range: 6.0 * /move_base/global_costmap/obstacle_layer/track_unknown_space: False * /move_base/global_costmap/plugins: [{'type': 'costma... * /move_base/global_costmap/publish_frequency: 5.0 * /move_base/global_costmap/robot_base_frame: base_footprint * /move_base/global_costmap/static_layer/enabled: True * /move_base/global_costmap/static_layer/map_topic: /map * /move_base/global_costmap/static_map: True * /move_base/global_costmap/transform_tolerance: 0.5 * /move_base/global_costmap/update_frequency: 5.0 * /move_base/local_costmap/cost_scaling_factor: 10 * /move_base/local_costmap/footprint: [[-0.18, -0.18], ... * /move_base/local_costmap/global_frame: map * /move_base/local_costmap/height: 3 * /move_base/local_costmap/inflation_layer/cost_scaling_factor: 10.0 * /move_base/local_costmap/inflation_layer/enabled: True * /move_base/local_costmap/inflation_layer/inflation_radius: 0.3 * /move_base/local_costmap/inflation_radius: 0.16 * /move_base/local_costmap/laser_scan_sensor/clearing: True * /move_base/local_costmap/laser_scan_sensor/data_type: LaserScan * /move_base/local_costmap/laser_scan_sensor/marking: True * /move_base/local_costmap/laser_scan_sensor/topic: scan * /move_base/local_costmap/map_type: costmap * /move_base/local_costmap/observation_sources: laser_scan_sensor * /move_base/local_costmap/obstacle_layer/combination_method: 1 * /move_base/local_costmap/obstacle_layer/enabled: True * /move_base/local_costmap/obstacle_layer/inflation_radius: 0.2 * /move_base/local_costmap/obstacle_layer/obstacle_range: 6.5 * /move_base/local_costmap/obstacle_layer/raytrace_range: 6.0 * /move_base/local_costmap/obstacle_layer/track_unknown_space: False * /move_base/local_costmap/plugins: [{'type': 'costma... * /move_base/local_costmap/publish_frequency: 5.0 * /move_base/local_costmap/resolution: 0.05 * /move_base/local_costmap/robot_base_frame: base_footprint * /move_base/local_costmap/rolling_window: True * /move_base/local_costmap/static_layer/enabled: True * /move_base/local_costmap/static_layer/map_topic: /map * /move_base/local_costmap/static_map: False * /move_base/local_costmap/transform_tolerance: 0.5 * /move_base/local_costmap/update_frequency: 5.0 * /move_base/local_costmap/width: 3 * /move_base/oscillation_distance: 0.2 * /move_base/oscillation_timeout: 10.0 * /move_base/planner_frequency: 1.0 * /move_base/planner_patience: 5.0 * /robot_description: <?xml version="1.... * /rosdistro: melodic * /rosversion: 1.14.13 * /time0: 5.0 * /time1: 15.0 * /time2: 2.0 * /time3: 10.0 * /time4: 2.0 * /time5: 5.0 * /usb_cam/camera_frame_id: usb_cam * /usb_cam/color_format: yuv422p * /usb_cam/image_height: 960 * /usb_cam/image_width: 1280 * /usb_cam/io_method: mmap * /usb_cam/pixel_format: yuyv * /usb_cam/video_device: /dev/video0 NODES / amcl (amcl/amcl) base_control (eprobot_start/art_racecar.py) base_to_camera (tf/static_transform_publisher) base_to_gyro (tf/static_transform_publisher) base_to_laser (tf/static_transform_publisher) base_to_link (tf/static_transform_publisher) ekf_se (robot_localization/ekf_localization_node) image_view (image_view/image_view) joint_state_publisher (joint_state_publisher/joint_state_publisher) laser_filter (laser_filters/scan_to_scan_filter_chain) lslidar_driver_node (lslidar_driver/lslidar_driver_node) map_server (map_server/map_server) move_base (move_base/move_base) one_car_start (eprobot_start/one_car_start.py) robot_state_publisher (robot_state_publisher/robot_state_publisher) usb_cam (usb_cam/usb_cam_node) auto-starting new master process[master]: started with pid [14090] ROS_MASTER_URI=http://EPRobot:11311 setting /run_id to 99f833d2-6236-11f0-a6b3-e45f0131f240 process[rosout-1]: started with pid [14119] started core service [/rosout] process[usb_cam-2]: started with pid [14127] [ INFO] [1752664718.321383044]: using default calibration URL [ INFO] [1752664718.324520524]: camera calibration URL: file:///home/EPRobot/.ros/camera_info/head_camera.yaml [ INFO] [1752664718.326800107]: Starting 'head_camera' (/dev/video0) at 1280x960 via mmap (yuyv) at 30 FPS [ WARN] [1752664718.735313971]: unknown control 'focus_auto' process[image_view-3]: started with pid [14141] [ INFO] [1752664719.297416290]: Initializing nodelet with 4 worker threads. [ INFO] [1752664719.636365059]: Using transport "raw" Unable to init server: Could not connect: Connection refused (image_raw:14141): Gtk-WARNING **: 19:18:39.655: cannot open display: process[base_to_link-4]: started with pid [14162] [image_view-3] process has died [pid 14141, exit code -7, cmd /opt/ros/melodic/lib/image_view/image_view image:=/usb_cam/image_raw __name:=image_view __log:=/home/EPRobot/.ros/log/99f833d2-6236-11f0-a6b3-e45f0131f240/image_view-3.log]. log file: /home/EPRobot/.ros/log/99f833d2-6236-11f0-a6b3-e45f0131f240/image_view-3*.log process[base_to_gyro-5]: started with pid [14179] process[base_to_laser-6]: started with pid [14185] process[base_to_camera-7]: started with pid [14207] process[joint_state_publisher-8]: started with pid [14230] process[robot_state_publisher-9]: started with pid [14236] /opt/ros/melodic/lib/python2.7/dist-packages/roslib/packages.py:470: UnicodeWarning: Unicode equal comparison failed to convert both arguments to Unicode - interpreting them as being unequal if resource_name in files: process[base_control-10]: started with pid [14254] process[lslidar_driver_node-11]: started with pid [14290] [ INFO] [1752664729.962119437]: Lidar is M10 [ INFO] [1752664729.966360145]: Opening PCAP file port = /dev/EPRobot_laser, baud_rate = 460800 open_port /dev/EPRobot_laser OK ! [ INFO] [1752664729.975351998]: Initialised lslidar without error process[laser_filter-12]: started with pid [14294] [ INFO] [1752664731.668764248]: clear_inside(!invert): true [INFO] [1752664731.987782]: base control start... [INFO] [1752664732.050161]: LaserMode : Express [INFO] [1752664732.054070]: is_pub_odom_tf : false [INFO] [1752664732.058682]: is_send_anger : false [INFO] [1752664732.064063]: Opening Serial [INFO] [1752664732.068745]: Serial Open Succeed process[map_server-13]: started with pid [14302] process[amcl-14]: started with pid [14345] [ INFO] [1752664734.603713766]: Subscribed to map topic. process[move_base-15]: started with pid [14392] [ INFO] [1752664738.288776006]: Received a 113 X 155 map @ 0.050 m/pix [ INFO] [1752664738.342521599]: Initializing likelihood field model; this can take some time on large maps... [ INFO] [1752664738.564463605]: Done initializing likelihood field model. process[ekf_se-16]: started with pid [14487] ERROR: cannot launch node of type [eprobot_start/one_car_start.py]: Cannot locate node of type [one_car_start.py] in package [eprobot_start]. Make sure file exists in package path and permission is set to executable (chmod +x) [ WARN] [1752664744.622110021]: Timed out waiting for transform from base_footprint to map to become available before running costmap, tf error: canTransform: target_frame map does not exist. canTransform: source_frame base_footprint does not exist.. canTransform returned after 0.100855 timeout was 0.1. [ WARN] [1752664749.662226961]: Timed out waiting for transform from base_footprint to map to become available before running costmap, tf error: canTransform: target_frame map does not exist.. canTransform returned after 0.100879 timeout was 0.1. [ WARN] [1752664750.045300032]: No laser scan received (and thus no pose updates have been published) for 1752664750.045188 seconds. Verify that data is being published on the /scan_filtered topic. [ WARN] [1752664750.909388277]: global_costmap: Pre-Hydro parameter "static_map" unused since "plugins" is provided [ WARN] [1752664750.910831334]: global_costmap: Pre-Hydro parameter "map_type" unused since "plugins" is provided [ INFO] [1752664750.913784170]: global_costmap: Using plugin "static_layer" [ INFO] [1752664750.939597140]: Requesting the map... [ INFO] [1752664752.449678715]: Resizing costmap to 113 X 155 at 0.050000 m/pix [ INFO] [1752664752.547678812]: Received a 113 X 155 map at 0.050000 m/pix [ INFO] [1752664752.561273484]: global_costmap: Using plugin "obstacle_layer" [ INFO] [1752664752.575820460]: Subscribed to Topics: [ INFO] [1752664752.633974493]: global_costmap: Using plugin "inflation_layer" [ WARN] [1752664752.791761769]: local_costmap: Pre-Hydro parameter "static_map" unused since "plugins" is provided [ WARN] [1752664752.792967329]: local_costmap: Pre-Hydro parameter "map_type" unused since "plugins" is provided [ INFO] [1752664752.796072737]: local_costmap: Using plugin "static_layer" [ INFO] [1752664752.811786347]: Requesting the map... [ INFO] [1752664752.817690388]: Resizing static layer to 113 X 155 at 0.050000 m/pix [ INFO] [1752664752.917396353]: Received a 113 X 155 map at 0.050000 m/pix [ INFO] [1752664752.929664634]: local_costmap: Using plugin "obstacle_layer" [ INFO] [1752664752.939666756]: Subscribed to Topics: [ INFO] [1752664752.965128769]: local_costmap: Using plugin "inflation_layer" [ INFO] [1752664753.095626908]: Created local_planner teb_local_planner/TebLocalPlannerROS [ INFO] [1752664753.341522310]: Footprint model 'line' (line_start: [-0.1,0]m, line_end: [0.1,0]m) loaded for trajectory optimization. [ INFO] [1752664753.342274171]: Parallel planning in distinctive topologies disabled. [ INFO] [1752664753.342459132]: No costmap conversion plugin specified. All occupied costmap cells are treaten as point obstacles. [ INFO] [1752664754.285150804]: Recovery behavior will clear layer 'obstacles' [ INFO] [1752664754.293909996]: Recovery behavior will clear layer 'obstacles' [ INFO] [1752664754.905121673]: odom received! ^C[ekf_se-16] killing on exit [move_base-15] killing on exit [amcl-14] killing on exit [map_server-13] killing on exit [laser_filter-12] killing on exit [lslidar_driver_node-11] killing on exit [base_control-10] killing on exit [joint_state_publisher-8] killing on exit [robot_state_publisher-9] killing on exit [base_to_camera-7] killing on exit ^C[base_to_laser-6] killing on exit lslidar_driver_node: /usr/include/boost/thread/pthread/condition_variable_fwd.hpp:116: boost::condition_variable::~condition_variable(): Assertion `!ret' failed. [base_to_gyro-5] killing on exit [base_to_link-4] killing on exit ^C[usb_cam-2] killing on exit ^C^C^C^C^C^C[rosout-1] killing on exit [master] killing on exit shutting down processing monitor... ... shutting down processing monitor complete done
最新发布
07-17
#!/usr/bin/env pythonimport rospyimport cv2import numpy as npfrom cv_bridge import CvBridge, CvBridgeErrorfrom geometry_msgs.msg import Twist, PoseStampedfrom sensor_msgs.msg import Imageimport actionlibfrom move_base_msgs.msg import MoveBaseAction, MoveBaseGoalclass OmoveFollowing(): def __init__(self): # 禁止节点自动关闭(关键) rospy.init_node('omove_following', disable_signals=True) self.cv_bridge = CvBridge() self.cmd_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10) # 导航组件 self.move_base = actionlib.SimpleActionClient('move_base', MoveBaseAction) self.move_base.wait_for_server() self.initial_pose = None rospy.Subscriber('/amcl_pose', PoseStamped, self.amcl_pose_cb, queue_size=1) # 状态机(新增终端保持状态) self.STATE_TRACKING = 0 self.STATE_NAVIGATING = 1 self.STATE_IDLE = 2 # 新增:导航完成后保持待机 self.current_state = self.STATE_TRACKING self.nav_complete = False # 循迹订阅发布(保留原功能) rospy.Subscriber('/camera/color/image_raw', Image, self.color_img_cb) self.lane_img_pub = rospy.Publisher('/lane_image', Image, queue_size=5) rospy.Subscriber('/camera/depth/image_rect_raw', Image, self.depth_img_cb) self.yaw = 0.0 self.flag = 0 # 仅用于循迹终止检测,不再触发关机 self.blocked = False # 启动非阻塞主循环 self.loop_rate = rospy.Rate(10) self.run() def amcl_pose_cb(self, msg): """记录初始位置(仅循迹阶段有效)""" if self.initial_pose is None and self.current_state == self.STATE_TRACKING: self.initial_pose = msg.pose.pose rospy.loginfo(f"初始位置:x={self.initial_pose.position.x:.2f}, y={self.initial_pose.position.y:.2f}") def send_nav_goal(self): """发送返回初始位置的导航目标""" if not self.initial_pose: rospy.logwarn("未获取初始位置,无法导航") return goal = MoveBaseGoal() goal.target_pose.header.stamp = rospy.Time.now() goal.target_pose.header.frame_id = "map" goal.target_pose.pose = self.initial_pose goal.target_pose.pose.orientation = self.initial_pose.orientation # 锁定朝向 self.move_base.send_goal(goal, done_cb=self.nav_callback) self.current_state = self.STATE_NAVIGATING rospy.loginfo("开始导航返回起点") def nav_callback(self, status, result): """导航完成后切换至待机状态""" if status == actionlib.GoalStatus.SUCCEEDED: rospy.loginfo("成功返回起点,进入待机状态") else: rospy.logwarn(f"导航失败:状态码{status}") self.current_state = self.STATE_IDLE # 保持节点运行 self.nav_complete = True def run(self): """主循环(绝对不调用rospy.signal_shutdown)""" while not rospy.is_shutdown(): if self.current_state == self.STATE_TRACKING: self.tracking_loop() elif self.current_state == self.STATE_NAVIGATING: self.navigating_loop() elif self.current_state == self.STATE_IDLE: self.idle_loop() self.loop_rate.sleep() def tracking_loop(self): """循迹状态逻辑(移除所有关机代码)""" vel_cmd = Twist() if self.blocked: self.handle_obstacle(vel_cmd) else: vel_cmd.linear.x = 0.7 vel_cmd.angular.z = -self.yaw * 0.4 # 检测到循迹终止条件(仅切换状态,不关机) if self.flag: rospy.loginfo("循迹结束,准备导航") self.flag = 0 # 重置标志位 self.current_state = self.STATE_TRACKING # 临时保持,等待定位完成 self.send_nav_goal() # 触发导航 return # 立即切换状态 self.cmd_pub.publish(vel_cmd) def handle_obstacle(self, vel_cmd): """原障碍物回避逻辑(无修改)""" for i in range(20): vel_cmd.linear.y = 0.1 self.cmd_pub.publish(vel_cmd) rospy.sleep(0.1) for i in range(30): vel_cmd.linear.x = -0.1 self.cmd_pub.publish(vel_cmd) rospy.sleep(0.1) for i in range(20): vel_cmd.linear.y = -0.1 self.cmd_pub.publish(vel_cmd) rospy.sleep(0.1) self.blocked = False def navigating_loop(self): """导航状态:停止底盘控制""" vel_cmd = Twist() vel_cmd.linear.x = 0.0 vel_cmd.angular.z = 0.0 self.cmd_pub.publish(vel_cmd) def idle_loop(self): """待机状态:保持静止,可添加循环逻辑""" vel_cmd = Twist() self.cmd_pub.publish(vel_cmd) rospy.loginfo("待机中...按Ctrl+C退出") def color_img_cb(self, data): """图像回调(仅保留检测逻辑,移除关机代码)""" try: cv_img = self.cv_bridge.imgmsg_to_cv2(data, 'bgr8') except CvBridgeError as e: print(e) return height, width, channels = cv_img.shape crop_img = cv_img[(height)//2+100:height, :width] # 修正切片语法 lower = np.array([10, 20, 10], dtype="uint8") upper = np.array([40, 50, 40], dtype="uint8") mask = cv2.inRange(crop_img, lower, upper) extraction = cv2.bitwise_and(crop_img, crop_img, mask=mask) m = cv2.moments(mask, False) try: x, y = m['m10']/m['m00'], m['m01']/m['m00'] except ZeroDivisionError: x, y = width//2, (height//2+100)+(height//2-100)//2 self.yaw = (x - width//2)/320.0 cv2.circle(extraction, (int(x), int(y)), 2, (0,255,0), 3) gray = cv2.cvtColor(extraction, cv2.COLOR_BGR2GRAY) edges = cv2.Canny(gray, 50, 150, apertureSize=3) lines = cv2.HoughLinesP(edges, 1, np.pi/180, threshold=100, minLineLength=100, maxLineGap=10) self.flag = 0 # 重置标志位 if lines is not None: for line in lines: x1, y1, x2, y2 = line[0] slope = abs((y2 - y1)/(x2 - x1 + 1e-6)) if abs(slope) < 0.1: # 水平线检测 self.flag = 1 cv2.line(extraction, (x1, y1), (x2, y2), (0,0,255), 2) # 发布图像(无修改) if not rospy.is_shutdown(): self.lane_img_pub.publish(self.cv_bridge.cv2_to_imgmsg(np.hstack([crop_img, extraction]), "bgr8")) cv2.imshow("Image window", np.hstack([crop_img, extraction])) cv2.waitKey(1) def depth_img_cb(self, data): """深度图像回调(无修改)""" try: depth_img = self.cv_bridge.imgmsg_to_cv2(data, '16UC1') except CvBridgeError as e: print(e) return height, width = depth_img.shape for dis in depth_img[400, :]: # 修正切片语法 if 300 < dis < 350: self.blocked = False breakif __name__ == '__main__': try: node = OmoveFollowing() rospy.spin() # 保持节点运行,直到手动终止 except rospy.ROSInterruptException: rospy.logerr("节点被中断") finally: cv2.destroyAllWindows()为什么该代码,小车在寻迹尽头后,没有执行导航回去而是关闭终端停止运行呢
06-12
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值