def test_updateState_PosOffsetPosArc(self): ''' Test the robot following a path starting from the same angle but with a positive offset and larger radius ''' pathSeg = PathSegmentMsg() pathSeg.seg_type = pathSeg.ARC pathSeg.seg_number = 1 pathSeg.seg_length = math.pi / 2.0 pathSeg.ref_point.x = 0.0 pathSeg.ref_point.y = 0.0 init_quat = quaternion_from_euler(0, 0, math.pi + math.pi / 2) pathSeg.init_tan_angle.w = init_quat[3] pathSeg.init_tan_angle.x = init_quat[0] pathSeg.init_tan_angle.y = init_quat[1] pathSeg.init_tan_angle.z = init_quat[2] pathSeg.curvature = 1.0 maxSpeed = TwistMsg() maxSpeed.linear.x = 1.0 maxSpeed.angular.z = 1.0 pathSeg.max_speeds = maxSpeed minSpeed = TwistMsg() pathSeg.min_speeds = minSpeed pathSeg.accel_limit = 1.0 pathSeg.decel_limit = -1.0 state = State(pathSeg) vel_cmd = TwistMsg() vel_cmd.linear.x = 0.5 vel_cmd.angular.z = 0.0 point = PointMsg() maxIter = 1000 count = 1 rhoDes = pathSeg.curvature - .3 angle = State.getYaw(pathSeg.init_tan_angle) r = 1 / abs(rhoDes) startAngle = angle - math.pi / 2.0 # extrapolate next point while (state.segDistDone < 1.0 and maxIter > count): # create where the robot should have moved dAng = pathSeg.seg_length * (count / (maxIter / 2.0)) * rhoDes arcAng = startAngle + dAng point.x = pathSeg.ref_point.x + r * math.cos(arcAng) point.y = pathSeg.ref_point.y + r * math.sin(arcAng) state.updateState(vel_cmd, point, 0.0) count += 1 self.assertTrue(count < maxIter) self.assertTrue(state.segDistDone >= 1.0)
def filter_invalid_scan_poses(self, cell_x, cell_y, pose_list): """ Filters poses, that are behind obstacles or unknown which nearby the given cell. :param cell_x: x coordinate of the cell :type: float :param cell_y: y coordinate of the cell :type: float :param pose_list: list of poses :type: [PoseStamped] :return: filtered list of poses :type: [PoseStamped] """ poses = deepcopy(pose_list) surr_cells = self.get_surrounding_cells8(cell_x, cell_y) for (c, x, y) in surr_cells: if not c.is_free(): p = Point(x, y, 0) p2 = Point(cell_x, cell_y, 0) cell_to_p = subtract_point(p, p2) for pose in pose_list: x_under_pose = pose.pose.position.x y_under_pose = pose.pose.position.y # filter poses that are pointing to unknowns or obstacles cell_to_pose = subtract_point( Point(x_under_pose, y_under_pose, 0), p2) angle = get_angle(cell_to_p, cell_to_pose) if angle <= pi / 4 + 0.0001 and pose in poses: poses.remove(pose) return poses
def test_updateState_NegOffsetNegArc(self): ''' Test the robot following a path starting from the same angle but with a negative offset and a smaller radius ''' pathSeg = PathSegmentMsg() pathSeg.seg_type = pathSeg.ARC pathSeg.seg_number = 1 pathSeg.seg_length = math.pi/2.0 pathSeg.ref_point.x = 0.0 pathSeg.ref_point.y = 0.0 init_quat = quaternion_from_euler(0,0,3*math.pi/4.0-math.pi/2) pathSeg.init_tan_angle.w = init_quat[3] pathSeg.init_tan_angle.x = init_quat[0] pathSeg.init_tan_angle.y = init_quat[1] pathSeg.init_tan_angle.z = init_quat[2] pathSeg.curvature = -1.0 maxSpeed = TwistMsg() maxSpeed.linear.x = 1.0 maxSpeed.angular.z = 1.0 pathSeg.max_speeds = maxSpeed minSpeed = TwistMsg() pathSeg.min_speeds = minSpeed pathSeg.accel_limit = 1.0 pathSeg.decel_limit = -1.0 state = State(pathSeg) vel_cmd = TwistMsg() vel_cmd.linear.x = 0.5 vel_cmd.angular.z = 0.0 point = PointMsg() maxIter = 1000 count = 1 rhoDes = pathSeg.curvature + .3 angle = State.getYaw(pathSeg.init_tan_angle) r=1/abs(rhoDes) startAngle = angle + math.pi/2.0 # extrapolate next point while(state.segDistDone < 1.0 and maxIter > count): # create where the robot should have moved dAng = pathSeg.seg_length*(count/(maxIter/2.0))*rhoDes arcAng = startAngle+dAng point.x = pathSeg.ref_point.x + r*math.cos(arcAng) point.y = pathSeg.ref_point.y + r*math.sin(arcAng) state.updateState(vel_cmd, point, 0.0) count += 1 self.assertTrue(count < maxIter) self.assertTrue(state.segDistDone >= 1.0)
def to_marker(self): """ :return: a marker representing the map. :type: Marker """ marker = Marker() marker.type = Marker.CUBE_LIST for x in range(0, len(self.field)): for y in range(0, len(self.field[0])): marker.header.frame_id = '/odom_combined' marker.ns = 'suturo_planning/map' marker.id = 23 marker.action = Marker.ADD (x_i, y_i) = self.index_to_coordinates(x, y) marker.pose.position.x = 0 marker.pose.position.y = 0 marker.pose.position.z = 0 marker.pose.orientation.w = 1 marker.scale.x = self.cell_size marker.scale.y = self.cell_size marker.scale.z = 0.01 p = Point() p.x = x_i p.y = y_i marker.points.append(p) c = self.get_cell_by_index(x, y) marker.colors.append(self.__cell_to_color(c)) marker.lifetime = rospy.Time(0) return marker
def test_stop(self): ''' Test the stop method ''' pathSeg = PathSegmentMsg() pathSeg.seg_type = pathSeg.LINE pathSeg.seg_number = 1 pathSeg.seg_length = math.sqrt(2)*2 pathSeg.ref_point.x = 0.0 pathSeg.ref_point.y = 0.0 pathSeg.ref_point.z = 0.0 init_quat = quaternion_from_euler(0,0,math.pi/4.0) pathSeg.init_tan_angle.w = init_quat[3] pathSeg.init_tan_angle.x = init_quat[0] pathSeg.init_tan_angle.y = init_quat[1] pathSeg.init_tan_angle.z = init_quat[2] pathSeg.curvature = 0.0 maxSpeed = TwistMsg() maxSpeed.linear.x = 1.0 maxSpeed.angular.z = 1.0 pathSeg.max_speeds = maxSpeed minSpeed = TwistMsg() pathSeg.min_speeds = minSpeed pathSeg.accel_limit = 1.0 pathSeg.decel_limit = -1.0 state = State(pathSeg) vel_cmd = TwistMsg() vel_cmd.linear.x = 0.5 vel_cmd.angular.z = 1.5 point = PointMsg() maxIter = 300 count = 1 # extrapolate next point while(state.segDistDone < 1.0 and maxIter > count): # create where the robot should have moved point.x = pathSeg.seg_length*(count/(maxIter/2.0))*math.cos(math.pi/4.0) point.y = pathSeg.seg_length*(count/(maxIter/2.0))*math.sin(math.pi/4.0) state.updateState(vel_cmd, point, 0.0) count += 1 self.assertTrue(count < maxIter) self.assertTrue(state.segDistDone >= 1.0) self.assertEquals(state.v, 0.5) self.assertEquals(state.o, 1.5) state.stop() self.assertEquals(state.v, 0.0) self.assertEquals(state.o, 0.0)
def main(x=None,y=None): global goal rospy.init_node('astar_alpha_goal_publisher') goalPub = rospy.Publisher('goal_point',GoalMsg) naptime = rospy.Rate(RATE) goal = GoalMsg() goal.new = True if(x is None or y is None): goal.none = True else: goal_point = PointMsg() goal_point.x = x goal_point.y = y goal.goal = goal_point while not rospy.is_shutdown(): goalPub.publish(goal) goal.new = False naptime.sleep()
def test_updateState_NegOffsetNegSpin(self): ''' Test the robot following a path starting from an angle with a positive offset ''' pathSeg = PathSegmentMsg() pathSeg.seg_type = pathSeg.SPIN_IN_PLACE pathSeg.seg_number = 1 pathSeg.seg_length = math.pi pathSeg.ref_point.x = 0.0 pathSeg.ref_point.y = 0.0 pathSeg.ref_point.z = 0.0 init_quat = quaternion_from_euler(0, 0, 0.0) pathSeg.init_tan_angle.w = init_quat[3] pathSeg.init_tan_angle.x = init_quat[0] pathSeg.init_tan_angle.y = init_quat[1] pathSeg.init_tan_angle.z = init_quat[2] pathSeg.curvature = 1.0 ref_point = PointMsg() ref_point.x = 1.0 ref_point.y = 1.0 pathSeg.ref_point = ref_point maxSpeed = TwistMsg() maxSpeed.linear.x = 1.0 maxSpeed.angular.z = 1.0 pathSeg.max_speeds = maxSpeed minSpeed = TwistMsg() pathSeg.min_speeds = minSpeed pathSeg.accel_limit = 1.0 pathSeg.decel_limit = -1.0 state = State(pathSeg) vel_cmd = TwistMsg() vel_cmd.linear.x = 0.0 vel_cmd.angular.z = 0.5 point = PointMsg() psi = 0.0 maxIter = 1000 count = 1 # extrapolate next point while (state.segDistDone < 1.0 and maxIter > count): # create where the robot should have moved psi = 2.0 * state.pathSeg.seg_length * count / maxIter + math.pi / 4.0 state.updateState(vel_cmd, point, psi) count += 1 self.assertTrue(count < maxIter) self.assertTrue(state.segDistDone >= 1.0)
def test_updateState_NegOffsetNegSpin(self): ''' Test the robot following a path starting from an angle with a positive offset ''' pathSeg = PathSegmentMsg() pathSeg.seg_type = pathSeg.SPIN_IN_PLACE pathSeg.seg_number = 1 pathSeg.seg_length = math.pi pathSeg.ref_point.x = 0.0 pathSeg.ref_point.y = 0.0 pathSeg.ref_point.z = 0.0 init_quat = quaternion_from_euler(0,0,0.0) pathSeg.init_tan_angle.w = init_quat[3] pathSeg.init_tan_angle.x = init_quat[0] pathSeg.init_tan_angle.y = init_quat[1] pathSeg.init_tan_angle.z = init_quat[2] pathSeg.curvature = 1.0 ref_point = PointMsg() ref_point.x = 1.0 ref_point.y = 1.0 pathSeg.ref_point = ref_point maxSpeed = TwistMsg() maxSpeed.linear.x = 1.0 maxSpeed.angular.z = 1.0 pathSeg.max_speeds = maxSpeed minSpeed = TwistMsg() pathSeg.min_speeds = minSpeed pathSeg.accel_limit = 1.0 pathSeg.decel_limit = -1.0 state = State(pathSeg) vel_cmd = TwistMsg() vel_cmd.linear.x = 0.0 vel_cmd.angular.z = 0.5 point = PointMsg() psi = 0.0 maxIter = 1000 count = 1 # extrapolate next point while(state.segDistDone < 1.0 and maxIter > count): # create where the robot should have moved psi = 2.0*state.pathSeg.seg_length*count/maxIter + math.pi/4.0 state.updateState(vel_cmd, point, psi) count += 1 self.assertTrue(count < maxIter) self.assertTrue(state.segDistDone >= 1.0)
def test_updateState_NegOffsetLine(self): ''' Test the robot following a path starting with a negative offset and crossing over the path ''' pathSeg = PathSegmentMsg() pathSeg.seg_type = pathSeg.LINE pathSeg.seg_number = 1 pathSeg.seg_length = math.sqrt(2) * 2 pathSeg.ref_point.x = 0.0 pathSeg.ref_point.y = 0.0 pathSeg.ref_point.z = 0.0 init_quat = quaternion_from_euler(0, 0, math.pi / 4.0) pathSeg.init_tan_angle.w = init_quat[3] pathSeg.init_tan_angle.x = init_quat[0] pathSeg.init_tan_angle.y = init_quat[1] pathSeg.init_tan_angle.z = init_quat[2] pathSeg.curvature = 0.0 maxSpeed = TwistMsg() maxSpeed.linear.x = 1.0 maxSpeed.angular.z = 1.0 pathSeg.max_speeds = maxSpeed minSpeed = TwistMsg() pathSeg.min_speeds = minSpeed pathSeg.accel_limit = 1.0 pathSeg.decel_limit = -1.0 state = State(pathSeg) vel_cmd = TwistMsg() vel_cmd.linear.x = 0.5 vel_cmd.angular.z = 0.0 point = PointMsg() actSegLength = 2.0 * math.sqrt(2) + 1.0 maxIter = 1000 count = 1 # extrapolate next point while (state.segDistDone < 1.0 and maxIter > count): # create where the robot should have moved point.x = actSegLength * (count / (maxIter / 2.0)) * math.cos( 3 * math.pi / 4.0) + 1.0 point.y = actSegLength * (count / (maxIter / 2.0)) * math.sin( 3 * math.pi / 4.0) state.updateState(vel_cmd, point, 0.0) count += 1 self.assertTrue(count < maxIter) self.assertTrue(state.segDistDone >= 1.0)
def test1_1(self): p = PoseStamped() p.header.frame_id = "/odom_combined" p.pose.position = Point(1,0,0) p.pose.orientation = euler_to_quaternion(0,pi,0) p2 = Point(0.7815,0,0) p1 = get_fingertip(p) self.assertTrue(abs(p1.point.x - p2.x) < 0.0001) self.assertTrue(abs(p1.point.y - p2.y) < 0.0001) self.assertTrue(abs(p1.point.z - p2.z) < 0.0001)
def getPoint(self): t = self.getTranslation() p = Point() p.x = t[0] p.y = t[1] p.z = t[2] return p #eof #eoc
def main(): global corner1, corner2, numCells global brush global position global pointList rospy.init_node('brushfire_alpha_main') corner1 = (-6.25, 8.2) corner2 = (15.75, 28.2) numCells = 100 brush = BrushFire(corner1, corner2, numCells, size=15) naptime = rospy.Rate(RATE) print "corner1: " print corner1 print "" print "corner2: " print corner2 print "" print "numCels: " print numCells print "" rospy.Subscriber('goal_point', GoalMsg, goalCallback) rospy.Subscriber('/costmap_alpha/costmap/obstacles', GridCellsMsg, obstaclesCallback) rospy.Subscriber('map_pos', PoseStampedMsg, poseCallback) pathPointPub = rospy.Publisher('point_list', PointListMsg) pointList = PointListMsg() t = Timer(2.0, resetPath) t.start() while not rospy.is_shutdown(): if (position is None or brush is None or brush.goal is None): naptime.sleep() continue brush.extractLocal(position.x, position.y) brush.brushfire() brush.computePath() pointList.points = [] for point in brush.pathList: pathPoint = PointMsg() pathPoint.x = point[0] pathPoint.y = point[1] pointList.points.append(pathPoint) pathPointPub.publish(pointList) pointList.new = False naptime.sleep()
def main(): global corner1, corner2, numCells global brush global position global pointList rospy.init_node('brushfire_alpha_main') corner1 = (-6.25,8.2) corner2 = (15.75,28.2) numCells = 100 brush = BrushFire(corner1,corner2,numCells,size=15) naptime = rospy.Rate(RATE) print "corner1: " print corner1 print "" print "corner2: " print corner2 print "" print "numCels: " print numCells print "" rospy.Subscriber('goal_point',GoalMsg,goalCallback) rospy.Subscriber('/costmap_alpha/costmap/obstacles', GridCellsMsg,obstaclesCallback) rospy.Subscriber('map_pos',PoseStampedMsg, poseCallback) pathPointPub = rospy.Publisher('point_list',PointListMsg) pointList = PointListMsg() t = Timer(2.0, resetPath) t.start() while not rospy.is_shutdown(): if(position is None or brush is None or brush.goal is None): naptime.sleep() continue brush.extractLocal(position.x,position.y) brush.brushfire() brush.computePath() pointList.points = [] for point in brush.pathList: pathPoint = PointMsg() pathPoint.x = point[0] pathPoint.y = point[1] pointList.points.append(pathPoint) pathPointPub.publish(pointList) pointList.new = False naptime.sleep()
def test_updateState_NegOffsetLine(self): ''' Test the robot following a path starting with a negative offset and crossing over the path ''' pathSeg = PathSegmentMsg() pathSeg.seg_type = pathSeg.LINE pathSeg.seg_number = 1 pathSeg.seg_length = math.sqrt(2)*2 pathSeg.ref_point.x = 0.0 pathSeg.ref_point.y = 0.0 pathSeg.ref_point.z = 0.0 init_quat = quaternion_from_euler(0,0,math.pi/4.0) pathSeg.init_tan_angle.w = init_quat[3] pathSeg.init_tan_angle.x = init_quat[0] pathSeg.init_tan_angle.y = init_quat[1] pathSeg.init_tan_angle.z = init_quat[2] pathSeg.curvature = 0.0 maxSpeed = TwistMsg() maxSpeed.linear.x = 1.0 maxSpeed.angular.z = 1.0 pathSeg.max_speeds = maxSpeed minSpeed = TwistMsg() pathSeg.min_speeds = minSpeed pathSeg.accel_limit = 1.0 pathSeg.decel_limit = -1.0 state = State(pathSeg) vel_cmd = TwistMsg() vel_cmd.linear.x = 0.5 vel_cmd.angular.z = 0.0 point = PointMsg() actSegLength = 2.0*math.sqrt(2)+1.0 maxIter = 1000 count = 1 # extrapolate next point while(state.segDistDone < 1.0 and maxIter > count): # create where the robot should have moved point.x = actSegLength*(count/(maxIter/2.0))*math.cos(3*math.pi/4.0) + 1.0 point.y = actSegLength*(count/(maxIter/2.0))*math.sin(3*math.pi/4.0) state.updateState(vel_cmd, point, 0.0) count += 1 self.assertTrue(count < maxIter) self.assertTrue(state.segDistDone >= 1.0)
def remove_puzzle_fixture(self, yaml): rospy.logdebug("remove_puzzle_fixture called!") if yaml.task_type == Task.TASK_5: #fixture_position = deepcopy(yaml.puzzle_fixture.position) #fixture_position = add_point(fixture_position, Point(0.115 if yaml.puzzle_fixture.position.x < 0 else -0.18, 0.165 if yaml.puzzle_fixture.position.y < 0 else 0.05, 0)) fixture_position = get_puzzle_fixture_center(yaml.puzzle_fixture) for x in range(0, len(self.field)): for y in range(0, len(self.field[x])): cell = self.get_cell_by_index(x, y) cell_coor_2d = self.index_to_coordinates(x, y) cell_x, cell_y = cell_coor_2d cell_coor = Point(cell_x, cell_y, 0) fixture_dist = euclidean_distance_in_2d( fixture_position, cell_coor) if fixture_dist < 0.35: rospy.logdebug("cell at (" + str(x) + "," + str(y) + ") marked as fixture cell") cell.set_mark() if cell.is_unknown(): cell.highest_z = 0.1 cell.average_z = 0.1 cell.set_obstacle() if cell.highest_z > 0.1: cell.highest_z = 0.1 if cell.average_z > 0.1: cell.average_z = 0.1 rospy.logdebug("the cell at (" + str(x) + "," + str(y) + "): " + str(cell)) self.publish_as_marker()
def get_message(self): object_id = self.object_name.split("_")[-1] msg = ModelDescription() # generate InstanceId msg.instance_id = InstanceId() msg.instance_id.class_name = self.object_type msg.instance_id.id = object_id # HACK this information should be in some ontology if 'ProductWithAN' in self.object_type: msg.instance_id.ns = '/IAISupermarket/Catalog' elif 'ShelfLabel' in self.object_type: msg.instance_id.ns = '/IAISupermarket/ShelfLabels' else: msg.instance_id.ns = '/IAISupermarket/Shelves' # generate MeshDescription # NOTE: default is to use the class name msg.mesh_description = MeshDescription() msg.mesh_description.path_to_mesh = '' msg.mesh_description.path_to_material = '' # generate Tag's msg.tags.append(self.get_tag('SemLog', 'LogType', 'Static')) msg.tags.append(self.get_tag('SemLog', 'Id', object_id)) msg.tags.append(self.get_tag('SemLog', 'Class', self.object_type)) # set the pose msg.pose = Pose() msg.pose.position = Point() msg.pose.position.x = self.transform[2][0] msg.pose.position.y = self.transform[2][1] msg.pose.position.z = self.transform[2][2] msg.pose.orientation = Quaternion() msg.pose.orientation.x = self.transform[3][0] msg.pose.orientation.y = self.transform[3][1] msg.pose.orientation.z = self.transform[3][2] msg.pose.orientation.w = self.transform[3][3] return msg
def get_yaw(q): """ Calculates the pitch of a quaternion. :param q: Quaternion :type: Quaternion :return: yaw of the quaternion :type: float """ gripper_direction = qv_mult(q, Point(1, 0, 0)) v = deepcopy(gripper_direction) v.z = 0 if 0 <= magnitude(v) <= 0.001: v = Point(1, 0, 0) yaw = get_angle(v, Point(1, 0, 0)) return yaw
def goto_shelf2(self): shelf = PoseStamped() header = Header() header.frame_id = 'map' shelf.header = header shelf.pose.position = Point(-1.0, self.dist_to_shelfs, 0.0) shelf.pose.orientation = Quaternion(0.0, 0.0, 0.0, 1.0) self(shelf)
def relative_pose(self, position, orientation): shelf = PoseStamped() header = Header() header.frame_id = 'base_footprint' shelf.header = header shelf.pose.position = Point(*position) shelf.pose.orientation = Quaternion(*orientation) self(shelf)
def goto_shelf4(self): shelf = PoseStamped() header = Header() header.frame_id = 'map' shelf.header = header shelf.pose.position = Point(-2.9, self.dist_to_shelfs, 0.000) shelf.pose.orientation = Quaternion(0.0, 0.0, 0.0, 1.0) # shelf.pose.orientation = Quaternion(*quaternion_from_euler(0,0,pi*.99)) self(shelf)
def goto_shelf1(self): # self.client.cancel_all_goals() shelf = PoseStamped() header = Header() header.frame_id = 'map' shelf.header = header shelf.pose.position = Point(-0.0, self.dist_to_shelfs, 0.0) shelf.pose.orientation = Quaternion(0.0, 0.0, 0.0, 1.0) self(shelf)
def np_array_to_pose_stampeds(self, nparray, orientation, header): pose_list = [] for p in nparray: pose = PoseStamped() pose.pose.position = Point(*p) pose.pose.orientation = orientation pose.header = header pose_list.append(pose) return pose_list
def main(fullList): global pose global pathList rospy.init_node('pathplanner_alpha_dummyNode') rospy.Subscriber('map_pos', PoseStampedMsg, poseCallback) dummyPointPub = rospy.Publisher('point_list', PointListMsg) pathList.new = True naptime = rospy.Rate(RATE) #point = PointMsg() #point.x = pose.pose.position.x #point.y = pose.pose.position.y #appendToList(point) with open(fullList, 'rb') as csvFile: dialect = csv.Sniffer().sniff(csvFile.read(1024)) # auto detect delimiters csvFile.seek(0) reader = csv.reader(csvFile, dialect) # open up a csv reader object with the csv file for i,row in enumerate(reader): point1 = PointMsg() try: point1.x = float(row[0]) except ValueError: print "x ValueError" try: point1.y = float(row[1]) except ValueError: print "y ValueError" appendToList(point1) while not rospy.is_shutdown(): dummyPointPub.publish(pathList) pathList.new = False naptime.sleep()
def make_scan_pose(point, distance, angle, frame="/odom_combined", n=8): """ Calculates "n" positions around and pointing to "point" with "distance" in an "angle" :param point: Point the positions will be pointing to :type: Point :param distance: distance from the point to tcp origin :type: float :param angle: pitch of the gripper, 0 = horizontally, pi/2 = downwards :type: float :param frame: the frame that the positions will have :type: str :param n: number of positions :type: float :return: list of scan poses :type: [PoseStamped """ #TODO: assumes odom_combined look_positions = [] alpha = pi/2 - angle r = sin(alpha) * distance h = cos(alpha) * distance h_vector = Point() h_vector.z = h muh = add_point(point, h_vector) for i in range(0, n): a = 2 * pi * ((i + 0.0) / (n + 0.0)) b = a - (pi / 2) look_point = Point(cos(a), sin(a), 0) look_point = set_vector_length(r, look_point) look_point = add_point(look_point, muh) roll_point = Point(cos(b), sin(b), 0) roll_point = add_point(roll_point, point) look_pose = PoseStamped() look_pose.header.frame_id = frame look_pose.pose.orientation = three_points_to_quaternion(look_point, point, roll_point) look_pose.pose.position = look_point look_positions.append(look_pose) look_positions.sort(key=lambda x: magnitude(x.pose.position)) return look_positions
def orientation_to_vector(orientation): """ Transforms a 1, 0, 0 vector by a quaternion, the "roll" information is lost. :param orientation: orientation :type: Quaternion :return: vector :type: Point """ return qv_mult(orientation, Point(1, 0, 0))
def multiply_point(s, p): """ p * s :param s: factor :type: float :param p: point :type: Point / (float(x), float(y), float(z)) :return: multiplied point :type: Point """ v = p if type(p) is Point: v = (p.x, p.y, p.z) return Point(*np.multiply(v, s))
def get_fingertip(hand_pose): """ Calculates the point between the finger tips, where objects will be hold. :param hand_pose: hand pose :type: PoseStamped :return: point between fingers :type: PointStamped """ grasp_point = PointStamped() grasp_point.point = set_vector_length(hand_length + finger_length, Point(1, 0, 0)) grasp_point.point = qv_mult(hand_pose.pose.orientation, grasp_point.point) grasp_point.point = add_point(hand_pose.pose.position, grasp_point.point) grasp_point.header.frame_id = hand_pose.header.frame_id return grasp_point
def test2_1(self): co = CollisionObject() co.operation = CollisionObject.ADD co.id = "muh" co.header.frame_id = "/odom_combined" cylinder = SolidPrimitive() cylinder.type = SolidPrimitive.CYLINDER cylinder.dimensions.append(0.3) cylinder.dimensions.append(0.03) co.primitives = [cylinder] co.primitive_poses = [Pose()] co.primitive_poses[0].position = Point(1.2185, 0,0) co.primitive_poses[0].orientation = Quaternion(0,0,0,1) box = SolidPrimitive() box.type = SolidPrimitive.BOX box.dimensions.append(0.1) box.dimensions.append(0.1) box.dimensions.append(0.1) co.primitives.append(box) co.primitive_poses.append(Pose()) co.primitive_poses[1].position = Point(1.1185, 0,0) co.primitive_poses[1].orientation = Quaternion(0,0,0,1) co.primitives.append(box) co.primitive_poses.append(Pose()) co.primitive_poses[2].position = Point(0, 0,0) co.primitive_poses[2].orientation = Quaternion(0,0,0,1) p = PoseStamped() p.header.frame_id = "/odom_combined" p.pose.position = Point(1,0,0) p.pose.orientation = euler_to_quaternion(0,0,0) self.assertEqual(get_grasped_part(co, get_fingertip(p))[1], 0)
def prolog_to_transform(frame_id, child_frame_id, translation, rotation): """ :type frame_id: str :type child_frame_id: str :type translation: list :type rotation: list :rtype: TransformStamped """ if child_frame_id==None or translation==None or rotation==None: return None t = TransformStamped() t.header.frame_id = frame_id.encode('utf-8') t.child_frame_id = child_frame_id.encode('utf-8') t.transform.translation = Point(*translation) t.transform.rotation = Quaternion(*rotation) return t
def normalize(p): """ Normalizes a point. :param p: point :type: Point / (float(x), float(y), float(z)) :return: normalized point :type: Point """ v = p if type(p) is Point: v = (p.x, p.y, p.z) l = magnitude(v) result = Point(*(x * 1 / l for x in v)) return result
def get_marker(self): marker = Marker() marker.header.frame_id = self.ref_frame marker.type = marker.MESH_RESOURCE marker.action = Marker.ADD marker.id = 1337 marker.ns = self.get_short_name() marker.color = self.color marker.scale.x = 1 marker.scale.y = 1 marker.scale.z = 1 marker.frame_locked = True marker.pose.position = Point(*self.transform[-2]) marker.pose.orientation = Quaternion(*self.transform[-1]) marker.mesh_resource = self.mesh_path[:-4] + '.dae' return marker
def get_puzzle_fixture_center(puzzle_fixture): ''' Calculates the center of the puzzle fixture. Assumed fixture dimensions: 32cm x 32cm x 10cm :param puzzle_fixture: geometry_msgs/Pose :return: Point ''' l = sqrt(2 * 0.30 * 0.30) / 2 v1 = orientation_to_vector( rotate_quaternion(deepcopy(puzzle_fixture.orientation), 0, 0, pi / 4)) p1 = puzzle_fixture.position v1norm = normalize(v1) print("v1 = " + str(v1)) print("v1norm = " + str(v1norm)) p2 = Point(v1norm.x * l, v1norm.y * l, v1norm.z * l) print("p2 len = " + str(sqrt(p2.x * p2.x + p2.y * p2.y))) return add_point(p1, p2)
def subtract_point(p1, p2): """ p1 - p2 :param p1: first point :type: Point/ (float(x), float(y), float(z)) :param p2: second point :type: Point/ (float(x), float(y), float(z)) :return: Point :type: Point """ v1 = p1 if type(p1) is Point: v1 = (p1.x, p1.y, p1.z) v2 = p2 if type(p2) is Point: v2 = (p2.x, p2.y, p2.z) return Point(*np.subtract(v1, v2))
def cross_product(p1, p2): """ p1 x p2 :param p1: first point :type: Point / (float(x), float(y), float(z)) :param p2: second point :type: Point / (float(x), float(y), float(z)) :return: cross product :type: Point """ v1 = p1 if type(p1) is Point: v1 = (p1.x, p1.y, p1.z) v2 = p2 if type(p2) is Point: v2 = (p2.x, p2.y, p2.z) return Point(*np.cross(v1, v2))
def calculate_grasp_position_box(collision_object, n=8): ''' Calculates grasp positions for a Box. :param collision_object: box :type: CollisionObject :param n: number of grasps around each side :type: int :return: list of possible graps :type: [PoseStamped] ''' grasp_positions = [] depth = finger_length x_len = collision_object.primitives[0].dimensions[shape_msgs.msg.SolidPrimitive.BOX_X] / 2.0 if finger_length < x_len: depth = x_len depth_x = depth + hand_length y_len = collision_object.primitives[0].dimensions[shape_msgs.msg.SolidPrimitive.BOX_Y] / 2.0 if finger_length < y_len: depth = y_len depth_y = depth + hand_length z_len = collision_object.primitives[0].dimensions[shape_msgs.msg.SolidPrimitive.BOX_Z] / 2.0 if finger_length < z_len: depth = z_len depth_z = depth + hand_length #TODO: abfangen wenn eine Seite zu gross ist #TODO: cooler machen for i in range(0, n): a = 2 * pi * ((i + 0.0) / (n + 0.0)) grasp_point = Point(cos(a), sin(a), 0) d = (abs(grasp_point.x) * depth_x + abs(grasp_point.y) * depth_y + abs(grasp_point.z) * depth_z) / (abs( grasp_point.x) + abs(grasp_point.y) + abs(grasp_point.z)) grasp_positions.append(make_grasp_pose(d, grasp_point, Point(0,0,1), collision_object.id)) grasp_point = Point(cos(a), 0, sin(a)) d = (abs(grasp_point.x) * depth_x + abs(grasp_point.y) * depth_y + abs(grasp_point.z) * depth_z) / (abs( grasp_point.x) + abs(grasp_point.y) + abs(grasp_point.z)) grasp_positions.append(make_grasp_pose(d, grasp_point, Point(0,1,0), collision_object.id)) grasp_point = Point(0, cos(a), sin(a)) d = (abs(grasp_point.x) * depth_x + abs(grasp_point.y) * depth_y + abs(grasp_point.z) * depth_z) / (abs( grasp_point.x) + abs(grasp_point.y) + abs(grasp_point.z)) grasp_positions.append(make_grasp_pose(d, grasp_point, Point(1,0,0), collision_object.id)) return grasp_positions
def test11_1(self): m = self.make_free_map() m.get_cell_by_index(9, 9).set_unknown() m.get_cell_by_index(9, 10).set_unknown() m.get_cell_by_index(9, 11).set_unknown() m.get_cell_by_index(10, 9).set_unknown() m.get_cell_by_index(10, 10).set_unknown() m.get_cell_by_index(10, 11).set_unknown() m.get_cell_by_index(11, 9).set_unknown() m.get_cell_by_index(11, 10).set_unknown() m.get_cell_by_index(11, 11).set_unknown() (x, y) = m.index_to_coordinates(10, 10) poses = make_scan_pose(Point(x, y, 0), 0.05, 0, n=16) poses = m.filter_invalid_scan_poses(x, y, poses) # print len(poses) self.assertTrue(len(poses) == 0, poses)
def getStartAndEndPoints(): if(currSeg.seg_type == PathSegmentMsg.LINE): p_s = PointMsg() p_f = PointMsg() p_s.x = currSeg.ref_point.x p_s.y = currSeg.ref_point.y p_f.x = currSeg.ref_point.x + currSeg.seg_length*cos(getYaw(currSeg.init_tan_angle)) p_f.y = currSeg.ref_point.y + currSeg.seg_length*sin(getYaw(currSeg.init_tan_angle)) return (p_s,p_f) elif(currSeg.seg_type == PathSegmentMsg.ARC): return (0.0,0.0) # TODO elif(currSeg.seg_type == PathSegmentMsg.SPIN_IN_PLACE): return (currSeg.ref_point,currSeg.ref_point) # spins should never move in x,y plane else: return (0.0,0.0) # not sure if this is the best default answer
def main(): global corner1, corner2, numCells global searcher, newPath rospy.init_node('astar_alpha_main') # set the parameters specified within the launch files # corner 1 and corner 2 specify the area in the map # that astar will consider # Corner1 if rospy.has_param('corner1x'): corner1x = rospy.get_param('corner1x') else: corner1x = -6.25 if rospy.has_param('corner1y'): corner1y = rospy.get_param('corner1y') else: corner1y = 8.2 # Corner2 if rospy.has_param('corner2x'): corner2x = rospy.get_param('corner2x') else: corner2x = 15.75 if rospy.has_param('corner2y'): corner2y = rospy.get_param('corner2y') else: corner2y = 28.2 corner1 = (corner1x,corner1y) corner2 = (corner2x,corner2y) # numCells specifies the number of cells to divide the # specified astar search space into if rospy.has_param('numCells'): numCells = rospy.get_param('numCells') else: numCells = 100 # topic that the node looks for the closed points on if rospy.has_param('inflatedTopic'): inflatedTopic = rospy.get_param('inflatedTopic') else: inflatedTopic = '/costmap_local_alpha/costmap_local/inflated_obstacles' # topic that the node looks for goal messages on if rospy.has_param('goalTopic'): goalTopic = rospy.get_param('goalTopic') else: goalTopic = 'goal_point' # initialize an instance of the Astar class searcher = Astar(corner1,corner2,numCells) naptime = rospy.Rate(RATE) print "corner1: " print corner1 print "" print "corner2: " print corner2 print "" print "numCells: %i" % numCells print "" print "goal topics: %s" % goalTopic print "" print "inflatedTopic: %s" % inflatedTopic rospy.Subscriber(goalTopic,GoalMsg,goalCallback) rospy.Subscriber(inflatedTopic,GridCellsMsg,inflatedObstaclesCallback) rospy.Subscriber('map_pos', PoseStampedMsg, poseCallback) pathPointPub = rospy.Publisher('point_list', PointListMsg) first_run = True pointList = PointListMsg() while not rospy.is_shutdown(): pointList.new = newPath print "searcher.start" print searcher.start print "" print "searcher.goal" print searcher.goal print "" print "newPath" print newPath print "" print "searcher.path" print searcher.path print "" pointList.points = [] for point in searcher.path: pathPoint = PointMsg() pathPoint.x = point[0] pathPoint.y = point[1] pointList.points.append(pathPoint) pathPointPub.publish(pointList) if newPath: newPath = False naptime.sleep()
def main(): global RATE global lastVCmd global lastOCmd global obs global obsDist global obsExists global stopped global seg_number global currSeg global nextSeg global pose global ping_angle rospy.init_node('velocity_profiler_alpha') desVelPub = rospy.Publisher('des_vel',TwistMsg) # Steering reads this and adds steering corrections on top of the desired velocities segStatusPub = rospy.Publisher('seg_status', SegStatusMsg) # Lets the other nodes know what path segment the robot is currently executing rospy.Subscriber("motors_enabled", BoolMsg, eStopCallback) # Lets velocity profiler know the E-stop is enabled rospy.Subscriber("obstacles", ObstaclesMsg, obstaclesCallback) # Lets velocity profiler know where along the path there is an obstacle rospy.Subscriber("cmd_vel", TwistMsg, velCmdCallback) # rospy.Subscriber("path_seg", PathSegmentMsg, pathSegmentCallback) rospy.Subscriber("map_pos", PoseStampedMsg, poseCallback) naptime = rospy.Rate(RATE) vel_cmd = TwistMsg() point = PointMsg() # while(not ros.Time.isValid()): #pass print "Entering main loop" while not rospy.is_shutdown(): if(currSeg is not None): # print "seg type %i " % currSeg.seg_type # print "ref_point %s " % currSeg.ref_point # print "curv %f" % currSeg.curvature print "dist done %f" % currState.segDistDone if stopped: stopForEstop(desVelPub,segStatusPub) continue if(currSeg is not None): # Eventually this should work with arcs, spins and lines, but right # now its only working with lines if(currSeg.seg_type == PathSegmentMsg.LINE): # if there is an obstacle and the obstacle is within the segment length print ping_angle if(obsExists and obsDist/currSeg.seg_length < 1.0-currState.segDistDone and ping_angle > 60 and ping_angle < 140): stopForObs(desVelPub,segStatusPub) continue vel_cmd.linear.x = lastVCmd vel_cmd.angular.z = lastOCmd point.x = pose.pose.position.x point.y = pose.pose.position.y point.z = pose.pose.position.z currState.updateState(vel_cmd, point, State.getYaw(pose.pose.orientation)) # update where the robot is at (sVAccel, sVDecel, sWAccel, sWDecel) = computeTrajectory(currSeg,nextSeg) # figure out the switching points in the trajectory #print "sVAccel: %f sVDecel: %f" % (sVAccel,sVDecel) #print "sWAccel: %f, sWDecel: %f" % (sWAccel,sWDecel) des_vel = getVelCmd(sVAccel, sVDecel, sWAccel, sWDecel) # figure out the robot commands given the current state and the desired trajectory desVelPub.publish(des_vel) # publish the commands publishSegStatus(segStatusPub) # let everyone else know the status of the segment # see if its time to switch segments yet if(currState.segDistDone > 1.0): print "Finished segment type %i" % (currState.pathSeg.seg_type) print "currState.segDistDone %f" % (currState.segDistDone) currSeg = None else: # try and get new segments if(nextSeg is not None): currSeg = nextSeg # move the nextSegment up in line nextSeg = None # assume no segment until code below is run else: # didn't have a next segment before try: # so try and get a new one from the queue currSeg = segments.get(False) except QueueEmpty: # if the queue is still empty then currSeg = None # just set it to None try: # see if a next segment is specified nextSeg = segments.get(False) # try and get it except QueueEmpty: # if nothing specified nextSeg = None # set to None point = PointMsg() point.x = pose.pose.position.x point.y = pose.pose.position.y point.z = pose.pose.position.z currState.newPathSegment(currSeg, point, pose.pose.orientation) des_vel = TwistMsg() desVelPub.publish(des_vel) # publish all zeros for the des_vel publishSegStatus(segStatusPub) # publish that there is no segment if(currSeg is not None): rospy.logwarn("Starting a new segment, type %i" %currSeg.seg_type) rospy.logwarn("\tcurvature: %f" % currSeg.curvature) rospy.logwarn("\tref_point: %s" % currSeg.ref_point) naptime.sleep() continue
if(goalList[i] > len(goalList)+1): goalPoint.none = True; goalPoint.point = goalList[i] def main(): global pose global goalList rospy.init_node('goal_planner_alpha') rospy.Subscriber('map_pos', PoseStampedMsg, poseCallback) goalPointPub = rospy.Publisher('goal_point', Goal) naptime = rospy.Rate(RATE) endOfObstacles = PointMsg() endOfObstacles.x = -3.48 endOfObstacles.y = 20.69 goalList.append(endOfObstacles) needsToTurn = PointMsg() needsToTurn.x = -2.37 needsToTurn.y = 21.34 goalList.append(needsToTurn) finalGoalPoint = PointMsg() finalGoalPoint.x = 5.47 finalGoalPoint.y = 12.04 goalList.append(finalGoalPoint)
def updateState(self, vel_cmd, point, psi): """ Integrates along the desired path vector and projects the actual path vector onto the desired vector Inputs ------ vel_cmd is expected to be of type Twist point is expected to be of type Point Returns ------- True if everything went okay False if an error occurred """ dt = 1.0/20.0 if(self.pathSeg.seg_type == PathSegmentMsg.LINE): approx_equal = lambda a,b,t:abs(a-b)<t # grab the angle of the path angle = State.getYaw(self.pathSeg.init_tan_angle) # grab the starting point p0 = self.pathSeg.ref_point # calculate another point along the line p1 = PointMsg() p1.x = p0.x + self.pathSeg.seg_length*cos(angle) p1.y = p0.y + self.pathSeg.seg_length*sin(angle) # line segment tanVecMag = pow(p0.x-p1.x,2) + pow(p0.y-p1.y,2) # intersection point u = ((point.x - p0.x)*(p1.x-p0.x) + (point.y-p0.y)*(p1.y-p0.y))/tanVecMag intersect = PointMsg() intersect.x = point.x + u*cos(angle) intersect.y = point.y + u*sin(angle) d = sqrt(pow(intersect.x-p0.x,2)+pow(intersect.y-p0.y,2)) # see where this point will lead if followed along the line for a distance d testX = intersect.x + d*cos(angle) testY = intersect.y + d*sin(angle) # if the distance gets smaller then the d should be negative # if the distance gets bigger then the d should be positive if(sqrt(pow(testX-p0.x,2)+pow(testY-p0.y,2)) < d): d = -d self.segDistDone = d/self.pathSeg.seg_length elif(self.pathSeg.seg_type == PathSegmentMsg.ARC): self.segDistDone += abs((vel_cmd.angular.z*dt)/(self.pathSeg.seg_length/abs(self.pathSeg.curvature))) """ tanAngStart = State.getYaw(self.pathSeg.init_tan_angle) rho = self.pathSeg.curvature r = 1/abs(rho) if(rho >= 0): startAngle = tanAngStart - pi/2 else: startAngle = tanAngStart + pi/2 ref_point = self.pathSeg.ref_point rVec = State.createVector(ref_point, point) theta = atan2(rVec.y,rVec.x) phi = startAngle % (2*pi) if(abs(phi-2*pi) < phi): phi = phi - 2*pi posPhi = phi % (2*pi) posTheta = theta % (2*pi) # figure out angle halfway between end and start angle if(rho >= 0): halfAngle = self.pathSeg.seg_length/(2*r) finAngle = self.pathSeg.seg_length/r else: halfAngle = self.pathSeg.seg_length/(2*r) finAngle = -self.pathSeg.seg_length/r # find theta in terms of starting angle if(rho >= 0): if(posTheta > posPhi): beta = posTheta - posPhi else: beta = 2*pi-posPhi+posTheta else: if(posTheta < posPhi): beta = posTheta - posPhi else: beta = posTheta-posPhi-(2*pi) # figure out what region the angle is in if(rho >= 0): if(beta >= 0 and beta <= halfAngle+pi): # beta is in the specified arc alpha = beta else: alpha = -(2*pi-beta) else: if(beta >= halfAngle-pi and beta <= 0): alpha = beta; else: alpha = beta + (2*pi) if(rho >= 0): self.segDistDone = r*alpha/self.pathSeg.seg_length else: self.segDistDone = -r*alpha/self.pathSeg.seg_length """ elif(self.pathSeg.seg_type == PathSegmentMsg.SPIN_IN_PLACE): rho = self.pathSeg.curvature phi = State.getYaw(self.pathSeg.init_tan_angle) posPhi = phi % (2*pi) posPsi = psi % (2*pi) if(rho >=0): halfAngle = self.pathSeg.seg_length/2.0 else: halfAngle = self.pathSeg.seg_length/2.0-pi # find beta in terms of starting angle if(posPsi > posPhi): beta = posPsi - posPhi else: beta = 2*pi-posPhi+posPsi # figure out what region the angle is in if(beta >= 0 and beta <= halfAngle+pi): # beta is in the specified arc alpha = beta else: alpha = beta - 2*pi self.segDistDone = alpha/self.pathSeg.seg_length else: pass # should probably throw an unknown segment type error # update the current velocity self.v = vel_cmd.linear.x self.w = vel_cmd.angular.z # update the current point and heading self.point= point self.psi = psi