def write_with_offset(outbag, topic, msg, t, offset_sec):
def add_offset(t, offset):
# ignore those which are not unix timestamps
return t + offset if is_unix_time(t.to_sec()) else t
try:
offset = Duration.from_sec(offset_sec)
stamp = 0
if hasattr(msg, 'header'):
msg.header.stamp = add_offset(msg.header.stamp, offset)
elif hasattr(msg, 'stamp'):
msg.stamp = add_offset(msg.stamp, offset)
elif hasattr(msg, 'transforms'):
for id,_ in enumerate(msg.transforms):
msg.transforms[id].header.stamp = add_offset(msg.transforms[id].header.stamp, offset)
# for RealSense metadata
elif hasattr(msg, 'value'):
try:
value = int(msg.value)
if is_unix_time(float(value) / 1000):
msg.value = str(value + int(offset_sec * 1000))
except ValueError:
try:
value = float(msg.value)
if is_unix_time(value / 1000):
msg.value = '%.6f' % (value + offset_sec * 1000)
except ValueError:
pass
except TypeError:
print (str(msg))
print ('Cannot process above message: topic=%s, t=%.9f, offset=%.9f' % (topic, t.to_sec(), offset_sec))
exit()
else:
t = add_offset(t, offset)
outbag.write(topic, msg, t)
def __init__(self,
controller='motion/controller/joint_state_head',
tolerance=0.17,
timeout=10.0,
joint_topic="joint_states",
outcomes=['done', 'failed', 'timeout']):
super(SetJointStateBase, self).__init__(outcomes=outcomes)
# Store topic parameter for later use.
self._controller = controller
self._joint_topic = joint_topic
self._tolerance = tolerance
self._timeout = Duration.from_sec(timeout)
# create proxies
self._action_client = ProxyActionClient(
{self._controller: SetOperationalAction})
self._pose_publisher = ProxyPublisher(
{self._controller + '/in_joints_ref': JointState})
self._pose_subscriber = ProxySubscriberCached(
{self._joint_topic: JointState})
# timestamp
self._timestamp = None
# error in enter hook
self._error = False
def execute(self, userdata):
if self._error:
return 'failed'
# check if time elasped
if Time.now() - self._start_timestamp > self._duration:
return 'done'
# check if we have tosend new point
if Time.now() - self._movement_timestamp > self._movement_duration:
# determine new interval
self._movement_timestamp = Time.now()
self._movement_duration = Duration.from_sec(
random.uniform(*self._interval))
# form message
msg = JointState()
msg.header = Header(stamp=Time.now())
msg.name = self._joints
for i in range(0, len(self._joints)):
x = random.uniform(0, 1)
msg.position.append(self._minimal[i] * x + self._maximal[i] *
(1 - x))
# send message
try:
self._publisher.publish(self._controller + '/in_joints_ref',
msg)
except Exception as e:
Logger.logwarn('Failed to send JointState message `' +
self._topic + '`:\n%s' % str(e))
def __init__(self, outcomes = ['unknown'], pose_ns = 'saved_msgs/joint_state', tolerance = 0.17, joint_topic = "joint_states", timeout = 1.0):
# Process supplied pose list. Note it must be processed befor superclass constructor is called,
# beacuse it changes 'outcomes' list due to side effect.
# check if 'unknown' outcome is present
if 'unknown' not in outcomes:
raise RuntimeError, '"unknown" should presents in state outcomes'
# remove 'unknown' while preserving items order
poses_names = [ outcome for outcome in outcomes if outcome != 'unknown' ]
# Load poses from paramere server. Pose is loaded in form dict(joint name -> position),
# but they are stored as a list of tuples (pose_name, pose_dict) to preserve order.
self._poses = [ (name, self.load_joint_state(pose_ns, name)) for name in poses_names ]
# Call superclass constructor.
super(CheckJointState, self).__init__(outcomes = outcomes)
# Subscribe to the joint topic.
self._joint_topic = joint_topic
self._pose_subscriber = ProxySubscriberCached({ self._joint_topic: JointState })
# Store topic parameter for later use.
self._tolerance = tolerance
self._timeout = Duration.from_sec(timeout)
# timestamp
self._timestamp = None
def execute(self, userdata):
if self._error:
return 'failed'
# check if time elasped
if Time.now() - self._start_timestamp > self._duration:
return 'done'
# check if we have tosend new point
if Time.now() - self._movement_timestamp > self._movement_duration:
# determine new interval
self._movement_timestamp = Time.now()
self._movement_duration = Duration.from_sec(
random.uniform(*self._interval))
# form message
msg = JointState()
msg.header = Header(stamp=Time.now())
msg.name = ['joint52', 'joint53', 'eyes_pitch', 'eyes_yaw']
# random durection
x = random.uniform(0, 1)
y = random.uniform(0, 1)
# compute head position
msg.position = [0, 0, 0, 0]
msg.position[0] = self._min2356[0] * x + self._max2356[0] * (1 - x)
msg.position[1] = self._min2356[1] * y + self._max2356[1] * (1 - y)
# compute eyes position
msg.position[2] = self._min2356[2] * y + self._max2356[2] * (1 - y)
msg.position[3] = self._min2356[3] * x + self._max2356[3] * (1 - x)
# send message
try:
self._publisher.publish(self._controller + '/in_joints_ref',
msg)
except Exception as e:
Logger.logwarn('Failed to send JointState message `' +
self._topic + '`:\n%s' % str(e))
def test_extrapolation(dir, m, pose1, pose2, service, time1, time_12):
# this stuff is done to test the accuracy of the extrapolation
# wait some time to get the expected position
rospy.sleep(Duration.from_sec(5))
resp = service("centroid,cuboid")
object3 = resp.objects[0].object
time3 = resp.stamp
pose3 = m.transform_to(object3)
# Get the duration between the first perception and now
time_13 = time3 - time1
# get the relation between the first and second duration
diff_time = float(time_13.to_sec()) / float(time_12.to_sec())
# table = [["stamps: ", time1, time2, time3], ["duration: ", time_12, time_13, diff_time]]
# print tabulate(table)
# calculate the new longer vector (scalar multiplication by hand)
dir_13 = numpy.array([diff_time * dir[0], diff_time * dir[1]])
# add this new vector on the points from the first perception
pose_comp = copy.deepcopy(pose1)
pose_comp.primitive_poses[0].position.x += dir_13[0]
pose_comp.primitive_poses[0].position.y += dir_13[1]
pose_comp.id = "red_cube"
print pose_comp
# print out the table for the different poses for debug
table = [get_position(pose1), get_position(pose2), get_position(pose3), get_position(pose_comp)]
print tabulate(table)
dev = get_position(pose3)[1] / get_position(pose_comp)[1] * 100
print dev
return pose_comp
def __init__(self,
topic='motion/controller/look_at/in_pose_ref',
duration=120,
interval=[3, 5],
maxXYZ=[1, 0.3, 0.5],
minXYZ=[1.0, -0.3, 0.0],
frame_xyz='base_link',
frame_out='odom_combined'):
super(RandPoseGenerator, self).__init__(outcomes=['done', 'failure'])
# Store topic parameter for later use.
if not isinstance(topic, str):
raise ValueError("Topic name must be string.")
if not isinstance(duration, (int, float)) or duration <= 0:
raise ValueError("Duration must be positive number.")
if len(interval) != 2 or any([
not isinstance(t, (int, float)) for t in interval
]) or any([t < 0 for t in interval]) or interval[0] > interval[1]:
raise ValueError("Interval must represent interval of time.")
if len(minXYZ) != 3 or any(
[not isinstance(v, (int, float)) for v in minXYZ]):
raise ValueError("minXYZ: list of three numbers was expected.")
if len(maxXYZ) != 3 or any(
[not isinstance(v, (int, float)) for v in maxXYZ]):
raise ValueError("maxXYZ: list of three numbers was expected.")
if not isinstance(frame_xyz, str) or not isinstance(frame_out, str):
raise ValueError("Frame names must be string.")
self._topic = topic
self._duration = Duration.from_sec(duration)
self._interval = interval
self._minXYZ = minXYZ
self._maxXYZ = maxXYZ
self._frame_in = frame_xyz
self._frame_out = frame_out
# create proxies
self._publisher = ProxyPublisher({self._topic: PoseStamped})
self._tf_listener = ProxyTransformListener().listener()
self._tf_listener.waitForTransform(self._frame_out, self._frame_in,
rospy.Time(), rospy.Duration(1))
if not self._tf_listener.canTransform(self._frame_out, self._frame_in,
rospy.Time(0)):
raise ValueError(
"Unable to perform transform between frames %s and %s." %
(self._frame_out, self._frame_in))
# state
self._start_timestamp = None
self._movement_timestamp = None
self._movement_duration = Duration()
# error in enter hook
self._error = False
def set_movement_parameters(self, duration, interval, min2356, max2356):
if not isinstance(duration, (int, float)) or duration < 0:
raise ValueError("Duration must be nonegative number.")
if len(interval) != 2 or any([
not isinstance(t, (int, float)) for t in interval
]) or any([t < 0 for t in interval]) or interval[0] > interval[1]:
raise ValueError("Interval must represent interval of time.")
if len(min2356) != 4 or any(
[not isinstance(v, (int, float)) for v in min2356]):
raise ValueError("min2356: list of four numbers was expected.")
if len(max2356) != 4 or any(
[not isinstance(v, (int, float)) for v in max2356]):
raise ValueError("max2356: list of four numbers was expected.")
self._duration = Duration.from_sec(duration)
self._interval = interval
self._min2356 = min2356
self._max2356 = max2356
def percieve_object(self):
rospy.logdebug('FastGrasp: Start percieving Object')
# create service
service = rospy.ServiceProxy("/suturo/GetGripper", GetGripper)
# get the first perception
rospy.logdebug('FastGrasp: call Perception Service')
resp = service("firstConveyorCall,cuboid")
if len(resp.objects) == 0 or not resp.objects[0].c_cuboid_success:
rospy.logdebug('FastGrasp: objects empty or no cuboid 1')
return -1
object1 = resp.objects[0].object
self.__perceived_pose_time = resp.stamp
# wait some time until the second perception
rospy.sleep(Duration.from_sec(0.5))
resp = service("cuboid")
if len(resp.objects) == 0 or not resp.objects[0].c_cuboid_success:
rospy.logdebug('FastGrasp: objects empty or no cuboid 2')
return -2
object2 = resp.objects[0].object
time2 = resp.stamp
# transform the points into odom_combined
self.__perceived_pose = utils.manipulation.transform_to(object1)
# utils.manipulation.get_planning_scene().add_object(self.__perceived_pose)
pose2 = utils.manipulation.transform_to(object2)
# calculate the vector from the points of first and second perception
self.__direction = numpy.array([
(pose2.primitive_poses[0].position.x -
self.__perceived_pose.primitive_poses[0].position.x),
(pose2.primitive_poses[0].position.y -
self.__perceived_pose.primitive_poses[0].position.y)
])
# calculate the time between the first and second perception
self.__time_between_poses = time2 - self.__perceived_pose_time
rospy.logdebug('FastGrasp: Return Object')
return 0
def percieve_object(self):
rospy.logdebug('FastGrasp: Start percieving Object')
# create service
service = rospy.ServiceProxy("/suturo/GetGripper", GetGripper)
# get the first perception
rospy.logdebug('FastGrasp: call Perception Service')
resp = service("firstConveyorCall,cuboid")
if len(resp.objects) == 0 or not resp.objects[0].c_cuboid_success:
rospy.logdebug('FastGrasp: objects empty or no cuboid 1')
return -1
object1 = resp.objects[0].object
self.__perceived_pose_time = resp.stamp
# wait some time until the second perception
rospy.sleep(Duration.from_sec(0.5))
resp = service("cuboid")
if len(resp.objects) == 0 or not resp.objects[0].c_cuboid_success:
rospy.logdebug('FastGrasp: objects empty or no cuboid 2')
return -2
object2 = resp.objects[0].object
time2 = resp.stamp
# transform the points into odom_combined
self.__perceived_pose = utils.manipulation.transform_to(object1)
# utils.manipulation.get_planning_scene().add_object(self.__perceived_pose)
pose2 = utils.manipulation.transform_to(object2)
# calculate the vector from the points of first and second perception
self.__direction = numpy.array(
[(pose2.primitive_poses[0].position.x - self.__perceived_pose.primitive_poses[0].position.x),
(pose2.primitive_poses[0].position.y - self.__perceived_pose.primitive_poses[0].position.y)])
# calculate the time between the first and second perception
self.__time_between_poses = time2 - self.__perceived_pose_time
rospy.logdebug('FastGrasp: Return Object')
return 0
def set_movement_parameters(self, duration, interval, joints, minimal,
maximal):
if not isinstance(duration, (int, float)) or duration < 0:
raise ValueError("Duration must be nonegative number.")
if len(interval) != 2 or any([
not isinstance(t, (int, float)) for t in interval
]) or any([t < 0 for t in interval]) or interval[0] > interval[1]:
raise ValueError("Interval must represent interval of time.")
if len(joints) != len(minimal) or len(joints) != len(maximal):
raise ValueError(
"joints, minimal and maximal arrays must have the same size.")
if any([not isinstance(v, str) for v in joints]):
raise ValueError("joints: list of string is expected.")
if any([not isinstance(v, (int, float)) for v in minimal]):
raise ValueError("minimal: list of numbers is expected.")
if any([not isinstance(v, (int, float)) for v in maximal]):
raise ValueError("maximal: list of numbers is expected.")
self._duration = Duration.from_sec(duration)
self._interval = interval
self._joints = joints
self._minimal = minimal
self._maximal = maximal
def execute(self, userdata):
if self._error:
return 'failure'
time_now = Time.now()
# check if time elasped
if time_now - self._start_timestamp > self._duration:
return 'done'
# check if we have tosend new point
if time_now - self._movement_timestamp > self._movement_duration:
# determine new interval
self._movement_timestamp = time_now
self._movement_duration = Duration.from_sec(
random.uniform(*self._interval))
# form message
msg = PoseStamped()
msg.header.frame_id = self._frame_in
msg.header.stamp = Time(0.0)
# random position
msg.pose.position.x = random.uniform(self._minXYZ[0],
self._maxXYZ[0])
msg.pose.position.y = random.uniform(self._minXYZ[1],
self._maxXYZ[1])
msg.pose.position.z = random.uniform(self._minXYZ[2],
self._maxXYZ[2])
# transform to output frame
try:
# transform
self._pose = self._tf_listener.transformPose(
self._frame_out, msg)
except tf.Exception as e:
Logger.logwarn('Unable to transform from %s to %s: %s' %
(self._frame_in, self._frame_out, e))
return 'failure'
# publish pose
msg.header.stamp = self._movement_timestamp
self._publisher.publish(self._topic, msg)
def pos_calc_test(m):
# create service
service = rospy.ServiceProxy("/suturo/GetGripper", GetGripper)
# get the first perception
resp = service("firstConveyorCall,centroid,cuboid")
object1 = resp.objects[0].object
time1 = resp.stamp
# wait some time until the second perception
rospy.sleep(Duration.from_sec(0.5))
resp = service("centroid,cuboid")
object2 = resp.objects[0].object
time2 = resp.stamp
# transform the points into odom_combined
pose1 = m.transform_to(object1)
pose2 = m.transform_to(object2)
# calculate the vector from the points of first and second perception
dir = numpy.array([(pose2.primitive_poses[0].position.x - pose1.primitive_poses[0].position.x), (
pose2.primitive_poses[0].position.y - pose1.primitive_poses[0].position.y)])
# dir_dist = sqrt(pow(dir[0], 2) + pow(dir[1], 2))
# get the duration between the two looks
time_12 = time2 - time1
# set the time in 5 secs
t_5 = rospy.Time.now() + rospy.Duration(8)
time_13 = t_5 - time1
diff_time = float(time_13.to_sec()) / float(time_12.to_sec())
dir_13 = numpy.array([diff_time * dir[0], diff_time * dir[1]])
# add this new vector on the points from the first perception
pose_comp = copy.deepcopy(pose1)
pose_comp.primitive_poses[0].position.x += dir_13[0]
pose_comp.primitive_poses[0].position.y += dir_13[1]
pose_comp.id = "red_cube"
# print out the table for the different poses for debug
table = [get_position(pose1), get_position(pose2), get_position(pose_comp)]
print tabulate(table)
# return test_extrapolation(dir, m, pose1, pose2, service, time1, time_12)
return pose_comp
def main():
topic_mocap = '/vrpn_client_node/RigidBody1/pose'
frame_mocap = 'RigidBody1'
odom_frame_id = 'base_odom'
odom_child_frame_id = 'base_link'
parser = argparse.ArgumentParser()
parser.add_argument("-s", "--sync-mocap-file", type=str, help="sync file for mocap", default="")
args, left = parser.parse_known_args()
if len(left) != 1:
print ("Usage: " + sys.argv[0] + " odom_and_mocap.bag [-s sync_time.txt]")
return
in_file = left[0]
sync_file = args.sync_mocap_file
out_file = 'preprocessed.bag'
##the time diff between mocap client and server
if sync_file != '':
mocap_offset = getMin_diff_ser_clit(sync_file)
print ('mocap time offset: %f' % mocap_offset)
mocap_offset = Duration.from_sec(mocap_offset)
else:
mocap_offset = None
with rosbag.Bag(in_file) as inbag:
print ("-------------------- Input: %s ------------------------------" % in_file)
print (inbag)
outbag = rosbag.Bag(out_file, 'w')
writer_mocap = TopicWriter(outbag, topic_mocap)
writer_mocap.set_median_filter(1.0, 0.5)
gx_poses = {}
gx_stamp_offset = None
for topic,msg,t in inbag.read_messages():
if topic=="/gx/RosPose2d":
if msg.timestamp in gx_poses:
old = gx_poses[msg.timestamp]
if msg == old:
continue
else:
print old
print msg
print ('different odom message with identical stamp!')
exit()
if gx_stamp_offset is None:
"""
Use bag time if stamp is abnormal
"""
stamp = gx_msg_stamp(msg)
if abs(stamp.to_sec() - t.to_sec()) > 1000:
gx_stamp_offset = t.to_sec() - stamp.to_sec()
print ('odom time offset: %f' % gx_stamp_offset)
else:
gx_stamp_offset = 0
gxPose2d_to_odom(msg,t,outbag,odom_frame_id,odom_child_frame_id,gx_stamp_offset)
gxPose2d_to_tf(msg,t,outbag,odom_frame_id,odom_child_frame_id,gx_stamp_offset)
gx_poses[msg.timestamp] = msg
elif topic == topic_mocap:
if mocap_offset is None:
print ('\nWarning: found mocap topic but sync file is not specified (perhaps run again with [-s sync_time.txt]?)\n')
mocap_offset = Duration.from_sec(0)
msg.header.stamp += mocap_offset
writer_mocap.write_sliding(msg, t)
elif topic=="/gx/RosBaseImu":
pass
elif topic=="/gx/RosBaseTicks":
pass
elif topic == '/tf' and msg.transforms[0].child_frame_id == frame_mocap:
# merge mocap tf into its topic
msg.transforms[0].header.stamp += mocap_offset
writer_mocap.write_sliding(tf_to_pose(msg), t)
else:
outbag.write(topic,msg,t)
outbag.close()
outbag = rosbag.Bag(out_file)
print ("------------------- Output: %s ------------------------------" % out_file)
print (outbag)
def decode(self, data):
""" Generate a rospy.rostime.Duration instance based on the given data
which should be a string representation of a float.
"""
return Duration.from_sec(float(data))
def execute(self, userdata):
rospy.logdebug('FastGrasp: Executing state FastGrasp')
self.__t_point.header.frame_id = "/odom_combined"
# TODO: Auf 10 Min grenzen testen
if not utils.manipulation.is_gripper_open():
rospy.logdebug('FastGrasp: Open Gripper')
utils.manipulation.open_gripper()
r = self.percieve_object()
i = 0
while r == -1:
if i == 2 and not userdata.request_second_object:
# TODO: Zeit anpassen, ab wann gecalled wird
rospy.logdebug('FastGrasp: Request next object')
rospy.ServiceProxy("/euroc_interface_node/request_next_object", RequestNextObject).call()
userdata.request_second_object = True
if i == 19 and userdata.request_second_object:
rospy.logdebug('FastGrasp: Time Expired')
return 'noObjectsLeft'
r = self.percieve_object()
i += 1
# TODO: Was passiert wenn das Objekt nur einmal gesehen wurde
rospy.logdebug('FastGrasp: Begin movement, Plan 1')
for j in range(0, 4):
self.extrapolate(j)
self.calculate_target_point(self.__pose_comp)
if utils.manipulation.move_to(self.__t_point):
rospy.logdebug("FastGrasp: Plan 1: moved!")
break
else:
rospy.logdebug("FastGrasp: Plan 1: No Plan fount in step " + str(j))
if j == 3:
return 'noPlanFound'
if userdata.object_index == 1:
offset = rospy.Duration(2)
elif userdata.object_index == 2:
offset = rospy.Duration(4)
else:
offset = rospy.Duration(4)
while rospy.get_rostime() < self.__t_point_time - offset:
rospy.sleep(0.01)
self.__t_point.pose.position.z -= 0.07
rospy.logdebug("FastGrasp: Plan 2")
if utils.manipulation.move_to(self.__t_point):
rospy.logdebug("FastGrasp: Plan 2: moved!")
else:
rospy.logdebug("FastGrasp: Plan 2: Cant grasp")
return 'noPlanFound'
utils.manipulation.close_gripper(self.__pose_comp)
self.__t_point.pose.position.z += 0.1
rospy.logdebug("FastGrasp: Plan 3")
for k in range(0, 4):
if utils.manipulation.move_to(self.__t_point):
rospy.logdebug("FastGrasp: Plan 3: moved!")
break
else:
if k == 3:
rospy.logdebug("FastGrasp: No Plan found to lift object")
return 'noPlanFound'
rospy.logdebug("FastGrasp: Plan 3: Cant lift: " + str(k))
if self.__t_point.pose.position.y > 0:
self.__t_point.pose.position.y -= 0.1
else:
self.__t_point.pose.position.y += 0.1
if self.__t_point.pose.position.x > 0:
self.__t_point.pose.position.x -= 0.1
else:
self.__t_point.pose.position.x += 0.1
rospy.sleep(Duration.from_sec(0.5))
tfs = TorqueForceService()
if tfs.is_free():
rospy.logdebug("FastGrasp: Grasp Fail")
return 'graspingFailed'
rospy.logdebug("FastGrasp: objectGrasped, finished")
userdata.object_index += 1
return 'objectGrasped'
def decode(self, _, data):
""" Generate a rospy.rostime.Duration instance based on the given data
which should be a string representation of a float.
"""
return Duration.from_sec(float(data))
def execute(self, userdata):
rospy.logdebug('FastGrasp: Executing state FastGrasp')
self.__t_point.header.frame_id = "/odom_combined"
# TODO: Auf 10 Min grenzen testen
if not utils.manipulation.is_gripper_open():
rospy.logdebug('FastGrasp: Open Gripper')
utils.manipulation.open_gripper()
r = self.percieve_object()
i = 0
while r == -1:
if i == 2 and not userdata.request_second_object:
# TODO: Zeit anpassen, ab wann gecalled wird
rospy.logdebug('FastGrasp: Request next object')
rospy.ServiceProxy("/euroc_interface_node/request_next_object",
RequestNextObject).call()
userdata.request_second_object = True
if i == 19 and userdata.request_second_object:
rospy.logdebug('FastGrasp: Time Expired')
return 'noObjectsLeft'
r = self.percieve_object()
i += 1
# TODO: Was passiert wenn das Objekt nur einmal gesehen wurde
rospy.logdebug('FastGrasp: Begin movement, Plan 1')
for j in range(0, 4):
self.extrapolate(j)
self.calculate_target_point(self.__pose_comp)
if utils.manipulation.move_to(self.__t_point):
rospy.logdebug("FastGrasp: Plan 1: moved!")
break
else:
rospy.logdebug("FastGrasp: Plan 1: No Plan fount in step " +
str(j))
if j == 3:
return 'noPlanFound'
if userdata.object_index == 1:
offset = rospy.Duration(2)
elif userdata.object_index == 2:
offset = rospy.Duration(4)
else:
offset = rospy.Duration(4)
while rospy.get_rostime() < self.__t_point_time - offset:
rospy.sleep(0.01)
self.__t_point.pose.position.z -= 0.07
rospy.logdebug("FastGrasp: Plan 2")
if utils.manipulation.move_to(self.__t_point):
rospy.logdebug("FastGrasp: Plan 2: moved!")
else:
rospy.logdebug("FastGrasp: Plan 2: Cant grasp")
return 'noPlanFound'
utils.manipulation.close_gripper(self.__pose_comp)
self.__t_point.pose.position.z += 0.1
rospy.logdebug("FastGrasp: Plan 3")
for k in range(0, 4):
if utils.manipulation.move_to(self.__t_point):
rospy.logdebug("FastGrasp: Plan 3: moved!")
break
else:
if k == 3:
rospy.logdebug("FastGrasp: No Plan found to lift object")
return 'noPlanFound'
rospy.logdebug("FastGrasp: Plan 3: Cant lift: " + str(k))
if self.__t_point.pose.position.y > 0:
self.__t_point.pose.position.y -= 0.1
else:
self.__t_point.pose.position.y += 0.1
if self.__t_point.pose.position.x > 0:
self.__t_point.pose.position.x -= 0.1
else:
self.__t_point.pose.position.x += 0.1
rospy.sleep(Duration.from_sec(0.5))
tfs = TorqueForceService()
if tfs.is_free():
rospy.logdebug("FastGrasp: Grasp Fail")
return 'graspingFailed'
rospy.logdebug("FastGrasp: objectGrasped, finished")
userdata.object_index += 1
return 'objectGrasped'
