def make_msg(self, Depth1Image, Depth2Image, detection_data, camera1_param, camera2_param): bboxes_from_camera1 = BoundingBoxes() bboxes_from_camera2 = BoundingBoxes() bboxes_from_camera1.header = Depth1Image.header bboxes_from_camera2.header = Depth2Image.header self.now = rospy.get_rostime() self.timebefore = detection_data.header.stamp # Add point obstacle self.obstacle_msg.header.stamp = detection_data.header.stamp self.obstacle_msg.header.frame_id = "odom" # CHANGE HERE: odom/map #opencvに変換 bridge = CvBridge() try: Depth1image = bridge.imgmsg_to_cv2(Depth1Image, desired_encoding="passthrough") Depth2image = bridge.imgmsg_to_cv2(Depth2Image, desired_encoding="passthrough") except CvBridgeError as e: print(e) for bbox in detection_data.bounding_boxes: if (bbox.ymin + bbox.ymax)/2 < Depth1image.shape[0]: if bbox.ymax > Depth1image.shape[0]: bbox.ymax = Depth1image.shape[0] bboxes_from_camera1.bounding_boxes.append(bbox) else: bbox.ymin = bbox.ymin - Depth1image.shape[0] bbox.ymax = bbox.ymax - Depth1image.shape[0] if bbox.ymin < 0: bbox.ymin = 0 bboxes_from_camera2.bounding_boxes.append(bbox) camera1_obstacle_msg, camera2_obstacle_msg = ObstacleArrayMsg(), ObstacleArrayMsg() camera1_marker_data, camera2_marker_data = MarkerArray(), MarkerArray() camera1_obstacle_msg, camera1_marker_data = self.bbox_to_position_in_odom(bboxes_from_camera1, Depth1image, camera1_param) obstacle_msg, marker_data = self.bbox_to_position_in_odom(bboxes_from_camera2, Depth2image, camera2_param, len(camera1_obstacle_msg.obstacles), camera1_obstacle_msg, camera1_marker_data) self.obstacle_msg.obstacles, self.marker_data.markers = self.update_obstacles(self.obstacle_msg, obstacle_msg, self.marker_data, marker_data)
def pub_timer_callback(self, event): obstacle_msg = ObstacleArrayMsg() obstacle_msg.header.stamp = rospy.Time.now() obstacle_msg.header.frame_id = "map" obstacle_msg.obstacles = self.heaps2cubes() obstacle_msg.obstacles.append(self.field) self.pub_obstacles.publish(obstacle_msg)
def publish_obstacle_msg(self): if self.args.no_ros: return else: from tf.transformations import quaternion_from_euler from geometry_msgs.msg import Point32, Quaternion from costmap_converter.msg import ObstacleArrayMsg, ObstacleMsg obstacles_msg = ObstacleArrayMsg() obstacles_msg.header.stamp = self.get_sim_time() obstacles_msg.header.frame_id = kFixedFrame for i in range(1, self.n_sim_agents): # Add point obstacle obst = ObstacleMsg() obst.id = i obst.polygon.points = [Point32()] obst.polygon.points[0].x = self.rlenv.virtual_peppers[i].pos[0] obst.polygon.points[0].y = self.rlenv.virtual_peppers[i].pos[1] obst.polygon.points[0].z = 0 obst.radius = self.rlenv.vp_radii[i] yaw = self.rlenv.virtual_peppers[i].pos[2] q = quaternion_from_euler(0,0,yaw) obst.orientation = Quaternion(*q) obst.velocities.twist.linear.x = self.rlenv.virtual_peppers[i].vel[0] obst.velocities.twist.linear.y = self.rlenv.virtual_peppers[i].vel[1] obst.velocities.twist.linear.z = 0 obst.velocities.twist.angular.x = 0 obst.velocities.twist.angular.y = 0 obst.velocities.twist.angular.z = self.rlenv.virtual_peppers[i].vel[2] obstacles_msg.obstacles.append(obst) self.obstpub.publish(obstacles_msg) return
def publish_obstacle_msg(self, _pose_x, _pose_y): obstacle_msg = ObstacleArrayMsg() obstacle_msg.header.stamp = rospy.Time.now() obstacle_msg.header.frame_id = "map" # CHANGE HERE: odom/map block_dis = 0.025 # Add polygon obstacle obstacle_msg.obstacles.append(ObstacleMsg()) obstacle_msg.obstacles[0].id = 99 # 座標系がx, y逆な気する この順番で四角:ロボットの初期正面方向がx軸, 右側がy軸マイナス方向 0.3mは結構でかいロボット全部埋まるくらい v1 = Point32() v1.x = block_dis + _pose_x v1.y = block_dis + _pose_y v2 = Point32() v2.x = block_dis + _pose_x v2.y = -1 * block_dis + _pose_y v3 = Point32() v3.x = -1 * block_dis + _pose_x v3.y = -1 * block_dis + _pose_y v4 = Point32() v4.x = -1 * block_dis + _pose_x v4.y = block_dis + _pose_y obstacle_msg.obstacles[0].polygon.points = [v1, v2, v3, v4] return obstacle_msg
def to_obsarray(): array = ObstacleArrayMsg() array.header.frame_id = 'field' oid = [0] def next_id(): oid[0] += 1 return oid[0] - 1 red_line = [to_point((0, 4.1)), to_point((1.9, 6))] blue_line = [to_point((4.1, 0)), to_point((6, 1.9))] for l in (blue_line, red_line): obs = ObstacleMsg(polygon=Polygon(l)) obs.id = next_id() array.obstacles.append(obs) for stationary in ((2, 4), (4, 2)): obs = ObstacleMsg(polygon=to_square(stationary, 0.127)) obs.id = next_id() array.obstacles.append(obs) for color, pos in world.goals: obs = ObstacleMsg(polygon=to_square(pos, 0.127)) obs.id = next_id() array.obstacles.append(obs) for pos in world.cones: obs = ObstacleMsg(polygon=to_square(pos, 0.076)) obs.id = next_id() array.obstacles.append(obs) return array
def publish_obstacle_msg(): pub = rospy.Publisher('/move_base/TebLocalPlannerROS/obstacles', ObstacleArrayMsg, queue_size=1) #pub = rospy.Publisher('/p3dx/move_base/TebLocalPlannerROS/obstacles', ObstacleArrayMsg, queue_size=1) rospy.init_node("test_obstacle_msg") print('Prepare message now') obstacle_msg = ObstacleArrayMsg() obstacle_msg.header.stamp = rospy.Time.now() obstacle_msg.header.frame_id = "map" # Add point obstacle obstacle_msg.obstacles.append(ObstacleMsg()) obstacle_msg.obstacles[0].id = 0 obstacle_msg.obstacles[0].polygon.points = [Point32()] obstacle_msg.obstacles[0].polygon.points[0].x = -1 obstacle_msg.obstacles[0].polygon.points[0].y = -1 obstacle_msg.obstacles[0].polygon.points[0].z = 0 print('Prepared, publish') # Add line obstacle obstacle_msg.obstacles.append(ObstacleMsg()) obstacle_msg.obstacles[1].id = 1 line_start = Point32() line_start.x = 2 line_start.y = 0 line_end = Point32() line_end.x = 4 line_end.y = 0 obstacle_msg.obstacles[1].polygon.points = [line_start, line_end] # Add polygon obstacle # obstacle_msg.obstacles.append(ObstacleMsg()) # obstacle_msg.obstacles[1].id = 2 # v1 = Point32() # v1.x = -1 # v1.y = -1 # v2 = Point32() # v2.x = -0.5 # v2.y = -1.5 # v3 = Point32() # v3.x = 0 # v3.y = -1 # obstacle_msg.obstacles[2].polygon.points = [v1, v2, v3] r = rospy.Rate(10) # 10hz # t = 0.0 while not rospy.is_shutdown(): # Vary y component of the point obstacle # obstacle_msg.obstacles[0].polygon.points[0].y = 1*math.sin(t) # t = t + 0.1 # print('Prepared, publish') pub.publish(obstacle_msg) r.sleep()
def update_obstacles(self, prev_obstacle_msg, detected_obstacle_msg, prev_marker_msg, marker_msg): self.tf_listener.waitForTransform("/odom", "/base_link", rospy.Time(0), rospy.Duration(0.3)) current_position = self.tf_listener.lookupTransform( "/odom", "/base_link", rospy.Time(0)) updated_obstacle_msg = ObstacleArrayMsg() updated_marker_data = MarkerArray() print(len(detected_obstacle_msg.obstacles)) for detected_obstacle, marker in zip(detected_obstacle_msg.obstacles, marker_msg.markers): """ for prev_obstacle, prev_marker in zip(prev_obstacle_msg.obstacles, prev_marker_msg.markers): if abs(current_position[0][0] - prev_obstacle.polygon.points[0].x) < 4 or abs(current_position[0][1] - prev_obstacle.polygon.points[0].y) < 4: if ((detected_obstacle.polygon.points[0].x - prev_obstacle.polygon.points[0].x) * (detected_obstacle.polygon.points[0].x - prev_obstacle.polygon.points[0].x) + (detected_obstacle.polygon.points[0].y - prev_obstacle.polygon.points[0].y) * (detected_obstacle.polygon.points[0].y - prev_obstacle.polygon.points[0].y)) < 1.0: prev_obstacle.polygon.points[0].x = detected_obstacle.polygon.points[0].x prev_obstacle.polygon.points[0].y = detected_obstacle.polygon.points[0].y prev_marker.pose.position.x = marker.pose.position.x prev_marker.pose.position.y = marker.pose.position.y updated_obstacle_msg.obstacles.append(detected_obstacle) updated_marker_data.markers.append(marker) """ updated_obstacle_msg.obstacles.append(detected_obstacle) updated_marker_data.markers.append(marker) return updated_obstacle_msg.obstacles, updated_marker_data.markers
def publish_obstacle_msg(): pub = rospy.Publisher('/test_optim_node/obstacles', ObstacleArrayMsg, queue_size=1) #pub = rospy.Publisher('/p3dx/move_base/TebLocalPlannerROS/obstacles', ObstacleArrayMsg, queue_size=1) rospy.init_node("test_obstacle_msg") obstacle_msg = ObstacleArrayMsg() obstacle_msg.header.stamp = rospy.Time.now() obstacle_msg.header.frame_id = "odom" # CHANGE HERE: odom/map # Add point obstacle obstacle_msg.obstacles.append(ObstacleMsg()) obstacle_msg.obstacles[0].id = 0 obstacle_msg.obstacles[0].polygon.points = [Point32()] obstacle_msg.obstacles[0].polygon.points[0].x = 1.5 obstacle_msg.obstacles[0].polygon.points[0].y = 0 obstacle_msg.obstacles[0].polygon.points[0].z = 0 # Add line obstacle obstacle_msg.obstacles.append(ObstacleMsg()) obstacle_msg.obstacles[1].id = 1 line_start = Point32() line_start.x = -2.5 line_start.y = 0.5 # line_start.y = -3 line_end = Point32() line_end.x = -2.5 line_end.y = 2 #line_end.y = -4 obstacle_msg.obstacles[1].polygon.points = [line_start, line_end] # Add polygon obstacle obstacle_msg.obstacles.append(ObstacleMsg()) obstacle_msg.obstacles[1].id = 2 v1 = Point32() v1.x = -1-2 v1.y = -1+1 v2 = Point32() v2.x = -0.5-2 v2.y = -1.5+1 v3 = Point32() v3.x = 0-2 v3.y = -1+1 obstacle_msg.obstacles[2].polygon.points = [v1, v2, v3] r = rospy.Rate(10) # 10hz t = 0.0 while not rospy.is_shutdown(): # Vary y component of the point obstacle obstacle_msg.obstacles[0].polygon.points[0].y = 1*math.sin(t) t = t + 0.1 pub.publish(obstacle_msg) r.sleep()
def __init__(self): self.map_pose = Pose() self.map_width = 600 self.map_height = 600 self.map = OccupancyGrid() self.map.header.frame_id = 'map' self.map.info.resolution = 1.0 self.map.info.width = self.map_width self.map.info.height = self.map_height self.map.data = range(self.map_width * self.map_height) grid = np.ndarray((self.map_width, self.map_height), buffer=np.zeros((self.map_width, self.map_height), dtype=np.int8), dtype=np.int8) grid.fill(np.int8(0)) grid[3:7, 3:7].fill(np.int8(100)) for i in range(self.map_width * self.map_height): self.map.data[i] = grid.flat[i] self.global_costmap = OccupancyGrid() self.global_costmap.info.width = 10 self.global_costmap.info.height = 10 self.global_costmap.info.resolution = 100 self.global_costmap.data = range(100) grid = np.ndarray((10, 10), buffer=np.zeros((10, 10), dtype=np.int8), dtype=np.int8) grid.fill(np.int8(0)) for i in range(100): self.map.data[i] = grid.flat[i] self.global_costmap.info.origin.position.x = -500 self.global_costmap.info.origin.position.y = -500 self.global_costmap.header.frame_id = 'map' self.obstacle_array = ObstacleArrayMsg() obstacle = ObstacleMsg() obstacle.polygon.points = [ Point32(3.0, 3.0, 0.0), Point32(3.0, 6.0, 0.0), Point32(6.0, 6.0, 0.0), Point32(6.0, 3.0, 0.0) ] obstacle.radius = 2.5 self.obstacle_array.obstacles.append(obstacle) self.odometry_sub = rospy.Subscriber("base_pose_ground_truth", Odometry, self.odom_callback) self.map_pub = rospy.Publisher('move_base/local_costmap/costmap', OccupancyGrid, queue_size=5) self.global_costmap_pub = rospy.Publisher( 'move_base/global_costmap/costmap', OccupancyGrid, queue_size=5) self.obstacle_pub = rospy.Publisher( 'move_base/TebLocalPlannerROS/obstacles', ObstacleArrayMsg, queue_size=5)
def __init__(self): # Publisherを作成 self.publisher = rospy.Publisher('/move_base/TebLocalPlannerROS/obstacles', ObstacleArrayMsg, queue_size=1) self.marker_publisher = rospy.Publisher("/visualized_obstacle", MarkerArray, queue_size = 1) self.obstacle_list = ["person"] self.tf_br = tf.TransformBroadcaster() self.tf_listener = tf.TransformListener() self.obstacle_msg = ObstacleArrayMsg() self.marker_data = MarkerArray() self.now = rospy.get_rostime() self.prev = rospy.get_rostime()
def publish_obstacle_msg(): pub = rospy.Publisher('/test_optim_node/obstacles', ObstacleArrayMsg, queue_size=1) #pub = rospy.Publisher('/p3dx/move_base/TebLocalPlannerROS/obstacles', ObstacleArrayMsg, queue_size=1) rospy.init_node("test_obstacle_msg") y_0 = -3.0 vel_x = 0.0 vel_y = 0.3 range_y = 6.0 obstacle_msg = ObstacleArrayMsg() obstacle_msg.header.stamp = rospy.Time.now() obstacle_msg.header.frame_id = "map" # CHANGE HERE: odom/map # Add point obstacle obstacle_msg.obstacles.append(ObstacleMsg()) obstacle_msg.obstacles[0].id = 99 obstacle_msg.obstacles[0].polygon.points = [Point32()] obstacle_msg.obstacles[0].polygon.points[0].x = -1.5 obstacle_msg.obstacles[0].polygon.points[0].y = 0 obstacle_msg.obstacles[0].polygon.points[0].z = 0 yaw = math.atan2(vel_y, vel_x) q = tf.transformations.quaternion_from_euler(0, 0, yaw) obstacle_msg.obstacles[0].orientation = Quaternion(*q) obstacle_msg.obstacles[0].velocities.twist.linear.x = vel_x obstacle_msg.obstacles[0].velocities.twist.linear.y = vel_y obstacle_msg.obstacles[0].velocities.twist.linear.z = 0 obstacle_msg.obstacles[0].velocities.twist.angular.x = 0 obstacle_msg.obstacles[0].velocities.twist.angular.y = 0 obstacle_msg.obstacles[0].velocities.twist.angular.z = 0 r = rospy.Rate(10) # 10hz t = 0.0 while not rospy.is_shutdown(): # Vary y component of the point obstacle if (vel_y >= 0): obstacle_msg.obstacles[0].polygon.points[0].y = y_0 + (vel_y * t) % range_y else: obstacle_msg.obstacles[0].polygon.points[0].y = y_0 + ( vel_y * t) % range_y - range_y t = t + 0.1 pub.publish(obstacle_msg) r.sleep()
def publish_obstacle_msg(): #pub = rospy.Publisher('/test_optim_node/obstacles', ObstacleArrayMsg, queue_size=1) pub = rospy.Publisher('/move_base/TebLocalPlannerROS/obstacles', ObstacleArrayMsg, queue_size=1) rospy.init_node("test_obstacle_msg") obstacle_msg = ObstacleArrayMsg() obstacle_msg.header.stamp = rospy.Time.now() obstacle_msg.header.frame_id = "map" # CHANGE HERE: odom/map # Add point obstacle obstacle_msg.obstacles.append(ObstacleMsg()) obstacle_msg.obstacles[0].id = 0 obstacle_msg.obstacles[0].polygon.points = [Point32()] obstacle_msg.obstacles[0].polygon.points[0].x = 5 obstacle_msg.obstacles[0].polygon.points[0].y = 2 obstacle_msg.obstacles[0].polygon.points[0].z = 0 obstacle_msg.obstacles.append(ObstacleMsg()) obstacle_msg.obstacles[1].id = 1 obstacle_msg.obstacles[1].polygon.points = [Point32()] obstacle_msg.obstacles[1].polygon.points[0].x = 5 obstacle_msg.obstacles[1].polygon.points[0].y = 4 obstacle_msg.obstacles[1].polygon.points[0].z = 0 obstacle_msg.obstacles.append(ObstacleMsg()) obstacle_msg.obstacles[2].id = 2 obstacle_msg.obstacles[2].polygon.points = [Point32()] obstacle_msg.obstacles[2].polygon.points[0].x = 5 obstacle_msg.obstacles[2].polygon.points[0].y = 6 obstacle_msg.obstacles[2].polygon.points[0].z = 0 obstacle_msg.obstacles.append(ObstacleMsg()) obstacle_msg.obstacles[3].id = 3 obstacle_msg.obstacles[3].polygon.points = [Point32()] obstacle_msg.obstacles[3].polygon.points[0].x = 5 obstacle_msg.obstacles[3].polygon.points[0].y = 8 obstacle_msg.obstacles[3].polygon.points[0].z = 0 r = rospy.Rate(10) # 10hz t = 0.0 while not rospy.is_shutdown(): # Vary y component of the point obstacle pub.publish(obstacle_msg) r.sleep()
def __init__(self): # Publisherを作成 self.publisher = rospy.Publisher('/move_base/TebLocalPlannerROS/obstacles', ObstacleArrayMsg, queue_size=1) self.marker_publisher = rospy.Publisher("/visualized_obstacle", MarkerArray, queue_size = 1) self.landmark_publisher = rospy.Publisher("/rtabmap/tag_detections", AprilTagDetectionArray, queue_size = 1) self.landmark_publisher = rospy.Publisher("/rtabmap/gps/fix", NavSatFix, queue_size = 1) self.obstacle_list = ["person", "tree", "landmark"] self.landmark_list = ["landmark"] self.tf_br = tf.TransformBroadcaster() self.tf_listener = tf.TransformListener() self.obstacle_msg = ObstacleArrayMsg() self.marker_data = MarkerArray() self.marker_data = AprilTagDetectionArray() self.now = rospy.get_rostime() self.prev = rospy.get_rostime()
def __init__(self): rospy.init_node("test_obstacle_msg") self.obstacle_pub = rospy.Publisher( '/move_base/TebLocalPlannerROS/obstacles', ObstacleArrayMsg, queue_size=1) self.marker_pub = rospy.Publisher('dynamic_obstacle_markers', MarkerArray, queue_size=1) self.obstacle_msg = ObstacleArrayMsg() self.obs_array_markers = MarkerArray() self.range_x = [] self.range_y = [] self.pose_x = [] self.pose_y = []
def publish_obstacle_msg_moving(pose): points = unit_test2() position = pose.position print('postiions: ', position) rospy.init_node("test_obstacle_msg") pub = rospy.Publisher('/test_optim_node/obstacles', ObstacleArrayMsg, queue_size=1) obstacle_msg = ObstacleArrayMsg() obstacle_msg.header.stamp = rospy.Time.now() obstacle_msg.header.frame_id = "odom" # CHANGE HERE: odom/map lines = [[1, 2, 3], [4, 5, 6]] scale_x = 250 scale_y = 190 shift_x = 4.5 shift_y = 1 xcoords = points[0]/scale_x ycoords = points[1]/scale_y j=0 for idx, xcoord in enumerate(xcoords): if(idx%50 == 0): obstacle_msg.obstacles.append(ObstacleMsg()) print(type(xcoord)) x = xcoord - position.x y = ycoords[idx] - position.y # rotate coordinates obstacle_msg.obstacles[j].polygon.points = [Point()] obstacle_msg.obstacles[j].polygon.points[0].x = y obstacle_msg.obstacles[j].polygon.points[0].y = -x obstacle_msg.obstacles[j].polygon.points[0].z = 0 j = j+1 r = rospy.Rate(10) # 10hz t = 0.0 timeout = time.time() + 10 # 2 seconds from now while True and not rospy.is_shutdown(): test = 0 if test == 5 or time.time() > timeout: print('timer about to break') break pub.publish(obstacle_msg) r.sleep() test - test - 1
def makebox(ax, ay, bx, by, cx, cy, dx, dy): pub = rospy.Publisher('/test_optim_node/obstacles', ObstacleArrayMsg, queue_size=1) # /park_planner/lines #pub = rospy.Publisher('/p3dx/move_base/TebLocalPlannerROS/obstacles', ObstacleArrayMsg, queue_size=1) obstacle_msg = ObstacleArrayMsg() obstacle_msg.header.stamp = rospy.Time.now() obstacle_msg.header.frame_id = "odom" # CHANGE HERE: odom/mapobstacle_msg.obstacles.append(ObstacleMsg()) obstacle_msg.obstacles.append(ObstacleMsg()) obstacle_msg.obstacles.append(ObstacleMsg()) obstacle_msg.obstacles.append(ObstacleMsg()) obstacle_msg.obstacles[0].id = 1 obstacle_msg.obstacles[1].id = 2 obstacle_msg.obstacles[2].id = 3 A = Point32() A.x = ax A.y = ay B = Point32() B.x = bx B.y = by C = Point32() C.x = cx C.y = cy D = Point32() D.x = dx D.y = dy obstacle_msg.obstacles[0].polygon.points = [A, B] obstacle_msg.obstacles[1].polygon.points = [B, C] obstacle_msg.obstacles[2].polygon.points = [C, D] #obstacle_msg.obstacles[0].polygon.points = [v1, v2, v3, v4] r = rospy.Rate(10) # 10hz t = 0.0 while not rospy.is_shutdown(): """# Vary y component of the point obstacle obstacle_msg.obstacles[0].polygon.points[0].y = 1*math.sin(t) t = t + 0.1""" pub.publish(obstacle_msg) r.sleep()
def people_tracker_callback(self, msg): obstacle_msg = ObstacleArrayMsg() #obstacle_msg.header = msg.header obstacle_msg.header.stamp = rospy.Time.now() obstacle_msg.header.frame_id = "world" i = 0 for j in msg.uuids: obstacle_msg.obstacles.append(ObstacleMsg()) obstacle_msg.obstacles[i].id = np.int64(msg.uuids[i]) obstacle_msg.obstacles[i].polygon.points = [Point32()] obstacle_msg.obstacles[i].polygon.points[0].x = msg.poses[ i].position.x obstacle_msg.obstacles[i].polygon.points[0].y = msg.poses[ i].position.y obstacle_msg.obstacles[i].polygon.points[0].z = msg.poses[ i].position.z obstacle_msg.obstacles[i].orientation.x = msg.poses[ i].orientation.x obstacle_msg.obstacles[i].orientation.y = msg.poses[ i].orientation.y obstacle_msg.obstacles[i].orientation.z = msg.poses[ i].orientation.z obstacle_msg.obstacles[i].orientation.w = msg.poses[ i].orientation.w obstacle_msg.obstacles[ i].velocities.twist.linear.x = msg.velocities[i].x obstacle_msg.obstacles[ i].velocities.twist.linear.y = msg.velocities[i].y obstacle_msg.obstacles[ i].velocities.twist.linear.z = msg.velocities[i].z obstacle_msg.obstacles[i].velocities.twist.angular.x = 0 obstacle_msg.obstacles[i].velocities.twist.angular.y = 0 obstacle_msg.obstacles[i].velocities.twist.angular.z = 0 i = i + 1 print("I am publishing Dynamic Obstacles message obstacle_msg.") self.pub.publish(obstacle_msg)
def to_obsarray_bulked(): array = ObstacleArrayMsg() array.header.frame_id = 'field' oid = [0] def next_id(): oid[0] += 1 return oid[0] - 1 lines = [ # small cone corner ((0, 0), (0, 2.125)), ((0, 0), (2.125, 0)), ((1, 0), (1, 2.125)), ((0, 1), (2.125, 1)), # long cone corner ((6, 6), (6, 6 - 3.125)), ((6, 6), (6 - 3.125, 6)), ((5, 6), (5, 6 - 3.125)), ((6, 5), (6 - 3.125, 5)), ( # center cones (2, 2), (2, 2.5), (3.5, 4), (4, 4), (4, 3.5), (2.5, 2)), # goal line ((0, 4.1), (1.9, 6)), ((4.1, 0), (6, 1.9)) ] for l in lines: obs = ObstacleMsg(polygon=Polygon([to_point(p) for p in l])) obs.id = next_id() array.obstacles.append(obs) for stationary in ((2, 4), (4, 2)): obs = ObstacleMsg(polygon=to_square(stationary, 0.127)) obs.id = next_id() array.obstacles.append(obs) return array
def publish_obstacle_msg(self): print("Testing_obstacle_msg") #obstacle_list = [ObstacleMsg()] obstacle_arr_msg = ObstacleArrayMsg() obstacle_arr_msg.header.stamp = rospy.Time.now() obstacle_arr_msg.header.frame_id = "vicon_world" # CHANGE HERE: odom/map # Add point obstacle obstacle_msg = ObstacleMsg() obstacle_msg.header.stamp = rospy.Time.now() obstacle_msg.header.frame_id = "vicon_world" # CHANGE HERE: odom/map obstacle_msg.radius = 0.5 obstacle_msg.id = 2 obstacle_msg.polygon.points = [Point32()] obstacle_msg.polygon.points[0].x = self.rob_1_pose_trans[0] obstacle_msg.polygon.points[0].y = self.rob_1_pose_trans[1] obstacle_msg.polygon.points[0].z = 0 obstacle_msg.orientation.x = self.rob_1_pose_rot[0] obstacle_msg.orientation.y = self.rob_1_pose_rot[1] obstacle_msg.orientation.z = self.rob_1_pose_rot[2] obstacle_msg.orientation.w = self.rob_1_pose_rot[3] obstacle_msg.velocities.twist.linear.x = self.vel_x obstacle_msg.velocities.twist.linear.y = self.vel_y obstacle_msg.velocities.twist.linear.z = 0 obstacle_msg.velocities.twist.angular.x = 0 obstacle_msg.velocities.twist.angular.y = 0 obstacle_msg.velocities.twist.angular.z = 0 obstacle_arr_msg.obstacles.append(obstacle_msg) #print(obstacle_arr_msg) r = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): self.rob_2_pub.publish(obstacle_arr_msg) r.sleep()
def run(): global h2_odom, h3_odom, tf_buffer, tf_listener_1 rospy.init_node('local_planning_test_script') tf_buffer = tf2_ros.Buffer(rospy.Duration(1200.0)) # tf buffer length tf_listener = tf2_ros.TransformListener(tf_buffer) tf_listener_1 = tf.TransformListener() pub_h2 = rospy.Publisher("/h2/cmd_vel", Twist, queue_size=1) pub_h3 = rospy.Publisher("/h3/cmd_vel", Twist, queue_size=1) pub_h1_goal = rospy.Publisher("/move_base_simple/goal", PoseStamped, queue_size=1) sub_h1_pose = rospy.Subscriber("/odometry/filtered", Odometry, h1_odom_cb) sub_h2_pose = rospy.Subscriber("/h2/odometry/filtered", Odometry, h2_odom_cb) sub_h3_pose = rospy.Subscriber("/h3/odometry/filtered", Odometry, h3_odom_cb) pub_h1_obstacless = rospy.Publisher( "/move_base/TebLocalPlannerROS/obstacles", ObstacleArrayMsg, queue_size=1) rate = rospy.Rate(20) count = 0 while not rospy.is_shutdown(): # try: # (trans, rot) = tf_listener.lookupTransform('/map', '/h2/odom', rospy.Time(0)) # except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): # continue # print(type(trans)) # print(type(rot)) # print(h2_odom) # print(h3_odom) if h2_odom is not None and h3_odom is not None: h2_obstacles = ObstacleMsg() h2_obstacles.header = h2_odom.header point = Point32() point.x = h2_odom.pose.pose.position.x point.y = h2_odom.pose.pose.position.y point.z = h2_odom.pose.pose.position.z h2_obstacles.polygon.points.append(point) h2_obstacles.radius = 0.5 h2_obstacles.velocities = h2_odom.twist #print("h2 velocity:{} {}".format(h2_odom.twist.twist.linear.x,h2_odom.twist.twist.linear.y)) h2_obstacles.orientation = h2_odom.pose.pose.orientation h3_obstacles = ObstacleMsg() point1 = Point32() point1.x = h3_odom.pose.pose.position.x point1.y = h3_odom.pose.pose.position.y point1.z = h3_odom.pose.pose.position.z h3_obstacles.header = h3_odom.header h3_obstacles.polygon.points.append(point1) h3_obstacles.radius = 0.5 h3_obstacles.velocities = h3_odom.twist h3_obstacles.orientation = h3_odom.pose.pose.orientation msg = ObstacleArrayMsg() msg.header = h2_obstacles.header msg.obstacles.append(h2_obstacles) msg.obstacles.append(h3_obstacles) pub_h1_obstacless.publish(msg) if count < 20: #print("begin pub") msg = PoseStamped() msg.header.seq = count msg.header.frame_id = "map" msg.pose.position.x = -6 msg.pose.position.y = 0 msg.pose.orientation.x = 0.0 msg.pose.orientation.y = 0.0 msg.pose.orientation.z = 0.0 msg.pose.orientation.w = 1.0 pub_h1_goal.publish(msg) # if 40 < count < 190: this seting will hit the robot if 20 < count < 380: msg_h2 = Twist() msg_h2.linear.x = 1.0 msg_h2.angular.z = 0.0 pub_h2.publish(msg_h2) if 20 < count < 1800: msg_h3 = Twist() msg_h3.linear.x = 0.30 #use teb set it 0.30,use steb set it 0.45 msg_h3.angular.z = 0.0 #pub_h3.publish(msg_h3) if count >= 2800: break count += 1 storage() rate.sleep() return
def dyn_obs_main(): RosNodeName = 'dyn_obs_polygon' rospy.init_node(RosNodeName, anonymous=True) lidarPointsLabelled_listener = LidarPointsLabelledListener() #lidarPoints_listener = LidarPointsListener() humanState_listener = HumanStateListener() #encoderIMU_listener = EncoderIMUListener() icp_listener = ICPListener() lvls = LvlSetGenerator("low") rate_init = rospy.Rate(10.0) rate_init.sleep() pub = rospy.Publisher('/move_base/TebLocalPlannerROS/obstacles', ObstacleArrayMsg, queue_size=1) #WP = Waypoint() #MPC=doMPC() while not rospy.is_shutdown(): start = time.time() #waypoint = WP.waypoint #if doGoalOnly == False: # rangeData = lidarPoints_listener.RangeData #else: # rangeData = [] staticData = lidarPointsLabelled_listener.StaticData personData = lidarPointsLabelled_listener.PersonData #robot_state = encoderIMU_listener.State # wrt to global origin robot_state = icp_listener.State # wrt to global origin #print "robot state", robot_state #robot_twist = encoderIMU_listener.Twist # wrt to global frame human_state = humanState_listener.State # wrt to GV body frame #print "robot state", robot_state obstacle_msg = ObstacleArrayMsg() obstacle_msg.header.stamp = rospy.Time.now() obstacle_msg.header.frame_id = "odom" # Add polygon obstacle for i in xrange(len(human_state)): obstacle_msg.obstacles.append(ObstacleMsg()) #print human_state vx = human_state[i][2] vy = human_state[i][3] print "vx", vx print "vy", vy if fabs(vx) <= 0.1 and fabs(vy) <= 0.1: vx = 0.0 vy = 0.0 if vx == 0.0: vx = 1e-5 t = atan(vy / vx) if vx * vy >= 0.0: if vx <= 0.0: t = t + pi else: if vx < 0.0: t = t + pi elif vy < 0.0: t = t + 2 * pi print "obs", i, human_state[i][0], human_state[i][1] #print "angle", t if human_state[i][0] == 0.0 and human_state[i][1] == 0.0: continue obstacle_msg.obstacles[i].id = 2 obstacle_msg.obstacles[i].polygon.points = [] X, Y = lvls.CalculateLevelSet(t) l = len(X) X = np.zeros((1, l), dtype=np.float) + X Y = np.zeros((1, l), dtype=np.float) + Y #X=np.array([[-1.0,0.0,1.0,0.0]]) #Y=np.array([[0.0,-1.0,0.0,1.0]]) X = X / 3 Y = Y / 3 t1 = -robot_state[2] R1 = [[cos(t1), -sin(t1)], [sin(t1), cos(t1)]] hs = np.matmul(R1, [[human_state[i][0]], [human_state[i][1]]]) #print hs x = np.ones((1, l), dtype=np.float) * (robot_state[0] + hs[0]) y = np.ones((1, l), dtype=np.float) * (robot_state[1] + hs[1]) xy = np.concatenate((x, y), axis=0) XY = np.concatenate((X, Y), axis=0) R = [[cos(t), -sin(t)], [sin(t), cos(t)]] XY = np.matmul(R, XY) XY = XY + xy XY = np.transpose(XY) #print "xy", xy #print "XY", XY Obshull = ConvexHull(XY) ObsCvxHull = XY[Obshull.vertices, :] len_poly = len(ObsCvxHull) #print ObsCvxHull #v=Point32() for j in xrange(len_poly): obstacle_msg.obstacles[i].polygon.points.append(Point32()) obstacle_msg.obstacles[i].polygon.points[j].x = ObsCvxHull[j][ 0] obstacle_msg.obstacles[i].polygon.points[j].y = ObsCvxHull[j][ 1] #print obstacle_msg.obstacles[0].polygon.points pub.publish(obstacle_msg) #else: # print "angle" """ v1 = Point32() v1.x = -1 v1.y = -1 v2 = Point32() v2.x = -0.5 v2.y = -1.5 v3 = Point32() v3.x = 0 v3.y = -1 #print v1 obstacle_msg.obstacles[0].polygon.points = [v1, v2, v3] obstacle_msg.obstacles[0].polygon.points.append(v1) #print obstacle_msg.obstacles[0].polygon.points """ rate_init.sleep() #if len(human_state): #print "human state", human_state[0] #if doPlot: #ax.plot(human_state[0][1], human_state[0][0],color='blue',marker='D') #Cluster = clusters(rangeData) #staticCluster = clusters(staticData) #personCluster = clusters(personData) if doPlot: ax.scatter(Cluster.clusterXY[:, 1], Cluster.clusterXY[:, 0], color='black') #ax.scatter(staticCluster.clusterXY[:,1], staticCluster.clusterXY[:,0], color='black') #ax.scatter(personCluster.clusterXY[:,1], personCluster.clusterXY[:,0], color='green') #elliHull = EnclosingEllipse(Cluster) #elliHull = EnclosingEllipse(staticCluster) #count = len(Cluster.clusterIdx) #count = len(staticCluster.clusterIdx) """ for i in xrange(count): #print elliHull.a[i], elliHull.b[i] centroid = [elliHull.centroid[i][1],elliHull.centroid[i][0]] if fabs(elliHull.m[i]) < 1e-6: if elliHull.m[i] >0: elliHull.m[i] = 0.001 else: elliHull.m[i] = -0.001 if doPlot: elli=patches.Ellipse(xy=centroid,width=2*elliHull.a[i], height=2*elliHull.b[i], angle=degrees(atan(1/(elliHull.m[i]))), fc='None') ax.add_artist(elli) if doPlot: ax.grid() ax.axis([-5,5,0,5]) ax.set_title('Obstacle position w.r.t to robot') plt.pause(0.05) #plt.draw() ax.cla() """ end = time.time()
def bbox_to_position_in_odom(self, bboxes, DepthImage, camera_param, i=0, obstacle_msg=ObstacleArrayMsg(), marker_data=MarkerArray()): if i == 0: del obstacle_msg.obstacles[:] del marker_data.markers[:] for bbox in bboxes.bounding_boxes: if bbox.Class in self.obstacle_list: try: tan_angle_x = camera_param[0][0] * ( bbox.xmin + bbox.xmax) / 2 + camera_param[0][1] * ( bbox.ymin + bbox.ymax) / 2 + camera_param[0][2] * 1 angle_x = math.atan(tan_angle_x) if abs(math.degrees(angle_x)) < 40: detected_area = DepthImage[bbox.ymin:bbox.ymax, bbox.xmin:bbox.xmax] distance_x = np.median(detected_area) / 1000 #distance_x = distance_x + 0.15 distance_x = distance_x - 0.9 distance_y = -distance_x * tan_angle_x if 1.0 < distance_x < 4.0: obstacle_msg.obstacles.append(ObstacleMsg()) marker_data.markers.append(Marker()) self.tf_br.sendTransform( (-distance_y, 0, distance_x), tf.transformations.quaternion_from_euler( 0, 0, 0), rospy.Time.now(), bbox.Class + str(i), bboxes.header.frame_id) self.tf_listener.waitForTransform( "/odom", "/" + bbox.Class + str(i), rospy.Time(0), rospy.Duration(0.5)) obstable_position = self.tf_listener.lookupTransform( "/odom", bbox.Class + str(i), rospy.Time(0)) obstacle_msg.obstacles[ i].header.stamp, obstacle_msg.obstacles[ i].header.frame_id = bboxes.header.stamp, "odom" obstacle_msg.obstacles[i].id = i obstacle_msg.obstacles[i].polygon.points = [ Point32() ] obstacle_msg.obstacles[i].polygon.points[ 0].x = obstable_position[0][0] obstacle_msg.obstacles[i].polygon.points[ 0].y = obstable_position[0][1] obstacle_msg.obstacles[i].polygon.points[ 0].z = obstable_position[0][2] marker_data.markers[ i].header.stamp, marker_data.markers[ i].header.frame_id = bboxes.header.stamp, "odom" marker_data.markers[i].ns, marker_data.markers[ i].id = bbox.Class, i marker_data.markers[i].action = Marker.ADD marker_data.markers[ i].pose.position.x, marker_data.markers[ i].pose.position.y, marker_data.markers[ i].pose.position.z = obstable_position[ 0][0], obstable_position[0][ 1], obstable_position[0][ 2] - 0.15 marker_data.markers[ i].pose.orientation.x, marker_data.markers[ i].pose.orientation.y, marker_data.markers[ i].pose.orientation.z, marker_data.markers[ i].pose.orientation.w = tf.transformations.quaternion_from_euler( 0, 0, 0) marker_data.markers[ i].color.r, marker_data.markers[ i].color.g, marker_data.markers[ i].color.b, marker_data.markers[ i].color.a = 1, 0, 0, 1 marker_data.markers[ i].scale.x, marker_data.markers[ i].scale.y, marker_data.markers[ i].scale.z = 0.2, 0.2, 1 marker_data.markers[i].type = 3 i = i + 1 except Exception as e: print(e) return obstacle_msg, marker_data
#!/usr/bin/env python import rospy, math, random, re from tf.transformations import quaternion_from_euler from geometry_msgs.msg import Twist, PolygonStamped, Quaternion, QuaternionStamped, TwistWithCovariance, Point32 from nav_msgs.msg import Odometry from costmap_converter.msg import ObstacleArrayMsg, ObstacleMsg # global variables obstacles_msg = ObstacleArrayMsg() def callback_base_pose_ground_truth(base_pose_ground_truth, obst_id): # Search for obstacle id and update fields if found if obst_id == 10: return # skip the Ball i = -1 # -1 is the last element for idx, obst in enumerate(obstacles_msg.obstacles): if obst.id == obst_id: i = idx if i == -1: obstacle_msg = ObstacleMsg() obstacle_msg.id = obst_id obstacles_msg.obstacles.append(obstacle_msg) # HEADER obstacles_msg.header.stamp = rospy.Time.now() obstacles_msg.header.frame_id = 'map' # OBSTACLES obstacles_msg.obstacles[i].header.stamp = obstacles_msg.header.stamp
MO_init = np.array([[-3.85, -0.22, ca.pi, -0.6, 0.3], [-4, 1, 0.0, 0.6, 0.3], [3.91, -11.63, -ca.pi, 0.6, 0.2], [-4.41, -9.5, ca.pi, -0.6, 0.3]]) # Define all the obstacles in the workspace. n_MO = len(MO_init[:, 0]) pub = rospy.Publisher('/MO_Obstacles', ObstacleArrayMsg, queue_size=1) # Setup the publisher and the topic pub1 = rospy.Publisher('/visualization_marker', MarkerArray, queue_size=1) rate = rospy.Rate(10) # Define the publishing rate step = 0.1 while not rospy.is_shutdown(): MO_msg = ObstacleArrayMsg() # Create a msg of type ObstacleArrayMsg MO_msg.header.stamp = rospy.Time.now() MO_msg.header.frame_id = '/map' markerArray = MarkerArray() for k in range(n_MO): # Calculate the next step using runge kutta 4. order k1 = np.array((MO_init[k, 3] * ca.cos(MO_init[k, 2]), MO_init[k, 3] * ca.sin(MO_init[k, 2]))) k2 = np.array( (MO_init[k, 3] * ca.cos(MO_init[k, 2]) + step / 2 * k1[0], MO_init[k, 3] * ca.sin(MO_init[k, 2]) + step / 2 * k1[1])) k3 = np.array( (MO_init[k, 3] * ca.cos(MO_init[k, 2]) + step / 2 * k2[0], MO_init[k, 3] * ca.sin(MO_init[k, 2]) + step / 2 * k2[1])) k4 = np.array((MO_init[k, 3] * ca.cos(MO_init[k, 2]) + step * k3[0],
import rospy def pt(x, y): p = Point32() p.x = x p.y = y p.z = 0 return p pub = rospy.Publisher("/obstacles", ObstacleArrayMsg, queue_size=1) rospy.init_node("talker") rate = rospy.Rate(10) msg = ObstacleArrayMsg() ob1 = ObstacleMsg() ob1.header.frame_id = "map" # ob1.polygon.points = [pt(-5.1, 4.9), pt(4.9,4.9), pt(4.9, -5.1), pt(5.1,-5.1), pt(5.1,5.1), pt(-5.1,5.1)] ob1.polygon.points = [pt(-5, 5), pt(-4, 6), pt(-3, 6), pt(-3, 2)] ob2 = ObstacleMsg() ob2.header.frame_id = "map" # ob2.polygon.points = [pt(-5.1,-5.1), pt(-4.9, -5.1), pt(-4.9, 4.9), pt(-5.1, 4.9)] ob2.polygon.points = [pt(5, 1), pt(2, 6), pt(4, 7), pt(6, 5), pt(6, 1)] ob3 = ObstacleMsg() ob3.header.frame_id = "map" # ob3.polygon.points = [pt(0,5), pt(0,10), pt(2,10), pt(2, 5)] ob3.polygon.points = [pt(-4, -3), pt(0, -5), pt(3, -4), pt(5, -1)]
def publish_obstacle_msg(): pub = rospy.Publisher('/move_base/TebLocalPlannerROS/obstacles', ObstacleArrayMsg, queue_size=1) rospy.init_node("obstacle_msg") obstacle_msg = ObstacleArrayMsg() obstacle_msg.header.stamp = rospy.Time.now() obstacle_msg.header.frame_id = "odom" # CHANGE HERE: odom/map # Add line obstacle obstacle_msg.obstacles.append(ObstacleMsg()) obstacle_msg.obstacles[0].id = 1 line_start = Point32() line_start.x = -2.0 line_start.y = 0.0 line_end = Point32() line_end.x = -0.5 line_end.y = 0.0 obstacle_msg.obstacles[0].polygon.points = [line_start, line_end] # Add line obstacle obstacle_msg.obstacles.append(ObstacleMsg()) obstacle_msg.obstacles[1].id = 2 line_start = Point32() line_start.x = -0.5 line_start.y = 0.0 line_end = Point32() line_end.x = -0.5 line_end.y = -0.5 obstacle_msg.obstacles[1].polygon.points = [line_start, line_end] # Add line obstacle obstacle_msg.obstacles.append(ObstacleMsg()) obstacle_msg.obstacles[2].id = 2 line_start = Point32() line_start.x = -0.5 line_start.y = -0.5 line_end = Point32() line_end.x = 0.5 line_end.y = -0.5 obstacle_msg.obstacles[2].polygon.points = [line_start, line_end] # Add line obstacle obstacle_msg.obstacles.append(ObstacleMsg()) obstacle_msg.obstacles[3].id = 3 line_start = Point32() line_start.x = 0.5 line_start.y = -0.5 line_end = Point32() line_end.x = 0.5 line_end.y = 0.0 obstacle_msg.obstacles[3].polygon.points = [line_start, line_end] # Add line obstacle obstacle_msg.obstacles.append(ObstacleMsg()) obstacle_msg.obstacles[4].id = 3 line_start = Point32() line_start.x = 0.5 line_start.y = 0.0 line_end = Point32() line_end.x = 2.0 line_end.y = 0.0 obstacle_msg.obstacles[4].polygon.points = [line_start, line_end] r = rospy.Rate(10) # 10hz t = 0.0 while not rospy.is_shutdown(): pub.publish(obstacle_msg) r.sleep()
def publish_obstacle_msg(): pub = rospy.Publisher('/test_optim_node/obstacles', ObstacleArrayMsg, queue_size=1) rospy.init_node("obstacle_msg") obstacle_msg = ObstacleArrayMsg() obstacle_msg.header.stamp = rospy.Time.now() obstacle_msg.header.frame_id = "odom/map" # CHANGE HERE: odom/map # Add bottom obstacle obstacle_msg.obstacles.append(ObstacleMsg()) obstacle_msg.obstacles[0].id = 0 v0 = Point32() v0.x = -12.0 v0.y = 0.0 v1 = Point32() v1.x = 0.0 v1.y = 0.0 v2 = Point32() v2.x = 0.0 v2.y = -2.5 v3 = Point32() v3.x = 7.5 v3.y = -2.5 v4 = Point32() v4.x = 7.5 v4.y = 0 v5 = Point32() v5.x = 17 v5.y = 0 v6 = Point32() v6.x = 17 v6.y = -10 v7 = Point32() v7.x = -12 v7.y = -10 obstacle_msg.obstacles[0].polygon.points = [v0, v1, v2, v3, v4, v5, v6, v7] # Add top obstacle obstacle_msg.obstacles.append(ObstacleMsg()) obstacle_msg.obstacles[1].id = 1 v0 = Point32() v0.x = 17.0 v0.y = 5.0 v1 = Point32() v1.x = -12.0 v1.y = 5.0 v2 = Point32() v2.x = -12.0 v2.y = 10.0 v3 = Point32() v3.x = 17.0 v3.y = 10.0 obstacle_msg.obstacles[1].polygon.points = [v0, v1, v2, v3] r = rospy.Rate(100) # 10hz while not rospy.is_shutdown(): pub.publish(obstacle_msg) r.sleep()
def publish_obstacles_scan(self, flow, dt): ''' Generate and publish ObstacleArrayMsg from flow vectors. ''' scan_resolution = 20.0 / self.num_r # scan_cartesian_one side(m)/index; resolution = 0.105 for index:190 obstacle_vels = flow / dt * scan_resolution # dt = 0.0249~~ # Generate obstacle_msg for TebLocalPlanner here. obstacle_msg = ObstacleArrayMsg() obstacle_msg.header.stamp = rospy.Time.now() obstacle_msg.header.frame_id = self.global_frame # get the scan sensor pose (= robot_pose) sensor_position, sensor_quaternion = self.listener.lookupTransform( '/base_scan', '/map', rospy.Time()) sensor_euler = tf.transformations.euler_from_quaternion( sensor_quaternion) # return (roll, pitch, yaw) sensor_angle = -sensor_euler[2] # angle = yaw _cos = math.cos(sensor_angle) _sin = math.sin(sensor_angle) robot_pose = (sensor_position[0], sensor_position[1], sensor_angle) obstacle_speed = obstacle_vels[:, 2:] obstacle_speed = np.linalg.norm(obstacle_speed, axis=1) #mask_img = gaussian_filter(self.scan_msg.ranges.T, 1.0) #obstacle_speed[mask_img < 40] = 0 for i in range(obstacle_vels.shape[0]): # Add point obstacle to the message depend on obstacle speed if obstacle_speed[i] > 0 and obstacle_speed[i] < 1: m = sensor_position[0] + obstacle_vels[i][0] / 3 n = sensor_position[1] + obstacle_vels[i][1] / 3 # k = _cos*m - _sin*n # l = _sin*m + _cos*n # flow_vector_position = ( # _cos*k + _sin*l, # - _sin*k + _cos*l # ) flow_vector_position = (m, n) # flow_vector_position = ( # _cos*sensor_position[0] + _sin*sensor_position[1] + obstacle_vels[i][0]/3, # _sin*sensor_position[0] + _cos*sensor_position[1] + obstacle_vels[i][1]/3 # ) # 첫 발행까지 시간소요 높음, 회전하면서 postion 동시에 변화 # flow_vector_position = ( # sensor_position[0] + (_cos*obstacle_vels[i][0] - _sin*obstacle_vels[i][1])/3, # sensor_position[1] + (_sin*obstacle_vels[i][0] + _cos*obstacle_vels[i][1])/3 # ) # 첫 발행까지 시간소요 높음, 회전하면서 postion 동시에 변화 line_scale = (1. - np.exp(-np.linalg.norm([ flow_vector_position[0] - robot_pose[0], flow_vector_position[1] - robot_pose[1] ]) / 2.)) * self.prediction_horizon # print(flow_vector_position) obstacle_msg.obstacles.append(ObstacleMsg()) obstacle_msg.obstacles[-1].id = len(obstacle_msg.obstacles) - 1 obstacle_msg.obstacles[-1].polygon.points = [ Point32(), Point32() ] obstacle_msg.obstacles[-1].polygon.points[ 0].x = flow_vector_position[0] obstacle_msg.obstacles[-1].polygon.points[ 0].y = flow_vector_position[1] obstacle_msg.obstacles[-1].polygon.points[0].z = 0 obstacle_msg.obstacles[-1].polygon.points[ 1].x = flow_vector_position[ 0] + obstacle_vels[i][2] * line_scale / 10 obstacle_msg.obstacles[-1].polygon.points[ 1].y = flow_vector_position[ 1] + obstacle_vels[i][3] * line_scale / 10 obstacle_msg.obstacles[-1].polygon.points[1].z = 0 self.obstacle_pub.publish(obstacle_msg) # Publish predicted obstacles print("one msg published")
def publish_obstacle_msg(points): rospy.init_node("test_obstacle_msg") pub = rospy.Publisher('/test_optim_node/obstacles', ObstacleArrayMsg, queue_size=1) obstacle_msg = ObstacleArrayMsg() obstacle_msg.header.stamp = rospy.Time.now() obstacle_msg.header.frame_id = "odom" # CHANGE HERE: odom/map lines = [[1, 2, 3], [4, 5, 6]] scale_x = 190 scale_y = 260 shift_x = 4.5 shift_y = 1 xcoords = points[0]/scale_x - shift_x ycoords = points[1]/scale_y - shift_y # xcoords = lines[0] # ycoords = lines[1] # obstacle_msg.obstacles[0].id = 0 j=0 for idx, xcoord in enumerate(xcoords): if(idx%50 == 0): obstacle_msg.obstacles.append(ObstacleMsg()) # print('point: x= ', xcoord, ' y= ', ycoords[idx], ' idx: ', idx) print(type(xcoord)) # Add point obstacle # v1 = Point32() # v1.x = xcoord # v1.y = ycoords[idx] # obstacle_msg.obstacles[idx].id = idx x = xcoord y = ycoords[idx] # rotate coordinates obstacle_msg.obstacles[j].polygon.points = [Point()] obstacle_msg.obstacles[j].polygon.points[0].x = y obstacle_msg.obstacles[j].polygon.points[0].y = -x obstacle_msg.obstacles[j].polygon.points[0].z = 0 j = j+1 # obstacle_msg.obstacles.append(ObstacleMsg()) # obstacle_msg.obstacles[1].id = 2 # v1 = Point32() # v1.x = -1 # v1.y = -1 # v2 = Point32() # v2.x = -0.5 # v2.y = -1.5 # v3 = Point32() # v3.x = 0 # v3.y = -1 # obstacle_msg.obstacles[2].polygon.points = [v1, v2, v3] r = rospy.Rate(10) # 10hz t = 0.0 timeout = time.time() + 5 # 10 seconds from now while True and not rospy.is_shutdown(): test = 0 if test == 5 or time.time() > timeout: print('timer about to break') break pub.publish(obstacle_msg) r.sleep() test - test - 1
def bbox_to_position_in_odom(self, bboxes, DepthImage, camera_param, i=0, obstacle_msg=ObstacleArrayMsg(), marker_data=MarkerArray()): if i == 0: del obstacle_msg.obstacles[:] del marker_data.markers[:] self.landmark_msg.header.stamp = bboxes.header.stamp self.landmark_msg.header.frame_id = "base_link" for bbox in bboxes.bounding_boxes: if bbox.Class in self.obstacle_list: tan_angle_x = camera_param[0][0] * ( bbox.xmin + bbox.xmax) / 2 + camera_param[0][1] * ( bbox.ymin + bbox.ymax) / 2 + camera_param[0][2] * 1 angle_x = math.atan(tan_angle_x) if abs(math.degrees(angle_x)) < 35: detected_area = DepthImage[bbox.ymin:bbox.ymax, bbox.xmin:bbox.xmax] distance_x = np.median(detected_area) / 1000 distance_x = distance_x + 0.15 if bboxes.header.frame_id == "camera2_color_optical_frame": distance_x = -distance_x distance_y = -distance_x * tan_angle_x if 1.0 < distance_x < 3.0: self.tf_br.sendTransform( (-distance_y, 0, distance_x), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), bbox.Class, bboxes.header.frame_id) self.tf_listener.waitForTransform( "/base_link", "/" + bbox.Class, rospy.Time(0), rospy.Duration(0.1)) landmark_position = self.tf_listener.lookupTransform( "/base_link", bboxes.header.frame_id, rospy.Time(0)) self.landmark_msg.detections[0].id = [1] self.landmark_msg.detections[0].size = [0.3] self.landmark_msg.detections[ 0].pose.header = bboxes.header self.landmark_msg.detections[ 0].pose.header.frame_id = bbox.Class self.landmark_msg.detections[ 0].pose.pose.pose.position.x = landmark_position[ 0][0] self.landmark_msg.detections[ 0].pose.pose.pose.position.y = landmark_position[ 0][1] self.landmark_msg.detections[ 0].pose.pose.pose.position.z = landmark_position[ 0][2] self.landmark_msg.detections[ 0].pose.pose.covariance = [ (self.landmark_msg.detections[0].pose.pose. pose.position.x * 0.01) * (self.landmark_msg.detections[0].pose.pose. pose.position.x * 0.01), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9999, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, (self.landmark_msg.detections[0].pose.pose. pose.position.z * 0.01) * (self.landmark_msg.detections[0].pose.pose. pose.position.z * 0.01), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9999 ] self.landmark_publisher.publish(self.landmark_msg) return obstacle_msg, marker_data