Skip to content

Commit

Permalink
eyantra hardware code upload
Browse files Browse the repository at this point in the history
  • Loading branch information
@krrish-jindal
krrish-jindal committed May 13, 2024
1 parent f97479c commit f446ff0
Show file tree
Hide file tree
Showing 15 changed files with 284 additions and 667 deletions.
4 changes: 2 additions & 2 deletions ebot_description/models/ebot/ebot.gazebo
View file
Original file line number Diff line number Diff line change
Expand Up @@ -194,7 +194,7 @@
<sensor name="laser" type="ray">
<pose> 0 0 0 0 0 0 </pose>
<visualize>false</visualize>
<update_rate>30</update_rate>
<update_rate>7</update_rate>
<ray>
<scan>
<horizontal>
Expand Down Expand Up @@ -329,7 +329,7 @@
<gravity>true</gravity>
<sensor name="imu_sensor" type="imu">
<always_on>true</always_on>
<update_rate>100</update_rate>
<update_rate>10</update_rate>
<visualize>true</visualize>
<topic>__default_topic__</topic>
<plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
Expand Down
43 changes: 23 additions & 20 deletions ebot_docking/scripts/ebot_docking_boilerplate.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@ def __init__(self):


# Subscribe to odometry data for robot pose information
self.odom_sub = self.create_subscription(Odometry, 'odom', self.odometry_callback, 10)
self.odom_sub = self.create_subscription(Odometry, '/odometry/filtered', self.odometry_callback, 10)

# Subscribe to ultrasonic sensor data for distance measurements
self.ultrasonic_rl_sub = self.create_subscription(Range, '/ultrasonic_rl/scan', self.ultrasonic_rl_callback, 10)
Expand Down Expand Up @@ -109,7 +109,7 @@ def normalize_angle(self, angle):
if angle < 0:
angle = math.pi + (math.pi + angle)
return angle


# Main control loop for managing docking behavior

Expand Down Expand Up @@ -181,7 +181,10 @@ def controller_loop(self):

elif self.flag == 1:
if abs(self.difference) > 0.1:
vel.angular.z = self.difference *0.6
if self.difference > 0:
vel.angular.z = self.difference *0.6
else:
vel.angular.z = self.difference *0.6
self.vel_pub.publish(vel)


Expand Down Expand Up @@ -212,39 +215,39 @@ def controller_loop(self):



# CORRECTING ULTRA SONIC DISTANCE ERROR
# # CORRECTING ULTRA SONIC DISTANCE ERROR

if self.usrleft_value > 0.15 and round(self.usrright_value,1) != round(self.usrleft_value,1):
# if self.usrleft_value > 0.15 and round(self.usrright_value,1) != round(self.usrleft_value,1):



# CORRECTING ULRA SONIC ERROR
# # CORRECTING ULRA SONIC ERROR

if abs(self.difference)<=0.02:
print(">>>>>=====>>>>>>")
vel.angular.z = self.usrdiff*0.2
# if abs(self.difference)<=0.02:
# print(">>>>>=====>>>>>>")
# vel.angular.z = self.usrdiff*0.2

self.vel_pub.publish(vel)
# self.vel_pub.publish(vel)

# CORRECTING RACK & BOT YAW ERROR
# # CORRECTING RACK & BOT YAW ERROR

else:
print(">>>>>>>>>>>>>>>")
vel.angular.z = self.difference*0.2
vel.linear.x = -0.2
# else:
# print(">>>>>>>>>>>>>>>")
# vel.angular.z = self.difference*0.2
# vel.linear.x = -0.2

self.vel_pub.publish(vel)
# self.vel_pub.publish(vel)


# vel.linear.x = -self.usrleft_value * 0.4
self.orientation_dock = False
self.linear_dock = False
# # vel.linear.x = -self.usrleft_value * 0.4
# self.orientation_dock = False
# self.linear_dock = False



# NO ULTRA SONIC DISTANCE ERROR DIRECT DOCKING

elif self.usrleft_value > 0.15 and round(self.usrright_value,1) == round(self.usrleft_value,1):
if self.usrleft_value > 0.15:
print("===============")
self.orientation_dock = False
vel.linear.x = -self.usrleft_value * 0.4
Expand Down
30 changes: 19 additions & 11 deletions ebot_docking/scripts/ebot_docking_boilerplate_hardware.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,7 @@ def __init__(self):


# Subscribe to odometry data for robot pose information
self.odom_sub = self.create_subscription(Odometry, 'odom', self.odometry_callback, 10)
self.odom_sub = self.create_subscription(Odometry, '/odom', self.odometry_callback, 10)


self.ultra_sub = self.create_subscription(Float32MultiArray, 'ultrasonic_sensor_std_float', self.ultra_callback, 10)
Expand All @@ -62,7 +62,7 @@ def __init__(self):
self.difference = 0
package_name = 'ebot_real_nav2'
config = "config/config.yaml"
self.flag =0
self.flag =1


ebot_nav2_dir = get_package_share_directory('ebot_real_nav2')
Expand Down Expand Up @@ -142,7 +142,7 @@ def controller_loop(self):
#
self.difference = self.normalize_yaw_rack - self.normalize_yaw_bot

if self.orientation_dock ==True:
if self.orientation_dock == True:
print("YAW------------YAW",(self.rack_yaw[int(self.rack_no)-1]))
print("LIST---------",self.rack_yaw,self.rack_no)

Expand All @@ -152,10 +152,11 @@ def controller_loop(self):
robot_head=str(self.robot_pose[2]/abs(self.robot_pose[2]))
print("ERRORS------",error,error2)

# # [0.9, 2.04, 0.0]



## X DIRECTION POSS CORRECTION
# ## X DIRECTION POSS CORRECTION

if 0.3 > abs(self.x_pose[int(self.rack_no) - 1][0] - self.robot_pose[0]) > 0.025 and robot_head== "1.0":

Expand All @@ -172,7 +173,7 @@ def controller_loop(self):



# Y DIRECTION POSS CORRECTION
# # # Y DIRECTION POSS CORRECTION

elif 0.3 > abs(self.x_pose[int(self.rack_no) - 1][1] - self.robot_pose[1]) > 0.025 and robot_head== "1.0":
print(self.robot_pose[0],self.robot_pose[1],"---------33333333333",abs(self.x_pose[int(self.rack_no) - 1][1] - self.robot_pose[1]))
Expand All @@ -186,7 +187,7 @@ def controller_loop(self):
self.vel_pub.publish(vel)


# DESIRE POSS REACH
# # # DESIRE POSS REACH

else:

Expand All @@ -200,10 +201,15 @@ def controller_loop(self):

# START ORIENTATION CORRECTION

elif self.flag == 1:
if abs(self.difference) > 0.1:
vel.angular.z = self.difference *0.9
if self.flag == 1:
print(abs(self.difference),"diff----")
if abs(self.difference)-0.01 > 0.07:
if self.difference > 0:
vel.angular.z = -self.difference *0.6 -0.2
else:
vel.angular.z = abs(self.difference) *0.6 +0.2
self.vel_pub.publish(vel)




Expand Down Expand Up @@ -265,11 +271,11 @@ def controller_loop(self):

# NO ULTRA SONIC DISTANCE ERROR DIRECT DOCKING

if self.usrleft_value >= 0.13:
if self.usrleft_value >= 0.23:
print("===============")
print(self.usrleft_value)
self.orientation_dock = False
vel.linear.x = -self.usrleft_value * 0.48
vel.linear.x = -self.usrleft_value * 0.3
vel.angular.z = 0.0
self.vel_pub.publish(vel)
self.linear_dock = False
Expand All @@ -286,6 +292,7 @@ def controller_loop(self):
self.linear_dock = True
self.is_docking = False
self.dock_aligned = True
self.orientation_dock = True


# Callback function for the DockControl service
Expand Down Expand Up @@ -321,6 +328,7 @@ def dock_control_callback(self, request, response):
self.get_logger().info("Waiting for alignment...")
self.controller_loop()
self.rate.sleep()


# Set the service response indicating success
response.success = True
Expand Down
8 changes: 4 additions & 4 deletions ebot_nav2/config/config.yaml
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
position:
- rack1: [6.5, 0.75, 1.57]
- rack2: [1.15, -2.2, 0.0]
- rack3: [0.5, 2.05, 0.0]
- rack1: [5.75, 0.6, 1.57]
- rack2: [1.1, -2.3, 0.0]
- rack3: [0.2, 1.8, 0.0]
- arm: [6.3, -0.05, 3.14]

package_id: [3, 1, 2]
package_id: [3, 2, 1]
10 changes: 5 additions & 5 deletions ebot_nav2/config/config_sim.yaml
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
position:
- rack1: [1.260054, 4.349990, 3.140017]
- rack2: [2.030010, 3.300094, -1.569944]
- rack3: [1.610650, -1.750544, 1.569966]
- arm: [1.600000, -2.400000, 3.140001]
- rack1: [5.75, 0.6, 1.57]
- rack2: [1.1, -2.3, 0.0]
- rack3: [0.2, 1.8, 0.0]
- arm: [6.3, -0.05, 0.0]

package_id: [3, 1, 2]
package_id: [3, 2, 1]
Loading

0 comments on commit f446ff0

Please sign in to comment.