Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

-for autonsystem and serial node #78

Merged
merged 1 commit into from
Apr 4, 2024
Merged

-for autonsystem and serial node #78

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
22 changes: 9 additions & 13 deletions rb_ws/src/buggy/scripts/auton/autonsystem.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -135,22 +135,21 @@ def init_check(self):
# (from both buggies if relevant)
# covariance is less than 1 meter
if (self.self_odom_msg == None):
print("WARNING: no available position estimate")
rospy.logwarn("WARNING: no available position estimate")
return False

if (self.self_odom_msg.pose.covariance[0] ** 2 + self.self_odom_msg.pose.covariance[7] ** 2 > 1**2):
print("checking position estimate certainty")
rospy.logwarn("checking position estimate certainty")
return False

# waits until covariance is acceptable to check heading
with self.lock:
self_pose, _ = self.get_world_pose_and_speed(self.self_odom_msg)
current_heading = self_pose.theta
closest_heading = self.cur_traj.get_heading_by_index(trajectory.get_closest_index_on_path(self_pose.x, self_pose.y))
print("current heading: ", np.rad2deg(current_heading))
rospy.loginfo("current heading: ", np.rad2deg(current_heading))
self.heading_publisher.publish(Float32(np.rad2deg(current_heading)))


# headings are originally between -pi and pi
# if they are negative, convert them to be between 0 and pi
if current_heading < 0:
Expand All @@ -160,25 +159,22 @@ def init_check(self):
closest_heading = 2*np.pi + closest_heading

if (abs(current_heading - closest_heading) >= np.pi/2):
print("WARNING: INCORRECT HEADING! restart stack. Current heading [-180, 180]: ", np.rad2deg(self_pose.theta))
rospy.logwarn("WARNING: INCORRECT HEADING! restart stack. Current heading [-180, 180]: ", np.rad2deg(self_pose.theta))
return False

return True

def tick_caller(self):


print("start checking initialization status")
rospy.loginfo("start checking initialization status")
while ((not rospy.is_shutdown()) and not self.init_check()):
self.init_check_publisher.publish(False)
rospy.sleep(0.001)
print("done checking initialization status")
rospy.loginfo("done checking initialization status")
self.init_check_publisher.publish(True)
# initialize global trajectory index

with self.lock:
_, _ = self.get_world_pose_and_speed(self.self_odom_msg)

t_planner = threading.Thread(target=self.planner_thread)
t_controller = threading.Thread(target=self.local_controller_thread)

Expand Down Expand Up @@ -314,9 +310,9 @@ def planner_tick(self):
profile = args.profile
left_curb_file = args.left_curb

print("\n\nStarting Controller: " + str(ctrl) + "\n\n")
print("\n\nUsing path: /rb_ws/src/buggy/paths/" + str(traj) + "\n\n")
print("\n\nStarting at distance: " + str(start_dist) + "\n\n")
rospy.loginfo("\n\nStarting Controller: " + str(ctrl) + "\n\n")
rospy.loginfo("\n\nUsing path: /rb_ws/src/buggy/paths/" + str(traj) + "\n\n")
rospy.loginfo("\n\nStarting at distance: " + str(start_dist) + "\n\n")

trajectory = Trajectory(json_filepath="/rb_ws/src/buggy/paths/" + traj)
left_curb = None
Expand Down
6 changes: 3 additions & 3 deletions rb_ws/src/buggy/scripts/serial/ros_to_bnyahaj.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,7 @@ def set_steering(self, msg):
self.fresh_steer = True

def writer_thread(self):
print('Starting sending alarm and steering to teensy!')
rospy.loginfo('Starting sending alarm and steering to teensy!')
while True:
if self.fresh_steer:
with self.lock:
Expand All @@ -70,12 +70,12 @@ def writer_thread(self):
self.steer_send_rate.sleep()

def reader_thread(self):
print('Starting reading odom from teensy!')
rospy.loginfo('Starting reading odom from teensy!')
while True:
packet = self.comms.read_packet()
# print("trying to read odom")
if isinstance(packet, Odometry):
# print("packet", packet.radio_seqnum, packet.gps_seqnum)
rospy.logdebug("packet", packet)
#Publish to odom topic x and y coord
odom = ROSOdom()
# convert to long lat
Expand Down
Loading