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)