Skip to content
This repository has been archived by the owner on Apr 12, 2024. It is now read-only.

Upgrade code to ROS Noetic #26

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
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
2 changes: 1 addition & 1 deletion src/CMakeLists.txt
1 change: 0 additions & 1 deletion src/kart_2dnav/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,6 @@ find_package(catkin REQUIRED COMPONENTS
global_planner
move_base
teb_local_planner
teb_local_planner_tutorials
#sbg_driver
#sbg_to_imu
#imu_filter_madgwick
Expand Down
1 change: 0 additions & 1 deletion src/kart_2dnav/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -63,7 +63,6 @@
<depend>global_planner</depend>
<depend>move_base</depend>
<depend>teb_local_planner</depend>
<depend>teb_local_planner_tutorials</depend>

<!-- The export tag contains other, unspecified, tags -->
<export>
Expand Down
8 changes: 4 additions & 4 deletions src/slam_mode_goal/src/slam_mode_goal/slam_mode_goal.py
Original file line number Diff line number Diff line change
Expand Up @@ -186,7 +186,7 @@ def weighted_resultant_vector(laserscan):
# Iterate through left half of the lazerscans and grab corresponding
# right lazerscan for each left one.
theta_tot = 0
for i in range(len(ranges) / 2):
for i in range(len(ranges) // 2):
l_dist = ranges[i]
l_ang = theta_arr[i]
r_dist = ranges[-1 - i]
Expand All @@ -200,7 +200,7 @@ def weighted_resultant_vector(laserscan):
# Add the theta of res vector
theta_tot += math.atan(y / x)

theta = theta_tot / (len(ranges) / 2)
theta = theta_tot / (len(ranges) // 2)

# If the vector (x_pos, y_pos) is too close to a wall, then
# decrease magnitude of vector and give more weight to the y position.
Expand All @@ -209,13 +209,13 @@ def weighted_resultant_vector(laserscan):

## Experimentally determining this based on simulation. Will want to get do this more thoroughly later
thresh_dist = 2.1 * KART_LENGTH
front_dist_from_wall = ranges[len(ranges) / 2] - KART_LENGTH
front_dist_from_wall = ranges[len(ranges) // 2] - KART_LENGTH

if front_dist_from_wall < thresh_dist:
x_pos *= 0.35
y_pos *= 0.85

print "theta: {}, x: {}, y: {}".format(theta, x_pos, y_pos)
print("theta: {}, x: {}, y: {}".format(theta, x_pos, y_pos))

# Orientation of kart at goal should be the resultant vect angle (may not be the best)
orient = math.atan(y_pos / x_pos)
Expand Down