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()
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