diff --git a/autonav_ws/src/autonav_nav/src/astar.py b/autonav_ws/src/autonav_nav/src/astar.py index 6008ec8..33bd596 100644 --- a/autonav_ws/src/autonav_nav/src/astar.py +++ b/autonav_ws/src/autonav_nav/src/astar.py @@ -23,9 +23,8 @@ CV_BRIDGE = cv_bridge.CvBridge() competition_waypoints = [ - [(42.66827, -83.21934), (42.668121, -83.219361), (42.668077, -83.219359), (42.667928, -83.219328)], # NORTH - [(42.667928, -83.219328), (42.668077, -83.219359), (42.668121, -83.219361), (42.66827, -83.21934)], # SOUTH - [(35.210475, -97.441927), (35.210476, -97.442321), (35.210599, -97.442323), (35.210599, -97.442096)], # NORMAN TEST WAYPOINTS + [(42.6682623, -83.2193709), (42.6681206, -83.2193606), (42.6680766, -83.2193592), (42.6679277, -83.2193276), (42.668130236144883, -83.21889785301433)], # NORTH + [(42.668086, -83.218446)], # [] ] @@ -117,21 +116,21 @@ def onReset(self): self.waypoint_start_time = 0 def get_waypoints_for_dir(self): - if not self.config.calculateWaypointDirection: - return simulation_waypoints[self.config.waypointDirection] if self.system_mode == SystemModeEnum.SIMULATION else competition_waypoints[self.config.waypointDirection] if self.system_mode == SystemModeEnum.COMPETITION else practice_waypoints[self.config.waypointDirection] + # if not self.config.calculateWaypointDirection: + return simulation_waypoints[self.config.waypointDirection] if self.system_mode == SystemModeEnum.SIMULATION else competition_waypoints[self.config.waypointDirection] if self.system_mode == SystemModeEnum.COMPETITION else practice_waypoints[self.config.waypointDirection] # Get out current heading and estimate within 180 degrees which direction we are facing (north or south, 0 and 1 respectively) - heading = self.position.theta - direction_index = 0 - heading_degrees = abs(heading * 180 / math.pi) - self.get_logger().info(f"Heading: {heading_degrees}") - if heading_degrees > 120 and heading_degrees < 240: - direction_index = 1 - self.get_logger().info("Facing South") - else: - self.get_logger().info("Facing North") - - return simulation_waypoints[direction_index] if self.system_mode == SystemModeEnum.SIMULATION else competition_waypoints[direction_index] if self.system_mode == SystemModeEnum.COMPETITION else practice_waypoints[direction_index] + # heading = self.position.theta + # direction_index = 0 + # heading_degrees = abs(heading * 180 / math.pi) + # self.get_logger().info(f"Heading: {heading_degrees}") + # if heading_degrees > 120 and heading_degrees < 240: + # direction_index = 1 + # self.get_logger().info("Facing South") + # else: + # self.get_logger().info("Facing North") + # + # return simulation_waypoints[direction_index] if self.system_mode == SystemModeEnum.SIMULATION else competition_waypoints[direction_index] if self.system_mode == SystemModeEnum.COMPETITION else practice_waypoints[direction_index] def system_state_transition(self, old: SystemState, updated: SystemState): if updated.state == SystemStateEnum.AUTONOMOUS and updated.mobility and len(self.waypoints) == 0: diff --git a/autonav_ws/src/scr_controller/package.xml b/autonav_ws/src/scr_controller/package.xml index e1ed5e7..9373dd4 100644 --- a/autonav_ws/src/scr_controller/package.xml +++ b/autonav_ws/src/scr_controller/package.xml @@ -3,7 +3,7 @@ scr_controller 1.0.0 - Manages state between all nodes + Manages state between all nodes (maybe) Dylan Zemlin MIT diff --git a/autonav_ws/src/scr_controller/src/core.cpp b/autonav_ws/src/scr_controller/src/core.cpp index c6b05af..6af6370 100644 --- a/autonav_ws/src/scr_controller/src/core.cpp +++ b/autonav_ws/src/scr_controller/src/core.cpp @@ -63,6 +63,8 @@ class CoreNode : public rclcpp::Node // Set mode timer mode_timer = this->create_wall_timer(std::chrono::seconds(5), std::bind(&CoreNode::mode_timer_callback, this)); + + // god help me } private: