Пример #1
0
from math import pi
from geometry_msgs.msg import Quaternion
import rospy
from suturo_planning_manipulation.force_test import plan_to
from suturo_planning_manipulation.manipulation import Manipulation, geometry_msgs
from suturo_planning_manipulation.mathemagie import euler_to_quaternion

__author__ = 'benny'

if __name__ == '__main__':
    rospy.init_node('force_test', anonymous=True)
    m = Manipulation()
    t_point = geometry_msgs.msg.Pose()
    t_point.position.x = -0.4
    t_point.position.y = 0
    t_point.position.z = 0.3
    t_point.orientation = euler_to_quaternion(0, pi, 0)
    m.direct_move(plan_to(t_point, m))
    m.open_gripper()
Пример #2
0
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
Пример #3
0
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