def run():
print 'hello world from pub_agents.py'
rospy.init_node('pub_agents', anonymous=False)
pub_clusters = rospy.Publisher('/JA01/clusters', Clusters, queue_size=10)
ego = EgoAgent()
init_angle = np.random.random(4) * 2 * 3.14159
sizes = random.sample([0.5, 0.5, 0.4, 0.4, 0.3, 0.3], NUM_CLUSTERS)
# print init_angle
pos = Vector3()
vel = Vector3()
for id in range(NUM_CLUSTERS):
labels.append(id)
pos = Vector3(RADIUS * np.cos(init_angle[id]),
RADIUS * np.sin(init_angle[id]), 0.0)
mean_points.append(pos)
vel = Vector3(random.uniform(0.1, 0.5), random.uniform(0, 3.14159 * 2),
0.0)
velocities.append(vel)
min_points.append(
Vector3(pos.x - sizes[id] / 2.0, pos.y - sizes[id] / 2.0, 0.0))
max_points.append(
Vector3(pos.x + sizes[id] / 2.0, pos.y + sizes[id] / 2.0, 0.0))
cluster = Clusters()
rate = rospy.Rate(1 / DT)
while not rospy.is_shutdown():
for id in range(NUM_CLUSTERS):
update(mean_points[id], min_points[id], max_points[id],
velocities[id])
cluster.labels = labels
cluster.velocities = velocities
cluster.mean_points = mean_points
cluster.min_points = min_points
cluster.max_points = max_points
pub_clusters.publish(cluster)
rate.sleep()
rospy.spin()
def run(self):
rospy.init_node('SampleNode', anonymous=True)
rate = rospy.Rate(10)
postTopic = rospy.Publisher('mitsumi_pose', PoseStamped, queue_size=1)
goalTopic = rospy.Publisher('mitsumi_goal', PoseStamped, queue_size=1)
velocityTopic = rospy.Publisher('mitsumi_velocity',
Vector3,
queue_size=1)
clustersTopic = rospy.Publisher('mitsumi_clusters',
Clusters,
queue_size=1)
actionsTopic = rospy.Publisher('mitsumi_safe_actions',
NNActions,
queue_size=1)
modeTopic = rospy.Publisher('mitsumi_mode', PlannerMode, queue_size=1)
mode = PlannerMode()
mode.mode = mode.NN
actions = NNActions()
# self.value_net = value_net
self.operation_mode = PlannerMode()
self.operation_mode.mode = self.operation_mode.NN
rospy.Subscriber('mitsumi_nn_cmd_vel', Twist, self.twisted)
pose = Pose()
pose.position.x = 2
pose.position.y = 5
pose.position.z = 0
sampleMessage = PoseStamped()
sampleMessage.pose = pose
goalpose = Pose()
goalpose.position.x = 5
goalpose.position.y = 5
goalpose.position.z = 0
goalMessage = PoseStamped()
goalMessage.pose = goalpose
velocity = Vector3()
velocity.x = 0.5
velocity.y = 0.6
cluster_all = Clusters()
cluster_all.labels = [0, 1, 2]
cluster_all.min_points = [
Point(1, 1, 0), Point(3, 1, 0),
Point(5, 1, 0)
]
cluster_all.mean_points = [
Point(1, 2, 0), Point(3, 2, 0),
Point(5, 2, 0)
]
cluster_all.max_points = [
Point(1, 3, 0), Point(3, 3, 0),
Point(5, 3, 0)
]
cluster_all.velocities = [
Vector3(0, 0, 0),
Vector3(0, 0, 0),
Vector3(0, 0, 0)
]
index = 0
whatever = True
while not rospy.is_shutdown() and not self.stop:
if (index % 100 == 0 or whatever):
postTopic.publish(sampleMessage)
clustersTopic.publish(cluster_all)
if (index % 200 == 0 or whatever):
goalTopic.publish(goalMessage)
if (index == 50 or whatever):
velocityTopic.publish(velocity)
actionsTopic.publish(actions)
modeTopic.publish(mode)
index = index + 1
# print(index)
rate.sleep()
def peds_callback(self, peds_msg):
peds_cluster = Clusters()
peds_cluster.header.stamp = rospy.Time.now()
peds_cluster.header.frame_id = "map"
peds_cluster.labels = []
peds_cluster.counts = []
peds_cluster.mean_points = []
peds_cluster.max_points = []
peds_cluster.min_points = []
peds_cluster.pointclouds = []
peds_cluster.velocities = []
peds_cluster.num.data = int(len(peds_msg.detections))
#markers = MarkerArray()
#print(len(peds_msg.detections))
if len(peds_msg.detections) > 0:
for i in range(len(peds_msg.detections)):
t = self.tf_listener.getLatestCommonTime(
"/map", "/base_footprint")
pose_in_baseframe = PointStamped()
pose_in_baseframe.header.frame_id = "base_footprint"
pose_in_baseframe.header.stamp = rospy.Time.now()
pose_in_baseframe.point = peds_msg.detections[
i].pose.pose.position
pose_in_map = PointStamped()
pose_in_map.header.frame_id = "map"
pose_in_map.header.stamp = rospy.Time.now()
pose_in_map = self.tf_listener.transformPoint(
"/map", pose_in_baseframe)
peds_cluster.mean_points.append(pose_in_map.point)
ped_vel = Vector3()
ped_vel.x = 0.4
ped_vel.y = 0.4
ped_vel.z = 0.0
peds_cluster.velocities.append(ped_vel)
peds_cluster.labels.append(i)
'''
marker = Marker()
marker.header.stamp = rospy.Time.now()
marker.header.frame_id = 'map'
marker.ns = 'other_agent'
marker.id = i
marker.type = marker.CYLINDER
marker.action = marker.ADD
marker.pose.position.x = pose_in_map.point.x
marker.pose.position.y = pose_in_map.point.y
# marker.pose.orientation = orientation
marker.scale = Vector3(x=0.2,y=0.2,z=0.2)
marker.color = ColorRGBA(r=1.0,g=0.4,a=1.0)
marker.lifetime = rospy.Duration(0.1)
markers.markers.append(marker)
'''
self.clusters_pub.publish(peds_cluster)
#self.pub_agent_markers.publish(markers)
goal = PoseStamped()
goal.header.stamp = rospy.Time.now()
goal.header.frame_id = "map"
goal.pose.position.x = 5.0
goal.pose.position.y = 0.0
self.goal_pub.publish(goal)