def filter_by_distance(features, max_distance): # type: (List[Feature], float) -> List[Feature]
cam_pixel_to_point = rospy.ServiceProxy('cam_pixel_to_point', CamPixelToPoint)
tf_listener = TransformListener()
filtered = []
for feature in features:
cam_pos = Vector3()
cam_pos.x, cam_pos.y = feature.centroid[0], feature.centroid[1]
point = cam_pixel_to_point(cam_pos).point # type: PointStamped
tf_listener.waitForTransform('camera_link', point.header.frame_id, rospy.Time(rospy.get_time()), rospy.Duration(1))
point = tf_listener.transformPoint('camera_link', point)
if point.point.x <= max_distance:
filtered.append(feature)
return filtered
def feature_depths(features):
cam_pixel_to_point = rospy.ServiceProxy('cam_pixel_to_point', CamPixelToPoint)
tf_listener = TransformListener()
depths = []
for feature in features:
cam_pos = Vector3()
cam_pos.x, cam_pos.y = feature.centroid[0], feature.centroid[1]
point = cam_pixel_to_point(cam_pos).point # type: PointStamped
tf_listener.waitForTransform('camera_link', point.header.frame_id, rospy.Time(rospy.get_time()),
rospy.Duration(1))
point = tf_listener.transformPoint('camera_link', point)
depths.append(np.linalg.norm([point.point.x, point.point.y, point.point.z]))
return depths
def transform_pose(_pose2D, src_frame='CameraTop_frame', dst_frame='/base_footprint', timeout=3):
tl = TransformListener()
pose_stamped = PoseStamped()
pose_stamped.header.stamp = rospy.Time()
pose_stamped.header.frame_id = src_frame
pose_stamped.pose = Pose(Point(_pose2D.x, _pose2D.y, 0.0), Quaternion())
try:
tl.waitForTransform(target_frame=dst_frame, source_frame=src_frame,
time=rospy.Time(), timeout=rospy.Duration(timeout))
pose_transf = tl.transformPose(dst_frame, pose_stamped)
except Exception as e:
rospy.logwarn("Transformation failed!!! %s" % str(e))
return _pose2D
return Pose2D(pose_transf.pose.position.x, pose_transf.pose.position.y, 0.0)
def select_center(features): # type: (List[Feature]) -> Feature
cam_pixel_to_point = rospy.ServiceProxy('cam_pixel_to_point', CamPixelToPoint)
tf_listener = TransformListener()
angles = []
center_ray = np.array([1.0, 0.0, 0.0])
for feature in features:
cam_pos = Vector3()
cam_pos.x, cam_pos.y = feature.centroid[0], feature.centroid[1]
point = cam_pixel_to_point(cam_pos).point # type: PointStamped
tf_listener.waitForTransform('camera_link', point.header.frame_id, rospy.Time(rospy.get_time()), rospy.Duration(1))
point = tf_listener.transformPoint('camera_link', point)
ray = np.array([point.point.x, point.point.y, point.point.z])
ray /= np.linalg.norm(ray)
angle = np.arccos(np.dot(center_ray, ray))
angles.append(abs(angle))
return features[np.argmin(angles)]
def transform_pose(pose, src_frame, dst_frame, timeout=10):
"""
Transforms the given pose from src_frame to dst_frame.
:param src_frame
:param dst_frame
:param timeout the amount of time allowed (in secs) for a transformation (default 3)
"""
if str(type(pose)) != str(type(Pose())):
rospy.logwarn(colors.BACKGROUND_RED + "The 'pose' should be a Pose object, not '%s'.%s" % (str(type(pose)).split('\'')[1], colors.NATIVE_COLOR))
from tf.listener import TransformListener
assert timeout >= 1
pose_stamped = PoseStamped()
pose_stamped.header.stamp = rospy.Time()
pose_stamped.header.frame_id = src_frame
pose_stamped.pose = pose
global _tl
if not _tl:
_tl = TransformListener()
rospy.sleep(0.5)
timeout -= 0.5
rospy.loginfo("Transforming position from %s to %s coordinates..." % (
src_frame, dst_frame))
# If something fails we'll return the original pose (for testing
# with mocks when tf isn't available)
result = pose
try:
_tl.waitForTransform(
target_frame=dst_frame, source_frame=src_frame,
time=rospy.Time(), timeout=rospy.Duration(timeout))
pose_transf = _tl.transformPose(dst_frame, pose_stamped)
result = pose_transf.pose
except Exception as e:
rospy.logwarn(colors.BACKGROUND_RED + "Warning! Pose transformation failed! %s%s" % (str(e), colors.NATIVE_COLOR))
return result
class abbRobot:
errorDict = {
0: "Successful",
-1: "Invalid goal",
-2: "Invalid joints",
-3: "Old header timestamp",
-4: "Path tolerance violated",
-5: "Goal tolerance violated"
}
def __init__(self):
self.manip = mic.MoveGroupCommander("manipulator")
self.client = act.SimpleActionClient('joint_trajectory_action',
trajAction)
rp.loginfo("Waiting for server joint_trajectory_action.")
self.client.wait_for_server()
rp.loginfo("Successfuly connected to server.")
self.tfListener = TransformListener()
def getEEPoint(self, start_link='base_link', end_link="tool0"):
self.tfListener.waitForTransform(start_link, end_link, rp.Time(),
rp.Duration(0.5))
ptData = self.tfListener.lookupTransform(start_link, end_link,
rp.Time())
pts = [round(pt, 5) for pt in ptData[0]]
orient = [round(pt, 5) for pt in ptData[1]]
return pts, orient
def move2Point(self,
points,
eAngles=[[0, 0, 0]],
ax='sxyz',
end_effector='link_6'):
"""
Moves to a point using end effector point space.\n
Usage:\n
- points - list of lists that contain x,y,z coordinates\n
- eAngles - default [[0,0,0]]\n
- list of lists that contain a,b,g euler angles
- if multiple points are passed and only one list of euler angles then those angles are going to be used for all points
- ax - specify convention to use for euler angles
- default: 'sxyz'
- end_effector - default link_6
- link whose point you want to move
"""
print "Number of points recieved: ", len(points)
t1 = t.time()
if (len(points) == len(eAngles) or len(eAngles) == 1):
for i in xrange(len(points)):
if rp.is_shutdown():
print "ROS has been shutdown. Exiting..."
break
print i + 1, ". point:", [round(pt, 5) for pt in points[i]]
p = Point(points[i][0], points[i][1], points[i][2])
index = i
if len(eAngles) == 1:
index = 0
orient = Quaternion(*qEuler(eAngles[index][0], eAngles[index]
[1], eAngles[index][2], ax))
self.manip.set_pose_target(Pose(p, orient),
end_effector_link=end_effector)
self.manip.go(True)
rp.loginfo("Moving to multiple points finished.")
self.__displayDuration(t1, t.time())
else:
print "Number of points recieved does not match number of euler angles received\nneither number of euler angles is 1. Please check the input parameters."
def cartesian2Point(self,
points,
eAngles,
ax='sxyz',
resolution=0.01,
jumpStep=0,
end_effector='link_6'):
"""
Moves to a point in a straight line using end effector point space.\n
Usage:\n
- points - list of lists that contain x,y,z coordinates\n
- eAngles - list that contains a,b,g euler angles for constraint
- ax - specify convention to use for euler angles
- default: 'sxyz'
- resolution - maximum distance between 2 generated points
- default: 0.01
- jumpStep - maximum jump distance between 2 generated points
- default: 0
- end_effector - link whose point you want to move
- default link_6
"""
print "Number of points recieved: ", len(points)
wpose = Pose()
self.manip.set_end_effector_link(end_effector)
constraint = Constraints()
orientation_constraint = OrientationConstraint()
orientation_constraint.link_name = end_effector
orientation_constraint.orientation = Quaternion(
*qEuler(eAngles[0], eAngles[1], eAngles[2], ax))
constraint.orientation_constraints.append(orientation_constraint)
self.manip.set_path_constraints(constraint)
t1 = t.time()
for pt in points:
if not rp.is_shutdown():
print "Going to point: ", [round(p, 5) for p in pt]
wpose.position.x = pt[0]
wpose.position.y = pt[1]
wpose.position.z = pt[2]
(plan, factor) = self.manip.compute_cartesian_path([wpose],
resolution,
jumpStep)
trajLen = len(plan.joint_trajectory.points)
if trajLen <= 100 and factor == 1.0:
self.manip.execute(plan)
else:
rp.loginfo(
"Error while planning. No execution attempted.\nNo. points planned:{}\nPercentage of path found:{}%"
.format(trajLen, round(factor * 100, 2)))
rp.loginfo("Moving to multiple points finished.")
self.__displayDuration(t1, t.time())
self.manip.set_path_constraints(None)
def jointAction(self, jointAngles):
"""
Moves to a point using joint space\n
Usage:\n
- jointAngles - list of lists that contain 6 angles in order from joint_1 to joint_6
"""
print "Number of joint angles recieved: ", len(jointAngles)
t1 = t.time()
jointGoal = goal()
jointGoal.trajectory.joint_names = [
'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6'
]
jointGoal.trajectory.points.append(JointTrajectoryPoint())
for joint in jointAngles:
print "Going to joint values[degrees]: ", [
round(i * 180 / pi, 2) for i in joint
]
jointGoal.trajectory.points[0].positions = joint
# jointGoal.trajectory.points[0].velocities=[2.,2.,2.,6.,6.,7.]
# jointGoal.trajectory.points[0].accelerations=[1.,1.,1.,1.,1.,1.]
# jointGoal.trajectory.points[0].time_from_start=rp.Duration(2,0)
self.client.send_goal(jointGoal, feedback_cb=self.__feedback)
self.client.wait_for_result()
print "[Result:] ", self.errorDict[
self.client.get_result().error_code]
spent = self.__displayDuration(t1, t.time())
@staticmethod
def __feedback(msg):
print "[Feedback:] ", msg.error
@staticmethod
def __displayDuration(t1, t2):
spent = [(int(t2 - t1) / 60), 0, 0]
spent[1] = int((t2 - t1) - spent[0] * 60)
spent[2] = int(((t2 - t1) - spent[0] * 60 - spent[1]) * 1000)
rp.loginfo("Execution took [min:sec.ms]: %i:%02i.%03i ", spent[0],
spent[1], spent[2])
class Arm(object):
"""Arm controls the robot's arm.
Joint space control:
joints = ArmJoints()
# Fill out joint states
arm = fetch_api.Arm()
arm.move_to_joints(joints)
"""
def __init__(self):
self._joint_client = actionlib.SimpleActionClient(
JOINT_ACTION_SERVER, control_msgs.msg.FollowJointTrajectoryAction)
self._joint_client.wait_for_server(rospy.Duration(10))
self._move_group_client = actionlib.SimpleActionClient(
MOVE_GROUP_ACTION_SERVER, MoveGroupAction)
self._move_group_client.wait_for_server(rospy.Duration(10))
self._compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
self._compute_fk = rospy.ServiceProxy('compute_fk', GetPositionFK)
self._tf_listener = TransformListener()
def move_to_joints(self, joint_state):
goal = control_msgs.msg.FollowJointTrajectoryGoal()
goal.trajectory.joint_names.extend(ArmJoints.names())
point = trajectory_msgs.msg.JointTrajectoryPoint()
point.positions.extend(joint_state.values())
point.time_from_start = rospy.Duration(TIME_FROM_START)
goal.trajectory.points.append(point)
self._joint_client.send_goal(goal)
self._joint_client.wait_for_result(rospy.Duration(10))
def move_to_joint_goal(self,
joints,
allowed_planning_time=10.0,
execution_timeout=rospy.Duration(15.0),
group_name='arm',
num_planning_attempts=1,
plan_only=False,
replan=False,
replan_attempts=5,
tolerance=0.01):
"""Moves the end-effector to a pose, using motion planning.
Args:
joints: A list of (name, value) for the arm joints.
allowed_planning_time: float. The maximum duration to wait for a
planning result.
execution_timeout: float. The maximum duration to wait for an arm
motion to execute (or for planning to fail completely), in
seconds.
group_name: string. Either 'arm' or 'arm_with_torso'.
num_planning_attempts: int. The number of times to compute the same
plan. The shortest path is ultimately used. For random
planners, this can help get shorter, less weird paths.
plan_only: bool. If True, then this method does not execute the
plan on the robot. Useful for determining whether this is
likely to succeed.
replan: bool. If True, then if an execution fails (while the arm is
moving), then come up with a new plan and execute it.
replan_attempts: int. How many times to replan if the execution
fails.
tolerance: float. The goal tolerance, in meters.
Returns:
string describing the error if an error occurred, else None.
"""
goal_builder = MoveItGoalBuilder()
goal_builder.set_joint_goal(*zip(*joints))
goal_builder.allowed_planning_time = allowed_planning_time
goal_builder.num_planning_attempts = num_planning_attempts
goal_builder.plan_only = plan_only
goal_builder.planner_id = ''
goal_builder.replan = replan
goal_builder.replan_attempts = replan_attempts
goal_builder.tolerance = tolerance
goal = goal_builder.build()
if goal is not None:
self._move_group_client.send_goal(goal)
# success = self._move_group_client.wait_for_result(
# rospy.Duration(execution_timeout))
success = self._move_group_client.wait_for_result(
execution_timeout)
if not success:
return moveit_error_string(MoveItErrorCodes.TIMED_OUT)
result = self._move_group_client.get_result()
if result:
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return None
else:
return moveit_error_string(result.error_code.val)
else:
return moveit_error_string(MoveItErrorCodes.TIMED_OUT)
def move_to_pose(self,
pose_stamped,
allowed_planning_time=10.0,
execution_timeout=15.0,
group_name='arm',
num_planning_attempts=1,
orientation_constraint=None,
plan_only=False,
replan=False,
replan_attempts=5,
tolerance=0.01):
"""Moves the end-effector to a pose, using motion planning.
Args:
pose_stamped: geometry_msgs/PoseStamped. The goal pose for the gripper.
allowed_planning_time: float. The maximum duration to wait for a
planning result.
execution_timeout: float. The maximum duration to wait for an arm
motion to execute (or for planning to fail completely), in
seconds.
group_name: string. Either 'arm' or 'arm_with_torso'.
num_planning_attempts: int. The number of times to compute the same
plan. The shortest path is ultimately used. For random
planners, this can help get shorter, less weird paths.
orientation_constraint: moveit_msgs/OrientationConstraint. An
orientation constraint for the entire path.
plan_only: bool. If True, then this method does not execute the
plan on the robot. Useful for determining whether this is
likely to succeed.
replan: bool. If True, then if an execution fails (while the arm is
moving), then come up with a new plan and execute it.
replan_attempts: int. How many times to replan if the execution
fails.
tolerance: float. The goal tolerance, in meters.
Returns:
string describing the error if an error occurred, else None.
"""
goal_builder = MoveItGoalBuilder()
goal_builder.set_pose_goal(pose_stamped)
if orientation_constraint is not None:
goal_builder.orientation_constraint = orientation_constraint
goal_builder.allowed_planning_time = allowed_planning_time
goal_builder.num_planning_attempts = num_planning_attempts
goal_builder.plan_only = plan_only
goal_builder.replan = replan
goal_builder.replan_attempts = replan_attempts
goal_builder.tolerance = tolerance
goal = goal_builder.build()
if goal is not None:
self._move_group_client.send_goal(goal)
self._move_group_client.wait_for_result(
rospy.Duration(execution_timeout))
result = self._move_group_client.get_result()
if result:
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return None
else:
return moveit_error_string(result.error_code.val)
else:
return moveit_error_string(MoveItErrorCodes.TIMED_OUT)
def move_to_pose_with_seed(self,
pose_stamped,
seed_state=None,
trajectory_constraint=[],
allowed_planning_time=10.0,
execution_timeout=15.0,
group_name='arm',
num_planning_attempts=1,
orientation_constraint=None,
plan_only=False,
replan=False,
replan_attempts=5,
tolerance=0.01):
"""Moves the end-effector to a pose, using motion planning.
Args:
pose_stamped: geometry_msgs/PoseStamped. The goal pose for the gripper.
seed_state: sensor_msgs/JointState. The start joint state
to search for solution.
trajectory_constraint: array of moveit_msgs/Constraints. The trajectory
constraint.
allowed_planning_time: float. The maximum duration to wait for a
planning result.
execution_timeout: float. The maximum duration to wait for an arm
motion to execute (or for planning to fail completely), in
seconds.
group_name: string. Either 'arm' or 'arm_with_torso'.
num_planning_attempts: int. The number of times to compute the same
plan. The shortest path is ultimately used. For random
planners, this can help get shorter, less weird paths.
orientation_constraint: moveit_msgs/OrientationConstraint. An
orientation constraint for the entire path.
plan_only: bool. If True, then this method does not execute the
plan on the robot. Useful for determining whether this is
likely to succeed.
replan: bool. If True, then if an execution fails (while the arm is
moving), then come up with a new plan and execute it.
replan_attempts: int. How many times to replan if the execution
fails.
tolerance: float. The goal tolerance, in meters.
Returns:
string describing the error if an error occurred, else None.
"""
goal_builder = MoveItGoalBuilder()
goal_builder.set_pose_goal(pose_stamped)
if orientation_constraint is not None:
goal_builder.add_path_orientation_constraint(
orientation_constraint)
goal_builder.allowed_planning_time = allowed_planning_time
goal_builder.num_planning_attempts = num_planning_attempts
goal_builder.plan_only = plan_only
goal_builder.replan = replan
goal_builder.replan_attempts = replan_attempts
goal_builder.tolerance = tolerance
if seed_state:
goal_builder.start_state.joint_state = seed_state
if trajectory_constraint:
goal_builder.add_trajectory_orientation_constraint(
trajectory_constraint)
goal = goal_builder.build()
if goal is not None:
self._move_group_client.send_goal(goal)
self._move_group_client.wait_for_result(
rospy.Duration(execution_timeout))
result = self._move_group_client.get_result()
if result:
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return None
else:
return moveit_error_string(result.error_code.val)
else:
return moveit_error_string(MoveItErrorCodes.TIMED_OUT)
def straight_move_to_pose(self, group, plan):
"""Moves the end-effector to a pose in a straight line.
Args:
group: moveit_commander.MoveGroupCommander. The planning group for
the arm.
plan: the plan for the arm to follow
Returns:
string describing the error if an error occurred, else None.
"""
# Execute path
result = group.execute(plan, wait=True)
if not result:
return moveit_error_string(MoveItErrorCodes.INVALID_MOTION_PLAN)
else:
return None
def straight_move_to_pose_check(self,
group,
pose_stamped,
ee_step=0.025,
jump_threshold=2.0,
avoid_collisions=True):
"""Checks if we can move the end-effector to a pose in a straight line.
Args:
group: moveit_commander.MoveGroupCommander. The planning group for
the arm.
pose_stamped: geometry_msgs/PoseStamped. The goal pose for the
gripper.
ee_step: float. The distance in meters to interpolate the path.
jump_threshold: float. The maximum allowable distance in the arm's
configuration space allowed between two poses in the path. Used to
prevent "jumps" in the IK solution.
avoid_collisions: bool. Whether to check for obstacles or not.
Returns:
the generated plan if the arm can move ina straight line, else None.
"""
# Transform pose into planning frame
self._tf_listener.waitForTransform(pose_stamped.header.frame_id,
group.get_planning_frame(),
rospy.Time.now(),
rospy.Duration(1.0))
try:
pose_transformed = self._tf_listener.transformPose(
group.get_planning_frame(), pose_stamped)
except (tf.LookupException, tf.ConnectivityException):
rospy.logerr('Unable to transform pose from frame {} to {}'.format(
pose_stamped.header.frame_id, group.get_planning_frame()))
return moveit_error_string(
MoveItErrorCodes.FRAME_TRANSFORM_FAILURE)
# Compute path
plan, fraction = group.compute_cartesian_path(
[group.get_current_pose().pose, pose_transformed.pose], ee_step,
jump_threshold, avoid_collisions)
if fraction < 1:
return None
else:
return plan
def check_pose(self,
pose_stamped,
allowed_planning_time=10.0,
group_name='arm',
tolerance=0.01):
return self.move_to_pose(pose_stamped,
allowed_planning_time=allowed_planning_time,
group_name=group_name,
tolerance=tolerance,
plan_only=True)
def compute_ik(self, pose_stamped, timeout=rospy.Duration(5)):
"""Computes inverse kinematics for the given pose.
Note: if you are interested in returning the IK solutions, we have
shown how to access them.
Args:
pose_stamped: geometry_msgs/PoseStamped.
timeout: rospy.Duration. How long to wait before giving up on the
IK solution.
Returns: A list of (name, value) for the arm joints if the IK solution
was found, False otherwise.
"""
request = GetPositionIKRequest()
request.ik_request.pose_stamped = pose_stamped
request.ik_request.group_name = 'arm'
request.ik_request.timeout = timeout
response = self._compute_ik(request)
error_str = moveit_error_string(response.error_code.val)
success = error_str == 'SUCCESS'
if not success:
return False
joint_state = response.solution.joint_state
joints = []
for name, position in zip(joint_state.name, joint_state.position):
if name in ArmJoints.names():
joints.append((name, position))
return joints
def compute_fk(self, joint_state):
"""Computes forward kinematics for the given joint state.
Args:
joint_state: sensor_msgs/JointState.
Returns: A geometry_msgs/PoseStamped of the wrist position, False otherwise.
"""
request = GetPositionFKRequest()
request.header.frame_id = 'base_link'
request.fk_link_names = ['wrist_roll_link']
request.robot_state.joint_state = joint_state
response = self._compute_fk(request)
error_str = moveit_error_string(response.error_code.val)
success = error_str == 'SUCCESS'
if not success:
return False
return response.pose_stamped
def get_cartesian_path(self,
group,
start_joint_state,
waypoints,
ee_step=0.025,
jump_threshold=2.0,
avoid_collisions=True):
"""Returns a robot trajectory which passes through all the waypoints specified.
Args:
group: moveit_commander.MoveGroupCommander. The planning group for
the arm.
start_joint_state: sensor_msgs/JointState.
waypoints: geometry_msgs/Pose[]. Waypoints to pass through.
pose_stamped: geometry_msgs/PoseStamped. The goal pose for the
gripper.
ee_step: float. The distance in meters to interpolate the path.
jump_threshold: float. The maximum allowable distance in the arm's
configuration space allowed between two poses in the path. Used to
prevent "jumps" in the IK solution.
avoid_collisions: bool. Whether to check for obstacles or not.
Returns: A RobotTrajectory to follow, False otherwise.
"""
# request = GetCartesianPathRequest()
# request.header.frame_id = 'base_link'
# request.start_state.joint_state = start_joint_state
# request.group_name = ARM_GROUP_NAME
# request.waypoints = waypoints
# request.max_step = 0.025
# request.jump_threshold = 2.0
# request.avoid_collisions = True
# response = self._get_cartesian_path(request)
# error_str = moveit_error_string(response.error_code.val)
# success = error_str == 'SUCCESS'
# if not success:
# return False
# return response.solution
# Compute path
plan, fraction = group.compute_cartesian_path(waypoints, ee_step,
jump_threshold,
avoid_collisions)
if fraction < 1:
return None
else:
return plan
def execute_trajectory(self, group, trajectory):
"""Moves the end-effector along the given trajectory.
Args:
group: moveit_commander.MoveGroupCommander. The planning group for
the arm.
trajectory: the RobotTrajectory to follow.
Return:
string describing the error if an error occurred, else None.
"""
result = group.execute(trajectory, wait=True)
if not result:
return moveit_error_string(MoveItErrorCodes.INVALID_MOTION_PLAN)
else:
return None
def cancel_all_goals(self):
self._move_group_client.cancel_all_goals()
self._joint_client.cancel_all_goals()
class DataCollectorAndLabeler:
def __init__(self, output_folder, server, menu_handler, marker_size, calibration_file):
if not os.path.exists(output_folder):
os.mkdir(output_folder) # Create the new folder
else:
while True:
msg = Fore.YELLOW + "To continue, the directory '{}' will be delete.\n"
msg = msg + "Do you wish to continue? [y/N] " + Style.RESET_ALL
answer = raw_input(msg.format(output_folder))
if len(answer) > 0 and answer[0].lower() in ('y', 'n'):
if answer[0].lower() == 'n':
sys.exit(1)
else:
break
else:
sys.exit(1) # defaults to N
shutil.rmtree(output_folder) # Delete old folder
os.mkdir(output_folder) # Recreate the folder
self.output_folder = output_folder
self.listener = TransformListener()
self.sensors = {}
self.sensor_labelers = {}
self.server = server
self.menu_handler = menu_handler
self.data_stamp = 0
self.collections = {}
self.bridge = CvBridge()
self.config = loadJSONConfig(calibration_file)
if self.config is None:
sys.exit(1) # loadJSON should tell you why.
self.world_link = self.config['world_link']
# Add sensors
print(Fore.BLUE + 'Sensors:' + Style.RESET_ALL)
print('Number of sensors: ' + str(len(self.config['sensors'])))
# Go through the sensors in the calib config.
for sensor_key, value in self.config['sensors'].items():
# Create a dictionary that describes this sensor
sensor_dict = {'_name': sensor_key, 'parent': value['link'],
'calibration_parent': value['parent_link'],
'calibration_child': value['child_link']}
# TODO replace by utils function
print("Waiting for message")
msg = rospy.wait_for_message(value['topic_name'], rospy.AnyMsg)
connection_header = msg._connection_header['type'].split('/')
ros_pkg = connection_header[0] + '.msg'
msg_type = connection_header[1]
print('Topic ' + value['topic_name'] + ' has type ' + msg_type)
sensor_dict['topic'] = value['topic_name']
sensor_dict['msg_type'] = msg_type
# If topic contains a message type then get a camera_info message to store along with the sensor data
if sensor_dict['msg_type'] == 'Image': # if it is an image must get camera_info
sensor_dict['camera_info_topic'] = os.path.dirname(sensor_dict['topic']) + '/camera_info'
from sensor_msgs.msg import CameraInfo
camera_info_msg = rospy.wait_for_message(sensor_dict['camera_info_topic'], CameraInfo)
from rospy_message_converter import message_converter
sensor_dict['camera_info'] = message_converter.convert_ros_message_to_dictionary(camera_info_msg)
# Get the kinematic chain form world_link to this sensor's parent link
now = rospy.Time()
self.listener.waitForTransform(value['link'], self.world_link, now, rospy.Duration(5))
chain = self.listener.chain(value['link'], now, self.world_link, now, self.world_link)
chain_list = []
for parent, child in zip(chain[0::], chain[1::]):
key = self.generateKey(parent, child)
chain_list.append({'key': key, 'parent': parent, 'child': child})
sensor_dict['chain'] = chain_list # Add to sensor dictionary
self.sensors[sensor_key] = sensor_dict
sensor_labeler = InteractiveDataLabeler(self.server, self.menu_handler, sensor_dict,
marker_size, self.config['calibration_pattern'])
self.sensor_labelers[sensor_key] = sensor_labeler
print('finished visiting sensor ' + sensor_key)
print(Fore.BLUE + sensor_key + Style.RESET_ALL + ':\n' + str(sensor_dict))
# print('sensor_labelers:')
# print(self.sensor_labelers)
self.abstract_transforms = self.getAllAbstractTransforms()
# print("abstract_transforms = " + str(self.abstract_transforms))
def getTransforms(self, abstract_transforms, time=None):
transforms_dict = {} # Initialize an empty dictionary that will store all the transforms for this data-stamp
if time is None:
time = rospy.Time.now()
for ab in abstract_transforms: # Update all transformations
self.listener.waitForTransform(ab['parent'], ab['child'], time, rospy.Duration(1.0))
(trans, quat) = self.listener.lookupTransform(ab['parent'], ab['child'], time)
key = self.generateKey(ab['parent'], ab['child'])
transforms_dict[key] = {'trans': trans, 'quat': quat, 'parent': ab['parent'], 'child': ab['child']}
return transforms_dict
def lockAllLabelers(self):
for sensor_name, sensor in self.sensors.iteritems():
self.sensor_labelers[sensor_name].lock.acquire()
print("Locked all labelers")
def unlockAllLabelers(self):
for sensor_name, sensor in self.sensors.iteritems():
self.sensor_labelers[sensor_name].lock.release()
print("Unlocked all labelers")
def getLabelersTimeStatistics(self):
stamps = [] # a list of the several time stamps of the stored messages
for sensor_name, sensor in self.sensors.iteritems():
stamps.append(copy.deepcopy(self.sensor_labelers[sensor_name].msg.header.stamp))
max_delta = getMaxTimeDelta(stamps)
average_time = getAverageTime(stamps) # For looking up transforms use average time of all sensor msgs
print('Times:')
for stamp in stamps:
printRosTime(stamp)
return stamps, average_time, max_delta
def saveCollection(self):
# --------------------------------------
# Collect sensor data and labels (images, laser scans, etc)
# --------------------------------------
# Lock the semaphore for all labelers
self.lockAllLabelers()
# Analyse message time stamps and decide if collection can be stored
stamps, average_time, max_delta = self.getLabelersTimeStatistics()
if max_delta.to_sec() > float(self.config['max_duration_between_msgs']): # times are close enough?
rospy.logwarn('Max duration between msgs in collection is ' + str(max_delta.to_sec()) +
'. Not saving collection.')
self.unlockAllLabelers()
return None
else: # test passed
rospy.loginfo('Max duration between msgs in collection is ' + str(max_delta.to_sec()))
# Collect all the transforms
transforms = self.getTransforms(self.abstract_transforms, average_time) # use average time of sensor msgs
printRosTime(average_time, "Collected transforms for time ")
all_sensor_data_dict = {}
all_sensor_labels_dict = {}
for sensor_name, sensor in self.sensors.iteritems():
print('collect sensor: ' + sensor_name)
msg = copy.deepcopy(self.sensor_labelers[sensor_name].msg)
labels = copy.deepcopy(self.sensor_labelers[sensor_name].labels)
# TODO add exception also for point cloud and depth image
# Update sensor data ---------------------------------------------
if sensor['msg_type'] == 'Image': # Special case of requires saving image data as png separate files
cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8") # Convert to opencv image and save image to disk
filename = self.output_folder + '/' + sensor['_name'] + '_' + str(self.data_stamp) + '.jpg'
filename_relative = sensor['_name'] + '_' + str(self.data_stamp) + '.jpg'
cv2.imwrite(filename, cv_image)
image_dict = message_converter.convert_ros_message_to_dictionary(msg) # Convert sensor data to dictionary
del image_dict['data'] # Remove data field (which contains the image), and replace by "data_file"
image_dict['data_file'] = filename_relative # Contains full path to where the image was saved
# Update the data dictionary for this data stamp
all_sensor_data_dict[sensor['_name']] = image_dict
else:
# Update the data dictionary for this data stamp
all_sensor_data_dict[sensor['_name']] = message_converter.convert_ros_message_to_dictionary(msg)
# Update sensor labels ---------------------------------------------
if sensor['msg_type'] in ['Image', 'LaserScan']:
all_sensor_labels_dict[sensor_name] = labels
else:
raise ValueError('Unknown message type.')
collection_dict = {'data': all_sensor_data_dict, 'labels': all_sensor_labels_dict, 'transforms': transforms}
self.collections[self.data_stamp] = collection_dict
self.data_stamp += 1
# Save to json file
D = {'sensors': self.sensors, 'collections': self.collections, 'calibration_config': self.config}
self.createJSONFile(self.output_folder + '/data_collected.json', D)
self.unlockAllLabelers()
def getAllAbstractTransforms(self):
rospy.sleep(0.5) # wait for transformations
# Get a list of all transforms to collect
transforms_list = []
now = rospy.Time.now()
all_frames = self.listener.getFrameStrings()
for frame in all_frames:
chain = self.listener.chain(frame, now, self.world_link, now, self.world_link)
for idx in range(0, len(chain) - 1):
parent = chain[idx]
child = chain[idx + 1]
transforms_list.append({'parent': parent, 'child': child, 'key': self.generateKey(parent, child)})
# https://stackoverflow.com/questions/31792680/how-to-make-values-in-list-of-dictionary-unique
uniq_l = list(map(dict, frozenset(frozenset(i.items()) for i in transforms_list)))
return uniq_l # get unique values
def createJSONFile(self, output_file, D):
print("Saving the json output file to " + str(output_file) + ", please wait, it could take a while ...")
f = open(output_file, 'w')
json.encoder.FLOAT_REPR = lambda f: ("%.6f" % f) # to get only four decimal places on the json file
print >> f, json.dumps(D, indent=2, sort_keys=True)
f.close()
print("Completed.")
@staticmethod
def generateKey(parent, child, suffix=''):
return parent + '-' + child + suffix
class MoveItGoalBuilder(object):
"""Builds a MoveGroupGoal.
Example:
# To do a reachability check from the current robot pose.
builder = MoveItGoalBuilder()
builder.set_pose_goal(pose_stamped)
builder.allowed_planning_time = 5
builder.plan_only = True
goal = builder.build()
# To move to a current robot pose with a few options changed.
builder = MoveItGoalBuilder()
builder.set_pose_goal(pose_stamped)
builder.replan = True
builder.replan_attempts = 10
goal = builder.build()
Here are the most common class attributes you might set before calling
build(), and their default values:
allowed_planning_time: float=10. How much time to allow the planner,
in seconds.
group_name: string='arm'. Either 'arm' or 'arm_with_torso'.
num_planning_attempts: int=1. How many times to compute the same plan (most
planners are randomized). The shortest plan will be used.
plan_only: bool=False. Whether to only compute the plan but not execute it.
replan: bool=False. Whether to come up with a new plan if there was an
error executing the original plan.
replan_attempts: int=5. How many times to do replanning.
replay_delay: float=1. How long to wait in between replanning, in seconds.
tolerance: float=0.01. The tolerance, in meters for the goal pose.
"""
def __init__(self):
self.allowed_planning_time = 10.0
self.fixed_frame = 'base_link'
self.gripper_frame = 'wrist_roll_link'
self.group_name = 'arm'
self.planning_scene_diff = moveit_msgs.msg.PlanningScene()
self.planning_scene_diff.is_diff = True
self.planning_scene_diff.robot_state.is_diff = True
self.look_around = False
self.max_acceleration_scaling_factor = 0
self.max_velocity_scaling_factor = 0
self.num_planning_attempts = 1
self.plan_only = False
self.planner_id = 'RRTConnectkConfigDefault'
self.replan = False
self.replan_attempts = 5
self.replan_delay = 1
self.start_state = moveit_msgs.msg.RobotState()
self.start_state.is_diff = True
self.tolerance = 0.01
self._orientation_contraints = []
self._pose_goal = None
self._joint_names = None
self._joint_positions = None
self._tf_listener = TransformListener()
def set_pose_goal(self, pose_stamped):
"""Sets a pose goal.
Pose and joint goals are mutually exclusive. The most recently set goal
wins.
Args:
pose_stamped: A geometry_msgs/PoseStamped.
"""
self._pose_goal = pose_stamped
self._joint_names = None
self._joint_positions = None
def set_joint_goal(self, joint_names, joint_positions):
"""Set a joint-space planning goal.
Args:
joint_names: A list of strings. The names of the joints in the goal.
joint_positions: A list of floats. The joint angles to achieve.
"""
self._joint_names = joint_names
self._joint_positions = joint_positions
self._pose_goal = None
def add_path_orientation_contraint(self, o_constraint):
"""Adds an orientation constraint to the path.
Args:
o_constraint: A moveit_msgs/OrientationConstraint.
"""
self._orientation_contraints.append(copy.deepcopy(o_constraint))
self.planner_id = 'RRTConnectkConfigDefault'
def build(self, tf_timeout=rospy.Duration(5.0)):
"""Builds the goal message.
To set a pose or joint goal, call set_pose_goal or set_joint_goal
before calling build. To add a path orientation constraint, call
add_path_orientation_constraint first.
Args:
tf_timeout: rospy.Duration. How long to wait for a TF message. Only
used with pose goals.
Returns: moveit_msgs/MoveGroupGoal
"""
goal = MoveGroupGoal()
# Set start state
goal.request.start_state = copy.deepcopy(self.start_state)
# Set goal constraint
if self._pose_goal is not None:
self._tf_listener.waitForTransform(self._pose_goal.header.frame_id,
self.fixed_frame,
rospy.Time.now(), tf_timeout)
try:
pose_transformed = self._tf_listener.transformPose(
self.fixed_frame, self._pose_goal)
except (tf.LookupException, tf.ConnectivityException):
return None
c1 = Constraints()
c1.position_constraints.append(PositionConstraint())
c1.position_constraints[0].header.frame_id = self.fixed_frame
c1.position_constraints[0].link_name = self.gripper_frame
b = BoundingVolume()
s = SolidPrimitive()
s.dimensions = [self.tolerance * self.tolerance]
s.type = s.SPHERE
b.primitives.append(s)
b.primitive_poses.append(self._pose_goal.pose)
c1.position_constraints[0].constraint_region = b
c1.position_constraints[0].weight = 1.0
c1.orientation_constraints.append(OrientationConstraint())
c1.orientation_constraints[0].header.frame_id = self.fixed_frame
c1.orientation_constraints[
0].orientation = pose_transformed.pose.orientation
c1.orientation_constraints[0].link_name = self.gripper_frame
c1.orientation_constraints[
0].absolute_x_axis_tolerance = self.tolerance
c1.orientation_constraints[
0].absolute_y_axis_tolerance = self.tolerance
c1.orientation_constraints[
0].absolute_z_axis_tolerance = self.tolerance
c1.orientation_constraints[0].weight = 1.0
goal.request.goal_constraints.append(c1)
elif self._joint_names is not None:
c1 = Constraints()
for i in range(len(self._joint_names)):
c1.joint_constraints.append(JointConstraint())
c1.joint_constraints[i].joint_name = self._joint_names[i]
c1.joint_constraints[i].position = self._joint_positions[i]
c1.joint_constraints[i].tolerance_above = self.tolerance
c1.joint_constraints[i].tolerance_below = self.tolerance
c1.joint_constraints[i].weight = 1.0
goal.request.goal_constraints.append(c1)
# Set path constraints
goal.request.path_constraints.orientation_constraints = self._orientation_contraints
# Set trajectory constraints
# Set planner ID (name of motion planner to use)
goal.request.planner_id = self.planner_id
# Set group name
goal.request.group_name = self.group_name
# Set planning attempts
goal.request.num_planning_attempts = self.num_planning_attempts
# Set planning time
goal.request.allowed_planning_time = self.allowed_planning_time
# Set scaling factors
goal.request.max_acceleration_scaling_factor = self.max_acceleration_scaling_factor
goal.request.max_velocity_scaling_factor = self.max_velocity_scaling_factor
# Set planning scene diff
goal.planning_options.planning_scene_diff = copy.deepcopy(
self.planning_scene_diff)
# Set is plan only
goal.planning_options.plan_only = self.plan_only
# Set look around
goal.planning_options.look_around = self.look_around
# Set replanning options
goal.planning_options.replan = self.replan
goal.planning_options.replan_attempts = self.replan_attempts
goal.planning_options.replan_delay = self.replan_delay
return goal
class Sensor:
def __init__(self, name, server, menu_handler, frame_world,
frame_opt_parent, frame_opt_child, frame_sensor,
marker_scale):
print('Creating a new sensor named ' + name)
self.name = name
self.server = server
self.menu_handler = menu_handler
self.listener = TransformListener()
self.br = tf.TransformBroadcaster()
self.marker_scale = marker_scale
# transforms
self.world_link = frame_world
self.opt_parent_link = frame_opt_parent
self.opt_child_link = frame_opt_child
self.sensor_link = frame_sensor
self.updateAll() # update all the transformations
# print('Collected pre, opt and pos transforms.')
#
# print('preT:\n' + str(self.preT))
# print('optT:\n' + str(self.optT))
# print('posT:\n' + str(self.posT))
self.optTInitial = copy.deepcopy(self.optT)
self.createInteractiveMarker() # create interactive marker
print('Created interactive marker.')
# Start publishing now
self.timer_callback = rospy.Timer(
rospy.Duration(.1),
self.publishTFCallback) # to periodically broadcast
def resetToInitalPose(self):
self.optT.matrix = self.optTInitial.matrix
trans = self.optT.getTranslation()
self.marker.pose.position.x = trans[0]
self.marker.pose.position.y = trans[1]
self.marker.pose.position.z = trans[2]
quat = self.optT.getQuaternion()
self.marker.pose.orientation.x = quat[0]
self.marker.pose.orientation.y = quat[1]
self.marker.pose.orientation.z = quat[2]
self.marker.pose.orientation.w = quat[3]
self.optTInitial = copy.deepcopy(self.optT)
self.menu_handler.reApply(self.server)
self.server.applyChanges()
def publishTFCallback(self, _):
trans = self.optT.getTranslation()
quat = self.optT.getQuaternion()
self.br.sendTransform((trans[0], trans[1], trans[2]),
(quat[0], quat[1], quat[2], quat[3]),
rospy.Time.now(), self.opt_child_link,
self.opt_parent_link)
def markerFeedback(self, feedback):
# print(' sensor ' + self.name + ' received feedback')
self.optT.setTranslationFromPosePosition(feedback.pose.position)
self.optT.setQuaternionFromPoseQuaternion(feedback.pose.orientation)
self.menu_handler.reApply(self.server)
self.server.applyChanges()
def updateAll(self):
self.updatePreT()
self.updateOptT()
self.updatePosT()
def updateOptT(self):
self.optT = self.updateT(self.opt_parent_link, self.opt_child_link,
rospy.Time.now())
def updatePreT(self):
self.preT = self.updateT(self.world_link, self.opt_parent_link,
rospy.Time.now())
def updatePosT(self):
self.posT = self.updateT(self.opt_child_link, self.sensor_link,
rospy.Time.now())
def updateT(self, parent_link, child_link, stamp):
# self.listener.waitForTransform(parent_link, child_link, stamp, rospy.Duration(3.0))
# (trans, quat) = self.listener.lookupTransform(parent_link, child_link, stamp)
self.listener.waitForTransform(parent_link, child_link, rospy.Time(),
rospy.Duration(1.0))
(trans, quat) = self.listener.lookupTransform(parent_link, child_link,
rospy.Time())
T = TransformationT(parent_link, child_link)
T.setTranslation(trans)
T.setQuaternion(quat)
return T
def createInteractiveMarker(self):
self.marker = InteractiveMarker()
self.marker.header.frame_id = self.opt_parent_link
trans = self.optT.getTranslation()
self.marker.pose.position.x = trans[0]
self.marker.pose.position.y = trans[1]
self.marker.pose.position.z = trans[2]
quat = self.optT.getQuaternion()
self.marker.pose.orientation.x = quat[0]
self.marker.pose.orientation.y = quat[1]
self.marker.pose.orientation.z = quat[2]
self.marker.pose.orientation.w = quat[3]
self.marker.scale = self.marker_scale
self.marker.name = self.name
self.marker.description = self.name + '_control'
# insert a box
control = InteractiveMarkerControl()
control.always_visible = True
marker_box = Marker()
marker_box.type = Marker.SPHERE
marker_box.scale.x = self.marker.scale * 0.3
marker_box.scale.y = self.marker.scale * 0.3
marker_box.scale.z = self.marker.scale * 0.3
marker_box.color.r = 0
marker_box.color.g = 1
marker_box.color.b = 0
marker_box.color.a = 0.2
control.markers.append(marker_box)
self.marker.controls.append(control)
self.marker.controls[
0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
self.marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 1
control.orientation.y = 0
control.orientation.z = 0
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
self.marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
self.marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 1
control.orientation.z = 0
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
self.marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
self.marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation.w = 1
control.orientation.x = 0
control.orientation.y = 0
control.orientation.z = 1
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
control.orientation_mode = InteractiveMarkerControl.FIXED
self.marker.controls.append(control)
self.server.insert(self.marker, self.markerFeedback)
self.menu_handler.apply(self.server, self.marker.name)
from tf.listener import TransformListener
from tf.broadcaster import TransformBroadcaster
from std_msgs.msg import Header
import numpy as np
from tf2_msgs.msg import TFMessage
rospy.init_node('map_to_odom')
listener = TransformListener()
publisher = TransformBroadcaster()
rate = rospy.Rate(100)
rospy.sleep(5)
time = rospy.Time.now()
listener.waitForTransform(target_frame='camera_link_slam',
source_frame='map',
time=time,
timeout=rospy.Duration(1))
listener.waitForTransform(target_frame='odom',
source_frame='camera_link',
time=time,
timeout=rospy.Duration(1))
trans = None
rot = None
camera_link_slam_stamp = None
camera_link_stamp = None
def set_stamp(message): # type: (TFMessage) -> None
global camera_link_slam_stamp, camera_link_stamp
if message.transforms[0].child_frame_id == 'camera_link_slam':
class DataCollectorAndLabeler:
def __init__(self, args, server, menu_handler):
self.output_folder = utilities.resolvePath(args['output_folder'])
if os.path.exists(
self.output_folder
) and not args['overwrite']: # dataset path exists, abort
print(
'\n' + Fore.RED + 'Error: Dataset ' + self.output_folder +
' exists.\nIf you want to replace it add a "--overwrite" flag.'
+ Style.RESET_ALL + '\n')
rospy.signal_shutdown()
elif os.path.exists(
self.output_folder
) and args['overwrite']: # move existing path to a backup location
now = datetime.now()
dt_string = now.strftime("%Y-%m-%d-%H-%M-%S")
basename = os.path.basename(self.output_folder)
backup_folder = '/tmp/' + basename + '_' + dt_string
time.sleep(2)
print('\n\nWarning: Dataset ' + Fore.YELLOW + self.output_folder +
Style.RESET_ALL + ' exists.\nMoving it to a new folder: ' +
Fore.YELLOW + backup_folder +
'\nThis will be deleted after a system reboot!' +
Style.RESET_ALL + '\n\n')
time.sleep(2)
execute('mv ' + self.output_folder + ' ' + backup_folder,
verbose=True)
os.mkdir(self.output_folder) # Recreate the folder
self.listener = TransformListener()
self.sensors = {}
self.sensor_labelers = {}
self.server = server
self.menu_handler = menu_handler
self.data_stamp = 0
self.collections = {}
self.bridge = CvBridge()
self.config = loadConfig(args['calibration_file'])
if self.config is None:
sys.exit(1) # loadJSON should tell you why.
self.world_link = self.config['world_link']
# Add sensors
print(Fore.BLUE + 'Sensors:' + Style.RESET_ALL)
print('Number of sensors: ' + str(len(self.config['sensors'])))
# Go through the sensors in the calib config.
for sensor_key, value in self.config['sensors'].items():
# Create a dictionary that describes this sensor
sensor_dict = {
'_name': sensor_key,
'parent': value['link'],
'calibration_parent': value['parent_link'],
'calibration_child': value['child_link']
}
# TODO replace by utils function
print("Waiting for message " + value['topic_name'] + ' ...')
msg = rospy.wait_for_message(value['topic_name'], rospy.AnyMsg)
print('... received!')
connection_header = msg._connection_header['type'].split('/')
ros_pkg = connection_header[0] + '.msg'
msg_type = connection_header[1]
print('Topic ' + value['topic_name'] + ' has type ' + msg_type)
sensor_dict['topic'] = value['topic_name']
sensor_dict['msg_type'] = msg_type
# If topic contains a message type then get a camera_info message to store along with the sensor data
if sensor_dict[
'msg_type'] == 'Image': # if it is an image must get camera_info
sensor_dict['camera_info_topic'] = os.path.dirname(
sensor_dict['topic']) + '/camera_info'
from sensor_msgs.msg import CameraInfo
print('Waiting for camera_info message on topic ' +
sensor_dict['camera_info_topic'] + ' ...')
camera_info_msg = rospy.wait_for_message(
sensor_dict['camera_info_topic'], CameraInfo)
print('... received!')
from rospy_message_converter import message_converter
sensor_dict[
'camera_info'] = message_converter.convert_ros_message_to_dictionary(
camera_info_msg)
# Get the kinematic chain form world_link to this sensor's parent link
now = rospy.Time()
print('Waiting for transformation from ' + value['link'] + ' to ' +
self.world_link)
self.listener.waitForTransform(value['link'], self.world_link, now,
rospy.Duration(5))
print('... received!')
chain = self.listener.chain(value['link'], now, self.world_link,
now, self.world_link)
chain_list = []
for parent, child in zip(chain[0::], chain[1::]):
key = self.generateKey(parent, child)
chain_list.append({
'key': key,
'parent': parent,
'child': child
})
sensor_dict['chain'] = chain_list # Add to sensor dictionary
self.sensors[sensor_key] = sensor_dict
sensor_labeler = InteractiveDataLabeler(
self.server, self.menu_handler, sensor_dict,
args['marker_size'], self.config['calibration_pattern'])
self.sensor_labelers[sensor_key] = sensor_labeler
print('Setup for sensor ' + sensor_key + ' is complete.')
print(Fore.BLUE + sensor_key + Style.RESET_ALL + ':\n' +
str(sensor_dict))
# print('sensor_labelers:')
# print(self.sensor_labelers)
self.abstract_transforms = self.getAllAbstractTransforms()
# print("abstract_transforms = " + str(self.abstract_transforms))
def getTransforms(self, abstract_transforms, time=None):
transforms_dict = {
} # Initialize an empty dictionary that will store all the transforms for this data-stamp
if time is None:
time = rospy.Time.now()
for ab in abstract_transforms: # Update all transformations
self.listener.waitForTransform(ab['parent'], ab['child'], time,
rospy.Duration(1.0))
(trans,
quat) = self.listener.lookupTransform(ab['parent'], ab['child'],
time)
key = self.generateKey(ab['parent'], ab['child'])
transforms_dict[key] = {
'trans': trans,
'quat': quat,
'parent': ab['parent'],
'child': ab['child']
}
return transforms_dict
def lockAllLabelers(self):
for sensor_name, sensor in self.sensors.iteritems():
self.sensor_labelers[sensor_name].lock.acquire()
print("Locked all labelers")
def unlockAllLabelers(self):
for sensor_name, sensor in self.sensors.iteritems():
self.sensor_labelers[sensor_name].lock.release()
print("Unlocked all labelers")
def getLabelersTimeStatistics(self):
stamps = [] # a list of the several time stamps of the stored messages
for sensor_name, sensor in self.sensors.iteritems():
stamps.append(
copy.deepcopy(
self.sensor_labelers[sensor_name].msg.header.stamp))
max_delta = getMaxTimeDelta(stamps)
# TODO : this is because of Andre's bag file problem. We should go back to the getAverageTime
# average_time = getAverageTime(stamps) # For looking up transforms use average time of all sensor msgs
average_time = getMaxTime(
stamps
) # For looking up transforms use average time of all sensor msgs
print('Times:')
for stamp, sensor_name in zip(stamps, self.sensors):
printRosTime(stamp, prefix=sensor_name + ': ')
return stamps, average_time, max_delta
def saveCollection(self):
# --------------------------------------
# Collect sensor data and labels (images, laser scans, etc)
# --------------------------------------
# Lock the semaphore for all labelers
self.lockAllLabelers()
# Analyse message time stamps and decide if collection can be stored
stamps, average_time, max_delta = self.getLabelersTimeStatistics()
if max_delta is not None: # if max_delta is None (only one sensor), continue
if max_delta.to_sec() > float(
self.config['max_duration_between_msgs']
): # times are close enough?
rospy.logwarn('Max duration between msgs in collection is ' +
str(max_delta.to_sec()) +
'. Not saving collection.')
self.unlockAllLabelers()
return None
else: # test passed
rospy.loginfo('Max duration between msgs in collection is ' +
str(max_delta.to_sec()))
# Collect all the transforms
transforms = self.getTransforms(
self.abstract_transforms,
average_time) # use average time of sensor msgs
printRosTime(average_time, "Collected transforms for time ")
all_sensor_data_dict = {}
all_sensor_labels_dict = {}
for sensor_key, sensor in self.sensors.iteritems():
print('collect sensor: ' + sensor_key)
msg = copy.deepcopy(self.sensor_labelers[sensor_key].msg)
labels = copy.deepcopy(self.sensor_labelers[sensor_key].labels)
print('sensor' + sensor_key)
# TODO add exception also for point cloud and depth image
# Update sensor data ---------------------------------------------
if sensor[
'msg_type'] == 'Image': # Special case of requires saving image data as png separate files
cv_image = self.bridge.imgmsg_to_cv2(
msg,
"bgr8") # Convert to opencv image and save image to disk
filename = self.output_folder + '/' + sensor[
'_name'] + '_' + str(self.data_stamp) + '.jpg'
filename_relative = sensor['_name'] + '_' + str(
self.data_stamp) + '.jpg'
cv2.imwrite(filename, cv_image)
image_dict = message_converter.convert_ros_message_to_dictionary(
msg) # Convert sensor data to dictionary
del image_dict[
'data'] # Remove data field (which contains the image), and replace by "data_file"
image_dict[
'data_file'] = filename_relative # Contains full path to where the image was saved
# Update the data dictionary for this data stamp
all_sensor_data_dict[sensor['_name']] = image_dict
else:
# Update the data dictionary for this data stamp
all_sensor_data_dict[sensor[
'_name']] = message_converter.convert_ros_message_to_dictionary(
msg)
# Update sensor labels ---------------------------------------------
if sensor['msg_type'] in ['Image', 'LaserScan', 'PointCloud2']:
all_sensor_labels_dict[sensor_key] = labels
else:
raise ValueError('Unknown message type.')
collection_dict = {
'data': all_sensor_data_dict,
'labels': all_sensor_labels_dict,
'transforms': transforms
}
self.collections[self.data_stamp] = collection_dict
self.data_stamp += 1
# Save to json file
D = {
'sensors': self.sensors,
'collections': self.collections,
'calibration_config': self.config
}
print('Saving file ' + self.output_folder + '/data_collected.json')
self.createJSONFile(self.output_folder + '/data_collected.json', D)
self.unlockAllLabelers()
def getAllAbstractTransforms(self):
# Get a list of all transforms to collect
transforms_list = []
now = rospy.Time.now()
all_frames = self.listener.getFrameStrings()
for frame in all_frames:
# print('Waiting for transformation from ' + frame + ' to ' + self.world_link + '(max 3 secs)')
try:
self.listener.waitForTransform(frame, self.world_link, now,
rospy.Duration(3))
chain = self.listener.chain(frame, now, self.world_link, now,
self.world_link)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.logerr('Could not get transform from ' + frame + ' to ' +
self.world_link + '(max 3 secs)')
continue
for idx in range(0, len(chain) - 1):
parent = chain[idx]
child = chain[idx + 1]
transforms_list.append({
'parent': parent,
'child': child,
'key': self.generateKey(parent, child)
})
# https://stackoverflow.com/questions/31792680/how-to-make-values-in-list-of-dictionary-unique
uniq_l = list(
map(dict,
frozenset(frozenset(i.items()) for i in transforms_list)))
return uniq_l # get unique values
def createJSONFile(self, output_file, D):
print("Saving the json output file to " + str(output_file) +
", please wait, it could take a while ...")
f = open(output_file, 'w')
json.encoder.FLOAT_REPR = lambda f: (
"%.6f" % f) # to get only four decimal places on the json file
print >> f, json.dumps(D, indent=2, sort_keys=True)
f.close()
print("Completed.")
@staticmethod
def generateKey(parent, child, suffix=''):
return parent + '-' + child + suffix
class Arm(object):
"""Arm controls the robot's arm.
Joint space control:
joints = ArmJoints()
# Fill out joint states
arm = fetch_api.Arm()
arm.move_to_joints(joints)
"""
def __init__(self):
self._joint_client = actionlib.SimpleActionClient(
JOINT_ACTION_SERVER, control_msgs.msg.FollowJointTrajectoryAction)
self._joint_client.wait_for_server(rospy.Duration(10))
self._move_group_client = actionlib.SimpleActionClient(
MOVE_GROUP_ACTION_SERVER, MoveGroupAction)
self._move_group_client.wait_for_server(rospy.Duration(10))
self._compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
self._tf_listener = TransformListener()
self.tuck_pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
def tuck(self):
"""
Uses motion-planning to tuck the arm within the footprint of the base.
:return: a string describing the error, or None if there was no error
"""
return self.move_to_joint_goal(zip(ArmJoints.names(), self.tuck_pose))
def tuck_unsafe(self):
"""
TUCKS BUT DOES NOT PREVENT SELF-COLLISIONS, WHICH ARE HIGHLY LIKELY.
Don't use this unless you have prior knowledge that the arm can safely return
to tucked from its current configuration. Most likely, you should only use this
method in simulation, where the arm can clip through the base without issue.
:return:
"""
return self.move_to_joints(ArmJoints.from_list(self.tuck_pose))
def move_to_joints(self, joint_state):
"""
Moves to an ArmJoints configuration
:param joint_state: an ArmJoints instance to move to
:return:
"""
goal = control_msgs.msg.FollowJointTrajectoryGoal()
goal.trajectory.joint_names.extend(ArmJoints.names())
point = trajectory_msgs.msg.JointTrajectoryPoint()
point.positions.extend(joint_state.values())
point.time_from_start = rospy.Duration(TIME_FROM_START)
goal.trajectory.points.append(point)
self._joint_client.send_goal(goal)
self._joint_client.wait_for_result(rospy.Duration(10))
def move_to_joint_goal(self,
joints,
allowed_planning_time=10.0,
execution_timeout=15.0,
group_name='arm',
num_planning_attempts=1,
plan_only=False,
replan=False,
replan_attempts=5,
tolerance=0.01):
"""Moves the end-effector to a pose, using motion planning.
Args:
joints: A list of (name, value) for the arm joints.
allowed_planning_time: float. The maximum duration to wait for a
planning result.
execution_timeout: float. The maximum duration to wait for an arm
motion to execute (or for planning to fail completely), in
seconds.
group_name: string. Either 'arm' or 'arm_with_torso'.
num_planning_attempts: int. The number of times to compute the same
plan. The shortest path is ultimately used. For random
planners, this can help get shorter, less weird paths.
plan_only: bool. If True, then this method does not execute the
plan on the robot. Useful for determining whether this is
likely to succeed.
replan: bool. If True, then if an execution fails (while the arm is
moving), then come up with a new plan and execute it.
replan_attempts: int. How many times to replan if the execution
fails.
tolerance: float. The goal tolerance, in meters.
Returns:
string describing the error if an error occurred, else None.
"""
goal_builder = MoveItGoalBuilder()
goal_builder.set_joint_goal(*zip(*joints))
goal_builder.allowed_planning_time = allowed_planning_time
goal_builder.num_planning_attempts = num_planning_attempts
goal_builder.plan_only = plan_only
goal_builder.planner_id = ''
goal_builder.replan = replan
goal_builder.replan_attempts = replan_attempts
goal_builder.tolerance = tolerance
goal = goal_builder.build()
if goal is not None:
self._move_group_client.send_goal(goal)
success = self._move_group_client.wait_for_result(
rospy.Duration(execution_timeout))
if not success:
return moveit_error_string(MoveItErrorCodes.TIMED_OUT)
result = self._move_group_client.get_result()
if result:
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return None
else:
return moveit_error_string(result.error_code.val)
else:
return moveit_error_string(MoveItErrorCodes.TIMED_OUT)
def move_to_pose(self,
pose_stamped,
allowed_planning_time=10.0,
execution_timeout=15.0,
group_name='arm',
num_planning_attempts=1,
orientation_constraint=None,
plan_only=False,
replan=False,
replan_attempts=5,
tolerance=0.01):
"""Moves the end-effector to a pose, using motion planning.
Args:
pose: geometry_msgs/PoseStamped. The goal pose for the gripper.
allowed_planning_time: float. The maximum duration to wait for a
planning result.
execution_timeout: float. The maximum duration to wait for an arm
motion to execute (or for planning to fail completely), in
seconds.
group_name: string. Either 'arm' or 'arm_with_torso'.
num_planning_attempts: int. The number of times to compute the same
plan. The shortest path is ultimately used. For random
planners, this can help get shorter, less weird paths.
orientation_constraint: moveit_msgs/OrientationConstraint. An
orientation constraint for the entire path.
plan_only: bool. If True, then this method does not execute the
plan on the robot. Useful for determining whether this is
likely to succeed.
replan: bool. If True, then if an execution fails (while the arm is
moving), then come up with a new plan and execute it.
replan_attempts: int. How many times to replan if the execution
fails.
tolerance: float. The goal tolerance, in meters.
Returns:
string describing the error if an error occurred, else None.
"""
goal_builder = MoveItGoalBuilder()
goal_builder.set_pose_goal(pose_stamped)
if orientation_constraint is not None:
goal_builder.add_path_orientation_contraint(orientation_constraint)
goal_builder.allowed_planning_time = allowed_planning_time
goal_builder.num_planning_attempts = num_planning_attempts
goal_builder.plan_only = plan_only
goal_builder.replan = replan
goal_builder.replan_attempts = replan_attempts
goal_builder.tolerance = tolerance
goal = goal_builder.build()
if goal is not None:
self._move_group_client.send_goal(goal)
self._move_group_client.wait_for_result(
rospy.Duration(execution_timeout))
result = self._move_group_client.get_result()
if result:
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return None
else:
return moveit_error_string(result.error_code.val)
else:
return moveit_error_string(MoveItErrorCodes.TIMED_OUT)
def straight_move_to_pose(self,
group,
pose_stamped,
ee_step=0.025,
jump_threshold=2.0,
avoid_collisions=True):
"""Moves the end-effector to a pose in a straight line.
Args:
group: moveit_commander.MoveGroupCommander. The planning group for
the arm.
pose_stamped: geometry_msgs/PoseStamped. The goal pose for the
gripper.
ee_step: float. The distance in meters to interpolate the path.
jump_threshold: float. The maximum allowable distance in the arm's
configuration space allowed between two poses in the path. Used to
prevent "jumps" in the IK solution.
avoid_collisions: bool. Whether to check for obstacles or not.
Returns:
string describing the error if an error occurred, else None.
"""
# Transform pose into planning frame
self._tf_listener.waitForTransform(pose_stamped.header.frame_id,
group.get_planning_frame(),
rospy.Time.now(),
rospy.Duration(1.0))
try:
pose_transformed = self._tf_listener.transformPose(
group.get_planning_frame(), pose_stamped)
except (tf.LookupException, tf.ConnectivityException):
rospy.logerr('Unable to transform pose from frame {} to {}'.format(
pose_stamped.header.frame_id, group.get_planning_frame()))
return moveit_error_string(
MoveItErrorCodes.FRAME_TRANSFORM_FAILURE)
# Compute path
plan, fraction = group.compute_cartesian_path(
[group.get_current_pose().pose, pose_transformed.pose], ee_step,
jump_threshold, avoid_collisions)
if fraction < 1 and fraction > 0:
rospy.logerr('Only able to compute {}% of the path'.format(
fraction * 100))
if fraction == 0:
return moveit_error_string(MoveItErrorCodes.PLANNING_FAILED)
# Execute path
result = group.execute(plan, wait=True)
if not result:
return moveit_error_string(MoveItErrorCodes.INVALID_MOTION_PLAN)
else:
return None
def check_pose(self,
pose_stamped,
allowed_planning_time=10.0,
group_name='arm',
tolerance=0.01):
return self.move_to_pose(pose_stamped,
allowed_planning_time=allowed_planning_time,
group_name=group_name,
tolerance=tolerance,
plan_only=True)
def compute_ik(self, pose_stamped, timeout=rospy.Duration(5)):
"""Computes inverse kinematics for the given pose.
Note: if you are interested in returning the IK solutions, we have
shown how to access them.
Args:
pose_stamped: geometry_msgs/PoseStamped.
timeout: rospy.Duration. How long to wait before giving up on the
IK solution.
Returns: A list of (name, value) for the arm joints if the IK solution
was found, False otherwise.
"""
request = GetPositionIKRequest()
request.ik_request.pose_stamped = pose_stamped
request.ik_request.group_name = 'arm'
request.ik_request.timeout = timeout
response = self._compute_ik(request)
error_str = moveit_error_string(response.error_code.val)
success = error_str == 'SUCCESS'
if not success:
return False
joint_state = response.solution.joint_state
joints = []
for name, position in zip(joint_state.name, joint_state.position):
if name in ArmJoints.names():
joints.append((name, position))
return joints
def cancel_all_goals(self):
self._move_group_client.cancel_all_goals()
self._joint_client.cancel_all_goals()
