

 @brief Runs the Ackerman object. */ void run(void);

/**
 @brief Sets the goal for the Ackerman object.
 @param goal_ The goal position. */ void reachGoal(geometry_msgs::Point&
goal_);
/**
 @brief Checks if the goal has been reached.
 @param goal_ The goal position.
 @return True if the goal has been reached, False otherwise. */ bool
goalReached(geometry_msgs::Point& goal_);
/**
 @brief Stops the mission.
 @return True if the mission is stopped successfully, False otherwise. */ bool
stopMission();
/**
 @brief Gets the goal at the specified index.
 @param i The index of the goal.
 @return The goal position. */ geometry_msgs::Point getGoal(int i);
/**
 @brief Checks the origin to destination for navigation.
 @param origin The origin position.
 @param goal The destination position.
 @param distance The distance between the origin and destination.
 @param time The time to reach the destination.
 @param estimatedGoalPose The estimated goal pose.
 @return True if the origin to destination check is successful, False otherwise. */
bool checkOriginToDestination(nav_msgs::Odometry origin,
geometry_msgs::Point goal, double& distance, double& time,
nav_msgs::Odometry& estimatedGoalPose);
/**
 @brief Calculates the distance to the goal.
 @param goal_ The goal position.
 @return The distance to the goal. */ double
distanceToGoal(geometry_msgs::Point goal_);
/**
 @brief Calculates the time to reach the goal.
 @return The time to reach the goal. */ double timeToGoal(void);
/**
 @brief Gets the current odometry.
 @return The current odometry. */ nav_msgs::Odometry getOdometry(void);
/**
 @brief Checks if the goal is within the road center.
 @param goal_ The goal position.
 @param roadCentre The road center position.
 @return True if the goal is within the road center, False otherwise. */ bool
goalCheck(const geometry_msgs::Point& goal_, const geometry_msgs::Point&
roadCentre);
/**
 @brief Publishes cones and road center markers.
@param conesPub The pose array of cones.
@param coneMarkers The marker array of cones.
@param roadCentreMarker The marker of the road center.
@return True if the publishing is successful, False otherwise. */ bool
publishCones(geometry_msgs::PoseArray conesPub,
visualization_msgs::MarkerArray coneMarkers, visualization_msgs::Marker
roadCentreMarker);
geometry_msgs::Point goal_; /*!< The current goal position. */ };
Copy code

This is initialized in a file called laserproccessing.cpp as geometry_msgs::Point

LaserProcessing::detectClosestCone(){ //This function searches the cones_ vector, populated from
countSegments(), for the closest cone. geometry_msgs::Point point; unsigned int count =
countSegments(); if (count == 0) { return {}; } double dclosest = laserScan_.range_max; unsigned
int idx = 0, i = 0; for (auto cone : cones_) { double d = pow(pow(cone.first, 2) + pow(cone.second,
2), 0.5); if (d < dclosest) { idx = i; dclosest = d; } //the cones_ vector is iterated through and
repeatedly compared to the previous closest cone until the minimum value is found. i++; }
point.x = cones_.at(idx).first; point.y = cones_.at(idx).second; return point; }