publish the global plan
(self)
| 222 | return camera_info |
| 223 | |
| 224 | def publish_plan(self): |
| 225 | """ |
| 226 | publish the global plan |
| 227 | """ |
| 228 | msg = Path() |
| 229 | msg.header.frame_id = "/map" |
| 230 | msg.header.stamp = rospy.Time.now() |
| 231 | for wp in self._global_plan_world_coord: |
| 232 | pose = PoseStamped() |
| 233 | pose.pose.position.x = wp[0].location.x |
| 234 | pose.pose.position.y = -wp[0].location.y |
| 235 | pose.pose.position.z = wp[0].location.z |
| 236 | quaternion = tf.transformations.quaternion_from_euler( |
| 237 | 0, 0, -math.radians(wp[0].rotation.yaw)) |
| 238 | pose.pose.orientation.x = quaternion[0] |
| 239 | pose.pose.orientation.y = quaternion[1] |
| 240 | pose.pose.orientation.z = quaternion[2] |
| 241 | pose.pose.orientation.w = quaternion[3] |
| 242 | msg.poses.append(pose) |
| 243 | |
| 244 | rospy.loginfo("Publishing Plan...") |
| 245 | self.waypoint_publisher.publish(msg) |
| 246 | |
| 247 | def sensors(self): |
| 248 | """ |