def main(): rospy.init_node('pioneer_trajectory', anonymous=False) rospy.loginfo("[Tra] Pioneer Demo Trajectory - Running") ## Trajectory kinematics = Kinematics() kinematics.publisher_(kinematics.module_control_pub, "manipulation_module", latch=True) # <-- Enable Manipulation mode kinematics.publisher_(kinematics.send_ini_pose_msg_pub, "ini_pose", latch=True) sleep(2) # mode = 'full' mode = 'left_arm' if mode == "full": # trajectory simulation circle kinematics.set_kinematics_pose("left_arm" , 2.0, **{ 'x': 0.20, 'y': 0.30, 'z': 0.80, 'roll': 0.00, 'pitch': 0.00, 'yaw': 0.00 }) kinematics.set_kinematics_pose("right_arm" , 2.0, **{ 'x': 0.20, 'y': -0.30, 'z': 0.80, 'roll': 0.00, 'pitch': 0.00, 'yaw': 0.00 }) sleep(0.1) while kinematics.left_tra == True and kinematics.right_tra == True: pass sleep(1) iter = 0 while not rospy.is_shutdown(): if mode == "full": kinematics.trajectory_sin(group="left_arm", x=0.30, y=0.20, z=0.90, roll=0.0, pitch=0.0, yaw=0.0, xc=-0.1, yc=-0.1, zc=-0.1, time=2, res=0.01) kinematics.trajectory_sin(group="right_arm", x=0.30, y=-0.20, z=0.90, roll=0.0, pitch=0.0, yaw=0.0, xc=-0.1, yc=0.1, zc=-0.1, time=2, res=0.01) sleep(1) # because pubslishing array msg using for loop while kinematics.left_arr == True and kinematics.right_arr == True: pass # no action kinematics.trajectory_sin(group="left_arm", x=0.20, y=0.30, z=0.85, roll=0.0, pitch=0.0, yaw=0.0, xc=0.1, yc=0.1, zc=0.1, time=2, res=0.01) kinematics.trajectory_sin(group="right_arm", x=0.20, y=-0.30, z=0.85, roll=0.0, pitch=0.0, yaw=0.0, xc=0.1, yc=-0.1, zc=0.1, time=2, res=0.01) sleep(1) # because pubslishing array msg using for loop while kinematics.left_arr == True and kinematics.right_arr == True: pass # no action iter += 1 rospy.loginfo("[Tra] Iteration : {}".format(iter)) if iter >= 2: mode = "none" elif mode == "left_arm": left_arm = kinematics.get_kinematics_pose("left_arm") print(left_arm) kinematics.set_kinematics_pose("left_arm" , 2.0, **{ 'x': 0.40, 'y': 0.50, 'z': 0.80, 'roll': 0.00, 'pitch': 0.00, 'yaw': 0.00 }) while kinematics.left_arr == True pass mode = "none" # pass elif mode == "none": break kinematics.publisher_(kinematics.send_ini_pose_msg_pub, "ini_pose", latch=True) kinematics.kill_threads()
def main(): rospy.init_node('pioneer_main', anonymous=False) rospy.loginfo("Pioneer Main - Running") # camera = Camera() # print(sys.version) ################## ## Kinematics ################## kinematics = Kinematics() kinematics.publisher_(kinematics.module_control_pub, "manipulation_module", latch=True) # <-- Enable Manipulation mode kinematics.publisher_(kinematics.send_ini_pose_msg_pub, "ini_pose", latch=True) sleep(2) kinematics.set_kinematics_pose( "left_arm", 1.0, **{ 'x': 0.20, 'y': 0.30, 'z': 0.80, 'roll': 0.00, 'pitch': 0.00, 'yaw': 0.00 }) kinematics.set_kinematics_pose( "right_arm", 1.0, **{ 'x': 0.20, 'y': -0.30, 'z': 0.80, 'roll': 0.00, 'pitch': 0.00, 'yaw': 0.00 }) sleep(1) a = 0 while not rospy.is_shutdown(): kinematics.trajectory_sin(group="left_arm", x=0.30, y=0.20, z=0.90, roll=0.0, pitch=0.0, yaw=0.0, xc=-0.1, yc=-0.1, zc=-0.1, time=2, res=0.01) kinematics.trajectory_sin(group="right_arm", x=0.30, y=-0.20, z=0.90, roll=0.0, pitch=0.0, yaw=0.0, xc=-0.1, yc=0.1, zc=-0.1, time=2, res=0.01) sleep(1) # because pubslishing array msg using for loop while kinematics.left_arr == True and kinematics.right_arr == True: pass # no action # input("Press enter") kinematics.trajectory_sin(group="left_arm", x=0.20, y=0.30, z=0.80, roll=0.0, pitch=0.0, yaw=0.0, xc=0.1, yc=0.1, zc=0.1, time=2, res=0.01) kinematics.trajectory_sin(group="right_arm", x=0.20, y=-0.30, z=0.80, roll=0.0, pitch=0.0, yaw=0.0, xc=0.1, yc=-0.1, zc=0.1, time=2, res=0.01) sleep(1) # because pubslishing array msg using for loop while kinematics.left_arr == True and kinematics.right_arr == True: pass # no action # input("Press enter1") a = a + 1 if a >= 3: break ################## ## Motor ################## # motor = Motor() # motor.publisher_(motor.module_control_pub, "none", latch=True) # rate = rospy.Rate(60) # while not rospy.is_shutdown(): # rospy.loginfo("motor") # # motor.set_joint_states(["l_arm_wr_p", "l_arm_wr_y", "r_arm_wr_y", "r_arm_wr_p"], \ # # [0, 0, 0, 0], [0, 0, 0, 0], [0, 0, 0, 0]) # # sleep(2) # # motor.set_joint_states(["l_arm_wr_p", "l_arm_wr_y", "r_arm_wr_y", "r_arm_wr_p"], \ # # [45, 45, 45, 45], [0, 0, 0, 0], [0, 0, 0, 0]) # # sleep(2) # # motor.set_joint_states(["l_arm_wr_p", "l_arm_wr_y", "l_arm_wr_r", "r_arm_wr_y", "r_arm_wr_p", "r_arm_wr_r"], False) # # motor.set_joint_states(["l_arm_thumb_y", "l_arm_thumb_p", "l_arm_index_p", "l_arm_middle_p", "l_arm_finger45_p", # # "r_arm_thumb_y", "r_arm_thumb_p", "r_arm_index_p", "r_arm_middle_p", "r_arm_finger45_p"], False) # motor.set_joint_states(["all"], False) # rate.sleep() ################## ## Walk ################## # walk = Walking() # walk.latching_publish(walk.walking_pub, "ini_pose") # sleep(4) # print("walking") # walk.latching_publish(walk.walking_pub, "set_mode") # <-- Enable walking mode # sleep(4) # print("walking balance on") # walk.latching_publish(walk.walking_pub, "balance_on") # sleep(4) # walk.walk_command("forward", 1, 1.0, 0.01, 0.05, 5) # while (walk.status_msg == "Walking_Started"): # rospy.loginfo("walking forward 1") # walk.walk_command("forward", 1, 1.0, 0.1, 0.05, 5) # while (walk.status_msg == "Walking_Started"): # rospy.loginfo("walking forward 2") # walk.walk_command("forward", 1, 1.0, 0.01, 0.05, 5) # while (walk.status_msg == "Walking_Started"): # rospy.loginfo("walking forward 3") # sleep(5) # walk.kill_threads() ################## ## Sensor ################## # sensor = Sensor() # sleep(5) # sensor.kill_threads() ################## ## Motion ################## # motion = Motion() # motion.init_motion() # print(motion.init_joint) # print(motion.init_pose) # sleep(3) # rospy.loginfo(motion.init_pose) # # motor.set_joint_states(motion.init_joint, motion.init_pose) # # sleep(5) # motor.kill_threads() # motor.set_joint_states(["l_arm_el_y"], [0.0], [0.0], [0.0]) # motor.set_joint_states(["l_arm_el_y"], [0.5]) # while not rospy.is_shutdown(): # # rospy.loginfo("infinite") # # print(camera.source_image) # cv2.imshow('img',camera.source_image) # cv2.waitKey(1) # # rate.sleep() kinematics.kill_threads()