求助大佬们:
主题发布如下
self.path_pub = self.create_publisher(Path,"/path_my_A_star", 10 )
def publisher_path(self):
print('Start Publishing the msg!!!')
time = 1
y1 = []
y2 = []
for i in range(self.path_map.shape[1]):
current_time = self.get_clock().now().to_msg()
current_pose = PoseStamped()
current_pose.pose.position.x, current_pose.pose.position.y= self.mapToWorld(self.path_map[0][i], self.path_map[0][i])
y1.append(self.mapToWorld(self.path_map[0][i], self.path_map[0][i])[0])
y2.append(self.mapToWorld(self.path_map[0][i], self.path_map[0][i])[1])
current_pose.pose.position.z = 0.0
current_pose.pose.orientation.x = 0.0
current_pose.pose.orientation.y = 0.0
current_pose.pose.orientation.z = 0.0
current_pose.pose.orientation.w = 1.0
time += 1
self.current_path.header.stamp = current_time
self.current_path.header.frame_id = "odom"
self.current_path.poses.append(current_pose)
self.path_pub.publish(self.current_path)
self.last_time = current_time
print('Finish Publishing the msg!')
rviz并不显示path
由于rviz无法显示path轨迹,利用topic echo检看msg但是结果如下
peter@ubuntu:~$ ros2 topic echo /path_my_A_starh
WARNING: topic [/path_my_A_starh] does not appear to be published yet
Could not determine the type for the passed topic