def yawToQuat(angle):
quatList = quaternion_from_euler(0.0,0.0,angle)
quat = QuaternionMsg()
quat.x = quatList[0]
quat.y = quatList[1]
quat.z = quatList[2]
quat.w = quatList[3]
return quat
def yawToQuat(angle):
quatList = quaternion_from_euler(0.0, 0.0, angle)
quat = QuaternionMsg()
quat.x = quatList[0]
quat.y = quatList[1]
quat.z = quatList[2]
quat.w = quatList[3]
return quat
def createQuat(x,y,z):
quatList = quaternion_from_euler(x,y,z)
quat = QuaternionMsg()
quat.x = quatList[0]
quat.y = quatList[1]
quat.z = quatList[2]
quat.w = quatList[3]
return quat
def createQuat(x, y, z):
quatList = quaternion_from_euler(x, y, z)
quat = QuaternionMsg()
quat.x = quatList[0]
quat.y = quatList[1]
quat.z = quatList[2]
quat.w = quatList[3]
return quat
def get_message(self):
object_id = self.object_name.split("_")[-1]
msg = ModelDescription()
# generate InstanceId
msg.instance_id = InstanceId()
msg.instance_id.class_name = self.object_type
msg.instance_id.id = object_id
# HACK this information should be in some ontology
if 'ProductWithAN' in self.object_type:
msg.instance_id.ns = '/IAISupermarket/Catalog'
elif 'ShelfLabel' in self.object_type:
msg.instance_id.ns = '/IAISupermarket/ShelfLabels'
else:
msg.instance_id.ns = '/IAISupermarket/Shelves'
# generate MeshDescription
# NOTE: default is to use the class name
msg.mesh_description = MeshDescription()
msg.mesh_description.path_to_mesh = ''
msg.mesh_description.path_to_material = ''
# generate Tag's
msg.tags.append(self.get_tag('SemLog', 'LogType', 'Static'))
msg.tags.append(self.get_tag('SemLog', 'Id', object_id))
msg.tags.append(self.get_tag('SemLog', 'Class', self.object_type))
# set the pose
msg.pose = Pose()
msg.pose.position = Point()
msg.pose.position.x = self.transform[2][0]
msg.pose.position.y = self.transform[2][1]
msg.pose.position.z = self.transform[2][2]
msg.pose.orientation = Quaternion()
msg.pose.orientation.x = self.transform[3][0]
msg.pose.orientation.y = self.transform[3][1]
msg.pose.orientation.z = self.transform[3][2]
msg.pose.orientation.w = self.transform[3][3]
return msg
def __init__(self):
"""Class printing the marker of the wheelchair in rviz
"""
""" MEMBERS """
self.markers = UI_Markers()
self.user_dir = Quaternion(
) # The user commanded direction, it can come from keyboard = key_cmd, voice = voice_cmd, or head = head_cmd
self.user_vel = Twist(
) # The user commanded direction, it can come from keyboard = key_cmd, voice = voice_cmd, or head = head_cmd
"""ROS PARAMETERS"""
self.tf_prefix = rospy.get_param(
'tf_prefix', '') # # Reads the tf_prefix from the ROS namespace
if self.tf_prefix is not '':
self.tf_prefix = '/' + self.tf_prefix
self.discrete_interface = rospy.get_param(
'~discrete_interface', False
) ## To enable/disable the discrete mode Set true when using the keyboard
""" ROS SUBSCRIBERS """
self.user_dir_sub = rospy.Subscriber("user_dir", Quaternion,
self.__user_dir_callback__, None,
1)
self.user_vel_sub = rospy.Subscriber("user_vel", Twist,
self.__user_vel_callback__, None,
1)
self.vocal_command_sub = rospy.Subscriber("recognizer/output", String,
self.__voice_callback__,
None, 1)
"""ROS PUBLISHERS"""
self.wheelchair_marker_pub = rospy.Publisher("wheelchair_marker",
Marker,
queue_size=1)
self.humans_marker_array_pub = rospy.Publisher("humans_marker_array",
MarkerArray,
queue_size=1)
self.dir_marker_pub = rospy.Publisher("dir_marker",
Marker,
queue_size=1)
self.vel_marker_pub = rospy.Publisher("vel_marker",
Marker,
queue_size=1)
self.text_marker_pub = rospy.Publisher('text_marker',
Marker,
queue_size=1)
"""Start
"""
r = rospy.Rate(10.0)
while not rospy.is_shutdown():
wheelchair_marker = self.fill_wheelchair_marker()
humans = self.fill_humans_marker_array()
self.wheelchair_marker_pub.publish(wheelchair_marker)
self.wheelchair_marker_pub.publish(self.fill_sit_human_marker(
)) # I send it in the same topic it works well.
self.humans_marker_array_pub.publish(humans)
self.print_vel()
r.sleep()
def goto_shelf2(self):
shelf = PoseStamped()
header = Header()
header.frame_id = 'map'
shelf.header = header
shelf.pose.position = Point(-1.0, self.dist_to_shelfs, 0.0)
shelf.pose.orientation = Quaternion(0.0, 0.0, 0.0, 1.0)
self(shelf)
def relative_pose(self, position, orientation):
shelf = PoseStamped()
header = Header()
header.frame_id = 'base_footprint'
shelf.header = header
shelf.pose.position = Point(*position)
shelf.pose.orientation = Quaternion(*orientation)
self(shelf)
def parse_pose(pose):
rospy.loginfo('parse_pose')
f_pose = Pose()
f_pose.position.x = pose[0]
f_pose.position.y = pose[1]
f_pose.position.z = pose[2]
f_pose.orientation = Quaternion(*quaternion_from_euler(pose[3], pose[4], pose[5]))
return f_pose
def goto_shelf4(self):
shelf = PoseStamped()
header = Header()
header.frame_id = 'map'
shelf.header = header
shelf.pose.position = Point(-2.9, self.dist_to_shelfs, 0.000)
shelf.pose.orientation = Quaternion(0.0, 0.0, 0.0, 1.0)
# shelf.pose.orientation = Quaternion(*quaternion_from_euler(0,0,pi*.99))
self(shelf)
def goto_shelf1(self):
# self.client.cancel_all_goals()
shelf = PoseStamped()
header = Header()
header.frame_id = 'map'
shelf.header = header
shelf.pose.position = Point(-0.0, self.dist_to_shelfs, 0.0)
shelf.pose.orientation = Quaternion(0.0, 0.0, 0.0, 1.0)
self(shelf)
def add_cam_mast(self, x, y):
"""
Adds a Cylinder where the cam mast is.
:param x: x coordinate
:type: float
:param y: y coordinate
:type: float
"""
pose = PoseStamped()
pose.header.frame_id = "/odom_combined"
pose.pose.position = Point(x, y, 0.55)
pose.pose.orientation = Quaternion(0, 0, 0, 1)
c1 = self.make_cylinder("cm1", pose, [1.1, 0.05])
self.safe_objects.append(c1.id)
self.add_object(c1)
pose.pose.position = Point(x, y, 1.0)
pose.pose.orientation = Quaternion(0, 0, 0, 1)
c1 = self.make_box("mast_cams", pose, [0.3, 0.3, 0.3])
self.safe_objects.append(c1.id)
self.add_object(c1)
def rotate_quaternion_by_quaternion(q1, q2):
"""
Rotates a quaternion by another quaternion
:param q1: first Quaternion
:type: Quaternion
:param q2: second Quaternion
:type: Quaternion
:return: rotated Quaternion
:type: Quaternion
"""
r = quaternion_multiply([q1.x, q1.y, q1.z, q1.w], [q2.x, q2.y, q2.z, q2.w])
return Quaternion(*r)
def add_ground(self, height=-0.01):
"""
Adds a big flat box to the planning scene.
:param height: z value
:type: float
"""
pose = PoseStamped()
pose.header.frame_id = "/odom_combined"
pose.pose.position = Point(0, 0, height)
pose.pose.orientation = Quaternion(0, 0, 0, 1)
box = self.make_box("ground" + str(height), pose, [3, 3, 0.01])
self.safe_objects.append(box.id)
self.add_object(box)
def euler_to_quaternion(roll, pitch, yaw):
"""
Creates a quaternion out of roll, pitch, yaw
:param roll:
:type: float
:param pitch:
:type: float
:param yaw:
:type: float
:return: Quaternion from roll pitch yaw
:type: Quaternion
"""
return Quaternion(*quaternion_from_euler(roll, pitch, yaw))
def test2_1(self):
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = "muh"
co.header.frame_id = "/odom_combined"
cylinder = SolidPrimitive()
cylinder.type = SolidPrimitive.CYLINDER
cylinder.dimensions.append(0.3)
cylinder.dimensions.append(0.03)
co.primitives = [cylinder]
co.primitive_poses = [Pose()]
co.primitive_poses[0].position = Point(1.2185, 0,0)
co.primitive_poses[0].orientation = Quaternion(0,0,0,1)
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions.append(0.1)
box.dimensions.append(0.1)
box.dimensions.append(0.1)
co.primitives.append(box)
co.primitive_poses.append(Pose())
co.primitive_poses[1].position = Point(1.1185, 0,0)
co.primitive_poses[1].orientation = Quaternion(0,0,0,1)
co.primitives.append(box)
co.primitive_poses.append(Pose())
co.primitive_poses[2].position = Point(0, 0,0)
co.primitive_poses[2].orientation = Quaternion(0,0,0,1)
p = PoseStamped()
p.header.frame_id = "/odom_combined"
p.pose.position = Point(1,0,0)
p.pose.orientation = euler_to_quaternion(0,0,0)
self.assertEqual(get_grasped_part(co, get_fingertip(p))[1], 0)
def prolog_to_transform(frame_id, child_frame_id, translation, rotation):
"""
:type frame_id: str
:type child_frame_id: str
:type translation: list
:type rotation: list
:rtype: TransformStamped
"""
if child_frame_id==None or translation==None or rotation==None: return None
t = TransformStamped()
t.header.frame_id = frame_id.encode('utf-8')
t.child_frame_id = child_frame_id.encode('utf-8')
t.transform.translation = Point(*translation)
t.transform.rotation = Quaternion(*rotation)
return t
def get_marker(self):
marker = Marker()
marker.header.frame_id = self.ref_frame
marker.type = marker.MESH_RESOURCE
marker.action = Marker.ADD
marker.id = 1337
marker.ns = self.get_short_name()
marker.color = self.color
marker.scale.x = 1
marker.scale.y = 1
marker.scale.z = 1
marker.frame_locked = True
marker.pose.position = Point(*self.transform[-2])
marker.pose.orientation = Quaternion(*self.transform[-1])
marker.mesh_resource = self.mesh_path[:-4] + '.dae'
return marker
def rotate_quaternion(q, roll, pitch, yaw):
'''
Rotates a quaternion by roll, pitch, yaw.
:param q: Quaternion
:type: Quaternion
:param roll: roll
:type: float
:param pitch: pitch
:type: float
:param yaw: yaw
:type: float
:return: rotated quaternion
:type: Quaternion
'''
angle = quaternion_from_euler(roll, pitch, yaw)
no = quaternion_multiply([q.x, q.y, q.z, q.w], angle)
return Quaternion(*no)
def addConveyorBelt(self, msg):
print "addConveyorBelt callback"
# print msg
# zu 1
drop_point = msg.conveyor_belt.drop_center_point
print drop_point
# zu 2
conveyor_belt_width = msg.conveyor_belt.drop_deviation.y * 2
print conveyor_belt_width
# zu 3
conveyor_belt_height = drop_point.z
print conveyor_belt_height
# zu 4
conveyor_belt_length = msg.conveyor_belt.move_direction_and_length.y
print conveyor_belt_length
pose = PoseStamped()
pose.header.frame_id = "/odom_combined"
# h w d
# pose.pose.position = Point(drop_point.x,
# (drop_point.y - abs(conveyor_belt_length / 2)),
# drop_point.z - abs(conveyor_belt_height / 2))
# pose.pose.orientation = Quaternion(0, 0, 0, 1)
l = math.sqrt(msg.conveyor_belt.move_direction_and_length.x *
msg.conveyor_belt.move_direction_and_length.x +
conveyor_belt_length * conveyor_belt_length)
alpha = math.pi / 2 - math.asin(conveyor_belt_length / l)
wx = conveyor_belt_width * math.cos(alpha)
wy = conveyor_belt_width * math.sin(alpha)
w = wx + wy
pose.pose.position = Point(
drop_point.x, (drop_point.y - abs(conveyor_belt_length / 2)),
drop_point.z - abs(conveyor_belt_height / 2))
pose.pose.orientation = Quaternion(0, 0, 0, 1)
print pose
cb_size = [w * 2, l, conveyor_belt_height]
co = utils.manipulation.get_planning_scene().make_box(
"conveyor_belt", pose, cb_size)
utils.manipulation.get_planning_scene().add_object(co)
def get_marker(self):
marker = Marker()
marker.header.frame_id = self.ref_frame
marker.type = marker.MESH_RESOURCE
marker.action = Marker.ADD
marker.id = 1337
marker.ns = self.object_name
marker.color = self.color
marker.scale.x = 1
marker.scale.y = 1
marker.scale.z = 1
marker.frame_locked = True
marker.pose.position = Point(*self.transform[-2])
marker.pose.orientation = Quaternion(*self.transform[-1])
if len(self.mesh_path) > 2 and self.mesh_path[0] == "'":
marker.mesh_resource = self.mesh_path[1:-1]
else:
marker.mesh_resource = self.mesh_path
return marker
def three_points_to_quaternion(origin, to, roll=None):
"""
Calculates a quaternion that points from "origin" to "to" and lies in the plane defined by "origin", "to" and "roll".
:param origin: form this point
:type: Point
:param to: to this point
:type: Point
:param roll: in a plane defined by this third point
:type: Point or None
:return: orientation
:type: Quaternion
"""
muh = False
if roll is None: #TODO buggy
roll = Point(0, 0, origin.z + 1.000001)
muh = True
n_1 = subtract_point(to, origin)
n_1 = normalize(n_1)
n = subtract_point(roll, origin)
n = normalize(n)
n_2 = subtract_point(n, multiply_point(dot_product(n, n_1), n_1))
n_2 = normalize(n_2)
n_3 = cross_product(n_1, n_2)
n_3 = normalize(n_3)
rm = np.array(((n_1.x, n_2.x, n_3.x, 0), (n_1.y, n_2.y, n_3.y, 0),
(n_1.z, n_2.z, n_3.z, 0), (0, 0, 0, 1)),
dtype=np.float64)
q = quaternion_from_matrix(rm)
q = Quaternion(*q)
if muh:
q = rotate_quaternion(q, -pi / 2, 0, 0)
return q
def make6DofMarker(self, interaction_mode, root_link, tip_link):
def normed_q(x, y, z, w):
return np.array([x, y, z, w]) / np.linalg.norm([x, y, z, w])
int_marker = InteractiveMarker()
p = PoseStamped()
p.header.frame_id = tip_link
p.pose.orientation.w = 1
int_marker.header.frame_id = tip_link
int_marker.pose.orientation.w = 1
int_marker.pose = self.transformPose(root_link, p).pose
int_marker.header.frame_id = root_link
int_marker.scale = .2
int_marker.name = "eef_{}_to_{}".format(root_link, tip_link)
# insert a box
self.makeBoxControl(int_marker)
int_marker.controls[0].interaction_mode = interaction_mode
if interaction_mode != InteractiveMarkerControl.NONE:
control_modes_dict = {
InteractiveMarkerControl.MOVE_3D: "MOVE_3D",
InteractiveMarkerControl.ROTATE_3D: "ROTATE_3D",
InteractiveMarkerControl.MOVE_ROTATE_3D: "MOVE_ROTATE_3D"
}
int_marker.name += "_" + control_modes_dict[interaction_mode]
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 0, 0, 1)
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 0, 0, 1)
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 1, 0, 1))
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 1, 0, 1))
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 0, 1, 1))
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 0, 1, 1))
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
return int_marker
def make_6dof_marker(self, interaction_mode, root_link, tip_link):
def normed_q(x, y, z, w):
return np.array([x, y, z, w]) / np.linalg.norm([x, y, z, w])
int_marker = InteractiveMarker()
int_marker.header.frame_id = tip_link
int_marker.pose.orientation.w = 1
int_marker.scale = MARKER_SCALE
int_marker.name = u'eef_{}_to_{}'.format(root_link, tip_link)
# insert a box
self.make_sphere_control(int_marker)
int_marker.controls[0].interaction_mode = interaction_mode
if interaction_mode != InteractiveMarkerControl.NONE:
control_modes_dict = {
InteractiveMarkerControl.MOVE_3D: u'MOVE_3D',
InteractiveMarkerControl.ROTATE_3D: u'ROTATE_3D',
InteractiveMarkerControl.MOVE_ROTATE_3D: u'MOVE_ROTATE_3D'
}
int_marker.name += u'_' + control_modes_dict[interaction_mode]
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 0, 0, 1)
control.name = u'rotate_x'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 0, 0, 1)
control.name = u'move_x'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 1, 0, 1))
control.name = u'rotate_z'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 1, 0, 1))
control.name = u'move_z'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 0, 1, 1))
control.name = u'rotate_y'
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 0, 1, 1))
control.name = u'move_y'
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
self.markers[int_marker.name] = int_marker
return int_marker
def pub_goal_marker(self, header, pose):
"""
:param header:
:type header: std_msgs.msg._Header.Header
:param pose:
:type pose: Pose
"""
ma = MarkerArray()
m = Marker()
m.action = Marker.ADD
m.type = Marker.CYLINDER
m.header = header
old_q = [
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
]
# x
m.pose = deepcopy(pose)
m.scale.x = 0.05 * MARKER_SCALE
m.scale.y = 0.05 * MARKER_SCALE
m.scale.z = MARKER_SCALE
muh = qv_mult(old_q, [m.scale.z / 2, 0, 0])
m.pose.position.x += muh[0]
m.pose.position.y += muh[1]
m.pose.position.z += muh[2]
m.pose.orientation = Quaternion(*quaternion_multiply(
old_q, quaternion_about_axis(np.pi / 2, [0, 1, 0])))
m.color.r = 1
m.color.g = 0
m.color.b = 0
m.color.a = 1
m.ns = u'interactive_marker_{}_{}'.format(self.root_link,
self.tip_link)
m.id = 0
ma.markers.append(m)
# y
m = deepcopy(m)
m.pose = deepcopy(pose)
muh = qv_mult(old_q, [0, m.scale.z / 2, 0])
m.pose.position.x += muh[0]
m.pose.position.y += muh[1]
m.pose.position.z += muh[2]
m.pose.orientation = Quaternion(*quaternion_multiply(
old_q, quaternion_about_axis(-np.pi / 2, [1, 0, 0])))
m.color.r = 0
m.color.g = 1
m.color.b = 0
m.color.a = 1
m.ns = u'interactive_marker_{}_{}'.format(self.root_link,
self.tip_link)
m.id = 1
ma.markers.append(m)
# z
m = deepcopy(m)
m.pose = deepcopy(pose)
muh = qv_mult(old_q, [0, 0, m.scale.z / 2])
m.pose.position.x += muh[0]
m.pose.position.y += muh[1]
m.pose.position.z += muh[2]
m.color.r = 0
m.color.g = 0
m.color.b = 1
m.color.a = 1
m.ns = u'interactive_marker_{}_{}'.format(self.root_link,
self.tip_link)
m.id = 2
ma.markers.append(m)
self.marker_pub.publish(ma)
joint_state.position = [
0.0174419, -1.33713, 1.47922, -1.05844, -0.00214559, -0.660072
]
# 0.0138173009806,
# -0.948155685642,
# 1.64415424887,
# 0.833014565285,
# -0.00345735390984,
# -1.52754142152, ]
test.send_joint_goal(joint_state)
goal = PoseStamped()
goal.header.frame_id = 'gripper_tool_frame'
goal.pose.position.x = 0.1
goal.pose.position.z = -0.2
goal.pose.orientation.w = 1.0
test.send_cart_goal(goal)
goal = PoseStamped()
goal.header.frame_id = 'gripper_tool_frame'
goal.pose.position = Point(0, 0, 0)
goal.pose.orientation = Quaternion(0.0, 0.0, 0.70710678, 0.70710678)
test.send_cart_goal(goal)
goal = PoseStamped()
goal.header.frame_id = 'gripper_tool_frame'
goal.pose.position = Point(0, -0.1, 0)
goal.pose.orientation = Quaternion(-0.35355339, -0.35355339, -0.14644661,
0.85355339)
test.send_cart_goal(goal)
def matrixToPose(matrix):
t = tuple(translation_from_matrix(matrix))
q = tuple(quaternion_from_matrix(matrix))
return Pose(Point(t[0], t[1], t[2]), Quaternion(q[0], q[1], q[2], q[3]))
def wiggle_cb(self, goal):
# check for invalid goals
if goal.arm != WiggleGoal.LEFT_ARM and goal.arm != WiggleGoal.RIGHT_ARM:
rospy.logerr('arm goal should be {} or {}'.format(
WiggleGoal.LEFT_ARM, WiggleGoal.RIGHT_ARM))
return self.wiggle_action_server.set_aborted(
text='arm goal should be {} or {}'.format(
WiggleGoal.LEFT_ARM, WiggleGoal.RIGHT_ARM))
if np.linalg.norm(
np.array([
goal.goal_pose.pose.orientation.x,
goal.goal_pose.pose.orientation.y,
goal.goal_pose.pose.orientation.z,
goal.goal_pose.pose.orientation.w
])) < 0.99:
rospy.logerr('invalid orientation in goal position')
return self.wiggle_action_server.set_aborted(
text='invalid orientation in goal position')
rospy.loginfo('wiggle wiggle wiggle')
movement_duration = goal.timeout.data.to_sec()
wiggle_per_sec = goal.cycle_time
number_of_wiggles = int(movement_duration / wiggle_per_sec)
number_of_wiggle_points = 10
radius_x = goal.upperbound_x
radius_y = goal.upperbound_y
hz = number_of_wiggle_points / wiggle_per_sec
r = rospy.Rate(hz)
tcp_goal_pose = self.transformPose('left_gripper_tool_frame',
goal.goal_pose)
spiral = self.create_spiral(goal_height=tcp_goal_pose.pose.position.z,
radius_x=radius_x,
radius_y=radius_y,
number_of_points=number_of_wiggle_points,
number_of_wiggles=number_of_wiggles)
header = Header()
header.frame_id = 'left_gripper_tool_frame'
pose_list = self.np_array_to_pose_stampeds(spiral,
Quaternion(0, 0, 0, 1),
header)
pose_list = [
deepcopy(self.transformPose('base_footprint', pose))
for pose in pose_list
]
end_pose = self.transformPose('base_footprint', goal.goal_pose)
for i, pose in enumerate(pose_list):
if self.check_for_stop():
return
self.pub_wiggle(pose, goal.arm)
wiggle_feedback = WiggleFeedback()
wiggle_feedback.progress = .9 * (i + 1) / float(len(pose_list))
self.wiggle_action_server.publish_feedback(wiggle_feedback)
r.sleep()
# move back to real goal
time_to_get_to_endpose = rospy.Duration(1.0)
deadline = rospy.get_rostime() + time_to_get_to_endpose
while rospy.get_rostime() < deadline:
if self.check_for_stop():
return
self.pub_wiggle(end_pose, goal.arm)
result = WiggleFeedback()
result.progress = 1.
self.wiggle_action_server.publish_feedback(result)
self.wiggle_action_server.set_succeeded(result)