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 add_obstacle(self, id, posx, posy, vel_x, vel_y, range_x, range_y): self.pose_x.append(posx) self.pose_y.append(posy) self.range_x.append(range_x) self.range_y.append(range_y) samp_obstacle = ObstacleMsg() samp_obstacle.id = id samp_obstacle.radius = 0.35 samp_obstacle.polygon.points = [Point32()] samp_obstacle.polygon.points[0].x = posx samp_obstacle.polygon.points[0].y = posy samp_obstacle.polygon.points[0].z = 0 yaw = math.atan2(vel_y, vel_x) q = tf.transformations.quaternion_from_euler(0, 0, yaw) samp_obstacle.orientation = Quaternion(*q) samp_obstacle.velocities.twist.linear.x = vel_x samp_obstacle.velocities.twist.linear.y = vel_y samp_obstacle.velocities.twist.linear.z = 0 samp_obstacle.velocities.twist.angular.x = 0 samp_obstacle.velocities.twist.angular.y = 0 samp_obstacle.velocities.twist.angular.z = 0 return samp_obstacle
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 publish_obstacles(self): with self.lock: if self.transport_data is None: return # non-leg obstacles timestamp, xx, yy, clusters, is_legs, cogs, radii, tf_rob_in_fix = self.transport_data # tracks track_ids = [] tracks_latest_pos, tracks_color = [], [] tracks_in_frame, tracks_velocities = [], [] tracks_radii = [] for trackid in self.tracker.active_tracks: track = self.tracker.active_tracks[trackid] xy = np.array(track.pos_history[-1]) is_track_in_frame = True if trackid in self.tracker.latest_matches: color = (0.,1.,0.,1.) # green elif trackid in self.tracker.new_tracks: color = (0.,0.,1.,1.) # blue else: color = (0.7, 0.7, 0.7, 1.) is_track_in_frame = False track_ids.append(trackid) tracks_latest_pos.append(xy) tracks_color.append(color) tracks_in_frame.append(is_track_in_frame) tracks_velocities.append(track.estimate_velocity()) tracks_radii.append(track.avg_radius()) # publish trackedpersons from spencer_tracking_msgs.msg import TrackedPersons, TrackedPerson pub = rospy.Publisher("/tracked_persons", TrackedPersons, queue_size=1) tp_msg = TrackedPersons() tp_msg.header.frame_id = self.kFixedFrame tp_msg.header.stamp = timestamp for trackid, xy, in_frame, vel, radius in zip(track_ids, tracks_latest_pos, tracks_in_frame, tracks_velocities, tracks_radii): # if not in_frame: # continue tp = TrackedPerson() tp.track_id = trackid tp.is_occluded = False tp.is_matched = in_frame tp.detection_id = trackid tp.pose.pose.position.x = xy[0] tp.pose.pose.position.y = xy[1] heading_angle = np.arctan2(vel[1], vel[0]) # guess heading from velocity from geometry_msgs.msg import Quaternion tp.pose.pose.orientation = Quaternion( *tf.transformations.quaternion_from_euler(0, 0, heading_angle)) tp.twist.twist.linear.x = vel[0] tp.twist.twist.linear.y = vel[1] tp.twist.twist.angular.z = 0 # unknown tp_msg.tracks.append(tp) pub.publish(tp_msg) pub = rospy.Publisher('/obstacles', ObstacleArrayMsg, queue_size=1) obstacles_msg = ObstacleArrayMsg() obstacles_msg.header.stamp = timestamp obstacles_msg.header.frame_id = self.kFixedFrame for trackid, xy, in_frame, vel, radius in zip(track_ids, tracks_latest_pos, tracks_in_frame, tracks_velocities, tracks_radii): if not in_frame: continue # Add point obstacle obst = ObstacleMsg() obst.id = trackid obst.polygon.points = [Point32()] obst.polygon.points[0].x = xy[0] obst.polygon.points[0].y = xy[1] obst.polygon.points[0].z = 0 obst.radius = radius yaw = np.arctan2(vel[1], vel[0]) q = tf.transformations.quaternion_from_euler(0,0,yaw) obst.orientation = Quaternion(*q) obst.velocities.twist.linear.x = vel[0] obst.velocities.twist.linear.y = 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 = 0 obstacles_msg.obstacles.append(obst) pub.publish(obstacles_msg) pub = rospy.Publisher('/close_nonleg_obstacles', ObstacleArrayMsg, queue_size=1) MAX_DIST_STATIC_CLUSTERS_M = 3. cogs_in_fix = [] for i in range(len(cogs)): cogs_in_fix.append(apply_tf(cogs[i], Pose2D(tf_rob_in_fix))) obstacles_msg = ObstacleArrayMsg() obstacles_msg.header.stamp = timestamp obstacles_msg.header.frame_id = self.kFixedFrame for c, cog_in_fix, cog_in_rob, r, is_leg in zip(clusters, cogs_in_fix, cogs, radii, is_legs): if np.linalg.norm(cog_in_rob) < self.kRobotRadius: continue if len(c) < self.kMinClusterSize: continue # leg obstacles are already published in the tracked obstacles topic if is_leg: continue # close obstacles only if np.linalg.norm(cog_in_rob) > MAX_DIST_STATIC_CLUSTERS_M: continue # Add point obstacle obst = ObstacleMsg() obst.id = 0 obst.polygon.points = [Point32()] obst.polygon.points[0].x = cog_in_fix[0] obst.polygon.points[0].y = cog_in_fix[1] obst.polygon.points[0].z = 0 obst.radius = r yaw = 0 q = tf.transformations.quaternion_from_euler(0,0,yaw) from geometry_msgs.msg import Quaternion obst.orientation = Quaternion(*q) obst.velocities.twist.linear.x = 0 obst.velocities.twist.linear.y = 0 obst.velocities.twist.linear.z = 0 obst.velocities.twist.angular.x = 0 obst.velocities.twist.angular.y = 0 obst.velocities.twist.angular.z = 0 obstacles_msg.obstacles.append(obst) pub.publish(obstacles_msg) pub = rospy.Publisher('/obstacle_markers', MarkerArray, queue_size=1) # delete all markers ma = MarkerArray() mk = Marker() mk.header.frame_id = self.kFixedFrame mk.ns = "obstacles" mk.id = 0 mk.type = 0 # ARROW mk.action = 3 # deleteall ma.markers.append(mk) pub.publish(ma) # publish tracks ma = MarkerArray() id_ = 0 # track endpoint for trackid, xy, in_frame, vel in zip(track_ids, tracks_latest_pos, tracks_in_frame, tracks_velocities): if not in_frame: continue normvel = np.linalg.norm(vel) if normvel == 0: continue mk = Marker() mk.header.frame_id = self.kFixedFrame mk.ns = "tracks" mk.id = trackid mk.type = 0 # ARROW mk.action = 0 mk.scale.x = np.linalg.norm(vel) mk.scale.y = 0.1 mk.scale.z = 0.1 mk.color.r = color[0] mk.color.g = color[1] mk.color.b = color[2] mk.color.a = color[3] mk.frame_locked = True mk.pose.position.x = xy[0] mk.pose.position.y = xy[1] mk.pose.position.z = 0.03 yaw = np.arctan2(vel[1], vel[0]) q = tf.transformations.quaternion_from_euler(0,0,yaw) mk.pose.orientation = Quaternion(*q) ma.markers.append(mk) pub.publish(ma)