From 302226d4902e4a32625bbdabef32fd88148ac3a8 Mon Sep 17 00:00:00 2001 From: noemoji041 Date: Mon, 12 Feb 2024 22:53:25 +0530 Subject: [PATCH] task5 hardware scripts added --- arm_picky/srv/ArmNew.srv | 2 +- .../scripts/ebot_docking_boilerplate.py | 7 +- .../ebot_docking_boilerplate_hardware.py | 21 +- .../ebot_docking_boilerplate_sim_test.py | 305 ------------ ebot_nav2/config/config_sim.yaml | 7 + ebot_nav2/scripts/ebot_nav_cmd.py | 2 +- ebot_nav2/scripts/ebot_nav_cmd_hardware.py | 197 ++++---- pymoveit2/examples/ex_collision_object.py | 101 ---- pymoveit2/examples/ex_joint_goal.py | 70 --- pymoveit2/examples/ex_pose_goal.py | 73 --- pymoveit2/examples/ex_servo.py | 66 --- pymoveit2/examples/new_hard.py | 406 ---------------- pymoveit2/examples/task1b.py | 384 --------------- pymoveit2/examples/task4b.py | 384 --------------- pymoveit2/examples/task4b_new.py | 428 ----------------- .../{something.py => task5_movit_hardware.py} | 121 ++--- pymoveit2/examples/task5_movit_sim.py | 0 ur_description/scripts/test.py | 447 ------------------ 18 files changed, 200 insertions(+), 2821 deletions(-) delete mode 100644 ebot_docking/scripts/ebot_docking_boilerplate_sim_test.py create mode 100644 ebot_nav2/config/config_sim.yaml delete mode 100755 pymoveit2/examples/ex_collision_object.py delete mode 100755 pymoveit2/examples/ex_joint_goal.py delete mode 100755 pymoveit2/examples/ex_pose_goal.py delete mode 100755 pymoveit2/examples/ex_servo.py delete mode 100644 pymoveit2/examples/new_hard.py delete mode 100755 pymoveit2/examples/task1b.py delete mode 100644 pymoveit2/examples/task4b.py delete mode 100755 pymoveit2/examples/task4b_new.py rename pymoveit2/examples/{something.py => task5_movit_hardware.py} (84%) create mode 100644 pymoveit2/examples/task5_movit_sim.py delete mode 100755 ur_description/scripts/test.py diff --git a/arm_picky/srv/ArmNew.srv b/arm_picky/srv/ArmNew.srv index 5a3ef3f..fdc913b 100644 --- a/arm_picky/srv/ArmNew.srv +++ b/arm_picky/srv/ArmNew.srv @@ -1,4 +1,4 @@ bool boom -string whack +string rack_no --- bool reply diff --git a/ebot_docking/scripts/ebot_docking_boilerplate.py b/ebot_docking/scripts/ebot_docking_boilerplate.py index cc12ee6..24c5df6 100755 --- a/ebot_docking/scripts/ebot_docking_boilerplate.py +++ b/ebot_docking/scripts/ebot_docking_boilerplate.py @@ -53,7 +53,7 @@ def __init__(self): self.normalize_yaw_rack = 0 self.difference = 0 package_name = 'ebot_nav2' - config = "config/config.yaml" + config = "config/config_sim.yaml" self.flag =0 pkg_share = FindPackageShare(package=package_name).find(package_name) @@ -267,10 +267,7 @@ def controller_loop(self): # Callback function for the DockControl service def dock_control_callback(self, request, response): - # Extract desired docking parameters from the service request - self.linear_dock = request.linear_dock - self.orientation_dock = request.orientation_dock - self.distance = request.distance + # Extract desired docking parametg self.orientation = request.orientation self.rack_no = request.rack_no # print(self.orientation) diff --git a/ebot_docking/scripts/ebot_docking_boilerplate_hardware.py b/ebot_docking/scripts/ebot_docking_boilerplate_hardware.py index b2e47be..2288306 100644 --- a/ebot_docking/scripts/ebot_docking_boilerplate_hardware.py +++ b/ebot_docking/scripts/ebot_docking_boilerplate_hardware.py @@ -113,8 +113,8 @@ def odometry_callback(self, msg): self.robot_pose[2] = yaw def ultra_callback(self,msg): - self.usrleft_value= msg.data[4] - self.usrright_value = msg.data[5] + self.usrleft_value= msg.data[4]/100 + self.usrright_value = msg.data[5]/100 # Utility function to normalize angles within the range of -π to π (OPTIONAL) @@ -139,10 +139,8 @@ def controller_loop(self): # Implement control logic here for linear and angular motion # For example P-controller is enough, what is P-controller go check it out ! # ... - print("After") + # self.difference = self.normalize_yaw_rack - self.normalize_yaw_bot - self.flag = 1 - self.orientation_dock = False if self.orientation_dock ==True: print("YAW------------YAW",(self.rack_yaw[int(self.rack_no)-1])) @@ -154,13 +152,12 @@ 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 -<<<<<<< Updated upstream if 0.3 > abs(self.x_pose[int(self.rack_no) - 1][0] - self.robot_pose[0]) > 0.025 and robot_head== "1.0": print(self.robot_pose[0],self.robot_pose[1],"---------44444444",abs(self.x_pose[int(self.rack_no) - 1][0] - self.robot_pose[0])) @@ -172,11 +169,6 @@ def controller_loop(self): elif 0.3 > abs(self.x_pose[int(self.rack_no) - 1][0] - self.robot_pose[0]) > 0.025 and robot_head== "-1.0": print(self.robot_pose[0],self.robot_pose[1],"---------44444444",abs(self.x_pose[int(self.rack_no) - 1][0] - self.robot_pose[0])) vel.linear.x = error *0.4 -======= - if abs(self.rack3_coordinates[0] - self.robot_pose[0]) > 0.01: - print(self.robot_pose[0],"---------44444444") - vel.linear.x = error ->>>>>>> Stashed changes self.vel_pub.publish(vel) @@ -274,11 +266,11 @@ def controller_loop(self): # NO ULTRA SONIC DISTANCE ERROR DIRECT DOCKING - if self.usrleft_value > 1: + if self.usrleft_value >= 0.13: print("===============") print(self.usrleft_value) self.orientation_dock = False - vel.linear.x = -self.usrleft_value * 0.2 + vel.linear.x = -self.usrleft_value * 0.48 vel.angular.z = 0.0 self.vel_pub.publish(vel) self.linear_dock = False @@ -290,6 +282,7 @@ def controller_loop(self): else: vel.linear.x = 0.0 self.vel_pub.publish(vel) + time.sleep(2) print("docking done") self.linear_dock = True self.is_docking = False diff --git a/ebot_docking/scripts/ebot_docking_boilerplate_sim_test.py b/ebot_docking/scripts/ebot_docking_boilerplate_sim_test.py deleted file mode 100644 index f77cf94..0000000 --- a/ebot_docking/scripts/ebot_docking_boilerplate_sim_test.py +++ /dev/null @@ -1,305 +0,0 @@ -#!/usr/bin/env python3 - -## Overview - -# ### -# This ROS2 script is designed to control a robot's docking behavior with a rack. -# It utilizes odometry data, ultrasonic sensor readings, and provides docking control through a custom service. -# The script handles both linear and angular motion to achieve docking alignment and execution. -# ### - -# Import necessary ROS2 packages and message types -import rclpy -from rclpy.node import Node -from nav_msgs.msg import Odometry -from geometry_msgs.msg import Twist -from sensor_msgs.msg import Range -from rclpy.callback_groups import ReentrantCallbackGroup -from rclpy.executors import MultiThreadedExecutor -from tf_transformations import euler_from_quaternion -from ebot_docking.srv import DockSw # Import custom service message -import math, statistics -from launch_ros.substitutions import FindPackageShare -from ament_index_python.packages import get_package_share_directory -import os -import yaml -import time -# Define a class for your ROS2 node -class MyRobotDockingController(Node): - - def __init__(self): - # Initialize the ROS2 node with a unique name - super().__init__('my_robot_docking_controller') - - # Create a callback group for managing callbacks - self.callback_group = ReentrantCallbackGroup() - - self.get_logger().info("Server Started") - - - # Subscribe to odometry data for robot pose information - self.odom_sub = self.create_subscription(Odometry, 'odom', 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) - # Add another one here - self.ultrasonic_rr_sub = self.create_subscription(Range, '/ultrasonic_rr/scan', self.ultrasonic_rr_callback, 10) - - - # Create a ROS2 service for controlling docking behavior, can add another custom service message - self.dock_control_srv = self.create_service(DockSw, 'dock_control', self.dock_control_callback, callback_group=self.callback_group) - - # Create a publisher for sending velocity commands to the robot - self.vel_pub = self.create_publisher(Twist, "/cmd_vel", 10) - - # Initialize all flags and parameters here - self.is_docking = False - self.robot_pose = [0,0,0] - self.updated_robot_pose = [0,0,0] - self.dock_aligned = False - self.kp = 1.0 - self.normalize_yaw_bot = 0 - self.normalize_yaw_rack = 0 - self.difference = 0 - package_name = 'ebot_nav2' - config = "config/config.yaml" - self.flag =0 - self.orient_diff=0 - - - ebot_nav2_dir = get_package_share_directory('ebot_nav2') - - pkg_share = FindPackageShare(package=package_name).find(package_name) - config_path = os.path.join(pkg_share, config) - with open(config_path, 'r') as infp: - pos_rack = infp.read() - - data_dict = yaml.safe_load(pos_rack) - self.x_pose = [] - positions = data_dict['position'] - self.rack1_coordinates = positions[0]['rack1'] - self.x_pose.append(self.rack1_coordinates) - self.rack2_coordinates = positions[1]['rack2'] - self.x_pose.append(self.rack2_coordinates) - self.rack3_coordinates = positions[2]['rack3'] - self.x_pose.append(self.rack3_coordinates) - - self.rack_yaw=[] - - orientation_rack_1 = self.rack1_coordinates[2] - self.rack_yaw.append(orientation_rack_1) - - orientation_rack_2 = self.rack2_coordinates[2] - self.rack_yaw.append(orientation_rack_2) - - orientation_rack_3 = self.rack3_coordinates[2] - self.rack_yaw.append(orientation_rack_3) - - - self.controller_timer = self.create_timer(0.1, self.controller_loop) - - # Callback function for odometry data - def odometry_callback(self, msg): - # Extract and update robot pose information from odometry message - self.robot_pose[0] = msg.pose.pose.position.x - self.robot_pose[1] = msg.pose.pose.position.y - quaternion_array = msg.pose.pose.orientation - orientation_list = [quaternion_array.x, quaternion_array.y, quaternion_array.z, quaternion_array.w] - _, _, yaw = euler_from_quaternion(orientation_list) - - self.robot_pose[2] = yaw - - # Callback function for the left ultrasonic sensor - def ultrasonic_rl_callback(self, msg): - self.usrleft_value = msg.range - - # Callback function for the right ultrasonic sensor - def ultrasonic_rr_callback(self, msg): - self.usrright_value = msg.range - - # Utility function to normalize angles within the range of -π to π (OPTIONAL) - def normalize_angle(self, angle): - if angle < 0: - angle = math.pi + (math.pi + angle) - return angle - - # Main control loop for managing docking behavior - - def controller_loop(self): - vel = Twist() - - if self.is_docking: - - print("After") - self.difference = self.normalize_yaw_rack - self.normalize_yaw_bot - - if self.orientation_dock ==True: - - print("YAW------------YAW",abs(self.rack_yaw[int(self.rack_no)-1])) - - error = self.x_pose[int(self.rack_no) - 1][0] - self.robot_pose[0] - error2 = self.x_pose[int(self.rack_no) - 1][1] - self.robot_pose[1] - self.orient_diff=self.robot_pose[2]-self.orientation - robot_head=str(self.orient_diff/abs(self.robot_pose[2]-self.orientation)) - - - - -# set error range for distangce to avoide rack yaw -# X DIRECTION - if abs(self.x_pose[int(self.rack_no) - 1][0] - self.robot_pose[0]) > 0.025 and abs(self.rack_yaw[int(self.rack_no)-1]) == 1.57 and robot_head== "+": - - print(self.robot_pose[0],self.robot_pose[1],"---------44444444",abs(self.x_pose[int(self.rack_no) - 1][0] - self.robot_pose[0])) - print("=============================================") - - print ("ROBOT_P---",self.robot_pose[0]) - vel.linear.x = error *0.3 - self.vel_pub.publish(vel) - - elif abs(self.x_pose[int(self.rack_no) - 1][0] - self.robot_pose[0]) > 0.025 and abs(self.rack_yaw[int(self.rack_no)-1]) == 1.57 and robot_head== "-": - print(self.robot_pose[0],self.robot_pose[1],"---------44444444",abs(self.x_pose[int(self.rack_no) - 1][0] - self.robot_pose[0])) - vel.linear.x = -error *0.3 - self.vel_pub.publish(vel) - - - - - -# Y DIRECTION - - elif abs(self.x_pose[int(self.rack_no) - 1][1] - self.robot_pose[1]) > 0.025 and (abs(self.rack_yaw[int(self.rack_no)-1]) == 3.14 or abs(self.rack_yaw[int(self.rack_no)-1]) == 0.0 ) and robot_head== "+": - print(self.robot_pose[0],self.robot_pose[1],"---------33333333333",abs(self.x_pose[int(self.rack_no) - 1][1] - self.robot_pose[1])) - vel.linear.x = error2 *0.1 - self.vel_pub.publish(vel) - - - elif abs(self.x_pose[int(self.rack_no) - 1][1] - self.robot_pose[1]) > 0.025 and (abs(self.rack_yaw[int(self.rack_no)-1]) == 3.14 or abs(self.rack_yaw[int(self.rack_no)-1]) == 0.0 ) and robot_head== "-": - print(self.robot_pose[0],self.robot_pose[1],"---------33333333333",abs(self.x_pose[int(self.rack_no) - 1][1] - self.robot_pose[1])) - vel.linear.x = -error2 *0.1 - self.vel_pub.publish(vel) - - - else: - - print("Before") - vel.linear.x = 0.0 - self.vel_pub.publish(vel) - self.flag = 1 - self.orientation_dock = False - - elif self.flag == 1: - if abs(self.difference) > 0.02: - vel.angular.z = self.difference *0.6 - self.vel_pub.publish(vel) - - else: - vel.angular.z = 0.0 - self.vel_pub.publish(vel) - self.orientation_dock = False - self.linear_dock = False - self.flag = 0 - - print("successfully oriented",self.linear_dock) - elif self.linear_dock == False: - print("FLAG---",self.flag) - print("++++++++++++++++++++++++++++++++++++") - self.usrdiff=self.usrleft_value-self.usrright_value - # print("L-",self.usrleft_value,"R-",self.usrright_value) - # print("Diff---",self.difference) - self.diff=self.usrleft_value-self.usrright_value - - if self.usrleft_value > 0.15 and round(self.usrright_value,1) != round(self.usrleft_value,1): - - - if abs(self.difference)<=0.02: - print(">>>>>=====>>>>>>") - vel.angular.z = self.usrdiff*0.2 - - self.vel_pub.publish(vel) - else: - vel.angular.z = self.difference*0.2 - vel.linear.x = -0.2 - - self.vel_pub.publish(vel) - - - print(">>>>>>>>>>>>>>>") - # vel.linear.x = -self.usrleft_value * 0.4 - self.orientation_dock = False - self.linear_dock = False - - - elif self.usrleft_value > 0.15 and round(self.usrright_value,1) == round(self.usrleft_value,1): - print("===============") - self.orientation_dock = False - vel.linear.x = -self.usrleft_value * 0.4 - vel.angular.z = 0.0 - self.vel_pub.publish(vel) - self.linear_dock = False - - - else: - vel.linear.x = 0.0 - self.vel_pub.publish(vel) - print("docking done") - self.linear_dock = True - self.is_docking = False - self.dock_aligned = True - - - - # Callback function for the DockControl service - def dock_control_callback(self, request, response): - # 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) - - self.normalize_yaw_rack = self.normalize_angle(self.orientation) - # print(self.normalize_yaw_rack) - self.normalize_yaw_bot = self.normalize_angle(self.robot_pose[2]) - # print(self.normalize_yaw_bot) - - - # Reset flags and start the docking process - self.dock_aligned = False - print(self.dock_aligned) - - # Log a message indicating that docking has started - self.get_logger().info("Docking started!") - # Create a rate object to control the loop frequency - self.rate = self.create_rate(2, self.get_clock()) - - # Wait until the robot is aligned for docking - while not self.dock_aligned: - self.normalize_yaw_rack = self.normalize_angle(self.orientation) - self.normalize_yaw_bot = self.normalize_angle(self.robot_pose[2]) - self.is_docking = True - self.get_logger().info("Waiting for alignment...") - self.controller_loop() - self.rate.sleep() - - # Set the service response indicating success - response.success = True - response.message = "Docking control initiated" - return response - -# Main function to initialize the ROS2 node and spin the executor -def main(args=None): - rclpy.init(args=args) - - my_robot_docking_controller = MyRobotDockingController() - - executor = MultiThreadedExecutor() - executor.add_node(my_robot_docking_controller) - - executor.spin() - - my_robot_docking_controller.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/ebot_nav2/config/config_sim.yaml b/ebot_nav2/config/config_sim.yaml new file mode 100644 index 0000000..f32d771 --- /dev/null +++ b/ebot_nav2/config/config_sim.yaml @@ -0,0 +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.500000, 3.140001] + +package_id: [3, 1, 2] \ No newline at end of file diff --git a/ebot_nav2/scripts/ebot_nav_cmd.py b/ebot_nav2/scripts/ebot_nav_cmd.py index 05c3c5b..6d3ce48 100755 --- a/ebot_nav2/scripts/ebot_nav_cmd.py +++ b/ebot_nav2/scripts/ebot_nav_cmd.py @@ -174,7 +174,7 @@ def navigate_and_dock(self, goal_pick, goal_drop, goal_int, orientation_rack, ra self.arm_request(rack_no = "3") def main(self): package_name = 'ebot_nav2' - config = "config/config.yaml" + config = "config/config_sim.yaml" ebot_nav2_dir = get_package_share_directory('ebot_nav2') diff --git a/ebot_nav2/scripts/ebot_nav_cmd_hardware.py b/ebot_nav2/scripts/ebot_nav_cmd_hardware.py index bbcd189..d16b55c 100644 --- a/ebot_nav2/scripts/ebot_nav_cmd_hardware.py +++ b/ebot_nav2/scripts/ebot_nav_cmd_hardware.py @@ -7,6 +7,7 @@ from rclpy.node import Node from ebot_docking.srv import DockSw from usb_relay.srv import RelaySw +from arm_picky.srv import ArmNew import yaml import os from launch_ros.substitutions import FindPackageShare @@ -23,8 +24,7 @@ def __init__(self): super().__init__('nav_dock') self.attach = self.create_client(srv_type=RelaySw, srv_name='/usb_relay_sw') - - + self.arm_check = self.create_client(srv_type=ArmNew, srv_name='arm_control') self.client_docking = self.create_client(srv_type=DockSw, srv_name='dock_control') self.vel_pub = self.create_publisher(Twist, "/cmd_vel", 10) self.vel_msg = Twist() @@ -43,6 +43,14 @@ def send_request(self, orientation, rack_no): rclpy.spin_until_future_complete(self, future) return future.result() + def arm_request(self, rack_no): + request_arm = ArmNew.Request() + request_arm.boom = True + 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() + def rack_attach(self, rack): req = RelaySw.Request() req.relaychannel = True @@ -59,7 +67,7 @@ def rack_attach(self, rack): def rack_detach(self, rack): req = RelaySw.Request() - req.relaychannel = False + req.relaychannel = True req.relaystate = False atc = self.attach.call_async(req) @@ -76,29 +84,26 @@ 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.65 + else: + d=1.0 + 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 - - - if (correct_angle > 3.14): - - flag_angle = correct_angle - 3.14 - correct_yaw = -3.14 + flag_angle + def nav_theta(self,angle,obj_type): + if obj_type =="rack": + correct_angle=angle-1.57 else: - correct_yaw = correct_angle - - x,y,z,w=quaternion_from_euler(0,0,correct_yaw) + correct_angle=angle + + + x,y,z,w=quaternion_from_euler(0,0,correct_angle) return (x,y,z,w) @@ -121,7 +126,6 @@ def nav_reach(self, goal): - def navigate_and_dock(self, goal_pick, goal_drop, goal_int, orientation_rack, rack,rack_no): self.rack_attach(rack) self.navigator.goToPose(goal_pick) @@ -129,14 +133,14 @@ def navigate_and_dock(self, goal_pick, goal_drop, goal_int, orientation_rack, ra self.send_request(orientation_rack, rack_no) #self.rack_attach(rack) - if rack_no == "3": - self.navigator.goToPose(goal_int) - self.nav_reach(goal_int) - else: - pass + + self.navigator.goToPose(goal_int) + self.nav_reach(goal_int) 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): @@ -169,25 +173,48 @@ def main(self): 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"] 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(theta_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(theta_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(theta_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") + + + 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_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' @@ -198,28 +225,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] - - # Define other goals... - goal_drop_int = PoseStamped() - goal_drop_int.header.frame_id = 'map' - goal_drop_int.header.stamp = self.navigator.get_clock().now().to_msg() - goal_drop_int.pose.position.x = -0.2 - goal_drop_int.pose.position.y = -2.5 - goal_drop_int.pose.orientation.x = 0.0 - goal_drop_int.pose.orientation.y = 0.0 - goal_drop_int.pose.orientation.z = 0.9999997 - goal_drop_int.pose.orientation.w = 0.0007963 - - 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 = 1.15 - goal_drop_1.pose.position.y = -2.455 - 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] @@ -229,6 +235,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() @@ -240,43 +247,71 @@ 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.650000 - goal_drop_2.pose.position.y = -3.684832 - 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 - - - # Define other drop goals... + 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] self.navigator.waitUntilNav2Active() - if package_id == 3: - self.navigate_and_dock(goal_pick_3, goal_drop_3, goal_drop_int, orientation_rack_3, rack_list[2],"3") - elif package_id == 2: - # Navigate for package_id 2 - pass - else: - # Navigate for package_id 1 - pass + 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") + exit(0) if __name__ == '__main__': nav_controller = NavigationController() - nav_controller.main() + nav_controller.main() \ No newline at end of file diff --git a/pymoveit2/examples/ex_collision_object.py b/pymoveit2/examples/ex_collision_object.py deleted file mode 100755 index 6eb6194..0000000 --- a/pymoveit2/examples/ex_collision_object.py +++ /dev/null @@ -1,101 +0,0 @@ -#!/usr/bin/env python3 -""" -Example of adding and removing a collision object with a mesh geometry. -Note: Python module `trimesh` is required for this example (`pip install trimesh`). -`ros2 run pymoveit2 ex_collision_object.py --ros-args -p action:="add" -p position:="[0.5, 0.0, 0.5]" -p quat_xyzw:="[0.0, 0.0, -0.707, 0.707]"` -`ros2 run pymoveit2 ex_collision_object.py --ros-args -p action:="add" -p filepath:="./my_favourity_mesh.stl"` -`ros2 run pymoveit2 ex_collision_object.py --ros-args -p action:="remove"` -""" - -from os import path -from threading import Thread - -import rclpy -from rclpy.callback_groups import ReentrantCallbackGroup -from rclpy.node import Node - -from pymoveit2 import MoveIt2 -from pymoveit2.robots import ur5 - -DEFAULT_EXAMPLE_MESH = path.join( - path.dirname(path.realpath(__file__)), "assets", "rack.stl" -) - - -def main(): - rclpy.init() - - # Create node for this example - node = Node("ex_collision_object") - - # Declare parameter for joint positions - node.declare_parameter( - "filepath", - "", - ) - node.declare_parameter( - "action", - "add", - ) - node.declare_parameter("position", [0.55, 0.05, -0.21]) - node.declare_parameter("quat_xyzw", [0.0, 0.0, 0.0, 0.707]) - - # Create callback group that allows execution of callbacks in parallel without restrictions - callback_group = ReentrantCallbackGroup() - - # Create MoveIt 2 interface - moveit2 = MoveIt2( - node=node, - joint_names=ur5.joint_names(), - base_link_name=ur5.base_link_name(), - end_effector_name=ur5.end_effector_name(), - group_name=ur5.MOVE_GROUP_ARM, - callback_group=callback_group, - ) - - # Spin the node in background thread(s) - executor = rclpy.executors.MultiThreadedExecutor(2) - executor.add_node(node) - executor_thread = Thread(target=executor.spin, daemon=True, args=()) - executor_thread.start() - - # Get parameters - filepath = node.get_parameter("filepath").get_parameter_value().string_value - action = node.get_parameter("action").get_parameter_value().string_value - position = node.get_parameter("position").get_parameter_value().double_array_value - quat_xyzw = node.get_parameter("quat_xyzw").get_parameter_value().double_array_value - - # Use the default example mesh if invalid - if not filepath: - node.get_logger().info(f"Using the default example mesh file") - filepath = DEFAULT_EXAMPLE_MESH - - # Make sure the mesh file exists - if not path.exists(filepath): - node.get_logger().error(f"File '{filepath}' does not exist") - rclpy.shutdown() - exit(1) - - # Determine ID of the collision mesh - mesh_id = path.basename(filepath).split(".")[0] - - if "add" == action: - # Add collision mesh - node.get_logger().info( - f"Adding collision mesh '{filepath}' {{position: {list(position)}, quat_xyzw: {list(quat_xyzw)}}}" - ) - # print(ur5.base_link_name()) - moveit2.add_collision_mesh( - filepath=filepath, id=mesh_id, position=position, quat_xyzw=quat_xyzw, frame_id=ur5.base_link_name() - ) - else: - # Remove collision mesh - node.get_logger().info(f"Removing collision mesh with ID '{mesh_id}'") - moveit2.remove_collision_mesh(id=mesh_id) - - rclpy.shutdown() - exit(0) - - -if __name__ == "__main__": - main() diff --git a/pymoveit2/examples/ex_joint_goal.py b/pymoveit2/examples/ex_joint_goal.py deleted file mode 100755 index 8ade6e2..0000000 --- a/pymoveit2/examples/ex_joint_goal.py +++ /dev/null @@ -1,70 +0,0 @@ -#!/usr/bin/env python3 -""" -Example of moving to a joint configuration. -`ros2 run pymoveit2 ex_joint_goal.py --ros-args -p joint_positions:="[1.57, -1.57, 0.0, -1.57, 0.0, 1.57]"` -""" - -from threading import Thread - -import rclpy -from rclpy.callback_groups import ReentrantCallbackGroup -from rclpy.node import Node - -from pymoveit2 import MoveIt2 -from pymoveit2.robots import ur5 - - -def main(): - rclpy.init() - - # Create node for this example - node = Node("ex_joint_goal") - - # Declare parameter for joint positions - node.declare_parameter( - "joint_positions", - [ - 0.0, - -2.39, - 2.4, - -3.15, - -1.58, - 3.15 - ], - ) - - # Create callback group that allows execution of callbacks in parallel without restrictions - callback_group = ReentrantCallbackGroup() - - # Create MoveIt 2 interface - moveit2 = MoveIt2( - node=node, - joint_names=ur5.joint_names(), - base_link_name=ur5.base_link_name(), - end_effector_name=ur5.end_effector_name(), - group_name=ur5.MOVE_GROUP_ARM, - callback_group=callback_group, - ) - - # Spin the node in background thread(s) - executor = rclpy.executors.MultiThreadedExecutor(2) - executor.add_node(node) - executor_thread = Thread(target=executor.spin, daemon=True, args=()) - executor_thread.start() - - # Get parameter - joint_positions = ( - node.get_parameter("joint_positions").get_parameter_value().double_array_value - ) - - # Move to joint configuration - node.get_logger().info(f"Moving to {{joint_positions: {list(joint_positions)}}}") - moveit2.move_to_configuration(joint_positions) - moveit2.wait_until_executed() - - rclpy.shutdown() - exit(0) - - -if __name__ == "__main__": - main() diff --git a/pymoveit2/examples/ex_pose_goal.py b/pymoveit2/examples/ex_pose_goal.py deleted file mode 100755 index bb52671..0000000 --- a/pymoveit2/examples/ex_pose_goal.py +++ /dev/null @@ -1,73 +0,0 @@ -#!/usr/bin/env python3 -""" -Example of moving to a pose goal. -`ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.25, 0.0, 1.0]" -p quat_xyzw:="[0.0, 0.0, 0.0, 1.0]" -p cartesian:=False` -""" - -from threading import Thread - -import rclpy -from rclpy.callback_groups import ReentrantCallbackGroup -from rclpy.node import Node - -from pymoveit2 import MoveIt2 -from pymoveit2.robots import ur5 - - -def main(): - rclpy.init() - - # Create node for this example - node = Node("ex_pose_goal") - - # Declare parameters for position and orientation - node.declare_parameter("position", [0.35, 0.10, 0.68]) - node.declare_parameter("quat_xyzw", [0.5, 0.5, 0.5, 0.5]) - node.declare_parameter("cartesian", False) - - # Create callback group that allows execution of callbacks in parallel without restrictions - callback_group = ReentrantCallbackGroup() - - # Create MoveIt 2 interface - moveit2 = MoveIt2( - node=node, - joint_names=ur5.joint_names(), - base_link_name=ur5.base_link_name(), - end_effector_name=ur5.end_effector_name(), - group_name=ur5.MOVE_GROUP_ARM, - callback_group=callback_group, - ) - - # Spin the node in background thread(s) - executor = rclpy.executors.MultiThreadedExecutor(2) - executor.add_node(node) - executor_thread = Thread(target=executor.spin, daemon=True, args=()) - executor_thread.start() - - # Get parameters - position = node.get_parameter("position").get_parameter_value().double_array_value - quat_xyzw = node.get_parameter("quat_xyzw").get_parameter_value().double_array_value - cartesian = node.get_parameter("cartesian").get_parameter_value().bool_value - - # Move to pose - node.get_logger().info( - f"Moving to {{position: {list(position)}, quat_xyzw: {list(quat_xyzw)}}}" - ) - switch = True - while switch == True: - try: - moveit2.move_to_pose(position=[0.30, 0.1, 0.68], quat_xyzw=[0.0, 0.0, 0.0, 0.0], cartesian=False,tolerance_position = 0.1,tolerance_orientation=0.1) - moveit2.wait_until_executed() - switch = False - except : - pass - # moveit2.move_to_pose(position=[0.194, -0.43, 0.701], quat_xyzw=[0.0, 0.0, 0.0, 0.0], cartesian=False) - # moveit2.wait_until_executed() - # moveit2.move_to_pose(position=[-0.37, 0.12, 0.397], quat_xyzw=[0.0, 0.0, 0.0, 0.0], cartesian=False) - # moveit2.wait_until_executed() - rclpy.shutdown() - exit(0) - - -if __name__ == "__main__": - main() diff --git a/pymoveit2/examples/ex_servo.py b/pymoveit2/examples/ex_servo.py deleted file mode 100755 index 7ddcd46..0000000 --- a/pymoveit2/examples/ex_servo.py +++ /dev/null @@ -1,66 +0,0 @@ -#!/usr/bin/env python3 -""" -Example of using MoveIt 2 Servo to perform a circular motion. -`ros2 run pymoveit2 ex_servo.py` -""" - - -from math import cos, sin -import math, time -from copy import deepcopy -import rclpy -import tf2_ros -from rclpy.callback_groups import ReentrantCallbackGroup -from rclpy.node import Node -from geometry_msgs.msg import TwistStamped -from pymoveit2.robots import ur5 -from rclpy.qos import ( - QoSDurabilityPolicy, - QoSHistoryPolicy, - QoSProfile, - QoSReliabilityPolicy, -) - -# Initialize message based on passed arguments - - - -def main(): - rclpy.init() - - # Create node for this example - node = Node("ex_servo") - - - # Create callback group that allows execution of callbacks in parallel without restrictions - callback_group = ReentrantCallbackGroup() - __twist_pub = node.create_publisher(TwistStamped, "/servo_node/delta_twist_cmds", 10) - - def servo_circular_motion(): - """Move in a circular motion using Servo""" - __twist_msg = TwistStamped() - __twist_msg.header.stamp = node.get_clock().now().to_msg() - __twist_msg.header.frame_id = ur5.base_link_name() - __twist_msg.twist.linear.x = -1.0 - __twist_msg.twist.linear.y = 0.0 - __twist_msg.twist.linear.z = 0.0 - __twist_msg.twist.angular.x = 0.0 - __twist_msg.twist.angular.y = 0.0 - __twist_msg.twist.angular.z = 0.0 - print(__twist_msg) - __twist_pub.publish(__twist_msg) - - # Create timer for moving in a circular motion - node.create_timer(0.02, servo_circular_motion) - - # Spin the node in background thread(s) - executor = rclpy.executors.MultiThreadedExecutor(2) - executor.add_node(node) - executor.spin() - - rclpy.shutdown() - exit(0) - - -if __name__ == "__main__": - main() diff --git a/pymoveit2/examples/new_hard.py b/pymoveit2/examples/new_hard.py deleted file mode 100644 index 2a51bf2..0000000 --- a/pymoveit2/examples/new_hard.py +++ /dev/null @@ -1,406 +0,0 @@ -#!/usr/bin/env python3 - -from os import path -from threading import Thread - -import rclpy -from rclpy.callback_groups import ReentrantCallbackGroup -from rclpy.node import Node -import time -from pymoveit2 import MoveIt2 -from pymoveit2.robots import ur5 -import math -import sys -import tf2_ros -from rclpy.node import Node -from geometry_msgs.msg import TransformStamped -from scipy.spatial.transform import Rotation as R -from builtin_interfaces.msg import Time -from geometry_msgs.msg import Quaternion -from tf_transformations import quaternion_from_euler -from tf_transformations import euler_from_quaternion -import tf_transformations -from tf2_msgs.msg import TFMessage -from geometry_msgs.msg import TwistStamped - -from math import cos, sin -import math, time -from copy import deepcopy -import rclpy -import tf2_ros -from rclpy.callback_groups import ReentrantCallbackGroup -from rclpy.node import Node -from geometry_msgs.msg import TwistStamped -from pymoveit2.robots import ur5 -from rclpy.qos import ( - QoSDurabilityPolicy, - QoSHistoryPolicy, - QoSProfile, - QoSReliabilityPolicy, -) - -from ur_msgs.srv import SetIO -from controller_manager_msgs.srv import SwitchController # module call - -endf_x = 0 -endf_y = 0 -endf_z = 0 - - -pos1 = [0.35, 0.1, 0.68] -pos2 = [0.35, 0.1, 0.68] -pos3 = [0.35, 0.1, 0.68] - -def truncate(f, n): - return math.floor(f * 10 ** n) / 10 ** n -class endf(Node): - def __init__(self): - super().__init__('move_ur5') - self.tf_buffer = tf2_ros.buffer.Buffer() # buffer time used for listening transforms - self.listener = tf2_ros.TransformListener(self.tf_buffer, self) - self.br = tf2_ros.TransformBroadcaster(self) - self.switch = False - self.__contolMSwitch = self.create_client(SwitchController, "/controller_manager/switch_controller") - self.tf_topic = self.create_subscription(TFMessage, '/tf', self.tf_cb , 10) - self.twist_pub = self.create_publisher(TwistStamped, "/servo_node/delta_twist_cmds", 10) - self.callback_group = ReentrantCallbackGroup() - self.pz = 0 - self.obj_aruco = "None" - self.last_obj = "None" - - self.moveit2 = MoveIt2( - node=self, - joint_names=ur5.joint_names(), - base_link_name=ur5.base_link_name(), - end_effector_name=ur5.end_effector_name(), - group_name=ur5.MOVE_GROUP_ARM, - callback_group=self.callback_group,) - self.declare_parameter( - "joint_positions_initial", - [ - 0.0, - -2.39, - 2.4, - -3.15, - -1.58, - 3.15 - ], - ) - self.declare_parameter( - "joint_positions_positive", - [ - 1.57, - -2.39, - 2.4, - -3.15, - -1.58, - 3.15 - ], - ) - - # self.declare_parameter( - # "joint_positions_positive_back", - # [ - # 1.57, - # -2.39, - # 2.4, - # -3.15, - # -1.58, - # 3.15 - # ], - # ) - self.declare_parameter( - "joint_positions_negative", - [ - -1.57, - -2.39, - 2.4, - -3.15, - -1.58, - 3.15 - ], - ) - self.declare_parameter( - "joint_positions_back", - [ - -2.96706, - -1.6057, - 1.67552, - -3.14159, - -1.58825, - 3.14159 - ], - ) - self.declare_parameter( - "joint_positions_final_1", - [ - -0.027, - -1.803, - -1.3658, - -3.039, - -1.52, - 3.15 - ], - ) - self.declare_parameter( - "joint_positions_final_2", - [ - 0.541052, - -2.35619, - -0.698132, - -3.14159, - -1.58825, - 3.14159 - ], - ) - self.declare_parameter( - "joint_positions_final_3", - [ - -2.967, - -0.715, - 0.89, - -3.14159, - -1.58825, - 3.14159 - ], - ) - - def trajactory(self): - switchParam = SwitchController.Request() - switchParam.activate_controllers = ["scaled_joint_trajectory_controller"]# for normal use of moveit - switchParam.deactivate_controllers = ["forward_position_controller"] - switchParam.strictness = 2 - switchParam.start_asap = False - - while not self.__contolMSwitch.wait_for_service(timeout_sec=5.0): - self.get_logger().warn(f"Service control Manager is not yet available...") - self.__contolMSwitch.call_async(switchParam) - print("[CM]: Switching Complete Trajec") - - def servo_active(self): - switchParam = SwitchController.Request() - switchParam.deactivate_controllers = ["scaled_joint_trajectory_controller"] - switchParam.activate_controllers = ["forward_position_controller"] # for servoing - switchParam.strictness = 2 - switchParam.start_asap = False - - while not self.__contolMSwitch.wait_for_service(timeout_sec=5.0): - self.get_logger().warn(f"Service control Manager is not yet available...") - self.__contolMSwitch.call_async(switchParam) - print("[CM]: Switching Complete Servo") - - while not self.__contolMSwitch.wait_for_service(timeout_sec=5.0): - self.get_logger().warn(f"Service control Manager is not yet available...") - self.__contolMSwitch.call_async(switchParam) - print("[CM]: Switching Complete") - - def gripper_call(self, state): - ''' - based on the state given as i/p the service is called to activate/deactivate - pin 16 of TCP in UR5 - i/p: node, state of pin:Bool - o/p or return: response from service call - ''' - gripper_control = self.create_client(SetIO, '/io_and_status_controller/set_io') - while not gripper_control.wait_for_service(timeout_sec=1.0): - self.get_logger().info('EEF Tool service not available, waiting again...') - req = SetIO.Request() - req.fun = 1 - req.pin = 16 - req.state = float(state) - gripper_control.call_async(req) - return state - - def tf_cb(self, data): - self.obj = "None" - try: - if data.transforms[0].header.frame_id == "base_link": - if "obj" in data.transforms[0].child_frame_id: - self.obj_aruco = data.transforms[0].child_frame_id - except Exception as e: - print(e) - - def servo(self, box_no): - while rclpy.ok(): - try: - tool0 = self.tf_buffer.lookup_transform('base_link', "tool0", rclpy.time.Time()) - box49 = self.tf_buffer.lookup_transform('base_link', f"obj_{box_no}", rclpy.time.Time()) - - while rclpy.ok(): - try: - box49 = self.tf_buffer.lookup_transform('base_link', f"obj_{box_no}", rclpy.time.Time()) - tool0 = self.tf_buffer.lookup_transform('base_link', "tool0", rclpy.time.Time()) - except Exception as e: - print(e) - print(f"y ={round((box49.transform.translation.y) - (tool0.transform.translation.y),4)}") - print(f"x ={round((box49.transform.translation.x) - (tool0.transform.translation.x),4)}") - print(f"z ={round((box49.transform.translation.z) - (tool0.transform.translation.z),4)}") - joint_positions_initial = (self.get_parameter("joint_positions_initial").get_parameter_value().double_array_value) - joint_positions_final_1 = (self.get_parameter("joint_positions_final_1").get_parameter_value().double_array_value) - joint_positions_final_2 = (self.get_parameter("joint_positions_final_2").get_parameter_value().double_array_value) - joint_positions_final_3 = (self.get_parameter("joint_positions_final_3").get_parameter_value().double_array_value) - joint_positions_back = (self.get_parameter("joint_positions_back").get_parameter_value().double_array_value) - - - # ARM Move To BOX - - if round((box49.transform.translation.y) - (tool0.transform.translation.y),4) > 0.01 or round((box49.transform.translation.x) - (tool0.transform.translation.x),4) > 0.01 or round((box49.transform.translation.z) - (tool0.transform.translation.z),4) > 0.01: - __twist_msg = TwistStamped() - __twist_msg.header.stamp = self.get_clock().now().to_msg() - __twist_msg.header.frame_id = ur5.base_link_name() - __twist_msg.twist.linear.y = round((box49.transform.translation.y) - (tool0.transform.translation.y),4) *2 - __twist_msg.twist.linear.x = round((box49.transform.translation.x) - (tool0.transform.translation.x),4) *2 - __twist_msg.twist.linear.z = round((box49.transform.translation.z) - (tool0.transform.translation.z),4) *2 - self.twist_pub.publish(__twist_msg) - -# Gripper ON - else : - print("trying to attach") - self.gripper_call(1) - break - - while rclpy.ok(): - try: - box49 = self.tf_buffer.lookup_transform('base_link', f"obj_{box_no}", rclpy.time.Time()) - tool0 = self.tf_buffer.lookup_transform('base_link', "tool0", rclpy.time.Time()) - roll , pitch , yaw = euler_from_quaternion([box49.transform.rotation.x, box49.transform.rotation.y, box49.transform.rotation.z, box49.transform.rotation.w]) - print(f"x = {round(tool0.transform.translation.x,2)}") - print(f"y = {round(tool0.transform.translation.y,2)}") - - except Exception as e: - print(e) - - -# ARM Adding Buffer POSS & Drop Location - - - if (round(yaw,2) >= 1.39 and round(yaw,2)<=1.91) and (round(tool0.transform.translation.x,2) > 0.23): - __twist_msg = TwistStamped() - __twist_msg.header.stamp = self.get_clock().now().to_msg() - __twist_msg.header.frame_id = ur5.base_link_name() - __twist_msg.twist.linear.z = 0.1 - __twist_msg.twist.linear.x = -0.4 - self.twist_pub.publish(__twist_msg) - elif (round(yaw,2) >= 1.39 and round(yaw,2)<=1.91) and (round(tool0.transform.translation.x,2) <= 0.23): - self.trajactory() - self.moveit2.move_to_configuration(joint_positions_final_1) - self.moveit2.wait_until_executed() - self.moveit2.move_to_configuration(joint_positions_final_1) - self.moveit2.wait_until_executed() - break - - elif round(yaw,2) >= -0.17 and round(yaw,2) <=0.52 and (round(tool0.transform.translation.y,2) < -0.26): - __twist_msg = TwistStamped() - __twist_msg.header.stamp = self.get_clock().now().to_msg() - __twist_msg.header.frame_id = ur5.base_link_name() - __twist_msg.twist.linear.z = 0.04 - __twist_msg.twist.linear.y = 0.2 - __twist_msg.twist.linear.x = -0.2 - self.twist_pub.publish(__twist_msg) - elif round(yaw,2) >= -0.17 and round(yaw,2) <=0.52 and (round(tool0.transform.translation.y,2) >= -0.26): - self.trajactory() - self.moveit2.move_to_configuration(joint_positions_back) - self.moveit2.wait_until_executed() - self.moveit2.move_to_configuration(joint_positions_back) - self.moveit2.wait_until_executed() - self.moveit2.move_to_configuration(joint_positions_final_3) - self.moveit2.wait_until_executed() - self.moveit2.move_to_configuration(joint_positions_final_3) - self.moveit2.wait_until_executed() - break - - elif round(yaw) ==((round(yaw,2) >= 2.79 and round(yaw,2)<= 3.14)) and (round(tool0.transform.translation.y,2) < 0.25): - print("----------------") - __twist_msg = TwistStamped() - __twist_msg.header.stamp = self.get_clock().now().to_msg() - __twist_msg.header.frame_id = ur5.base_link_name() - __twist_msg.twist.linear.z = 0.1 - __twist_msg.twist.linear.y = -0.2 - self.twist_pub.publish(__twist_msg) - elif round(yaw) == ((round(yaw,2) >= 2.79 and round(yaw,2)<= 3.14)) and (round(tool0.transform.translation.y,2) >= 0.25): - print("========") - - self.trajactory() - self.moveit2.move_to_configuration(joint_positions_final_2) - self.moveit2.wait_until_executed() - self.moveit2.move_to_configuration(joint_positions_final_2) - self.moveit2.wait_until_executed() - break - - - - - -# Gripper OFF - - print("trying to detach") - self.gripper_call(0) - break - - except Exception as e: - print(e) - - -def main(): - - rclpy.init() - enftf = endf() - - executor = rclpy.executors.MultiThreadedExecutor(2) - executor.add_node(enftf) - executor_thread = Thread(target=executor.spin, daemon=True, args=()) - executor_thread.start() - - joint_positions_initial = (enftf.get_parameter("joint_positions_initial").get_parameter_value().double_array_value) - joint_positions_positive = (enftf.get_parameter("joint_positions_positive").get_parameter_value().double_array_value) - joint_positions_negative = (enftf.get_parameter("joint_positions_negative").get_parameter_value().double_array_value) - joint_positions_back = (enftf.get_parameter("joint_positions_back").get_parameter_value().double_array_value) - - - - while rclpy.ok(): - try: - obj = enftf.obj_aruco[4:] - print(obj) - box = enftf.tf_buffer.lookup_transform('base_link', enftf.obj_aruco, rclpy.time.Time()) - roll , pitch , yaw = euler_from_quaternion([box.transform.rotation.x, box.transform.rotation.y, box.transform.rotation.z, box.transform.rotation.w]) - print(round(yaw,2)) - - if enftf.last_obj == obj: - pass - - elif round(yaw,2) >= -0.17 and round(yaw,2) <=0.52: - enftf.trajactory() - enftf.moveit2.move_to_configuration(joint_positions_negative) - enftf.moveit2.wait_until_executed() - enftf.servo_active() - enftf.servo(obj) - enftf.trajactory() - enftf.moveit2.move_to_configuration(joint_positions_back) - enftf.moveit2.wait_until_executed() - enftf.moveit2.move_to_configuration(joint_positions_initial) - enftf.moveit2.wait_until_executed() - - elif (round(yaw,2) >= 2.79 and round(yaw,2)<= 3.14) : - enftf.trajactory() - enftf.moveit2.move_to_configuration(joint_positions_positive) - enftf.moveit2.wait_until_executed() - enftf.servo_active() - enftf.servo(obj) - enftf.trajactory() - enftf.moveit2.move_to_configuration(joint_positions_initial) - enftf.moveit2.wait_until_executed() - - elif round(yaw,2) >= 1.39 and round(yaw,2)<=1.91: - enftf.servo_active() - enftf.servo(obj) - enftf.trajactory() - enftf.moveit2.move_to_configuration(joint_positions_initial) - enftf.moveit2.wait_until_executed() - enftf.last_obj = obj - - except Exception as e: - print(e) - -if __name__ == "__main__": - main() \ No newline at end of file diff --git a/pymoveit2/examples/task1b.py b/pymoveit2/examples/task1b.py deleted file mode 100755 index 16b1721..0000000 --- a/pymoveit2/examples/task1b.py +++ /dev/null @@ -1,384 +0,0 @@ -#!/usr/bin/env python3 - -from os import path -from threading import Thread - -import rclpy -from rclpy.callback_groups import ReentrantCallbackGroup -from rclpy.node import Node -import time -from pymoveit2 import MoveIt2 -from pymoveit2.robots import ur5 -import math -import sys -import tf2_ros -from rclpy.node import Node -from geometry_msgs.msg import TransformStamped -from scipy.spatial.transform import Rotation as R -from builtin_interfaces.msg import Time -from geometry_msgs.msg import Quaternion -from tf_transformations import quaternion_from_euler -import tf_transformations -from geometry_msgs.msg import TwistStamped -from linkattacher_msgs.srv import AttachLink -from linkattacher_msgs.srv import DetachLink -from math import cos, sin -import math, time -from copy import deepcopy -import rclpy -import tf2_ros -from rclpy.callback_groups import ReentrantCallbackGroup -from rclpy.node import Node -from geometry_msgs.msg import TwistStamped -from pymoveit2.robots import ur5 -from rclpy.qos import ( - QoSDurabilityPolicy, - QoSHistoryPolicy, - QoSProfile, - QoSReliabilityPolicy, -) - -RACK_MESH = path.join( - path.dirname(path.realpath(__file__)), "assets", "rack1.stl" -) -BASE_MESH = path.join( - path.dirname(path.realpath(__file__)), "assets", "arm_base_40.stl" -) - -endf_x = 0 -endf_y = 0 -endf_z = 0 - - -pos1 = [0.35, 0.1, 0.68] -pos2 = [0.35, 0.1, 0.68] -pos3 = [0.35, 0.1, 0.68] - -def truncate(f, n): - return math.floor(f * 10 ** n) / 10 ** n -class endf(Node): - def __init__(self): - super().__init__('move_ur5') - self.tf_buffer = tf2_ros.buffer.Buffer() # buffer time used for listening transforms - self.listener = tf2_ros.TransformListener(self.tf_buffer, self) - self.br = tf2_ros.TransformBroadcaster(self) - self.switch = False - self.twist_pub = self.create_publisher(TwistStamped, "/servo_node/delta_twist_cmds", 10) - self.gripper_control = self.create_client(AttachLink, '/GripperMagnetON') - self.gripper_control_off = self.create_client(DetachLink, '/GripperMagnetOFF') - self.callback_group = ReentrantCallbackGroup() - self.pz = 0 - - self.moveit2 = MoveIt2( - node=self, - joint_names=ur5.joint_names(), - base_link_name=ur5.base_link_name(), - end_effector_name=ur5.end_effector_name(), - group_name=ur5.MOVE_GROUP_ARM, - callback_group=self.callback_group,) - self.declare_parameter( - "joint_positions_initial", - [ - 0.0, - -2.39, - 2.4, - -3.15, - -1.58, - 3.15 - ], - ) - self.declare_parameter( - "joint_positions_positive", - [ - 1.57, - -2.39, - 2.4, - -3.15, - -1.58, - 3.15 - ], - ) - self.declare_parameter( - "joint_positions_negative", - [ - -1.57, - -2.39, - 2.4, - -3.15, - -1.58, - 3.15 - ], - ) - self.declare_parameter( - "joint_positions_final_1", - [ - 0.0, - -2.07694, - -0.418879, - -3.26377, - -1.41372, - 3.05432 - ], - ) - self.declare_parameter( - "joint_positions_final_2", - [ - 0.541052, - -2.35619, - -0.698132, - -3.14159, - -1.58825, - 3.14159 - ], - ) - self.declare_parameter( - "joint_positions_final_3", - [ - 0.244346, - -2.16921, - -0.820305, - -3.14159, - -1.58825, - 3.14159 - ], - ) - - def servo(self, box_no): - while rclpy.ok(): - try: - tool0 = self.tf_buffer.lookup_transform('base_link', "tool0", rclpy.time.Time()) - box49 = self.tf_buffer.lookup_transform('base_link', f"obj_{box_no}", rclpy.time.Time()) - - - - # while rclpy.ok(): - # try: - # tool0 = enftf.tf_buffer.lookup_transform('base_link', "tool0", rclpy.time.Time()) - # box49 = enftf.tf_buffer.lookup_transform('base_link', "obj_49", rclpy.time.Time()) - # except: - # pass - - - # roll_box , pitch_box, yaw_box = tf_transformations.euler_from_quaternion([box49.transform.rotation.x, box49.transform.rotation.y, box49.transform.rotation.z, box49.transform.rotation.w]) - # print(yaw_box) - # roll_tool , pitch_tool, yaw_tool = tf_transformations.euler_from_quaternion([tool0.transform.rotation.x, tool0.transform.rotation.y, tool0.transform.rotation.z, tool0.transform.rotation.w]) - - # print(f"y ={round((yaw_tool) - (yaw_box),4)}") - - # if round((yaw_tool) - (yaw_box),4) < 0.003: - # __twist_msg = TwistStamped() - # __twist_msg.header.stamp = enftf.get_clock().now().to_msg() - # __twist_msg.header.frame_id = ur5.base_link_name() - # __twist_msg.twist.angular.z = round((yaw_tool) - (yaw_box),4) *2 - # __twist_pub.publish(__twist_msg) - # else: - # break - - - - - while rclpy.ok(): - try: - box49 = self.tf_buffer.lookup_transform('base_link', f"obj_{box_no}", rclpy.time.Time()) - tool0 = self.tf_buffer.lookup_transform('base_link', "tool0", rclpy.time.Time()) - except: - pass - print(f"y ={round((box49.transform.translation.y) - (tool0.transform.translation.y),4)}") - print(f"x ={round((box49.transform.translation.x) - (tool0.transform.translation.x),4)}") - print(f"z ={round((box49.transform.translation.z) - (tool0.transform.translation.z),4)}") - joint_positions_initial = (self.get_parameter("joint_positions_initial").get_parameter_value().double_array_value) - joint_positions_final_1 = (self.get_parameter("joint_positions_final_1").get_parameter_value().double_array_value) - joint_positions_final_2 = (self.get_parameter("joint_positions_final_2").get_parameter_value().double_array_value) - joint_positions_final_3 = (self.get_parameter("joint_positions_final_3").get_parameter_value().double_array_value) - - if round((box49.transform.translation.y) - (tool0.transform.translation.y),4) > 0.003 or round((box49.transform.translation.x) - (tool0.transform.translation.x),4) > 0.003 or round((box49.transform.translation.z) - (tool0.transform.translation.z),4) > 0.003: - __twist_msg = TwistStamped() - __twist_msg.header.stamp = self.get_clock().now().to_msg() - __twist_msg.header.frame_id = ur5.base_link_name() - __twist_msg.twist.linear.y = round((box49.transform.translation.y) - (tool0.transform.translation.y),4) *2 - __twist_msg.twist.linear.x = round((box49.transform.translation.x) - (tool0.transform.translation.x),4) *2 - __twist_msg.twist.linear.z = round((box49.transform.translation.z) - (tool0.transform.translation.z),4) *2 - self.twist_pub.publish(__twist_msg) - else : - while not self.gripper_control.wait_for_service(timeout_sec=1.0): - self.get_logger().info('EEF service not available, waiting again...') - print("tryiing to attach") - req = AttachLink.Request() - req.model1_name = f'box{box_no}' - req.link1_name = 'link' - req.model2_name = 'ur5' - req.link2_name = 'wrist_3_link' - self.gripper_control.call_async(req) - break - while rclpy.ok(): - try: - box49 = self.tf_buffer.lookup_transform('base_link', f"obj_{box_no}", rclpy.time.Time()) - tool0 = self.tf_buffer.lookup_transform('base_link', "tool0", rclpy.time.Time()) - print(f"x = {round(tool0.transform.translation.x,2)}") - print(f"y = {round(tool0.transform.translation.x,2)}") - - except: - pass - if box_no == str(1) and (round(tool0.transform.translation.x,2) > 0.21): - __twist_msg = TwistStamped() - __twist_msg.header.stamp = self.get_clock().now().to_msg() - __twist_msg.header.frame_id = ur5.base_link_name() - __twist_msg.twist.linear.z = -0.04 - __twist_msg.twist.linear.x = -0.4 - self.twist_pub.publish(__twist_msg) - elif box_no == str(49) and (round(tool0.transform.translation.y,2) > 0.21): - __twist_msg = TwistStamped() - __twist_msg.header.stamp = self.get_clock().now().to_msg() - __twist_msg.header.frame_id = ur5.base_link_name() - __twist_msg.twist.linear.z = 0.04 - __twist_msg.twist.linear.y = -0.2 - self.twist_pub.publish(__twist_msg) - elif box_no == str(3) and (round(tool0.transform.translation.y,2) < -0.22): - __twist_msg = TwistStamped() - __twist_msg.header.stamp = self.get_clock().now().to_msg() - __twist_msg.header.frame_id = ur5.base_link_name() - __twist_msg.twist.linear.z = 0.04 - __twist_msg.twist.linear.y = 0.2 - __twist_msg.twist.linear.x = -0.2 - self.twist_pub.publish(__twist_msg) - else: - break - if self.pz == 0: - self.moveit2.move_to_configuration(joint_positions_initial) - self.moveit2.wait_until_executed() - self.moveit2.move_to_configuration(joint_positions_final_1) - self.moveit2.wait_until_executed() - self.moveit2.move_to_configuration(joint_positions_final_1) - self.moveit2.wait_until_executed() - - self.pz = self.pz +1 - elif self.pz == 1: - self.moveit2.move_to_configuration(joint_positions_initial) - self.moveit2.wait_until_executed() - self.moveit2.move_to_configuration(joint_positions_final_2) - self.moveit2.wait_until_executed() - self.moveit2.move_to_configuration(joint_positions_final_2) - self.moveit2.wait_until_executed() - - self.pz = self.pz +1 - else: - self.moveit2.move_to_configuration(joint_positions_initial) - self.moveit2.wait_until_executed() - self.moveit2.move_to_configuration(joint_positions_final_3) - self.moveit2.wait_until_executed() - self.moveit2.move_to_configuration(joint_positions_final_3) - self.moveit2.wait_until_executed() - - - while not self.gripper_control.wait_for_service(timeout_sec=1.0): - self.enftf.get_logger().info('EEF service not available, waiting again...') - print("tryiing to attach") - req = DetachLink.Request() - req.model1_name = f'box{box_no}' - req.link1_name = 'link' - req.model2_name = 'ur5' - req.link2_name = 'wrist_3_link' - self.gripper_control_off.call_async(req) - break - - except: - pass - - -def main(): - rclpy.init() - enftf = endf() - - - twist_msg = TwistStamped() - #twist_msg.header.frame_id = "auto" - - #node = Node("move_ur5") - - executor = rclpy.executors.MultiThreadedExecutor(2) - executor.add_node(enftf) - executor_thread = Thread(target=executor.spin, daemon=True, args=()) - executor_thread.start() - - filepath1 = RACK_MESH - filepath2 = BASE_MESH - position_r1 = [0.52,0.05,0.17] - quat_xyzw_r1 = [0.00,0.00,0.00,0.00] - position_r2 = [0.25,-0.62,0.17] - quat_xyzw_r2 = [0, 0, 0.7068252, 0.7073883] - position_r3 = [0.25,0.74,0.17] - quat_xyzw_r3 = [0, 0, -0.7068252, 0.7073883] - # position_b1 = [0.25,-0.57,0.18] - # quat_xyzw_b1 = [0, 0, 0.7068252, 0.7073883] - # position_b2 = [0.25,-0.57,0.55] - # quat_xyzw_b2 = [-0.0000221, 0.001126, 0.6931103, 0.7208306] - # position_b3 = [0.45,0.06,0.55] - # quat_xyzw_b3 = [0, 0, 0.9999997, 0.0007963] - rack_pos = [position_r1,position_r2,position_r3] - rack_quat = [quat_xyzw_r1,quat_xyzw_r2,quat_xyzw_r3] - - pos_b = [-0.20, 0, -0.26] - quat_b= [0, 0, 1, 0.000796327] - base_pos=[pos_b,quat_b] - # box_pos = [position_b1,position_b2,position_b3] - # box_quat = [quat_xyzw_b1,quat_xyzw_b2,quat_xyzw_b3] - #check if exists - - # if not path.exists(filepath1): - # node.get_logger().error(f"File '{filepath1}' does not exist") - # rclpy.shutdown() - # exit(1) - # if not path.exists(filepath2): - # node.get_logger().error(f"File '{filepath2}' does not exist") - # rclpy.shutdown() - # exit(1) - - mesh_id1 = path.basename(filepath1).split(".")[0] - mesh_id2 = path.basename(filepath2).split(".")[0] - mesh_box_id = ["box_1","box_2","box_3"] - mesh_rack_id = ["rack_1","rack_2","rack_3"] - joint_positions_initial = (enftf.get_parameter("joint_positions_initial").get_parameter_value().double_array_value) - joint_positions_positive = (enftf.get_parameter("joint_positions_positive").get_parameter_value().double_array_value) - joint_positions_negative = (enftf.get_parameter("joint_positions_negative").get_parameter_value().double_array_value) - - - for i in range(len(position_r1)): - time.sleep(2) - enftf.moveit2.add_collision_mesh( - filepath=filepath1, id=mesh_rack_id[i], position=rack_pos[i], quat_xyzw=rack_quat[i], frame_id=ur5.base_link_name() - ) - time.sleep(2) - # print(filepath1) - # moveit2.add_collision_mesh( - # filepath=filepath2, id=mesh_box_id[i], position=box_pos[i], quat_xyzw=box_quat[i], frame_id=ur5.base_link_name() - # ) - # time.sleep(2) - enftf.moveit2.add_collision_mesh( - filepath=filepath2, id="base", position=base_pos[0], quat_xyzw=base_pos[1], frame_id=ur5.base_link_name()) - enftf.servo(str(1)) - - enftf.moveit2.move_to_configuration(joint_positions_initial) - enftf.moveit2.wait_until_executed() - enftf.moveit2.move_to_configuration(joint_positions_positive) - enftf.moveit2.wait_until_executed() - enftf.servo(str(49)) - - enftf.moveit2.move_to_configuration(joint_positions_initial) - enftf.moveit2.wait_until_executed() - enftf.moveit2.move_to_configuration(joint_positions_negative) - enftf.moveit2.wait_until_executed() - enftf.servo(str(3)) - - - - - - - - - - - - -if __name__ == "__main__": - main() diff --git a/pymoveit2/examples/task4b.py b/pymoveit2/examples/task4b.py deleted file mode 100644 index e23fdef..0000000 --- a/pymoveit2/examples/task4b.py +++ /dev/null @@ -1,384 +0,0 @@ -#!/usr/bin/env python3 - -from os import path -from threading import Thread - -import rclpy -from rclpy.callback_groups import ReentrantCallbackGroup -from rclpy.node import Node -import time -from pymoveit2 import MoveIt2 -from pymoveit2.robots import ur5 -import math -import sys -import tf2_ros -from rclpy.node import Node -from geometry_msgs.msg import TransformStamped -from scipy.spatial.transform import Rotation as R -from builtin_interfaces.msg import Time -from geometry_msgs.msg import Quaternion -from tf_transformations import quaternion_from_euler -from tf_transformations import euler_from_quaternion -import tf_transformations -from tf2_msgs.msg import TFMessage -from geometry_msgs.msg import TwistStamped -from linkattacher_msgs.srv import AttachLink -from linkattacher_msgs.srv import DetachLink -from math import cos, sin -import math, time -from copy import deepcopy -import rclpy -import tf2_ros -from rclpy.callback_groups import ReentrantCallbackGroup -from rclpy.node import Node -from geometry_msgs.msg import TwistStamped -from pymoveit2.robots import ur5 -from rclpy.qos import ( - QoSDurabilityPolicy, - QoSHistoryPolicy, - QoSProfile, - QoSReliabilityPolicy, -) -from ur_msgs.srv import SetIO -from controller_manager_msgs.srv import SwitchController # module call - - - -RACK_MESH = path.join( - path.dirname(path.realpath(__file__)), "assets", "rack1.stl" -) -BASE_MESH = path.join( - path.dirname(path.realpath(__file__)), "assets", "arm_base_40.stl" -) - -endf_x = 0 -endf_y = 0 -endf_z = 0 - - -pos1 = [0.35, 0.1, 0.68] -pos2 = [0.35, 0.1, 0.68] -pos3 = [0.35, 0.1, 0.68] - -def truncate(f, n): - return math.floor(f * 10 ** n) / 10 ** n -class endf(Node): - def __init__(self): - super().__init__('move_ur5') - self.tf_buffer = tf2_ros.buffer.Buffer() # buffer time used for listening transforms - self.listener = tf2_ros.TransformListener(self.tf_buffer, self) - self.br = tf2_ros.TransformBroadcaster(self) - self.switch = False - self.tf_topic = self.create_subscription(TFMessage, '/tf', self.tf_cb , 10) - self.twist_pub = self.create_publisher(TwistStamped, "/servo_node/delta_twist_cmds", 10) - self.callback_group = ReentrantCallbackGroup() - self.pz = 0 - self.obj_aruco = "None" - self.last_obj = "None" - self.__contolMSwitch = self.create_client(SwitchController, "/controller_manager/switch_controller") - - self.moveit2 = MoveIt2( - node=self, - joint_names=ur5.joint_names(), - base_link_name=ur5.base_link_name(), - end_effector_name=ur5.end_effector_name(), - group_name=ur5.MOVE_GROUP_ARM, - callback_group=self.callback_group,) - self.declare_parameter( - "joint_positions_initial", - [ - 0.0, - -2.398, - 2.43, - -3.15, - -1.58, - 3.15 - ], - ) - self.declare_parameter( - "joint_positions_positive", - [ - 1.57, - -2.39, - 2.4, - -3.15, - -1.58, - 3.15 - ], - ) - self.declare_parameter( - "joint_positions_negative", - [ - -1.57, - -2.39, - 2.4, - -3.15, - -1.58, - 3.15 - ], - ) - self.declare_parameter( - "joint_positions_final_1", - [ - -0.027, - -1.803, - -1.3658, - -3.039, - -1.52, - 3.15 - ], - ) - self.declare_parameter( - "joint_positions_final_2", - [ - 0.541052, - -2.35619, - -0.698132, - -3.14159, - -1.58825, - 3.14159 - ], - ) - self.declare_parameter( - "joint_positions_final_3", - [ - 0.244346, - -2.16921, - -0.820305, - -3.14159, - -1.58825, - 3.14159 - ], - ) - - def trajactory(self): - switchParam = SwitchController.Request() - switchParam.activate_controllers = ["scaled_joint_trajectory_controller"] # for normal use of moveit - switchParam.strictness = 2 - switchParam.start_asap = False - - while not self.__contolMSwitch.wait_for_service(timeout_sec=5.0): - self.get_logger().warn(f"Service control Manager is not yet available...") - self.__contolMSwitch.call_async(switchParam) - print("[CM]: Switching Complete") - - def servo_active(self): - switchParam = SwitchController.Request() - switchParam.deactivate_controllers = ["forward_position_controller"] # for servoing - switchParam.strictness = 2 - switchParam.start_asap = False - - while not self.__contolMSwitch.wait_for_service(timeout_sec=5.0): - self.get_logger().warn(f"Service control Manager is not yet available...") - self.__contolMSwitch.call_async(switchParam) - print("[CM]: Switching Complete") - - def gripper_call(self, state): - ''' - based on the state given as i/p the service is called to activate/deactivate - pin 16 of TCP in UR5 - i/p: node, state of pin:Bool - o/p or return: response from service call - ''' - gripper_control = self.create_client(SetIO, '/io_and_status_controller/set_io') - while not gripper_control.wait_for_service(timeout_sec=1.0): - self.get_logger().info('EEF Tool service not available, waiting again...') - req = SetIO.Request() - req.fun = 1 - req.pin = 16 - req.state = float(state) - gripper_control.call_async(req) - return state - - def tf_cb(self, data): - self.obj = "None" - try: - if data.transforms[0].header.frame_id == "base_link": - if "obj" in data.transforms[0].child_frame_id: - self.obj_aruco = data.transforms[0].child_frame_id - except Exception as e: - print(e) - - def servo(self, box_no): - while rclpy.ok(): - try: - tool0 = self.tf_buffer.lookup_transform('base_link', "tool0", rclpy.time.Time()) - box49 = self.tf_buffer.lookup_transform('base_link', f"obj_{box_no}", rclpy.time.Time()) - - while rclpy.ok(): - try: - box49 = self.tf_buffer.lookup_transform('base_link', f"obj_{box_no}", rclpy.time.Time()) - tool0 = self.tf_buffer.lookup_transform('base_link', "tool0", rclpy.time.Time()) - except Exception as e: - print(e) - print(f"y ={round((box49.transform.translation.y) - (tool0.transform.translation.y),4)}") - print(f"x ={round((box49.transform.translation.x) - (tool0.transform.translation.x),4)}") - print(f"z ={round((box49.transform.translation.z) - (tool0.transform.translation.z),4)}") - joint_positions_initial = (self.get_parameter("joint_positions_initial").get_parameter_value().double_array_value) - joint_positions_final_1 = (self.get_parameter("joint_positions_final_1").get_parameter_value().double_array_value) - joint_positions_final_2 = (self.get_parameter("joint_positions_final_2").get_parameter_value().double_array_value) - joint_positions_final_3 = (self.get_parameter("joint_positions_final_3").get_parameter_value().double_array_value) - - if round((box49.transform.translation.y) - (tool0.transform.translation.y),4) > 0.004 or round((box49.transform.translation.x) - (tool0.transform.translation.x),4) > 0.004 or round((box49.transform.translation.z) - (tool0.transform.translation.z),4) > 0.004: - __twist_msg = TwistStamped() - __twist_msg.header.stamp = self.get_clock().now().to_msg() - __twist_msg.header.frame_id = ur5.base_link_name() - __twist_msg.twist.linear.y = round((box49.transform.translation.y) - (tool0.transform.translation.y),4) *2 - __twist_msg.twist.linear.x = round((box49.transform.translation.x) - (tool0.transform.translation.x),4) *2 - __twist_msg.twist.linear.z = round((box49.transform.translation.z) - (tool0.transform.translation.z),4) *2 - self.twist_pub.publish(__twist_msg) - else : - print("trying to attach") - self.gripper_call(1) - break - while rclpy.ok(): - try: - box49 = self.tf_buffer.lookup_transform('base_link', f"obj_{box_no}", rclpy.time.Time()) - tool0 = self.tf_buffer.lookup_transform('base_link', "tool0", rclpy.time.Time()) - roll , pitch , yaw = euler_from_quaternion([box49.transform.rotation.x, box49.transform.rotation.y, box49.transform.rotation.z, box49.transform.rotation.w]) - print(f"x = {round(tool0.transform.translation.x,2)}") - print(f"y = {round(tool0.transform.translation.x,2)}") - - except Exception as e: - print(e) - if (round(yaw) == 2 or round(yaw) == 1) and (round(tool0.transform.translation.x,2) > 0.23): - __twist_msg = TwistStamped() - __twist_msg.header.stamp = self.get_clock().now().to_msg() - __twist_msg.header.frame_id = ur5.base_link_name() - __twist_msg.twist.linear.z = -0.08 - __twist_msg.twist.linear.x = -0.4 - self.twist_pub.publish(__twist_msg) - elif round(yaw) == 3 and (round(tool0.transform.translation.y,2) > 0.21): - __twist_msg = TwistStamped() - __twist_msg.header.stamp = self.get_clock().now().to_msg() - __twist_msg.header.frame_id = ur5.base_link_name() - __twist_msg.twist.linear.z = 0.04 - __twist_msg.twist.linear.y = -0.2 - self.twist_pub.publish(__twist_msg) - elif round(yaw) == 0 and (round(tool0.transform.translation.y,2) < -0.22): - __twist_msg = TwistStamped() - __twist_msg.header.stamp = self.get_clock().now().to_msg() - __twist_msg.header.frame_id = ur5.base_link_name() - __twist_msg.twist.linear.z = 0.04 - __twist_msg.twist.linear.y = 0.2 - __twist_msg.twist.linear.x = -0.2 - self.twist_pub.publish(__twist_msg) - else: - break - if self.pz == 0: - # self.moveit2.move_to_configuration(joint_positions_initial) - # self.moveit2.wait_until_executed() - self.trajactory() - self.moveit2.move_to_configuration(joint_positions_final_1) - self.moveit2.wait_until_executed() - self.moveit2.move_to_configuration(joint_positions_final_1) - self.moveit2.wait_until_executed() - - self.pz = self.pz +1 - elif self.pz == 1: - # self.moveit2.move_to_configuration(joint_positions_initial) - # self.moveit2.wait_until_executed() - self.trajactory() - self.moveit2.move_to_configuration(joint_positions_final_2) - self.moveit2.wait_until_executed() - self.moveit2.move_to_configuration(joint_positions_final_2) - self.moveit2.wait_until_executed() - - self.pz = self.pz +1 - else: - # self.moveit2.move_to_configuration(joint_positions_initial) - # self.moveit2.wait_until_executed() - self.trajactory() - self.moveit2.move_to_configuration(joint_positions_final_3) - self.moveit2.wait_until_executed() - self.moveit2.move_to_configuration(joint_positions_final_3) - self.moveit2.wait_until_executed() - - print("trying to detach") - self.gripper_call(0) - break - - except Exception as e: - print(e) - - -def main(): - - rclpy.init() - enftf = endf() - - executor = rclpy.executors.MultiThreadedExecutor(2) - executor.add_node(enftf) - executor_thread = Thread(target=executor.spin, daemon=True, args=()) - executor_thread.start() - - filepath1 = RACK_MESH - filepath2 = BASE_MESH - position_r1 = [0.52,0.05,0.17] - quat_xyzw_r1 = [0.00,0.00,0.00,0.00] - position_r2 = [0.25,-0.62,0.17] - quat_xyzw_r2 = [0, 0, 0.7068252, 0.7073883] - position_r3 = [0.25,0.74,0.17] - quat_xyzw_r3 = [0, 0, -0.7068252, 0.7073883] - - rack_pos = [position_r1,position_r2,position_r3] - rack_quat = [quat_xyzw_r1,quat_xyzw_r2,quat_xyzw_r3] - - pos_b = [-0.20, 0, -0.26] - quat_b= [0, 0, 1, 0.000796327] - base_pos=[pos_b,quat_b] - mesh_rack_id = ["rack_1","rack_2","rack_3"] - joint_positions_initial = (enftf.get_parameter("joint_positions_initial").get_parameter_value().double_array_value) - joint_positions_positive = (enftf.get_parameter("joint_positions_positive").get_parameter_value().double_array_value) - joint_positions_negative = (enftf.get_parameter("joint_positions_negative").get_parameter_value().double_array_value) - - - for i in range(len(position_r1)): - time.sleep(2) - enftf.moveit2.add_collision_mesh( - filepath=filepath1, id=mesh_rack_id[i], position=rack_pos[i], quat_xyzw=rack_quat[i], frame_id=ur5.base_link_name() - ) - time.sleep(2) - - enftf.moveit2.add_collision_mesh( - filepath=filepath2, id="base", position=base_pos[0], quat_xyzw=base_pos[1], frame_id=ur5.base_link_name()) - - while rclpy.ok(): - try: - obj = enftf.obj_aruco[4:] - print(obj) - box = enftf.tf_buffer.lookup_transform('base_link', enftf.obj_aruco, rclpy.time.Time()) - roll , pitch , yaw = euler_from_quaternion([box.transform.rotation.x, box.transform.rotation.y, box.transform.rotation.z, box.transform.rotation.w]) - print(yaw) - if enftf.last_obj == obj: - pass - elif round(yaw) == 0: - enftf.trajactory() - enftf.moveit2.move_to_configuration(joint_positions_negative) - enftf.moveit2.wait_until_executed() - enftf.servo_active() - enftf.servo(obj) - enftf.trajactory() - enftf.moveit2.move_to_configuration(joint_positions_initial) - enftf.moveit2.wait_until_executed() - elif round(yaw) == 3: - enftf.moveit2.move_to_configuration(joint_positions_positive) - enftf.moveit2.wait_until_executed() - enftf.servo_active() - enftf.servo(obj) - enftf.trajactory() - enftf.moveit2.move_to_configuration(joint_positions_initial) - enftf.moveit2.wait_until_executed() - elif round(yaw) == 2 or round(yaw) == 1: - enftf.servo_active() - enftf.servo(obj) - enftf.trajactory() - enftf.moveit2.move_to_configuration(joint_positions_initial) - enftf.moveit2.wait_until_executed() - enftf.last_obj = obj - - except Exception as e: - print(e) - -if __name__ == "__main__": - main() diff --git a/pymoveit2/examples/task4b_new.py b/pymoveit2/examples/task4b_new.py deleted file mode 100755 index 0a962db..0000000 --- a/pymoveit2/examples/task4b_new.py +++ /dev/null @@ -1,428 +0,0 @@ -#!/usr/bin/env python3 - -from os import path -from threading import Thread - -import rclpy -from rclpy.callback_groups import ReentrantCallbackGroup -from rclpy.node import Node -import time -from pymoveit2 import MoveIt2 -from pymoveit2.robots import ur5 -import math -import sys -import tf2_ros -from rclpy.node import Node -from geometry_msgs.msg import TransformStamped -from scipy.spatial.transform import Rotation as R -from builtin_interfaces.msg import Time -from geometry_msgs.msg import Quaternion -from tf_transformations import quaternion_from_euler -from tf_transformations import euler_from_quaternion -import tf_transformations -from tf2_msgs.msg import TFMessage -from geometry_msgs.msg import TwistStamped -from linkattacher_msgs.srv import AttachLink -from linkattacher_msgs.srv import DetachLink -from math import cos, sin -import math, time -from copy import deepcopy -import rclpy -import tf2_ros -from rclpy.callback_groups import ReentrantCallbackGroup -from rclpy.node import Node -from geometry_msgs.msg import TwistStamped -from pymoveit2.robots import ur5 -from arm_picky.srv import ArmNew -from rclpy.qos import ( - QoSDurabilityPolicy, - QoSHistoryPolicy, - QoSProfile, - QoSReliabilityPolicy, -) - -from ur_msgs.srv import SetIO -from controller_manager_msgs.srv import SwitchController # module call - - -RACK_MESH = path.join( - path.dirname(path.realpath(__file__)), "assets", "rack1.stl" -) -BASE_MESH = path.join( - path.dirname(path.realpath(__file__)), "assets", "arm_base_40.stl" -) - -endf_x = 0 -endf_y = 0 -endf_z = 0 - - -pos1 = [0.35, 0.1, 0.68] -pos2 = [0.35, 0.1, 0.68] -pos3 = [0.35, 0.1, 0.68] - -def truncate(f, n): - return math.floor(f * 10 ** n) / 10 ** n - -class endf(Node): - def __init__(self): - super().__init__('move_ur5') - self.tf_buffer = tf2_ros.buffer.Buffer() # buffer time used for listening transforms - self.listener = tf2_ros.TransformListener(self.tf_buffer, self) - self.br = tf2_ros.TransformBroadcaster(self) - self.switch = False - self.__contolMSwitch = self.create_client(SwitchController, "/controller_manager/switch_controller") - self.arm_srv = self.create_service(ArmNew, 'arm_control', self.arm_control_callback, callback_group=self.callback_group) - self.tf_topic = self.create_subscription(TFMessage, '/tf', self.tf_cb , 10) - self.twist_pub = self.create_publisher(TwistStamped, "/servo_node/delta_twist_cmds", 10) - self.callback_group = ReentrantCallbackGroup() - self.pz = 0 - self.obj_aruco = "None" - self.last_obj = "None" - - self.moveit2 = MoveIt2( - node=self, - joint_names=ur5.joint_names(), - base_link_name=ur5.base_link_name(), - end_effector_name=ur5.end_effector_name(), - group_name=ur5.MOVE_GROUP_ARM, - callback_group=self.callback_group,) - self.declare_parameter( - "joint_positions_initial", - [ - 0.0, - -2.39, - 2.4, - -3.15, - -1.58, - 3.15 - ], - ) - self.declare_parameter( - "joint_positions_positive", - [ - 1.57, - -2.39, - 2.4, - -3.15, - -1.58, - 3.15 - ], - ) - self.declare_parameter( - "joint_positions_negative", - [ - -1.57, - -2.39, - 2.4, - -3.15, - -1.58, - 3.15 - ], - ) - self.declare_parameter( - "joint_positions_back", - [ - -2.96706, - -1.6057, - 1.67552, - -3.14159, - -1.58825, - 3.14159 - ], - ) - self.declare_parameter( - "joint_positions_final_1", - [ - -0.027, - -1.803, - -1.3658, - -3.039, - -1.52, - 3.15 - ], - ) - self.declare_parameter( - "joint_positions_final_2", - [ - 0.541052, - -2.35619, - -0.698132, - -3.14159, - -1.58825, - 3.14159 - ], - ) - self.declare_parameter( - "joint_positions_final_3", - [ - -2.967, - -0.715, - 0.89, - -3.14159, - -1.58825, - 3.14159 - ], - ) - - def trajactory(self): - switchParam = SwitchController.Request() - switchParam.activate_controllers = ["scaled_joint_trajectory_controller"] # for normal use of moveit - switchParam.strictness = 2 - switchParam.start_asap = False - - while not self.__contolMSwitch.wait_for_service(timeout_sec=5.0): - self.get_logger().warn(f"Service control Manager is not yet available...") - self.__contolMSwitch.call_async(switchParam) - print("[CM]: Switching Complete") - - def servo_active(self): - switchParam = SwitchController.Request() - switchParam.deactivate_controllers = ["forward_position_controller"] # for servoing - switchParam.strictness = 2 - switchParam.start_asap = False - - while not self.__contolMSwitch.wait_for_service(timeout_sec=5.0): - self.get_logger().warn(f"Service control Manager is not yet available...") - self.__contolMSwitch.call_async(switchParam) - print("[CM]: Switching Complete") - - def gripper_call(self, state): - ''' - based on the state given as i/p the service is called to activate/deactivate - pin 16 of TCP in UR5 - i/p: node, state of pin:Bool - o/p or return: response from service call - ''' - gripper_control = self.create_client(SetIO, '/io_and_status_controller/set_io') - while not gripper_control.wait_for_service(timeout_sec=1.0): - self.get_logger().info('EEF Tool service not available, waiting again...') - req = SetIO.Request() - req.fun = 1 - req.pin = 16 - req.state = float(state) - gripper_control.call_async(req) - return state - - def tf_cb(self, data): - self.obj = "None" - try: - if data.transforms[0].header.frame_id == "base_link": - if "obj" in data.transforms[0].child_frame_id: - self.obj_aruco = data.transforms[0].child_frame_id - except Exception as e: - print(e) - - def arm_control_callback(self, request, response): - self.variable_a = request.boom - self.variable_b = request.whack - self.get_logger().info('Incoming request\na: %d b: %d' % (request.boom, request.whack)) - response.cum = True - return response - def servo(self, box_no): - while rclpy.ok(): - try: - tool0 = self.tf_buffer.lookup_transform('base_link', "tool0", rclpy.time.Time()) - box49 = self.tf_buffer.lookup_transform('base_link', f"obj_{box_no}", rclpy.time.Time()) - - while rclpy.ok(): - try: - box49 = self.tf_buffer.lookup_transform('base_link', f"obj_{box_no}", rclpy.time.Time()) - tool0 = self.tf_buffer.lookup_transform('base_link', "tool0", rclpy.time.Time()) - except Exception as e: - print(e) - print(f"y ={round((box49.transform.translation.y) - (tool0.transform.translation.y),4)}") - print(f"x ={round((box49.transform.translation.x) - (tool0.transform.translation.x),4)}") - print(f"z ={round((box49.transform.translation.z) - (tool0.transform.translation.z),4)}") - joint_positions_initial = (self.get_parameter("joint_positions_initial").get_parameter_value().double_array_value) - joint_positions_final_1 = (self.get_parameter("joint_positions_final_1").get_parameter_value().double_array_value) - joint_positions_final_2 = (self.get_parameter("joint_positions_final_2").get_parameter_value().double_array_value) - joint_positions_final_3 = (self.get_parameter("joint_positions_final_3").get_parameter_value().double_array_value) - joint_positions_back = (self.get_parameter("joint_positions_back").get_parameter_value().double_array_value) - - - # ARM Move To BOX - - if round((box49.transform.translation.y) - (tool0.transform.translation.y),4) > 0.004 or round((box49.transform.translation.x) - (tool0.transform.translation.x),4) > 0.004 or round((box49.transform.translation.z) - (tool0.transform.translation.z),4) > 0.004: - __twist_msg = TwistStamped() - __twist_msg.header.stamp = self.get_clock().now().to_msg() - __twist_msg.header.frame_id = ur5.base_link_name() - __twist_msg.twist.linear.y = round((box49.transform.translation.y) - (tool0.transform.translation.y),4) *2 - __twist_msg.twist.linear.x = round((box49.transform.translation.x) - (tool0.transform.translation.x),4) *2 - __twist_msg.twist.linear.z = round((box49.transform.translation.z) - (tool0.transform.translation.z),4) *2 - self.twist_pub.publish(__twist_msg) - -# Gripper ON - else : - print("trying to attach") - self.gripper_call(1) - break - - while rclpy.ok(): - try: - box49 = self.tf_buffer.lookup_transform('base_link', f"obj_{box_no}", rclpy.time.Time()) - tool0 = self.tf_buffer.lookup_transform('base_link', "tool0", rclpy.time.Time()) - roll , pitch , yaw = euler_from_quaternion([box49.transform.rotation.x, box49.transform.rotation.y, box49.transform.rotation.z, box49.transform.rotation.w]) - print(f"x = {round(tool0.transform.translation.x,2)}") - print(f"y = {round(tool0.transform.translation.x,2)}") - - except Exception as e: - print(e) - - -# ARM Adding Buffer POSS & Drop Location - - - if (round(yaw) == 2 or round(yaw) == 1) and (round(tool0.transform.translation.x,2) > 0.23): - __twist_msg = TwistStamped() - __twist_msg.header.stamp = self.get_clock().now().to_msg() - __twist_msg.header.frame_id = ur5.base_link_name() - __twist_msg.twist.linear.z = -0.08 - __twist_msg.twist.linear.x = -0.4 - self.twist_pub.publish(__twist_msg) - elif (round(yaw) == 2 or round(yaw) == 1) and (round(tool0.transform.translation.x,2) <= 0.23): - self.trajactory() - self.moveit2.move_to_configuration(joint_positions_final_1) - self.moveit2.wait_until_executed() - self.moveit2.move_to_configuration(joint_positions_final_1) - self.moveit2.wait_until_executed() - break - - elif round(yaw) == 3 and (round(tool0.transform.translation.y,2) > 0.23): - __twist_msg = TwistStamped() - __twist_msg.header.stamp = self.get_clock().now().to_msg() - __twist_msg.header.frame_id = ur5.base_link_name() - __twist_msg.twist.linear.z = 0.04 - __twist_msg.twist.linear.y = -0.2 - self.twist_pub.publish(__twist_msg) - elif round(yaw) == 3 and (round(tool0.transform.translation.y,2) <= 0.23): - self.trajactory() - self.moveit2.move_to_configuration(joint_positions_final_2) - self.moveit2.wait_until_executed() - self.moveit2.move_to_configuration(joint_positions_final_2) - self.moveit2.wait_until_executed() - break - - elif round(yaw) == 0 and (round(tool0.transform.translation.y,2) < -0.26): - __twist_msg = TwistStamped() - __twist_msg.header.stamp = self.get_clock().now().to_msg() - __twist_msg.header.frame_id = ur5.base_link_name() - __twist_msg.twist.linear.z = 0.04 - __twist_msg.twist.linear.y = 0.2 - __twist_msg.twist.linear.x = -0.2 - self.twist_pub.publish(__twist_msg) - elif round(yaw) == 0 and (round(tool0.transform.translation.y,2) >= -0.26): - self.trajactory() - self.moveit2.move_to_configuration(joint_positions_back) - self.moveit2.wait_until_executed() - self.moveit2.move_to_configuration(joint_positions_back) - self.moveit2.wait_until_executed() - self.moveit2.move_to_configuration(joint_positions_final_3) - self.moveit2.wait_until_executed() - self.moveit2.move_to_configuration(joint_positions_final_3) - self.moveit2.wait_until_executed() - break - - - -# Gripper OFF - - print("trying to detach") - self.gripper_call(0) - break - - except Exception as e: - print(e) - - -def main(): - - rclpy.init() - enftf = endf() - - executor = rclpy.executors.MultiThreadedExecutor(2) - executor.add_node(enftf) - executor_thread = Thread(target=executor.spin, daemon=True, args=()) - executor_thread.start() - - filepath1 = RACK_MESH - filepath2 = BASE_MESH - position_r1 = [0.52,0.05,0.17] - quat_xyzw_r1 = [0.00,0.00,0.00,0.00] - position_r2 = [0.25,-0.62,0.17] - quat_xyzw_r2 = [0, 0, 0.7068252, 0.7073883] - position_r3 = [0.25,0.74,0.17] - quat_xyzw_r3 = [0, 0, -0.7068252, 0.7073883] - - rack_pos = [position_r1,position_r2,position_r3] - rack_quat = [quat_xyzw_r1,quat_xyzw_r2,quat_xyzw_r3] - - pos_b = [-0.20, 0, -0.26] - quat_b= [0, 0, 1, 0.000796327] - base_pos=[pos_b,quat_b] - mesh_rack_id = ["rack_1","rack_2","rack_3"] - joint_positions_initial = (enftf.get_parameter("joint_positions_initial").get_parameter_value().double_array_value) - joint_positions_positive = (enftf.get_parameter("joint_positions_positive").get_parameter_value().double_array_value) - joint_positions_negative = (enftf.get_parameter("joint_positions_negative").get_parameter_value().double_array_value) - joint_positions_back = (enftf.get_parameter("joint_positions_back").get_parameter_value().double_array_value) - - - for i in range(len(position_r1)): - time.sleep(2) - enftf.moveit2.add_collision_mesh( - filepath=filepath1, id=mesh_rack_id[i], position=rack_pos[i], quat_xyzw=rack_quat[i], frame_id=ur5.base_link_name() - ) - time.sleep(2) - - enftf.moveit2.add_collision_mesh( - filepath=filepath2, id="base", position=base_pos[0], quat_xyzw=base_pos[1], frame_id=ur5.base_link_name()) - - - while rclpy.ok(): - try: - - obj = enftf.obj_aruco[4:] - print(obj) - - box = enftf.tf_buffer.lookup_transform('base_link', enftf.obj_aruco, rclpy.time.Time()) - roll , pitch , yaw = euler_from_quaternion([box.transform.rotation.x, box.transform.rotation.y, box.transform.rotation.z, box.transform.rotation.w]) - print(yaw) - if enftf.variable_a == True: - if round(yaw) == 0: - enftf.trajactory() - enftf.moveit2.move_to_configuration(joint_positions_negative) - enftf.moveit2.wait_until_executed() - enftf.servo_active() - enftf.servo(obj) - enftf.trajactory() - enftf.moveit2.move_to_configuration(joint_positions_back) - enftf.moveit2.wait_until_executed() - enftf.moveit2.move_to_configuration(joint_positions_initial) - enftf.moveit2.wait_until_executed() - enftf.variable_a = False - - elif round(yaw) == 3: - enftf.trajactory() - enftf.moveit2.move_to_configuration(joint_positions_positive) - enftf.moveit2.wait_until_executed() - enftf.servo_active() - enftf.servo(obj) - enftf.trajactory() - enftf.moveit2.move_to_configuration(joint_positions_initial) - enftf.moveit2.wait_until_executed() - enftf.variable_a = False - - elif round(yaw) == 2 or round(yaw) == 1: - enftf.servo_active() - enftf.servo(obj) - enftf.trajactory() - enftf.moveit2.move_to_configuration(joint_positions_initial) - enftf.moveit2.wait_until_executed() - enftf.variable_a = False - - except Exception as e: - print(e) - - -if __name__ == "__main__": - main() diff --git a/pymoveit2/examples/something.py b/pymoveit2/examples/task5_movit_hardware.py similarity index 84% rename from pymoveit2/examples/something.py rename to pymoveit2/examples/task5_movit_hardware.py index 17d7233..ac298fe 100644 --- a/pymoveit2/examples/something.py +++ b/pymoveit2/examples/task5_movit_hardware.py @@ -9,6 +9,7 @@ import time from pymoveit2 import MoveIt2 from pymoveit2.robots import ur5 +from arm_picky.srv import ArmNew import math import sys import tf2_ros @@ -56,14 +57,15 @@ def truncate(f, n): class endf(Node): def __init__(self): super().__init__('move_ur5') + self.callback_group = ReentrantCallbackGroup() self.tf_buffer = tf2_ros.buffer.Buffer() # buffer time used for listening transforms self.listener = tf2_ros.TransformListener(self.tf_buffer, self) self.br = tf2_ros.TransformBroadcaster(self) self.switch = False + self.arm_srv = self.create_service(ArmNew, 'arm_control', self.arm_control_callback, callback_group=self.callback_group) self.__contolMSwitch = self.create_client(SwitchController, "/controller_manager/switch_controller") self.tf_topic = self.create_subscription(TFMessage, '/tf', self.tf_cb , 10) self.twist_pub = self.create_publisher(TwistStamped, "/servo_node/delta_twist_cmds", 10) - self.callback_group = ReentrantCallbackGroup() self.pz = 0 self.obj_aruco = "None" self.last_obj = "None" @@ -153,6 +155,13 @@ def __init__(self): ], ) + def arm_control_callback(self, request, response): + print("I am here") + self.variable_a = request.boom + self.variable_b = request.rack_no + response.reply = True + return response + def trajactory(self): switchParam = SwitchController.Request() switchParam.activate_controllers = ["scaled_joint_trajectory_controller"]# for normal use of moveit @@ -167,6 +176,7 @@ def trajactory(self): def servo_active(self): switchParam = SwitchController.Request() + print("hello ") switchParam.deactivate_controllers = ["scaled_joint_trajectory_controller"] switchParam.activate_controllers = ["forward_position_controller"] # for servoing switchParam.strictness = 2 @@ -177,6 +187,11 @@ def servo_active(self): self.__contolMSwitch.call_async(switchParam) print("[CM]: Switching Complete Servo") + while not self.__contolMSwitch.wait_for_service(timeout_sec=5.0): + self.get_logger().warn(f"Service control Manager is not yet available...") + self.__contolMSwitch.call_async(switchParam) + print("[CM]: Switching Complete") + def gripper_call(self, state): ''' based on the state given as i/p the service is called to activate/deactivate @@ -201,7 +216,7 @@ def tf_cb(self, data): if "obj" in data.transforms[0].child_frame_id: self.obj_aruco = data.transforms[0].child_frame_id except Exception as e: - print(e) + pass def servo(self, box_no): while rclpy.ok(): @@ -214,10 +229,10 @@ def servo(self, box_no): box49 = self.tf_buffer.lookup_transform('base_link', f"obj_{box_no}", rclpy.time.Time()) tool0 = self.tf_buffer.lookup_transform('base_link', "tool0", rclpy.time.Time()) except Exception as e: - print(e) - # print(f"y ={round((box49.transform.translation.y) - (tool0.transform.translation.y),4)}") - # print(f"x ={round((box49.transform.translation.x) - (tool0.transform.translation.x),4)}") - # print(f"z ={round((box49.transform.translation.z) - (tool0.transform.translation.z),4)}") + pass + print(f"y ={round((box49.transform.translation.y) - (tool0.transform.translation.y),4)}") + print(f"x ={round((box49.transform.translation.x) - (tool0.transform.translation.x),4)}") + print(f"z ={round((box49.transform.translation.z) - (tool0.transform.translation.z),4)}") joint_positions_initial = (self.get_parameter("joint_positions_initial").get_parameter_value().double_array_value) joint_positions_final_1 = (self.get_parameter("joint_positions_final_1").get_parameter_value().double_array_value) joint_positions_final_2 = (self.get_parameter("joint_positions_final_2").get_parameter_value().double_array_value) @@ -235,11 +250,13 @@ def servo(self, box_no): __twist_msg.twist.linear.x = round((box49.transform.translation.x) - (tool0.transform.translation.x),4) *2 __twist_msg.twist.linear.z = round((box49.transform.translation.z) - (tool0.transform.translation.z),4) *2 self.twist_pub.publish(__twist_msg) + print("SERVO_START") # Gripper ON else : print("trying to attach") self.gripper_call(1) + print("DONE") break while rclpy.ok(): @@ -247,75 +264,69 @@ def servo(self, box_no): box49 = self.tf_buffer.lookup_transform('base_link', f"obj_{box_no}", rclpy.time.Time()) tool0 = self.tf_buffer.lookup_transform('base_link', "tool0", rclpy.time.Time()) roll , pitch , yaw = euler_from_quaternion([box49.transform.rotation.x, box49.transform.rotation.y, box49.transform.rotation.z, box49.transform.rotation.w]) - # print(f"x = {round(tool0.transform.translation.x,2)}") - # print(f"y = {round(tool0.transform.translation.x,2)}") + except Exception as e: - print(e) + pass # ARM Adding Buffer POSS & Drop Location - - if (round(yaw) == 2 or round(yaw) == 1) and (round(tool0.transform.translation.x,2) > 0.23): - print(obj) + if self.variable_b == "2" and (round(tool0.transform.translation.y,2) > 0.25): __twist_msg = TwistStamped() __twist_msg.header.stamp = self.get_clock().now().to_msg() __twist_msg.header.frame_id = ur5.base_link_name() - __twist_msg.twist.linear.z = 0.4 - __twist_msg.twist.linear.x = -0.4 - print("1") + __twist_msg.twist.linear.z = 0.1 + __twist_msg.twist.linear.y = -0.2 self.twist_pub.publish(__twist_msg) - elif (round(yaw) == 2 or round(yaw) == 1) and (round(tool0.transform.translation.x,2) <= 0.23): + elif self.variable_b == "2" and (round(tool0.transform.translation.y,2) <= 0.25): self.trajactory() - self.moveit2.move_to_configuration(joint_positions_final_1) + self.moveit2.move_to_configuration(joint_positions_initial) self.moveit2.wait_until_executed() - self.moveit2.move_to_configuration(joint_positions_final_1) - print("2") + self.moveit2.move_to_configuration(joint_positions_initial) self.moveit2.wait_until_executed() - break + self.moveit2.move_to_configuration(joint_positions_final_2) + self.moveit2.wait_until_executed() + self.moveit2.move_to_configuration(joint_positions_final_2) + self.moveit2.wait_until_executed() + break - elif round(yaw) == None and (round(tool0.transform.translation.y,2) > 0.25): + elif self.variable_b == "1" and (round(tool0.transform.translation.x,2) > 0.23): __twist_msg = TwistStamped() __twist_msg.header.stamp = self.get_clock().now().to_msg() __twist_msg.header.frame_id = ur5.base_link_name() - __twist_msg.twist.linear.z = 0.4 - __twist_msg.twist.linear.y = -0.2 - print("3") + __twist_msg.twist.linear.z = 0.1 + __twist_msg.twist.linear.x = -0.4 self.twist_pub.publish(__twist_msg) - elif round(yaw) == None and (round(tool0.transform.translation.y,2) <= 0.25): + elif self.variable_b == "1" and (round(tool0.transform.translation.x,2) <= 0.23): self.trajactory() - self.moveit2.move_to_configuration(joint_positions_final_2) + self.moveit2.move_to_configuration(joint_positions_final_1) self.moveit2.wait_until_executed() - self.moveit2.move_to_configuration(joint_positions_final_2) + self.moveit2.move_to_configuration(joint_positions_final_1) self.moveit2.wait_until_executed() - print("4") break - - elif round(yaw) == 3 and (round(tool0.transform.translation.y,2) < -0.26): + + elif self.variable_b == "0" and (round(tool0.transform.translation.x,2) > 0.23): __twist_msg = TwistStamped() __twist_msg.header.stamp = self.get_clock().now().to_msg() __twist_msg.header.frame_id = ur5.base_link_name() - __twist_msg.twist.linear.z = 0.04 - __twist_msg.twist.linear.y = 0.2 - __twist_msg.twist.linear.x = -0.2 - print("5") + __twist_msg.twist.linear.z = 0.4 + __twist_msg.twist.linear.x = -0.4 self.twist_pub.publish(__twist_msg) - elif round(yaw) == 3 and (round(tool0.transform.translation.y,2) >= -0.26): + elif self.variable_b == "0" and (round(tool0.transform.translation.x,2) <= 0.23): self.trajactory() - self.moveit2.move_to_configuration(joint_positions_back) - self.moveit2.wait_until_executed() - self.moveit2.move_to_configuration(joint_positions_back) - self.moveit2.wait_until_executed() - self.moveit2.move_to_configuration(joint_positions_final_3) + self.moveit2.move_to_configuration(joint_positions_final_1) self.moveit2.wait_until_executed() - self.moveit2.move_to_configuration(joint_positions_final_3) + self.moveit2.move_to_configuration(joint_positions_final_1) self.moveit2.wait_until_executed() - print("7") break + + + + # Gripper OFF print("trying to detach") @@ -323,6 +334,7 @@ def servo(self, box_no): break except Exception as e: + print("Servo Function 2nd Part") print(e) @@ -349,13 +361,12 @@ def main(): print(obj) box = enftf.tf_buffer.lookup_transform('base_link', enftf.obj_aruco, rclpy.time.Time()) roll , pitch , yaw = euler_from_quaternion([box.transform.rotation.x, box.transform.rotation.y, box.transform.rotation.z, box.transform.rotation.w]) - print(yaw) - - if obj == "15": - if enftf.last_obj == obj: - pass + print(round(yaw,2)) + if enftf.variable_a == True: + + print("You are my special") - elif round(yaw) == 3: + if enftf.variable_b == "0": enftf.trajactory() enftf.moveit2.move_to_configuration(joint_positions_negative) enftf.moveit2.wait_until_executed() @@ -366,8 +377,9 @@ def main(): enftf.moveit2.wait_until_executed() enftf.moveit2.move_to_configuration(joint_positions_initial) enftf.moveit2.wait_until_executed() + enftf.variable_a == False - elif round(yaw) == None: + elif enftf.variable_b == "2": enftf.trajactory() enftf.moveit2.move_to_configuration(joint_positions_positive) enftf.moveit2.wait_until_executed() @@ -376,19 +388,18 @@ def main(): enftf.trajactory() enftf.moveit2.move_to_configuration(joint_positions_initial) enftf.moveit2.wait_until_executed() + enftf.variable_a == False - elif round(yaw) == 2 or round(yaw) == 1: - print(obj) + elif enftf.variable_b == "1": enftf.servo_active() - print("alok") enftf.servo(obj) enftf.trajactory() enftf.moveit2.move_to_configuration(joint_positions_initial) enftf.moveit2.wait_until_executed() - enftf.last_obj = obj + enftf.variable_a == False except Exception as e: - print(e) + print("Main Function") if __name__ == "__main__": - main() + main() \ No newline at end of file diff --git a/pymoveit2/examples/task5_movit_sim.py b/pymoveit2/examples/task5_movit_sim.py new file mode 100644 index 0000000..e69de29 diff --git a/ur_description/scripts/test.py b/ur_description/scripts/test.py deleted file mode 100755 index f24a908..0000000 --- a/ur_description/scripts/test.py +++ /dev/null @@ -1,447 +0,0 @@ -#!/usr/bin/env python3 - - -''' -***************************************************************************************** -* -* =============================================== -* Cosmo Logistic (CL) Theme (eYRC 2023-24) -* =============================================== -* -* This script should be used to implement Task 1A of Cosmo Logistic (CL) Theme (eYRC 2023-24). -* -* This software is made available on an "AS IS WHERE IS BASIS". -* Licensee/end user indemnifies and will keep e-Yantra indemnified from -* any and all claim(s) that emanate from the use of the Software or -* breach of the terms of this agreement. -* -***************************************************************************************** -''' - -# Team ID: [ Team-ID ] -# Author List: [ Names of team members worked on this file separated by Comma: Name1, Name2, ... ] -# Filename: task1a.py -# Functions: -# [ Comma separated list of functions in this file ] -# Nodes: Add your publishing and subscribing node -# Example: -# Publishing Topics - [ /tf ] -# Subscribing Topics - [ /camera/aligned_depth_to_color/image_raw, /etc... ] - - -################### IMPORT MODULES ####################### - -import rclpy -import sys -import cv2 -import math -import tf2_ros -import numpy as np -from rclpy.node import Node -from cv_bridge import CvBridge, CvBridgeError -from geometry_msgs.msg import TransformStamped -from scipy.spatial.transform import Rotation as R -from sensor_msgs.msg import CompressedImage, Image -from cv2 import aruco -from builtin_interfaces.msg import Time -from geometry_msgs.msg import Quaternion -from tf_transformations import quaternion_from_euler -import tf_transformations - - -##################### FUNCTION DEFINITIONS ####################### -def calculate_rectangle_area(coordinates): - # global width - ''' - Description: Function to calculate the area of a detected ArUco marker - - Args: - coordinates (list): coordinates of detected ArUco (4 sets of (x, y) coordinates) - - Returns: - area (float): area of detected ArUco - width (float): width of detected ArUco - height (float): height of detected ArUco - ''' - height = ((coordinates[0] - coordinates[4])**2 + (coordinates[1] - coordinates[5])**2)**0.5 - width = ((coordinates[2] - coordinates[4])**2 + (coordinates[3] - coordinates[5])**2)**0.5 - - # Calculate the area - area = width * height - - return area, width - -def detect_aruco(image, depth): - ''' - Description: Function to perform aruco detection and return each detail of aruco detected - such as marker ID, distance, angle, width, center point location, etc. - - Args: - image (Image): Input image frame received from respective camera topic - - Returns: - center_aruco_list (list): Center points of all aruco markers detected - distance_from_rgb_list (list): Distance value of each aruco markers detected from RGB camera - angle_aruco_list (list): Angle of all pose estimated for aruco marker - width_aruco_list (list): Width of all detected aruco markers - ids (list): List of all aruco marker IDs detected in a single frame - ''' - - center_aruco_list = [] - distance_from_rgb_list = [] - angle_aruco_list = [] - width_aruco_list = [] - ids = [] - flat_list =[] - aruco_area_threshold = 1500 - - # The camera matrix is defined as per camera info loaded from the plugin used. - # You may get this from /camer_info topic when camera is spawned in gazebo. - # Make sure you verify this matrix once if there are calibration issues. - cam_mat = np.array([[931.1829833984375, 0.0, 640.0], [0.0, 931.1829833984375, 360.0], [0.0, 0.0, 1.0]],dtype=np.float32) - - # The distortion matrix is currently set to 0. - # We will be using it during Stage 2 hardware as Intel Realsense Camera provides these camera info. - dist_mat = np.array([0.0,0.0,0.0,0.0,0.0]) - - # We are using 150x150 aruco marker size - size_of_aruco_cm = 15 - dictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50) - - # arucoDict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_4X4_50) - arucoParams = cv2.aruco.DetectorParameters() - brightness = 10 - contrast = 2.3 - image = cv2.addWeighted(image, contrast, np.zeros(image.shape, image.dtype), 0, brightness) - image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) - try: - (corners, aruco_id, rejected) = cv2.aruco.detectMarkers(image, dictionary, parameters=arucoParams) - for i in range(len(aruco_id)): - - x1= corners[i][0][0][0] - y1= corners[i][0][0][1] - x2= corners[i][0][2][0] - y2= corners[i][0][2][1] - x3= corners[i][0][3][0] - y3= corners[i][0][3][1] - - coordinates= [x1,y1,x2,y2,x3,y3] - area, width = calculate_rectangle_area(coordinates) - - if area >= aruco_area_threshold: - rvec , tvec, _ = cv2.aruco.estimatePoseSingleMarkers(corners, size_of_aruco_cm, cam_mat, dist_mat) - ids.append(int(aruco_id[i])) - current_rvec = rvec[i] - current_tvec = tvec[i] - - # center_aruco = current_tvec.squeeze() - center_aruco = np.mean(corners[i][0], axis=0) - center_x, center_y = map(int, center_aruco) - # distance_from_rgb = cv2.norm(current_tvec) - center_aruco_list.append((center_x,center_y)) - # depth_data = depth[center_y, center_x] - # distance_from_rgb_list.append(depth_data) - - # Angle cal - list_1 = current_rvec.tolist() - flat_list.append(list_1) - angle_aruco_list = [item[0] for item in flat_list] - rotation_mat, _ = cv2.Rodrigues(current_rvec) - - width_aruco_list.append(width) - cv2.aruco.drawDetectedMarkers(image, corners) - # cv2.putText(image, f"Distance: {distance_from_rgb_list[i]:.2f} cm", (int(corners[i][0][0][0]), int(corners[i][0][0][1]) + 20), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2) - # cv2.putText(image, f"center{center_x, center_y}", (int(center_x), int(center_y)), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2) - cv2.putText(image, f"ID: {aruco_id[i]}", (int(corners[i][0][0][0]), int(corners[i][0][0][1]) - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2) - # cv2.putText(image, f"{angle_aruco_list[i]}", (int(center_x), int(center_y)), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2) - cv2.drawFrameAxes(image,cam_mat, dist_mat, current_rvec[0] , current_tvec[0],2,1) - cv2.circle(image, (center_x ,center_y), 2, (255, 0, 0), 6) - print(aruco_id[i]) - cv2.imshow("Aruco Detection", image) - cv2.waitKey(1) # Wait for 1ms - - - # cv2.destroyAllWindows() - return center_aruco_list, distance_from_rgb_list, angle_aruco_list, width_aruco_list, ids, tvec - - except: - pass -##################### CLASS DEFINITION ####################### - -class aruco_tf(Node): - ''' - ___CLASS___ - - Description: Class which servers purpose to define process for detecting aruco marker and publishing tf on pose estimated. - ''' - - def __init__(self): - ''' - Description: Initialization of class aruco_tf - All classes have a function called __init__(), which is always executed when the class is being initiated. - The __init__() function is called automatically every time the class is being used to create a new object. - You can find more on this topic here -> https://www.w3schools.com/python/python_classes.asp - ''' - - super().__init__('aruco_tf_publisher') # registering node - self.cv_image = None # Initialize cv_image as None - self.depth_image = None - ############ Topic SUBSCRIPTIONS ############ - - self.color_cam_sub = self.create_subscription(Image, '/camera/color/image_raw', self.colorimagecb, 10) - self.depth_cam_sub = self.create_subscription(Image, '/camera/aligned_depth_to_color/image_raw', self.depthimagecb, 10) - - ############ Constructor VARIABLES/OBJECTS ############ - - image_processing_rate = 2.5 # rate of time to process image (seconds) - self.bridge = CvBridge() # initialise CvBridge object for image conversion - self.tf_buffer = tf2_ros.buffer.Buffer() # buffer time used for listening transforms - self.listener = tf2_ros.TransformListener(self.tf_buffer, self) - self.br = tf2_ros.TransformBroadcaster(self) # object as transform broadcaster to send transform wrt some frame_id - self.timer = self.create_timer(image_processing_rate, self.process_image) # creating a timer based function which gets called on every 0.2 seconds (as defined by 'image_processing_rate' variable) - - def depthimagecb(self, data): - ''' - Description: Callback function for aligned depth camera topic. - Use this function to receive image depth data and convert to CV2 image - - Args: - data (Image): Input depth image frame received from aligned depth camera topic - - Returns: - ''' - br = CvBridge() - - self.depth_image = br.imgmsg_to_cv2(data, data.encoding) - ############ ADD YOUR CODE HERE ############ - - # INSTRUCTIONS & HELP : - - # -> Use data variable to convert ROS Image message to CV2 Image type - - # -> HINT: You may use CvBridge to do the same - - ############################################ - - - def colorimagecb(self, data): - - ''' - Description: Callback function for colour camera raw topic. - Use this function to receive raw image data and convert to CV2 image - - Args: - data (Image): Input coloured raw image frame received from image_raw camera topic - - Returns: - ''' - br = CvBridge() - - self.cv_image = br.imgmsg_to_cv2(data, "bgr8") - self.gray = cv2.cvtColor(self.cv_image, cv2.COLOR_BGR2GRAY) - ############ ADD YOUR CODE HERE ############ - - # INSTRUCTIONS & HELP : - - # -> Use data variable to convert ROS Image message to CV2 Image type - - # -> HINT: You may use CvBridge to do the same - # Check if you need any rotation or flipping image as input data maybe different than what you expect to be. - # You may use cv2 functions such as 'flip' and 'rotate' to do the same - - ############################################ - - - def process_image(self): - ''' - Description: Timer function used to detect aruco markers and publish tf on estimated poses. - - Args: - Returns: - ''' - - ############ Function VARIABLES ############ - - # These are the variables defined from camera info topic such as image pixel size, focalX, focalY, etc. - # Make sure you verify these variable values once. As it may affect your result. - # You can find more on these variables here -> http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/CameraInfo.html - - sizeCamX = 1280 - sizeCamY = 720 - centerCamX = 640 - centerCamY = 360 - focalX = 931.1829833984375 - focalY = 931.1829833984375 - - center_list, distance_list, angle_list, width_list, ids,tvec = detect_aruco(self.cv_image, self.depth_image) - rpy = [] - list_rpy = [] - # Add your image processing code here - for i in range(len(ids)): - aruco_id = ids[i] - # distance = distance_list[i] - angle_aruco = angle_list[i] - width_aruco = width_list[i] - center=center_list[i] - print(angle_aruco) - angle_aruco = ((0.788*angle_aruco[2]) - ((angle_aruco[2]**2)/3160)) - if round(angle_aruco) == 0: - angle_aruco = (angle_aruco) + math.pi - roll, pitch, yaw = 0.4, 0, angle_aruco - else: - roll, pitch, yaw = 0, -0.261, angle_aruco - - print(angle_aruco) - - rpy.append(roll) - rpy.append(pitch) - rpy.append(yaw) - list_rpy.append(rpy) - rpy_np = np.array(list_rpy) - rot_mat, _ = cv2.Rodrigues(rpy_np) - - transform_matrix = np.array([[0, 0, 1], - [-1, 0, 0], - [0, 1, 0]]) - - good_mat = np.dot(rot_mat, transform_matrix) - - euler_good = tf_transformations.euler_from_matrix(good_mat) - - euler_list = euler_good - roll = euler_list[0] - pitch = euler_list[1] - yaw = euler_list[2] - list_rpy = [] - rpy = [] - - q_rot = quaternion_from_euler(roll,pitch,yaw) - - r = R.from_euler('xyz', [roll, pitch, yaw], degrees=True) - quaternion = r.as_quat() - - qy = q_rot[1] - qz = q_rot[2] - qx = q_rot[0] - qw = q_rot[3] - - x = tvec[i][0][0] - y = tvec[i][0][1] - z = tvec[i][0][2] - # y_1 = distance/1000 * (sizeCamX - center_list[i][0] - centerCamX) / focalX - # z_1 = distance/1000 * (sizeCamY - center_list[i][1] - centerCamY) / focalY - # x_1 = distance/1000 - - # transform_msg = TransformStamped() - # transform_msg.header.stamp = self.get_clock().now().to_msg() - # transform_msg.header.frame_id = 'camera_link' - # transform_msg.child_frame_id = f'cam_{aruco_id}' - # transform_msg.transform.translation.x = x_1 - # transform_msg.transform.translation.y = y_1 - # transform_msg.transform.translation.z = z_1 - # transform_msg.transform.rotation.x = qx - # transform_msg.transform.rotation.y = qy - # transform_msg.transform.rotation.z = qz - # transform_msg.transform.rotation.w = qw - # self.br.sendTransform(transform_msg) - - # try: - # t = self.tf_buffer.lookup_transform('base_link', transform_msg.child_frame_id, rclpy.time.Time()) - # transform_msg.header.stamp = self.get_clock().now().to_msg() - - # transform_msg.header.frame_id = 'base_link' - # transform_msg.child_frame_id = f'obj_{aruco_id}' - # transform_msg.transform.translation.x = t.transform.translation.x - # transform_msg.transform.translation.y = t.transform.translation.y - # transform_msg.transform.translation.z = t.transform.translation.z - # transform_msg.transform.rotation.x = t.transform.rotation.x - # transform_msg.transform.rotation.y = t.transform.rotation.y - # transform_msg.transform.rotation.z = t.transform.rotation.z - # transform_msg.transform.rotation.w = t.transform.rotation.w - # self.br.sendTransform(transform_msg) - # except: - # pass - - - ############ ADD YOUR CODE HERE ############ - - # INSTRUCTIONS & HELP : - - # -> Get aruco center, distance from rgb, angle, width and ids list from 'detect_aruco_center' defined above - - # -> Loop over detected box ids received to calculate position and orientation transform to publish TF - - # -> Use this equation to correct the input aruco angle received from cv2 aruco function 'estimatePoseSingleMarkers' here - # It's a correction formula- - # angle_aruco = (0.788*angle_aruco) - ((angle_aruco**2)/3160) - - # -> Then calculate quaternions from roll pitch yaw (where, roll and pitch are 0 while yaw is corrected aruco_angle) - - # -> Use center_aruco_list to get realsense depth and log them down. (divide by 1000 to convert mm to m) - - # -> Use this formula to rectify x, y, z based on focal length, center value and size of image - # x = distance_from_rgb * (sizeCamX - cX - centerCamX) / focalX - # y = distance_from_rgb * (sizeCamY - cY - centerCamY) / focalY - # z = distance_from_rgb - # where, - # cX, and cY from 'center_aruco_list' - # distance_from_rgb is depth of object calculated in previous step - # sizeCamX, sizeCamY, centerCamX, centerCamY, focalX and focalY are defined above - - # -> Now, mark the center points on image frame using cX and cY variables with help of 'cv2.cirle' function - - # -> Here, till now you receive coordinates from camera_link to aruco marker center position. - # So, publish this transform w.r.t. camera_link using Geometry Message - TransformStamped - # so that we will collect it's position w.r.t base_link in next step. - # Use the following frame_id- - # frame_id = 'camera_link' - # child_frame_id = 'cam_' Ex: cam_20, where 20 is aruco marker ID - - # -> Then finally lookup transform between base_link and obj frame to publish the TF - # You may use 'lookup_transform' function to pose of obj frame w.r.t base_link - - # -> And now publish TF between object frame and base_link - # Use the following frame_id- - # frame_id = 'base_link' - # child_frame_id = 'obj_' Ex: obj_20, where 20 is aruco marker ID - - # -> At last show cv2 image window having detected markers drawn and center points located using 'cv2.imshow' function. - # Refer MD book on portal for sample image -> https://portal.e-yantra.org/ - - # -> NOTE: The Z axis of TF should be pointing inside the box (Purpose of this will be known in task 1B) - # Also, auto eval script will be judging angular difference aswell. So, make sure that Z axis is inside the box (Refer sample images on Portal - MD book) - - ############################################ - - -##################### FUNCTION DEFINITION ####################### - -def main(): - ''' - Description: Main function which creates a ROS node and spin around for the aruco_tf class to perform it's task - ''' - - rclpy.init(args=sys.argv) # initialisation - - node = rclpy.create_node('aruco_tf_process') # creating ROS node - - node.get_logger().info('Node created: Aruco tf process') # logging information - - aruco_tf_class = aruco_tf() # creating a new object for class 'aruco_tf' - - rclpy.spin(aruco_tf_class) # spining on the object to make it alive in ROS 2 DDS - - aruco_tf_class.destroy_node() # destroy node after spin ends - - rclpy.shutdown() # shutdown process - - -if __name__ == '__main__': - ''' - Description: If the python interpreter is running that module (the source file) as the main program, - it sets the special __name__ variable to have a value “__main__”. - If this file is being imported from another module, __name__ will be set to the module’s name. - You can find more on this here -> https://www.geeksforgeeks.org/what-does-the-if-__name__-__main__-do/ - ''' - - main() \ No newline at end of file