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)
