def filter_poses(detections):
print("filtering detection poses")
filtered = []
for detect3d in detections:
print("bounding box: " + str(detect3d.bbox))
pose = PoseStamped()
pose.pose = detect3d.bbox.center
pose.header = detect3d.header
# print("pose: \n" + str(pose))
transform = getTFPos(pose.header.frame_id, "map")
# print("transform: \n" + str(transform))
map_center = tf2_geometry_msgs.do_transform_pose(pose, transform)
# print("center point: \n" + str(map_center))
surface = map_center.pose.position.z + detect3d.bbox.size.z
if map_center < 0:
print("filtered negative center heigth")
continue
print("surface@: " + str(surface))
if surface < 0.2:
print("filtered low surface heigth < 0.3m")
continue
filtered.append(detect3d)
return filtered
pose.position.x = tf_Stamped.transform.translation.x
pose.position.y = tf_Stamped.transform.translation.y
pose.position.z = tf_Stamped.transform.translation.z
pose.orientation = tf_Stamped.transform.rotation
def to_PoseStamped(klampt_se3, stamp='now'):
"""From Klamp't se3 element to ROS PoseStamped
"""
if stamp == 'now':
stamp = rospy.Time.now()
elif isinstance(stamp, (int, float)):
stamp = rospy.Time(stamp)
ros_pose = PoseStamped()
ros_pose.header.stamp = stamp
ros_pose.orientation = to_Quaternion(klampt_se3[0])
ros_pose.position = to_Point(klampt_se3[0])
return ros_pose
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('cobra_test_cartesian', anonymous=True)
arm = MoveGroupCommander(ARM_GROUP)
arm.allow_replanning(True)
rospy.sleep(2)
pose = PoseStamped().pose
# same orientation for all
q = quaternion_from_euler(0.0, 0.0, 0.0) # roll, pitch, yaw
pose.orientation = Quaternion(*q)
i = 1
while i <= 10:
waypoints = list()
j = 1
while j <= 5:
# random pose
rand_pose = arm.get_random_pose(arm.get_end_effector_link())
pose.position.x = rand_pose.pose.position.x
pose.position.y = rand_pose.pose.position.y
pose.position.z = rand_pose.pose.position.z
waypoints.append(copy.deepcopy(pose))
j += 1
(plan, fraction) = arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
print "====== waypoints created ======="
# print waypoints
# ======= show cartesian path ====== #
arm.go()
rospy.sleep(10)
i += 1
print "========= end ========="
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def find_frontier(self, msg):
"""
Returns a dictionary of frontiers given by {'key',(x,y,size)} in the occupancy grid.
:return {'key',(x,y,size)} A dictionary of frontiers and their sizes.
"""
# Get a map from the map_expander node
mapdata = FindFrontier.request_map()
# Begins counting the number of centroids
centroid_number = 0
# Establishes the dictionary of frontiers and the list of visited points
# frontiers = {}
visited_points = []
frontier = [-1515, -1515]
priority = 0
[current_x,
current_y] = FindFrontier.world_to_grid(mapdata,
msg.start.pose.position)
# Loop through all cells in the occupancy grid
for h in range(mapdata.info.height - 1):
for w in range(mapdata.info.width - 1):
# Calculates index of the point in the grid
this_index = FindFrontier.grid_to_index(mapdata, w, h)
# If the point has not been checked
if this_index not in visited_points:
# Add this point to the list of visited points
visited_points.append(this_index)
# If this point is unknown
if mapdata.data[this_index] == -1:
# Increase the total number of centroids by one
centroid_number += 1
# Set the initial size to one
size = 1
# Adds to the total x and y for averaging later
total_x = w
total_y = h
# Add any adjacent unknown points to the list of unknowns
list_of_unknown_points = FindFrontier.unknown_neighbors_of_8(
w, h, mapdata, [])
# While there are unknown neighbors
while not len(list_of_unknown_points) == 0:
# Get a point from the list
this = list_of_unknown_points.pop
this_x = this[0]
this_y = this[1]
# Increase the size by one
size += 1
# Add the current x and y to the totals
total_x += this_x
total_y += this_y
# Add any adjacent unknown points to the list of unknowns
list_of_unknown_points.append(
FindFrontier.unknown_neighbors_of_8(
this_x, this_y, mapdata, visited_points))
# Get the index for this point
this_index = FindFrontier.grid_to_index(
mapdata, this_x, this_y)
# Add this point to the visited points list
visited_points.append(this_index)
# Calculate the centroid using the total xs and ys
centroid_x = total_x / size
centroid_y = total_y / size
centroid_distance = FindFrontier.euclidean_distance(
centroid_x, centroid_y, current_x, current_y)
centroid_priority = size / centroid_distance
if centroid_priority > priority:
priority = centroid_priority
frontier = (centroid_x, centroid_y)
# Add the frontier to the dictionary
# frontiers[centroid_number] = (centroid_x, centroid_y, size)
# Return the dictionary
best_point = PoseStamped()
best_point.header.frame_id = "frontier"
best_point.position = [frontier[0], frontier[1], 0]
best_point.orientation = [0, 0, 0, 0]
return best_point
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('cobra_move_cartesian', anonymous=True)
arm = MoveGroupCommander(ARM_GROUP)
arm.allow_replanning(True)
# arm.set_goal_position_tolerance(0.01)
arm.set_named_target(START_POSITION)
arm.go()
rospy.sleep(2)
waypoints = list()
pose = PoseStamped().pose
# start with the current pose
waypoints.append(
copy.deepcopy(
arm.get_current_pose(arm.get_end_effector_link()).pose))
# same orientation for all
q = quaternion_from_euler(0.0, 0.0, 0.0) # roll, pitch, yaw
pose.orientation = Quaternion(*q)
# first pose
pose.position.x = 0.35
pose.position.y = 0.25
pose.position.z = 0.3
waypoints.append(copy.deepcopy(pose))
# second pose
pose.position.x = 0.25
pose.position.y = -0.3
pose.position.z = 0.3
waypoints.append(copy.deepcopy(pose))
# third pose
pose.position.x += 0.15
waypoints.append(copy.deepcopy(pose))
# fourth pose
pose.position.y += 0.15
waypoints.append(copy.deepcopy(pose))
(plan, fraction) = arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
print "====== waypoints created ======="
# print waypoints
# ======= show cartesian path ====== #
arm.go()
rospy.sleep(10)
print "========= end ========="
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('cobra_move_cartesian', anonymous=True)
arm = MoveGroupCommander(ARM_GROUP)
arm.allow_replanning(True)
# arm.set_goal_position_tolerance(0.01)
arm.set_named_target(START_POSITION)
arm.go()
rospy.sleep(2)
waypoints = list()
pose = PoseStamped().pose
# start with the current pose
waypoints.append(copy.deepcopy(arm.get_current_pose(arm.get_end_effector_link()).pose))
# same orientation for all
q = quaternion_from_euler(0.0, 0.0, 0.0) # roll, pitch, yaw
pose.orientation = Quaternion(*q)
# first pose
pose.position.x = 0.35
pose.position.y = 0.25
pose.position.z = 0.3
waypoints.append(copy.deepcopy(pose))
# second pose
pose.position.x = 0.25
pose.position.y = -0.3
pose.position.z = 0.3
waypoints.append(copy.deepcopy(pose))
# third pose
pose.position.x += 0.15
waypoints.append(copy.deepcopy(pose))
# fourth pose
pose.position.y += 0.15
waypoints.append(copy.deepcopy(pose))
(plan, fraction) = arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
print "====== waypoints created ======="
# print waypoints
# ======= show cartesian path ====== #
arm.go()
rospy.sleep(10)
print "========= end ========="
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
