class ManipulationNode(object): def __init__(self): print("init ManipulationNode") rospy.init_node("Manipulation_Control") self.__manipulation = Manipulation() rospy.Service(MOVE_SERVICE, Move, self.__handle_move) rospy.Service(PLAN_SERVICE, Plan, self.__handle_plan) rospy.Service(MOVE_WITH_PLAN_SERVICE, MoveWithPlan, self.__handle_move_with_plan) rospy.Service(ADD_COLLISION_OBJECTS_SERVICE, AddCollisionObjects, self.__handle_add_objects) rospy.Service(GET_COLLSISION_OBJECT_SERVICE, GetCollisionObject, self.__handle_get_collision_object) rospy.Service(MOVE_MASTCAM_SERVICE, MoveMastCam, self.__handle_mast_cam) rospy.Service(OPEN_GRIPPER_SERVICE, OpenGripper, self.__handle_open_gripper) rospy.Service(CLOSE_GRIPPER_SERVICE, CloseGripper, self.__handle_close_gripper) self.__base_publisher = rospy.Publisher(BASE_ORIGIN_TOPIC, PointStamped) self.__eef_position_publisher = rospy.Publisher( GET_EEF_POSITION_TOPIC, PointStamped) print( "ASDFASDFASFLKJKDJSFKJLASDFKJLDKFDKJLFKDSFHDSKFK############################################################################" ) def __handle_move(self, msg): goal_pose = self.__get_goal_pose(msg) if msg.type == MoveRequest.ACTION_MOVE_ARM_TO: result = self.__manipulation.move_to(goal_pose, msg.do_not_blow_up_list) elif msg.type == MoveRequest.ACTION_MOVE_ARM_AND_BASE_TO: result = self.__manipulation.move_arm_and_base_to( goal_pose, msg.do_not_blow_up_list) elif msg.type == MoveRequest.ACTION_MOVE_BASE: result = self.__manipulation.move_base(goal_pose) else: result = False return MoveResponse(result) def __handle_plan(self, msg): func = None if msg.move_group == PlanRequest.MOVE_GROUP_ARM: func = self.__manipulation.plan_arm_to elif msg.move_group == PlanRequest.MOVE_GROUP_ARM_BASE: func = self.__manipulation.plan_arm_and_base_to goal = self.__get_goal_pose(msg) start_state = None if msg.has_start_state: start_state = msg.start_state plan = func(goal, start_state) return PlanResponse(plan) def __handle_move_with_plan(self, msg): result = self.__manipulation.move_with_plan_to(msg.plan) return MoveWithPlanResponse(result=result) def __handle_add_objects(self, msg): self.__manipulation.get_planning_scene().add_objects(msg.objects) return AddCollisionObjectsResponse() def __handle_mast_cam(self, msg): message = self.__manipulation.pan_tilt(msg.pan, msg.tilt) result = False if message.find("path finished") != -1: result = True return MoveMastCamResponse(result) def __handle_get_collision_object(self, msg): return self.__manipulation.get_planning_scene().get_collision_object( msg.name) def __handle_open_gripper(self, msg): position = manipulation_constants.gripper_max_pose if msg.position is not None: position = msg.position return self.__manipulation.open_gripper(position) def __handle_close_gripper(self, msg): obj = msg.object if obj == CollisionObject(): obj = None grasp_point = msg.grasp_point if grasp_point == PointStamped(): grasp_point = None return self.__manipulation.close_gripper(obj, grasp_point) def __publish(self): rate = rospy.Rate(10) while not rospy.is_shutdown(): self.__base_publisher.publish( self.__manipulation.get_base_origin()) self.__eef_position_publisher.publish( self.__manipulation.get_eef_position()) rate.sleep() def start(self): publisher_thread = Thread(target=self.__publish) publisher_thread.start() while not rospy.is_shutdown(): try: rospy.spin() except KeyboardInterrupt: break def __get_goal_pose(self, msg): if msg.goal_pose_name != '': return msg.goal_pose_name else: return msg.goal_pose
class ManipulationNode(object): def __init__(self): print("init ManipulationNode") rospy.init_node("Manipulation_Control") self.__manipulation = Manipulation() rospy.Service(MOVE_SERVICE, Move, self.__handle_move) rospy.Service(PLAN_SERVICE, Plan, self.__handle_plan) rospy.Service(MOVE_WITH_PLAN_SERVICE, MoveWithPlan, self.__handle_move_with_plan) rospy.Service(ADD_COLLISION_OBJECTS_SERVICE, AddCollisionObjects, self.__handle_add_objects) rospy.Service(GET_COLLSISION_OBJECT_SERVICE, GetCollisionObject, self.__handle_get_collision_object) rospy.Service(MOVE_MASTCAM_SERVICE, MoveMastCam, self.__handle_mast_cam) rospy.Service(OPEN_GRIPPER_SERVICE, OpenGripper, self.__handle_open_gripper) rospy.Service(CLOSE_GRIPPER_SERVICE, CloseGripper, self.__handle_close_gripper) self.__base_publisher = rospy.Publisher(BASE_ORIGIN_TOPIC, PointStamped) self.__eef_position_publisher = rospy.Publisher(GET_EEF_POSITION_TOPIC, PointStamped) print("ASDFASDFASFLKJKDJSFKJLASDFKJLDKFDKJLFKDSFHDSKFK############################################################################") def __handle_move(self, msg): goal_pose = self.__get_goal_pose(msg) if msg.type == MoveRequest.ACTION_MOVE_ARM_TO: result = self.__manipulation.move_to(goal_pose, msg.do_not_blow_up_list) elif msg.type == MoveRequest.ACTION_MOVE_ARM_AND_BASE_TO: result = self.__manipulation.move_arm_and_base_to(goal_pose, msg.do_not_blow_up_list) elif msg.type == MoveRequest.ACTION_MOVE_BASE: result = self.__manipulation.move_base(goal_pose) else: result = False return MoveResponse(result) def __handle_plan(self, msg): func = None if msg.move_group == PlanRequest.MOVE_GROUP_ARM: func = self.__manipulation.plan_arm_to elif msg.move_group == PlanRequest.MOVE_GROUP_ARM_BASE: func = self.__manipulation.plan_arm_and_base_to goal = self.__get_goal_pose(msg) start_state = None if msg.has_start_state: start_state = msg.start_state plan = func(goal, start_state) return PlanResponse(plan) def __handle_move_with_plan(self, msg): result = self.__manipulation.move_with_plan_to(msg.plan) return MoveWithPlanResponse(result=result) def __handle_add_objects(self, msg): self.__manipulation.get_planning_scene().add_objects(msg.objects) return AddCollisionObjectsResponse() def __handle_mast_cam(self, msg): message = self.__manipulation.pan_tilt(msg.pan, msg.tilt) result = False if message.find("path finished") != -1: result = True return MoveMastCamResponse(result) def __handle_get_collision_object(self, msg): return self.__manipulation.get_planning_scene().get_collision_object(msg.name) def __handle_open_gripper(self, msg): position = manipulation_constants.gripper_max_pose if msg.position is not None: position = msg.position return self.__manipulation.open_gripper(position) def __handle_close_gripper(self, msg): obj = msg.object if obj == CollisionObject(): obj = None grasp_point = msg.grasp_point if grasp_point == PointStamped(): grasp_point = None return self.__manipulation.close_gripper(obj, grasp_point) def __publish(self): rate = rospy.Rate(10) while not rospy.is_shutdown(): self.__base_publisher.publish(self.__manipulation.get_base_origin()) self.__eef_position_publisher.publish(self.__manipulation.get_eef_position()) rate.sleep() def start(self): publisher_thread = Thread(target=self.__publish) publisher_thread.start() while not rospy.is_shutdown(): try: rospy.spin() except KeyboardInterrupt: break def __get_goal_pose(self, msg): if msg.goal_pose_name != '': return msg.goal_pose_name else: return msg.goal_pose
# test_task1(m) # m.grasp("cyan_cylinder") # print m.get_eef_position() t_point = geometry_msgs.msg.PoseStamped() t_point.header.frame_id = "/odom_combined" p = Point(0.3, 0, 0.7) # p = Point(0.91,0.66,0.37341) t_point.pose.position = p t_point.pose.orientation = euler_to_quaternion(0, 0, 0) # visualize_poses([t_point]) # m.move_arm_and_base_to([0.92, 0, 0,0,0,0,0,0,0]) # m.move_arm_and_base_to([-0.92, 0, 0,0,0,0,0,0,0]) # m.move_arm_and_base_to(t_point) m.move_arm_and_base_to(t_point) # j = JointState() # j.header.frame_id = "/odom_combined" # print m.get_arm_base_move_group().get_joints() # print m.get_arm_base_move_group().get_joints() # m.move_arm_and_base_to(t_point) # rospy.sleep(5) # plan = m.plan_arm_and_base_to(t_point) # print m.get_end_state(plan) # # # p = Point(-0.3,-0.3,0.5) # # p = Point(0.91,0.66,0.37341) # t_point.pose.position = p # t_point.pose.orientation = euler_to_quaternion(0, pi/2, 0)
# test_task1(m) # m.grasp("cyan_cylinder") # print m.get_eef_position() t_point = geometry_msgs.msg.PoseStamped() t_point.header.frame_id = "/odom_combined" p = Point(0.3, 0, 0.7) # p = Point(0.91,0.66,0.37341) t_point.pose.position = p t_point.pose.orientation = euler_to_quaternion(0, 0, 0) # visualize_poses([t_point]) # m.move_arm_and_base_to([0.92, 0, 0,0,0,0,0,0,0]) # m.move_arm_and_base_to([-0.92, 0, 0,0,0,0,0,0,0]) # m.move_arm_and_base_to(t_point) m.move_arm_and_base_to(t_point) # j = JointState() # j.header.frame_id = "/odom_combined" # print m.get_arm_base_move_group().get_joints() # print m.get_arm_base_move_group().get_joints() # m.move_arm_and_base_to(t_point) # rospy.sleep(5) # plan = m.plan_arm_and_base_to(t_point) # print m.get_end_state(plan) # # # p = Point(-0.3,-0.3,0.5) # # p = Point(0.91,0.66,0.37341) # t_point.pose.position = p # t_point.pose.orientation = euler_to_quaternion(0, pi/2, 0)