sim & hardware fix bug

ebot_docking/scripts/ebot_docking_boilerplate.py

@@ -267,7 +267,10 @@ def controller_loop(self):
 
 	# Callback function for the DockControl service
 	def dock_control_callback(self, request, response):
-		# Extract desired docking parametg
+		# Extract desired docking parameters from the service request
+		self.linear_dock = request.linear_dock
+		self.orientation_dock = request.orientation_dock
+		self.distance = request.distance
 		self.orientation = request.orientation
 		self.rack_no = request.rack_no
 		# print(self.orientation)

ebot_docking/scripts/ebot_docking_boilerplate_hardware.py

@@ -152,7 +152,6 @@ 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]
 
 
 

ebot_nav2/CMakeLists.txt

@@ -43,6 +43,7 @@ install(
     maps
     params
     rviz
+    scripts
   DESTINATION
     share/${PROJECT_NAME}/
 )

ebot_nav2/config/config.yaml

@@ -1,6 +1,7 @@
 position:
-    - rack1: [1.26, 4.34, 3.14]
-    - rack2: [2.03, 3.30, -1.57]
-    - rack3: [1.61, -1.75, 1.57]
+    - rack1: [6.5, 0.75, 1.57]
+    - rack2: [1.15, -2.2, 0.0]
+    - rack3: [0.5, 2.05, 0.0]
+    - arm: [6.3, -0.05, 3.14]
 
-package_id: [3, 1, 2]
+package_id: [3, 1, 2]

ebot_nav2/scripts/ebot_nav_cmd.py

@@ -50,7 +50,7 @@ def send_request(self, orientation, rack_no):
 	def arm_request(self, rack_no):
 		request_arm = ArmNew.Request()
 		request_arm.boom = True
-		request_arm.whack = str(rack_no)
+		request_arm.rack_no = str(rack_no)
 		gojo = self.arm_check.call_async(request_arm)
 		rclpy.spin_until_future_complete(self, gojo)
 		return gojo.result()
@@ -93,28 +93,24 @@ def normalize_angle(self, angle):
 			angle = math.pi + (math.pi + angle)
 		return angle
 
-	def nav_coordinate(self,angle,x,y):
-		d=0.75
+	def nav_coordinate(self,angle,x,y,poss):
+
+		if poss=="final":
+			d=0.75
+		else:
+			d=0.95
 
 		self.a=x+(d*math.cos(angle))
 		self.b=y+(d*math.sin(angle))
 		return (self.a,self.b)
 
-	def nav_theta(self,angle):
-		correct_angle=angle-1.57
-
-		# if (correct_angle > 6.28):
-		# 	correct_angle = correct_angle - 6.28
-
+
+	def nav_theta(self,angle,obj_type):
 
-		# if (correct_angle > 3.14):
-
-		# 	flag_angle = correct_angle - 3.14
-		# 	correct_yaw = -3.14 + flag_angle
-
-		# else:
-		# 	correct_yaw = correct_angle
-
+		if obj_type =="rack":
+			correct_angle=angle-1.57
+		else:
+			correct_angle=angle
 		x,y,z,w=quaternion_from_euler(0,0,correct_angle)
 		return (x,y,z,w)
 
@@ -171,7 +167,9 @@ def navigate_and_dock(self, goal_pick, goal_drop, goal_int, orientation_rack, ra
 		self.navigator.goToPose(goal_drop)
 		self.nav_reach(goal_drop)
 		self.rack_detach(rack)
-		self.arm_request(rack_no = "3")
+		self.arm_request(rack_no = str(rack_no))
+
+
 	def main(self):
 		package_name = 'ebot_nav2'
 		config = "config/config_sim.yaml"
@@ -185,33 +183,68 @@ def main(self):
 
 		data_dict = yaml.safe_load(pos_rack)
 
+
 		positions = data_dict['position']
 		rack1_coordinates = positions[0]['rack1']
 		rack2_coordinates = positions[1]['rack2']
 		rack3_coordinates = positions[2]['rack3']
+		arm_coordinates=positions[3]['arm']
 		package_id = data_dict['package_id'][0]
 
 		orientation_rack_1 = rack1_coordinates[2]
 		orientation_rack_2 = rack2_coordinates[2]
 		orientation_rack_3 = rack3_coordinates[2]
+		orientation_arm = arm_coordinates[2]
 
 		rack_list = ["rack1", "rack2", "rack3"]
 		print("-------------------")
 		print("RACK_1--",orientation_rack_1)
 		print("RACK_2--",orientation_rack_2)
 		print("RACK_3--",orientation_rack_3)
 		print("-------------------")
+
+
 		theta_1=self.normalize_angle(orientation_rack_1)
-		bot_pose_1=self.nav_coordinate(theta_1,rack1_coordinates[0],rack1_coordinates[1])
-		goal_theta_1= self.nav_theta(orientation_rack_1)
+		bot_pose_1=self.nav_coordinate(theta_1,rack1_coordinates[0],rack1_coordinates[1],"final")
+		goal_theta_1= self.nav_theta(orientation_rack_1,"rack")
 
 		theta_2=self.normalize_angle(orientation_rack_2)
-		bot_pose_2=self.nav_coordinate(theta_2,rack2_coordinates[0],rack2_coordinates[1])
-		goal_theta_2= self.nav_theta(orientation_rack_2)
+		bot_pose_2=self.nav_coordinate(theta_2,rack2_coordinates[0],rack2_coordinates[1],"final")
+		goal_theta_2= self.nav_theta(orientation_rack_2,"rack")
 
 		theta_3=self.normalize_angle(orientation_rack_3)
-		bot_pose_3=self.nav_coordinate(theta_3,rack3_coordinates[0],rack3_coordinates[1])
-		goal_theta_3= self.nav_theta(orientation_rack_3)
+		bot_pose_3=self.nav_coordinate(theta_3,rack3_coordinates[0],rack3_coordinates[1],"final")
+		goal_theta_3= self.nav_theta(orientation_rack_3,"rack")
+
+
+
+		#    DROP LOCATION
+
+		theta_4=self.normalize_angle(orientation_arm)
+		arm_pose_1=self.nav_coordinate(orientation_arm+90.0,arm_coordinates[0],arm_coordinates[1],"final")
+		goal_theta_4= self.nav_theta(orientation_arm+90.0,"arm")
+
+		theta_5=self.normalize_angle(orientation_arm)
+		arm_pose_2=self.nav_coordinate(orientation_arm,arm_coordinates[0],arm_coordinates[1],"final")
+		goal_theta_5= self.nav_theta(orientation_arm,"arm")
+
+		theta_6=self.normalize_angle(orientation_arm)
+		arm_pose_3=self.nav_coordinate(orientation_arm-90.0,arm_coordinates[0],arm_coordinates[1],"final")
+		goal_theta_6= self.nav_theta(orientation_arm-90.0,"arm")
+
+
+
+		#   INIT ARM poss DROP LOCATION
+
+		init_arm_pose_1=self.nav_coordinate(orientation_arm+90.0,arm_coordinates[0],arm_coordinates[1],"initial")
+		init_goal_theta_4= self.nav_theta(orientation_arm+90.0,"arm")
+
+		init_arm_pose_2=self.nav_coordinate(orientation_arm,arm_coordinates[0],arm_coordinates[1],"initial")
+		init_goal_theta_5= self.nav_theta(orientation_arm,"arm")
+
+		init_arm_pose_3=self.nav_coordinate(orientation_arm-90.0,arm_coordinates[0],arm_coordinates[1],"initial")
+		init_goal_theta_6= self.nav_theta(orientation_arm-90.0,"arm")
+
 
 		goal_pick_1 = PoseStamped()
 		goal_pick_1.header.frame_id = 'map'
@@ -222,38 +255,7 @@ def main(self):
 		goal_pick_1.pose.orientation.y = goal_theta_1[1]
 		goal_pick_1.pose.orientation.z = goal_theta_1[2]
 		goal_pick_1.pose.orientation.w = goal_theta_1[3]
-
-		goal_drop_int_1 = PoseStamped()
-		goal_drop_int_1.header.frame_id = 'map'
-		goal_drop_int_1.header.stamp = self.navigator.get_clock().now().to_msg()
-		goal_drop_int_1.pose.position.x = -0.2
-		goal_drop_int_1.pose.position.y = -2.7
-		goal_drop_int_1.pose.orientation.x = 0.0
-		goal_drop_int_1.pose.orientation.y = 0.0
-		goal_drop_int_1.pose.orientation.z = 0.9999997
-		goal_drop_int_1.pose.orientation.w = 0.0007963
-
-		goal_drop_int_2 = PoseStamped()
-		goal_drop_int_2.header.frame_id = 'map'
-		goal_drop_int_2.header.stamp = self.navigator.get_clock().now().to_msg()
-		goal_drop_int_2.pose.position.x = 1.65
-		goal_drop_int_2.pose.position.y = -4.5
-		goal_drop_int_2.pose.orientation.x = 0.0
-		goal_drop_int_2.pose.orientation.y = 0.0
-		goal_drop_int_2.pose.orientation.z =  -0.70398
-		goal_drop_int_2.pose.orientation.w =  0.7102198
-
-		goal_drop_1 = PoseStamped()
-		goal_drop_1.header.frame_id = 'map'
-		goal_drop_1.header.stamp = self.navigator.get_clock().now().to_msg()
-		goal_drop_1.pose.position.x = 0.99
-		goal_drop_1.pose.position.y = -2.6
-		goal_drop_1.pose.orientation.x = 0.0
-		goal_drop_1.pose.orientation.y = 0.0
-		goal_drop_1.pose.orientation.z = 0.9999997
-		goal_drop_1.pose.orientation.w = 0.0007963
-
-
+
 		goal_pick_2 = PoseStamped()
 		goal_pick_2.header.frame_id = 'map'
 		goal_pick_2.pose.position.x = bot_pose_2[0]
@@ -263,6 +265,7 @@ def main(self):
 		goal_pick_2.pose.orientation.z = goal_theta_2[2]
 		goal_pick_2.pose.orientation.w = goal_theta_2[3]
 
+
 		goal_pick_3 = PoseStamped()
 		goal_pick_3.header.frame_id = 'map'
 		goal_pick_3.header.stamp = self.navigator.get_clock().now().to_msg()
@@ -274,45 +277,73 @@ def main(self):
 		goal_pick_3.pose.orientation.w = goal_theta_3[3]
 
 
+		goal_drop_init_1 = PoseStamped()
+		goal_drop_init_1.header.frame_id = 'map'
+		goal_drop_init_1.header.stamp = self.navigator.get_clock().now().to_msg()
+		goal_drop_init_1.pose.position.x = init_arm_pose_1[0]
+		goal_drop_init_1.pose.position.y = init_arm_pose_1[1]
+		goal_drop_init_1.pose.orientation.x = init_goal_theta_4[0]
+		goal_drop_init_1.pose.orientation.y = init_goal_theta_4[1]
+		goal_drop_init_1.pose.orientation.z = init_goal_theta_4[2]
+		goal_drop_init_1.pose.orientation.w = init_goal_theta_4[3]
+
+		goal_drop_init_2 = PoseStamped()
+		goal_drop_init_2.header.frame_id = 'map'
+		goal_drop_init_2.pose.position.x = init_arm_pose_2[0]
+		goal_drop_init_2.pose.position.y = init_arm_pose_2[1]
+		goal_drop_init_2.pose.orientation.x = init_goal_theta_5[0]
+		goal_drop_init_2.pose.orientation.y = init_goal_theta_5[1]
+		goal_drop_init_2.pose.orientation.z = init_goal_theta_5[2]
+		goal_drop_init_2.pose.orientation.w = init_goal_theta_5[3]
+
+		goal_drop_init_3 = PoseStamped()
+		goal_drop_init_3.header.frame_id = 'map'
+		goal_drop_init_3.pose.position.x = init_arm_pose_3[0]
+		goal_drop_init_3.pose.position.y = init_arm_pose_3[1]
+		goal_drop_init_3.pose.orientation.x = init_goal_theta_6[0]
+		goal_drop_init_3.pose.orientation.y = init_goal_theta_6[1]
+		goal_drop_init_3.pose.orientation.z = init_goal_theta_6[2]
+		goal_drop_init_3.pose.orientation.w = init_goal_theta_6[3]
 
+		goal_drop_1 = PoseStamped()
+		goal_drop_1.header.frame_id = 'map'
+		goal_drop_1.header.stamp = self.navigator.get_clock().now().to_msg()
+		goal_drop_1.pose.position.x = arm_pose_1[0]
+		goal_drop_1.pose.position.y = arm_pose_1[1]
+		goal_drop_1.pose.orientation.x = goal_theta_4[0]
+		goal_drop_1.pose.orientation.y = goal_theta_4[1]
+		goal_drop_1.pose.orientation.z = goal_theta_4[2]
+		goal_drop_1.pose.orientation.w = goal_theta_4[3]
+
 		goal_drop_2 = PoseStamped()
 		goal_drop_2.header.frame_id = 'map'
-		goal_drop_2.header.stamp = self.navigator.get_clock().now().to_msg()
-		goal_drop_2.pose.position.x = 1.65
-		goal_drop_2.pose.position.y = -3.1
-		goal_drop_2.pose.orientation.x = 0.0
-		goal_drop_2.pose.orientation.y = 0.0
-		goal_drop_2.pose.orientation.z =  -0.70398
-		goal_drop_2.pose.orientation.w =  0.7102198
+		goal_drop_2.pose.position.x = arm_pose_2[0]
+		goal_drop_2.pose.position.y = arm_pose_2[1]
+		goal_drop_2.pose.orientation.x = goal_theta_5[0]
+		goal_drop_2.pose.orientation.y = goal_theta_5[1]
+		goal_drop_2.pose.orientation.z = goal_theta_5[2]
+		goal_drop_2.pose.orientation.w = goal_theta_5[3]
 
 		goal_drop_3 = PoseStamped()
 		goal_drop_3.header.frame_id = 'map'
-		goal_drop_3.header.stamp = self.navigator.get_clock().now().to_msg()
-		goal_drop_3.pose.position.x =1.550000
-		goal_drop_3.pose.position.y = -1.298586
-		goal_drop_3.pose.orientation.x = 0.0
-		goal_drop_3.pose.orientation.y = 0.0
-		goal_drop_3.pose.orientation.z =  0.6921004
-		goal_drop_3.pose.orientation.w = 0.7218012 
+		goal_drop_3.pose.position.x = arm_pose_3[0]
+		goal_drop_3.pose.position.y = arm_pose_3[1]
+		goal_drop_3.pose.orientation.x = goal_theta_6[0]
+		goal_drop_3.pose.orientation.y = goal_theta_6[1]
+		goal_drop_3.pose.orientation.z = goal_theta_6[2]
+		goal_drop_3.pose.orientation.w = goal_theta_6[3]
 
 
 		# Define other drop goals...
 
 		self.navigator.waitUntilNav2Active()
+
 		self.arm_request(rack_no = "3")
-		#self.navigate_and_dock(goal_pick_3, goal_drop_3, goal_drop_int, orientation_rack_3, rack_list[2], "3")
-		self.navigate_and_dock(goal_pick_1, goal_drop_2, goal_drop_int_2, orientation_rack_1, rack_list[0], "1")
+		self.navigate_and_dock(goal_pick_2, goal_drop_2, goal_drop_init_2, orientation_rack_2, rack_list[1], "2")
 		self.move_with_linear_x(2.0,0.5,-0.95)
-		self.navigate_and_dock(goal_pick_2, goal_drop_1, goal_drop_int_1, orientation_rack_2, rack_list[1], "2")
+		self.navigate_and_dock(goal_pick_1, goal_drop_1, goal_drop_init_1, orientation_rack_1, rack_list[0], "1")
 		self.move_with_linear_x(2.0,0.5,-0.95)
 
-		#elif package_id == 2:
-			# Navigate for package_id 2
-		#	pass
-		#else:
-			# Navigate for package_id 1
-		#	pass
-
 		exit(0)
 
 if __name__ == '__main__':

ebot_nav2/scripts/ebot_nav_cmd_hardware.py

@@ -106,6 +106,30 @@ def nav_theta(self,angle,obj_type):
 		x,y,z,w=quaternion_from_euler(0,0,correct_angle)
 		return (x,y,z,w)
 
+
+	def move_with_linear_x(self,duration, linear_x, angular_z):
+		start_time = time.time()  # Get the current time
+		end_time = start_time + duration
+		end_time2 = end_time + duration
+
+		# Create a Twist message to control the linear.x (forward movement)
+		vel_msg = Twist()
+
+		vel_msg.linear.x = linear_x
+
+		while time.time() < end_time:
+			self.vel_pub.publish(vel_msg)  # Publish the Twist message to control the robot's movement
+
+		vel_msg.linear.x = 0.0
+		vel_msg.angular.z = angular_z
+
+		while time.time() < end_time2:
+			self.vel_pub.publish(vel_msg) 
+
+		vel_msg.linear.x = 0.0
+		vel_msg.angular.z = 0.0
+		self.vel_pub.publish(vel_msg)
+
 
 	def nav_reach(self, goal):
 		while not self.navigator.isTaskComplete():
@@ -139,21 +163,8 @@ def navigate_and_dock(self, goal_pick, goal_drop, goal_int, orientation_rack, ra
 		self.navigator.goToPose(goal_drop)
 		self.nav_reach(goal_drop)
 		self.rack_detach(rack)
-		self.arm_request(rack_no = "3")
-
-
-
-	# def navigate_and_dock(self, goal_pick, goal_drop, orientation_rack, rack,bot_coordinates):
-	#     self.navigator.goToPose(goal_pick)
-	#     self.nav_reach(goal_pick)
-	#     # self.correct(bot_coordinates)
-	#     print("POSE----",self.robot_pose)
-	#     if self.flag == False:
-	#         self.send_request(orientation_rack)
-	#         self.rack_attach(rack)
-	#         self.navigator.goToPose(goal_drop)
-	#         self.nav_reach(goal_drop)
-	#         self.rack_detach(rack)        
+		self.arm_request(rack_no = str(rack_no))
+
 
 
 	def main(self):
@@ -305,11 +316,13 @@ def main(self):
 
 		self.navigator.waitUntilNav2Active()
 
-		self.arm_request(rack_no = "0")
-		self.navigate_and_dock(goal_pick_3, goal_drop_2, goal_drop_init_2, orientation_rack_3, rack_list[2], "3")
 		self.arm_request(rack_no = "1")
+		self.navigate_and_dock(goal_pick_3, goal_drop_3, goal_drop_init_3, orientation_rack_3, rack_list[2], "3")
+		self.move_with_linear_x(2.0,0.5,-0.95)
 
-
+		self.navigate_and_dock(goal_pick_2, goal_drop_2, goal_drop_init_2, orientation_rack_2, rack_list[1], "2")
+		self.move_with_linear_x(2.0,0.5,-0.95)
+
 		exit(0)
 
 if __name__ == '__main__':