def viacon_callback(self, msg): if self.last_pos is None: self.last_pos = -msg.transform.translation.x, -msg.transform.translation.y, msg.header.stamp.to_sec( ) return people = People() person = Person() people.header.stamp = msg.header.stamp people.header.frame_id = "map" last_x, last_y, last_t = self.last_pos self.last_pos = -msg.transform.translation.x, -msg.transform.translation.y, msg.header.stamp.to_sec( ) if msg.header.stamp.to_sec() - last_t <= 0: return self.vx = (1 - self.gamma) * self.vx + \ self.gamma * (-msg.transform.translation.x - last_x) / (msg.header.stamp.to_sec() - last_t) self.vy = (1 - self.gamma) * self.vy + \ self.gamma * (-msg.transform.translation.y - last_y) / (msg.header.stamp.to_sec() - last_t) person.position.x = -msg.transform.translation.x person.position.y = -msg.transform.translation.y self.last_pos = -msg.transform.translation.x, -msg.transform.translation.y, msg.header.stamp.to_sec( ) people.people = [person] self.p_pub.publish(people)
def run(self): self.start_time = rospy.Time.now() while not rospy.is_shutdown(): if ( rospy.Time.now() - self.start_time < rospy.Duration(self.delayed_start) ): rospy.sleep(0.1) continue poses = [] persons = [] for w in self.walkers: if random() < self.prob_switch_visible: self.visible[w.name] = (random() < self.prob_visible) if self.visible[w.name]: rospy.loginfo( "switch sensor %s to see %s" % (self.name, w.name)) else: rospy.loginfo( "switch sensor %s to NOT see %s" % (self.name, w.name)) if self.visible[w.name]: ps = w.get_pose() ps.position.x += nprnd.randn() * self.noise[0] ps.position.y += nprnd.randn() * self.noise[1] poses.append(ps) p = w.get_person() p.position.x = ps.position.x p.position.y = ps.position.y if self.anonymous: p.name = '' persons.append(p) pa = PoseArray() pa.header.frame_id = '/base_link' pa.header.stamp = rospy.Time.now() pa.poses = poses self.pub_poses.publish(pa) ppl = People() ppl.header.frame_id = '/base_link' ppl.header.stamp = rospy.Time.now() ppl.people = persons self.pub_people.publish(ppl) sleep_rate = ( self.avg_rate + nprnd.randn() * self.std_rate) sleep_rate = sleep_rate if sleep_rate > .1 else .1 rospy.sleep(1.0 / sleep_rate)
def publish(self): """ """ data = self.data groups = [] group = [] persons = [] if not data.poses: groups = [] else: for pose in data.poses: rospy.loginfo("Person Detected") quartenion = [ pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w ] (_, _, yaw) = tf.transformations.euler_from_quaternion(quartenion) pose_person = [pose.position.x, pose.position.y, yaw] persons.append(pose_person) if persons: p = People() p.header.frame_id = "/base_footprint" p.header.stamp = rospy.Time.now() for person in persons: p1 = Person() p1.position.x = person[0] p1.position.y = person[1] p1.velocity.x = 1 / math.tan(person[2]) p1.velocity.y = 1 p.people.append(p1) self.pub.publish(p) else: p = People() p.header.frame_id = "/base_footprint" p.header.stamp = rospy.Time.now() self.pub.publish(p)
def __init__(self, tf, rospy, global_frame, base_footprint, portable_number): #コンストラクタ self.output_line_ = "" #アウトプット用文字列 self.tf_ = tf #クラス用tf self.back_key_ = False self.status_key_ = False self.detect_mode_ = False self.mainloop_wheel_key_ = False self.mainloop_human_key_ = False self.rospy_ = rospy #クラス用rospy self.global_frame_ = global_frame #ポータブル自身のグローバルフレーム self.base_footprint_ = base_footprint #ポータブル自身のベースフットプリント self.portable_number_ = portable_number #ポータブル自身のナンバー self.listener_ = tf.TransformListener() #tfのリスナー self.odom_ = Odometry() self.twist_ = Twist() self.status_ = MoveBaseActionResult() self.chairbot_position_ = PointStamped() self.human_position_ = tracking_points() self.people_position_ = People() self.detect_start_command = "roslaunch tms_rc_pot detect_object.launch" self.detect_end_command = "rosnode kill /pot" + portable_number + "/human_tracker /pot" + portable_number + "/reflec_pot" self.rate_ = self.rospy_.Rate(10) # 10hz self.start_pub_ = self.rospy_.Publisher('initialpose', PoseWithCovarianceStamped, queue_size=10) self.goal_pub_ = self.rospy_.Publisher('move_base_simple/goal', PoseStamped, queue_size=10) self.straight_pub_ = self.rospy_.Publisher('mobile_base/commands/velocity', Twist, queue_size=10) rospy.Subscriber('odom', Odometry, self.callback_odom, queue_size=1) #オドメトリサブスクライバー rospy.Subscriber('move_base/result', MoveBaseActionResult, self.callback_status, queue_size=1) #リザルトスクライバー rospy.Subscriber('lrf_pose', PointStamped, self.callback_wheelchair_position, queue_size=1) #lrf_poseサブスクライバー rospy.Subscriber('people', People, self.callback_human_position, queue_size=1) #lrf_poseサブスクライバー rospy.Subscriber('command_msg', String, self.callback_command, queue_size=10) #commandサブスクライバー
def publish_human_position(self): #人の座標を出力 obstacle_people = People() #people_msg obstacle_people.header.frame_id = 'map' obstacle_people.header.stamp = self.rospy_.Time.now() marker_array_vis = MarkerArray() #people_visalizaion ids = map(lambda x: x.id, self.human_class) vanished_ids = list(set(self.prev_ids) - set(ids)) self.prev_ids = map(lambda x: x.id, self.human_class) for i in vanished_ids: # xrange(10000): marker_vis = Marker() #marker marker_vis.header.frame_id = 'map' marker_vis.header.stamp = self.rospy_.Time.now() marker_vis.id = i marker_vis.type = 3 #3 cylinder 9 text marker_vis.action = 2 #delete marker_array_vis.markers.append(marker_vis) self.human_vis_pub_.publish(marker_array_vis) marker_array_vis = MarkerArray() #people_visalizaion for i in range(0, len(self.human_class)): tmp_human = Person() #person tmp_human.name = str(self.human_class[i].id) tmp_human.position.x = self.human_class[i].x tmp_human.position.y = self.human_class[i].y tmp_human.position.z = 0.0 tmp_human.velocity.x = self.human_class[i].x_vel tmp_human.velocity.y = self.human_class[i].y_vel tmp_human.velocity.z = 0.0 tmp_human.reliability = 1.0 obstacle_people.people.append(tmp_human) marker_vis = Marker() #marker marker_vis.header.frame_id = 'map' marker_vis.header.stamp = self.rospy_.Time.now() marker_vis.id = self.human_class[i].id marker_vis.type = 3 #3 cylinder 9 text marker_vis.action = 0 #add marker_vis.scale.x = 0.1 marker_vis.scale.y = 0.1 marker_vis.scale.z = 1.0 marker_vis.color.r = 1 marker_vis.color.g = 0 marker_vis.color.b = 0 marker_vis.color.a = 1.0 marker_vis.lifetime = self.rospy_.Duration(0.2) marker_vis.pose.position.x = self.human_class[i].x marker_vis.pose.position.y = self.human_class[i].y marker_vis.pose.position.z = 0.5 marker_vis.pose.orientation.x = 0.0 marker_vis.pose.orientation.y = 0.0 marker_vis.pose.orientation.z = 0.0 marker_vis.pose.orientation.w = 1.0 #marker_vis.text="aaaaaaaaa" marker_array_vis.markers.append(marker_vis) self.human_pub_.publish(obstacle_people) self.human_vis_pub_.publish(marker_array_vis)
def __init__(self, model_to_track_list): # This are the models that we will generate information about. self.gz_model_obj = GazeboModel(model_to_track_list) self.people = People() self.person = Person() self.pub = rospy.Publisher("/people", People, queue_size=10) self.pub2 = rospy.Publisher("/person_pose", People, queue_size=10)
def __init__(self, tf, rospy, portable_number): #コンストラクタ self.output_line_ = "" #アウトプット用文字列 self.tf_ = tf #クラス用tf self.back_key_ = False self.status_key_ = False self.detect_mode_ = False self.mainloop_wheel_key_ = False self.mainloop_human_key_ = False self.rospy_ = rospy #クラス用rospy self.global_frame_ = 'map' + portable_number #グローバルフレーム self.base_footprint_ = 'base_footprint' + portable_number #ベースフットプリント self.portable_number_ = portable_number #ポータブルのナンバー self.listener_ = tf.TransformListener() #tfのリスナー self.odom_ = Odometry() self.twist_ = Twist() self.status_ = GoalStatusArray() self.chairbot_position_ = PointStamped() self.human_position_ = tracking_points() self.people_position_ = People() self.start_pos_ = PoseWithCovarianceStamped() #potスタートポジ self.goal_pos_ = PoseStamped() #potゴールポジ self.err_count_ = 0 self.detect_end_time = rospy.Time.from_sec(time.time()).to_sec() self.detect_start_command = "roslaunch tms_rc_pot detect_object.launch" # self.detect_end_command = "rosnode kill /pot"+portable_number+"/human_tracker /pot"+portable_number+"/reflec_pot" self.detect_end_command = "rosnode kill /pot" + portable_number + "/reflec_pot" self.rate_ = self.rospy_.Rate(10) # 10hz self.start_pub_ = self.rospy_.Publisher('initialpose', PoseWithCovarianceStamped, queue_size=10) self.goal_pub_ = self.rospy_.Publisher('move_base_simple/goal', PoseStamped, queue_size=10) self.vel_pub_ = self.rospy_.Publisher('mobile_base/commands/velocity', Twist, queue_size=10) rospy.Subscriber('odom', Odometry, self.callback_odom, queue_size=1) #オドメトリサブスクライバー rospy.Subscriber('move_base/status', GoalStatusArray, self.callback_status, queue_size=1) #リザルトスクライバー rospy.Subscriber('lrf_pose', PointStamped, self.callback_wheelchair_position, queue_size=1) #lrf_poseサブスクライバー rospy.Subscriber('people', People, self.callback_human_position, queue_size=1) #lrf_poseサブスクライバー rospy.Subscriber('command_msg', String, self.callback_command, queue_size=10) #commandサブスクライバー rospy.wait_for_service('move_base/clear_costmaps') self.rect_status = rospy.ServiceProxy('move_base/clear_costmaps', Empty)
def publish(self): gen.counter = 0 pl = People() pl.header.frame_id = None for p in self.people.values(): p.publish_markers(self.mpub) frame, person = p.get_person() pl.header.frame_id = frame pl.people.append(person) self.ppub.publish(pl)
def __init__(self): rospy.init_node("imu_anomaly_detection_layer_hack") self.tf_listener = TransformListener() rospy.Subscriber('/android/imu', Imu, self.imuCB, queue_size=1) rospy.Subscriber('mode', Bool, self.modeCB) self.pub = rospy.Publisher("/people", People, queue_size=1) self.fb_people = People() self.fb_people.header.frame_id = 'map' self.is_training = True self.clf = OneClassSVM(nu=0.1, kernel="poly", degree=3) rospy.spin()
def __init__(self): rospy.init_node("people_layer_hack") self.fb_people = People() self.fb_people.header.frame_id = 'map' #self.tf_listener = TransformListener() self.sub = rospy.Subscriber("/darknet_ros/bounding_boxes", BoundingBoxes, self.sub_cb, queue_size=10) self.pub = rospy.Publisher("/people", People, queue_size=10) self.found_objects = dict() rospy.spin()
def __init__(self): rospy.init_node("people_layer_hack") self.tf_listener = TransformListener() self.sub = rospy.Subscriber("/darknet_ros/found_object", Int8, self.sub_cb, queue_size=10) self.pub = rospy.Publisher("/people", People, queue_size=10) self.step = 0.4 self.counter = 1 self.fb_people = People() self.fb_people.header.frame_id = 'map' rospy.spin()
def __init__(self): self.people = People() self.angles = PoseArray() self.pub = rospy.Publisher('/people_yolo_detector', People, queue_size=10) self.pub2 = rospy.Publisher('/people_yolo_angles', PoseArray, queue_size=1) self.sub = rospy.Subscriber("/darknet_ros/bounding_boxes", BoundingBoxes, self.callback)
def __init__(self): self.r_ = rospy.Rate(100) #initializing message self.msg_tracked_persons_ = TrackedPersons() self.msg_people_ = People() self.msg_person_ = Person() #Subscriber setup rospy.Subscriber('/pedsim/tracked_persons', TrackedPersons, self.callbackTrackedPersons) #Publisher setup self.pub_people_ = rospy.Publisher('/people', People, queue_size=10) #Dummy flag self.FIRST_MSG_ = False
def spin(self): rate = rospy.Rate(10) while not rospy.is_shutdown(): pv = Person() pl = People() pl.header.stamp = rospy.Time.now() pl.header.frame_id = '/map' pv.position.x = self.x pv.position.y = self.y pv.position.z = .5 pv.velocity.x = 0 pv.velocity.y = 0 pv.name = 'Steve' pv.reliability = .90 pl.people.append(pv) self.ppub.publish(pl) rate.sleep()
def spin(self): rate = rospy.Rate(10) while not rospy.is_shutdown(): pv = Person() pl = People() pl.header.stamp = rospy.Time.now() pl.header.frame_id = '/base_link' pv.position.x = float(sys.argv[1]) pv.position.y = float(sys.argv[2]) pv.position.z = .5 pv.velocity.x = float(sys.argv[3]) pv.velocity.y = float(sys.argv[4]) pv.name = 'asdf' pv.reliability = .90 pl.people.append(pv) self.ppub.publish(pl) rate.sleep()
def publish(self): gen.counter = 0 pl = People() pl.header.frame_id = None pl.header.stamp = self.last_stamp for p in self.people.values(): p.publish_markers(self.mpub) frame, person = p.get_person() pl.header.frame_id = frame point = PointStamped() point.header = pl.header point.point = person.position base_link_point = self.transform_listener.transformPoint( "laser", point) if base_link_point.point.x < 4.0 and base_link_point.point.y < 2.0 and base_link_point.point.y > -2.0: print frame print person.name print base_link_point.point print "---" pl.people.append(person) self.ppub.publish(pl)
def predict(self, tracked_persons): tracks = tracked_persons.tracks track_dict = {} for track in tracks: #print track #print track.pose.pose.position.x track_dict[track.track_id] = [ track.pose.pose.position.x, track.pose.pose.position.y ] del self.prev_frames[0] self.prev_frames.append(track_dict) if len(tracks) == 0: return input_data = self.__generate_input(tracks) #print input_data.shape #print input_data grid_batch = getSequenceGridMask(input_data, self.dimensions, self.saved_args.neighborhood_size, self.saved_args.grid_size) obs_traj = input_data[:self.obs_length] obs_grid = grid_batch[:self.obs_length] print "********************** PREDICT NEW TRAJECTORY ******************************" complete_traj = self.social_lstm_model.sample(self.sess, obs_traj, obs_grid, self.dimensions, input_data, self.pred_length) #print complete_traj # Initialize the markers array prediction_markers = MarkerArray() # Publish them people_predictions = PeoplePrediction() for frame_index in range(self.pred_length): people = People() people.header.stamp = tracked_persons.header.stamp + rospy.Duration( frame_index * self.time_resolution) people.header.frame_id = tracked_persons.header.frame_id predicted_frame_index = frame_index + self.obs_length for person_index in range(self.max_pedestrians): track_id = complete_traj[predicted_frame_index, person_index, 0] x_coord = complete_traj[predicted_frame_index, person_index, 1] y_coord = complete_traj[predicted_frame_index, person_index, 2] if track_id == 0: break person = Person() person.name = str(track_id) point = Point() point.x = x_coord point.y = y_coord person.position = point people.people.append(person) self.prediction_marker.header.frame_id = tracked_persons.header.frame_id self.prediction_marker.header.stamp = tracked_persons.header.stamp self.prediction_marker.id = int(track_id) self.prediction_marker.pose.position.x = person.position.x self.prediction_marker.pose.position.y = person.position.y #self.prediction_marker.color.a = 1 - (frame_index * 1.0 / (self.pred_length * 1.0)) self.prediction_marker.color.a = 1.0 prediction_markers.markers.append(self.prediction_marker) people_predictions.predicted_people.append(people) #print people_predictions self.pedestrian_prediction_pub.publish(people_predictions) self.prediction_marker_pub.publish(prediction_markers)
def people_callback(self, people): assert len(people.people) == 1 person = people.people[0] predictions = PeoplePrediction() markers = MarkerArray() markers.markers = [] predictions.predicted_people = [] # if the message stamp is empty, we assign the current time if people.header.stamp == rospy.Time(): people.header.stamp = rospy.get_rostime() #delta_t = [] tdist = 0.0 for goal in self.goals: goal.update_distance(person) dist = -(goal.heuristic_distance[-1] - goal.heuristic_distance[0]) dist = max(dist, 0.0) tdist += dist**2.0 #delta_t.append(dist) if tdist > 0.0: # to avoid singularities for goal in self.goals: dist = -(goal.heuristic_distance[-1] - goal.heuristic_distance[0]) dist = max(dist, 0.0) goal.probability = dist**2.0 / tdist #print(tdist, [goal.probability for goal in self.goals], sum([goal.probability for goal in self.goals])) #if sum(delta_t) > 0.0: # print(tdist, delta_t, sum(delta_t), delta_t[0]/sum(delta_t)) #else: # print(tdist, delta_t, sum(delta_t)) predictions.path_probabilities = probabilities = [] for goal in self.goals: goal.predict_path(person) probabilities.append(Float64()) probabilities[-1].data = goal.probability k = 0 for i in range(self.num_predictions): people_one_timestep = People() people_one_timestep.people = [] for goal in self.goals[::-1]: person_prediction = goal.path_prediction[i] people_one_timestep.people.append(person_prediction) prediction_marker = self.get_marker() prediction_marker.header.frame_id = people.header.frame_id prediction_marker.header.stamp = people.header.stamp prediction_marker.id = k k += 1 prediction_marker.pose.position = person_prediction.position # the opacity of the marker is adjusted according to the prediction step prediction_marker.color.a = ( 1 - i / float(self.num_predictions)) * goal.probability markers.markers.append(prediction_marker) # fill the header people_one_timestep.header.frame_id = people.header.frame_id people_one_timestep.header.stamp = people.header.stamp + \ rospy.Duration(i * self.time_resolution) # push back the predictions for this time step to the prediction container predictions.predicted_people.append(people_one_timestep) self.prediction_pub.publish(predictions) self.marker_pub.publish(markers)
class Recognizer(object): def __init__(self, model, cascade_filename, run_local, wait, rp): self.rp = rp self.wait = wait self.doRun = True self.model = model self.restart = False self.ros_restart_request = False self.detector = CascadedDetector(cascade_fn=cascade_filename, minNeighbors=5, scaleFactor=1.1) if run_local: print ">> Error: Run local selected in ROS based Recognizer" sys.exit(1) else: self.bridge = CvBridge() def signal_handler(signal, frame): print ">> ROS Exiting" self.doRun = False signal.signal(signal.SIGINT, signal_handler) def image_callback(self, ros_data): try: cv_image = self.bridge.imgmsg_to_cv2(ros_data, "bgr8") except Exception, ex: print ex return # Resize the frame to half the original size for speeding up the detection process. # In ROS we can control the size, so we are sending a 320*240 image by default. img = cv2.resize(cv_image, (320, 240), interpolation=cv2.INTER_CUBIC) # img = cv2.resize(cv_image, (cv_image.shape[1] / 2, cv_image.shape[0] / 2), interpolation=cv2.INTER_CUBIC) # img = cv_image imgout = img.copy() # Remember the Persons found in current image persons = [] for _i, r in enumerate(self.detector.detect(img)): x0, y0, x1, y1 = r # (1) Get face, (2) Convert to grayscale & (3) resize to image_size: face = img[y0:y1, x0:x1] face = cv2.cvtColor(face, cv2.COLOR_BGR2GRAY) face = cv2.resize(face, self.model.image_size, interpolation=cv2.INTER_CUBIC) prediction = self.model.predict(face) predicted_label = prediction[0] classifier_output = prediction[1] # Now let's get the distance from the assuming a 1-Nearest Neighbor. # Since it's a 1-Nearest Neighbor only look take the zero-th element: distance = classifier_output['distances'][0] # Draw the face area in image: cv2.rectangle(imgout, (x0, y0), (x1, y1), (0, 0, 255), 2) # Draw the predicted name (folder name...): draw_str(imgout, (x0 - 20, y0 - 40), "Label " + self.model.subject_names[predicted_label]) draw_str(imgout, (x0 - 20, y0 - 20), "Feature Distance " + "%1.1f" % distance) msg = Person() point = Point() # Send the center of the person's bounding box mid_x = float(x1 + (x1 - x0) * 0.5) mid_y = float(y1 + (y1 - y0) * 0.5) point.x = mid_x point.y = mid_y # Z is "mis-used" to represent the size of the bounding box point.z = x1 - x0 msg.position = point msg.name = str(self.model.subject_names[predicted_label]) msg.reliability = float(distance) persons.append(msg) if len(persons) > 0: h = Header() h.stamp = rospy.Time.now() h.frame_id = '/ros_cam' msg = People() msg.header = h for p in persons: msg.people.append(p) self.rp.publisher.publish(msg) cv2.imshow('OCVFACEREC < ROS STREAM', imgout) cv2.waitKey(self.wait) try: z = self.ros_restart_request if z: self.restart = True self.doRun = False except Exception, e: pass
def __init__(self): rospy.init_node('yolo_detection_to_depth') rospy.on_shutdown(self.shutdown) # Displacement of the depth camera back from the front of the robot in meters self.camera_displacement = 0.612 self.display_depth_image = False self.base_link = rospy.get_param('~base_link', 'sibot/base_link') self.depth_frame = rospy.get_param('~depth_frame', 'sibot/camera_rgb_optical_frame') self.tfBuffer = tf2_ros.Buffer() tf_listener = tf2_ros.TransformListener(self.tfBuffer) # Keep a list of peope as a People message self.people = People() self.people.header.frame_id = self.base_link # Publish results as people_msgs/People messages self.people_pub = rospy.Publisher("people", People, queue_size=5) # Keep a list of peope poses as PoseArray message self.people_poses = PoseArray() self.people_poses.header.frame_id = self.base_link # Publish detections as a pose array self.people_poses_pub = rospy.Publisher("people_poses", PoseArray, queue_size=5) # Keep a list of people 3D bounding boxes as a BoundingBoxArray message self.people_bounding_boxes = BoundingBoxArray() self.people_bounding_boxes.header.frame_id = self.depth_frame # Publish detections as a JSK BoundingBoxArray for viewing in RViz self.people_bounding_boxes_pub = rospy.Publisher( "people_bounding_boxes", BoundingBoxArray, queue_size=5) # Keep a list of detected people using the Spencer DetectedPersons message type so we can display nice graphcis in RViz self.detected_persons = DetectedPersons() self.detected_persons.header.frame_id = self.base_link # Publish detections as a DetectedPersons array for viewing in RViz self.detected_persons_pub = rospy.Publisher("detected_persons", DetectedPersons, queue_size=5) # Publish person pointclouds #self.people_cloud_pub = rospy.Publisher("people_clouds", PointCloud2, queue_size=5) # Use message filters to time synchronize the bboxes and the pointcloud self.bbox_sub = message_filters.Subscriber("yolo_bboxes", bbox_array_stamped, queue_size=10) self.pointcloud_sub = message_filters.Subscriber('point_cloud', PointCloud2, queue_size=10) # Register the synchronized callback self.time_sync = message_filters.ApproximateTimeSynchronizer( [self.bbox_sub, self.pointcloud_sub], 10, 2) self.time_sync.registerCallback(self.get_bbox_cog) rospy.loginfo("Getting detections from YOLO...")
def modelCallback(states_msg): # People publisher ppl_pub = rospy.Publisher('people', People, queue_size=1) # Marker publisher marker_pub = rospy.Publisher('people_viz_arr', MarkerArray, queue_size=1) # List to hold the indexes of the people objects in the message ppl_idxs = [] # Loop through the names array to determine how many people objects are in the sim and their indexes for i in range(len(states_msg.name)): model_name = states_msg.name[i] # Check if the model name is that of person if model_name[:-2] == "person_standing": # Append the index to the list ppl_idxs.append(i) # # Marker array to hold the markers for each person # marker_list = MarkerArray() # # Create marker message and initialize with values for external person model # people_marker = Marker() # people_marker.header.frame_id = "map" # # people_marker.ns = "model" # people_marker.type = Marker.MESH_RESOURCE # people_marker.action = Marker.MODIFY # people_marker.mesh_resource = "package://social_nav_simulation/gazebo/models/human/meshes/standing.dae" # people_marker.mesh_use_embedded_materials = True # people_marker.scale.x = 1.0 # people_marker.scale.y = 1.0 # people_marker.scale.z = 1.0 # Create people and person msgs all_people = People() one_person = Person() # Loop through all the indexes of the people models found for i in range(len(ppl_idxs)): # Get the index of the person model idx = ppl_idxs[i] # Add the person's model name one_person.name = states_msg.name[idx] # Add the person's x and y position one_person.position.x = states_msg.pose[idx].position.x one_person.position.y = states_msg.pose[idx].position.y # Set name of marker # people_marker.ns = one_person.name # # Create marker at the person's position # people_marker.pose.position.x = one_person.position.x # people_marker.pose.position.y = one_person.position.y # people_marker.pose.orientation = states_msg.pose[idx].orientation # marker_list.markers.append(copy.deepcopy(people_marker)) # marker_pub.publish(people_marker) # Add the person's velocity one_person.velocity.x = states_msg.twist[idx].linear.x one_person.velocity.y = states_msg.twist[idx].linear.y # ! Need to append a deep copy of the person object or or else each one will get overwritten # by the last person object appeneded to the list person_to_append = copy.deepcopy(one_person) # Add the person to the people message all_people.people.append(person_to_append) # Add paraent frame and time to header all_people.header.frame_id = "map" all_people.header.stamp = rospy.Time.now() # Publish list of all people ppl_pub.publish(all_people)