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 __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 execute(self, userdata):
rospy.logdebug('CamToDropzone: Executing state CamToDropzone')
listener = TransformListener()
if utils.manipulation is None:
utils.manipulation = Manipulation()
utils.manipulation.set_turbo_mode()
utils.manipulation.set_movement_time(9)
# TODO: Exception schmeissen wenn abort_after vergangen ist
abort_after = 15
then = int(time.time())
now = int(time.time())
while not listener.frameExists("drop_point"):
if not (now - then < abort_after):
rospy.logdebug('drop_point frame not found!')
return 'fail'
rospy.loginfo("wait for drop_point frame")
rospy.sleep(2.)
now = int(time.time())
then = int(time.time())
now = int(time.time())
while not listener.frameExists("mdl_middle"):
if not (now - then < abort_after):
rospy.logdebug('CamToDropzone: mdl_middle frame not found!')
return 'fail'
rospy.sleep(2.)
now = int(time.time())
if len(userdata.scan_conveyor_pose) == 0:
rospy.logdebug('CamToDropzone: No scanPose, calculate now...')
p = utils.manipulation.scan_conveyor_pose()
userdata.scan_conveyor_pose.append(p)
else:
rospy.logdebug('CamToDropzone: scanPose found!')
p = userdata.scan_conveyor_pose[0]
rospy.logdebug('CamToDropzone: Move arm to scan_conveyor_pose')
for i in range(0, 3):
if utils.manipulation.move_to(p):
return 'scanPoseReached'
else:
if i == 2:
rospy.logdebug('CamToDropzone: Cant move arm to scan_conveyor_pose')
return 'fail'
def execute(self, userdata):
rospy.loginfo('Executing state CamToDropzone')
if utils.manipulation is None:
utils.manipulation = Manipulation()
rospy.sleep(2)
# 1. Foerderband in PS haun, wenns noch nicht drin ist.
subscriber = rospy.Subscriber("suturo/yaml_pars0r", Task,
self.addConveyorBelt)
# 1: Dropzone Punkt ueber Vektor dp finden
# 2: Breite des Bandes ueber 2*drop_deviation[1] bestimmen
# 3: Hoehe des Bandes ueber drop_center_point[2] (z wert) + Banddicke (0.01m) holen
# 4: Lange des Bandes mit mdl Vektor bestimmen + 0.10m fuer Laenge hinter dem dp Vektor
# TODO 5: Orientierung de Bandes mit mdl Vektor bestimmen
return 'success'
def execute(self, userdata):
rospy.loginfo('Executing state SearchObject')
if self._missing_objects is None:
self._missing_objects = []
for obj in userdata.yaml.objects:
print("adding missing object" + obj.name)
self._missing_objects.append(obj.name)
if utils.manipulation is None:
utils.manipulation = Manipulation()
rospy.sleep(2)
# Add the found objects
for obj in userdata.fitted_objects:
# Check if the object was already found
if [
x for x in self._found_objects
if x.mpe_object.id == obj.mpe_object.id
]:
rospy.loginfo('Object %s was already found.' %
obj.mpe_object.id)
else:
self._found_objects.append(obj)
if obj.mpe_object.id in self._missing_objects:
self._missing_objects.remove(obj.mpe_object.id)
userdata.objects_found = self._found_objects
rospy.loginfo('##########################################')
rospy.loginfo('Objects found:')
for obj in self._found_objects:
rospy.loginfo(obj.mpe_object.id)
rospy.loginfo('##########################################')
# Check if all objects were found
if not self._missing_objects:
return 'noObjectsLeft'
return 'searchObject'
def execute(self, userdata):
rospy.logdebug('PlaceTask6: Executing state PlaceTask6')
listener = TransformListener()
if utils.manipulation is None:
utils.manipulation = Manipulation()
# TODO: Exception schmeissen wenn abort_after vergangen ist
abort_after = 15
then = int(time.time())
now = int(time.time())
while not listener.frameExists("target_zone"):
if not now - then < abort_after:
rospy.logdebug("PlaceTask6: target_zone frame not found")
return 'fail'
rospy.loginfo("wait for target_zone frame")
rospy.sleep(2.)
now = int(time.time())
self.__place_pose = self.get_place_point(userdata)
rospy.logdebug('PlaceTask6: Move Arm to Place Pose')
rospy.logdebug(self.__place_pose)
for i in range(0, 7):
if utils.manipulation.move_to(self.__place_pose):
rospy.logdebug('PlaceTask6: OpenGripper')
utils.manipulation.open_gripper()
return 'success'
else:
rospy.logdebug('PlaceTask6: Calculate new place position...')
if i <= 2:
self.__place_pose = self.get_place_point(userdata)
if i > 2:
self.__place_pose = self.get_place_point(userdata, 3)
if i == 6:
rospy.logdebug('PlaceTask6: Cant find place position!')
utils.manipulation.open_gripper()
return 'fail'
def create_manipulation(self):
utils.manipulation = Manipulation()
#!/usr/bin/env python
import rospy
from suturo_planning_manipulation.manipulation import Manipulation
if __name__ == '__main__':
# print (lambda x: a(2, x))(3)
rospy.init_node('head_mover', anonymous=True)
#
mani = Manipulation()
print mani.get_arm_base_move_group().get_current_joint_values()
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
# dest.point = Point( -0.85, 0.85, 0.00)
# mani.place_and_move(dest)
#
# mani.grasp_and_move("green_cylinder")
#
# dest = PointStamped()
# dest.header.frame_id = "/odom_combined"
# dest.point = Point(-0.85, -0.85, 0)
# mani.place_and_move(dest)
if __name__ == '__main__':
rospy.init_node('head_mover', anonymous=True)
mani = Manipulation()
# test_task3(mani)
num_of_scans = 12
max_rad = 5.9
rad_per_step = max_rad / num_of_scans
#x is zwischen 0 und num_of_scans
x = 5
rad = x * rad_per_step - 2.945
rospy.loginfo('Turning arm %s' % str(rad))
mani.turn_arm(rad)
# co = mani.get_planning_scene().get_collision_object("red_cube")
# print mani.calc_object_weight(co, 2710)
# print mani.get_arm_move_group().get_current_joint_values()
# dest.header.frame_id = "/odom_combined"
# dest.point = Point( -0.85, 0.85, 0.00)
# mani.place_and_move(dest)
# mani.grasp_and_move("green_cylinder")
#
# dest = PointStamped()
# dest.header.frame_id = "/odom_combined"
# dest.point = Point(-0.85, -0.85, 0)
# mani.place_and_move(dest)
if __name__ == '__main__':
rospy.init_node('head_mover', log_level=rospy.DEBUG)
m = Manipulation()
# print m.get_arm_base_move_group().get_goal_orientation_tolerance()
# print m.get_arm_base_move_group().get_goal_position_tolerance()
# print m.get_arm_base_move_group().get_goal_joint_tolerance()
# print m.get_arm_base_move_group().get_goal_tolerance()
# print m.get_arm_move_group().get
# m.grasp("cyan_cylinder")
# m.open_gripper()
# m.close_gripper()
# m.get_arm_move_group().set_named_target("scan_pose1")
# print m.get_arm_move_group().get_goal_tolerance()
# print m.get_arm_move_group().get_goal_position_tolerance()
# print m.get_arm_move_group().get_goal_orientation_tolerance()
# rospy.sleep(2)
# test_task1(m)
def execute(self, userdata):
rospy.loginfo('Executing state ScanMapMastCam')
if utils.manipulation is None:
utils.manipulation = Manipulation(userdata.yaml)
rospy.sleep(2)
utils.map = Map(2)
# arm_base = utils.manipulation.get_base_origin()
# rospy.sleep(4)
utils.manipulation.pan_tilt(0.2, 0.5)
rospy.sleep(utils.waiting_time_before_scan)
utils.map.add_point_cloud(scene_cam=True)
utils.manipulation.pan_tilt(0.2825, 0.775)
rospy.sleep(utils.waiting_time_before_scan)
utils.map.add_point_cloud(scene_cam=True)
utils.manipulation.pan_tilt(0, 1.1)
rospy.sleep(utils.waiting_time_before_scan)
utils.map.add_point_cloud(scene_cam=True)
utils.manipulation.pan_tilt(-0.2825, 0.775)
rospy.sleep(utils.waiting_time_before_scan)
utils.map.add_point_cloud(scene_cam=True)
utils.manipulation.pan_tilt(-0.2, 0.5)
rospy.sleep(utils.waiting_time_before_scan)
utils.map.add_point_cloud(scene_cam=True)
utils.manipulation.pan_tilt(0, 0.45)
if not userdata.enable_movement:
return 'mapScanned'
co = utils.map.to_collision_object()
utils.manipulation.get_planning_scene().add_object(co)
# utils.manipulation.get_planning_scene().add_object(co)
rospy.sleep(2)
n = 8
for i in range(0, n):
a = 2 * pi * ((i + 0.0) / (n + 0.0))
goal_base_pose = PoseStamped()
goal_base_pose.header.frame_id = "/odom_combined"
goal_base_pose.pose.position = Point(cos(a), sin(a), 0)
if goal_base_pose.pose.position.x < 0 and goal_base_pose.pose.position.y < 0 or \
goal_base_pose.pose.position.x > 0 and goal_base_pose.pose.position.y > 0:
continue
goal_base_pose.pose.position = set_vector_length(
0.25, goal_base_pose.pose.position)
goal_base_pose.pose.orientation.w = 1
if utils.manipulation.move_base(goal_base_pose):
rospy.sleep(utils.waiting_time_before_scan)
utils.map.add_point_cloud(
arm_origin=utils.manipulation.get_base_origin().point,
scene_cam=True)
break
# utils.map.add_point_cloud(arm_origin=arm_base, scene_cam=True)
# utils.map.add_point_cloud(arm_origin=arm_base, scene_cam=True)
# utils.map.publish_as_marker()
if userdata.yaml.task_type == Task.TASK_5:
utils.map.all_unknowns_to_obstacle()
#utils.map.remove_puzzle_fixture(userdata.yaml)
co = utils.map.to_collision_object()
utils.manipulation.get_planning_scene().add_object(co)
return 'mapScanned'
# dest.header.frame_id = "/odom_combined"
# # dest.point = Point(-0.3, -0.4, 0.03)
# dest.point = Point( -0.85, 0.85, 0.00)
# mani.place_and_move(dest)
#
# mani.grasp_and_move("green_cylinder")
#
# dest = PointStamped()
# dest.header.frame_id = "/odom_combined"
# dest.point = Point(-0.85, -0.85, 0)
# mani.place_and_move(dest)
if __name__ == '__main__':
rospy.init_node('head_mover', anonymous=True)
mani = Manipulation()
# test_task3(mani)
num_of_scans = 12
max_rad = 5.9
rad_per_step = max_rad / num_of_scans
#x is zwischen 0 und num_of_scans
x = 5
rad = x * rad_per_step - 2.945
rospy.loginfo('Turning arm %s' % str(rad))
mani.turn_arm(rad)
# co = mani.get_planning_scene().get_collision_object("red_cube")
# print mani.calc_object_weight(co, 2710)
# print mani.get_arm_move_group().get_current_joint_values()
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
# dest.header.frame_id = "/odom_combined"
# dest.point = Point( -0.85, 0.85, 0.00)
# mani.place_and_move(dest)
# mani.grasp_and_move("green_cylinder")
#
# dest = PointStamped()
# dest.header.frame_id = "/odom_combined"
# dest.point = Point(-0.85, -0.85, 0)
# mani.place_and_move(dest)
if __name__ == "__main__":
rospy.init_node("head_mover", log_level=rospy.DEBUG)
m = Manipulation()
# print m.get_arm_base_move_group().get_goal_orientation_tolerance()
# print m.get_arm_base_move_group().get_goal_position_tolerance()
# print m.get_arm_base_move_group().get_goal_joint_tolerance()
# print m.get_arm_base_move_group().get_goal_tolerance()
# print m.get_arm_move_group().get
# m.grasp("cyan_cylinder")
# m.open_gripper()
# m.close_gripper()
# m.get_arm_move_group().set_named_target("scan_pose1")
# print m.get_arm_move_group().get_goal_tolerance()
# print m.get_arm_move_group().get_goal_position_tolerance()
# print m.get_arm_move_group().get_goal_orientation_tolerance()
# rospy.sleep(2)
# test_task1(m)
