def __init__(self):
self.num_poses = 0
self.sub_pose = rospy.Subscriber('/odom', Odometry, self.cbPose)
self.pub_obst_odom = rospy.Publisher('/obst_odom',
Clusters,
queue_size=1)
# static
# self.sub_pose = rospy.Subscriber('/flatland_server/debug/layer/static',MarkerArray,self.getStaticMap)
self.obstacles = {}
self.num_obst = 0
self.cluster = Clusters()
self.add_obst = True
# obst vel
self.vel = Twist()
self.vel.angular.z = 0.3
self.vel.linear.x = 0.1
self.radius = 0.5
# static map
self.static_map_obst = Clusters()
self.pub_timer = rospy.Timer(rospy.Duration(0.1), self.pubOdom)
def cbPose(self, msg):
# get all topics
topics = rospy.get_published_topics()
obst_ns = "/obstacle_"
obst_topics = []
# filter topics with ns (obst)
for t_list in topics:
for t in t_list:
if obst_ns in t and "odom" in t:
obst_topics.append(t)
# add obst topic to dictionary
if self.add_obst:
for topic in obst_topics:
rospy.Subscriber(topic,Odometry,self.cbLog, topic)
v_topic = topic.replace("odometry/ground_truth", "cmd_vel")
# publish velocity to move obstacles
# self.pubVel(v_topic,self.vel)
# rospy.sleep(0.2)
self.add_obst = False
# reset cluster
self.cluster = Clusters()
# fill cluster with obstacle odom
for i in self.obstacles:
tmp_point = Point()
tmp_point.x = self.obstacles[i].pose.pose.position.x
tmp_point.y = self.obstacles[i].pose.pose.position.y
tmp_vel = self.obstacles[i].twist.twist.linear
self.cluster.mean_points.append(tmp_point)
self.cluster.velocities.append(tmp_vel)
self.cluster.labels.append(self.obstacles[i].header.seq)
self.num_poses += 1
self.pub_obst_odom.publish(self.cluster)
self.add_obst = True
if self.num_obst != len(self.obstacles):
self.num_obst = len(self.obstacles)
print("========================================")
print("Number of obstacles: "+str(self.num_obst))
for t in obst_topics:
print(t)
def __init__(self):
self.num_poses = 0
self.sub_pose = rospy.Subscriber('/odom',Odometry,self.cbPose)
# self.pub_pose_marker = rospy.Publisher('/obst_marker',Marker,queue_size=1)
self.pub_obst_odom = rospy.Publisher('/obst_odom',Clusters,queue_size=1)
self.obstacles = {}
self.num_obst = 0
self.cluster = Clusters()
self.add_obst = True
self.vel = Twist()
self.vel.angular.z = 0.1
self.vel.linear.x = 0.12
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 __init__(self, veh_name, veh_data, nn, actions):
#tb3
self.obst_rad = 0.5
# canon
self.node_name = rospy.get_name()
self.prev_other_agents_state = []
# vehicle info
self.veh_name = veh_name
self.veh_data = veh_data
# neural network
self.nn = nn
self.actions = actions
# self.value_net = value_net
self.operation_mode = PlannerMode()
self.operation_mode.mode = self.operation_mode.NN
# for subscribers
self.pose = PoseStamped()
self.vel = Vector3()
self.psi = 0.0
self.ped_traj_vec = []
self.other_agents_state = []
# for publishers
self.global_goal = PoseStamped()
self.goal = PoseStamped()
self.goal.pose.position.x = veh_data['goal'][0]
self.goal.pose.position.y = veh_data['goal'][1]
self.desired_position = PoseStamped()
self.desired_action = np.zeros((2,))
# handle obstacles close to vehicle's front
self.stop_moving_flag = False
self.d_min = 0.2
self.new_global_goal_received = False
# visualization
self.path_marker = Marker()
# Clusters
self.prev_clusters = Clusters()
self.current_clusters = Clusters()
# subscribers and publishers
self.num_poses = 0
self.num_actions_computed = 0.0
self.pub_others = rospy.Publisher('~other_vels',Vector3,queue_size=1)
self.pub_twist = rospy.Publisher('~nn_cmd_vel',Twist,queue_size=1)
self.pub_pose_marker = rospy.Publisher('~pose_marker',Marker,queue_size=1)
self.pub_agent_marker = rospy.Publisher('~agent_marker',Marker,queue_size=1)
self.pub_agent_markers = rospy.Publisher('~agent_markers',MarkerArray,queue_size=1)
self.pub_path_marker = rospy.Publisher('~path_marker',Marker,queue_size=1)
self.pub_goal_path_marker = rospy.Publisher('~goal_path_marker',Marker,queue_size=1)
# sub
self.sub_pose = rospy.Subscriber('~pose',Odometry,self.cbPose)
self.sub_mode = rospy.Subscriber('~mode',PlannerMode, self.cbPlannerMode)
self.sub_global_goal = rospy.Subscriber('~goal',PoseStamped, self.cbGlobalGoal)
self.sub_subgoal = rospy.Subscriber('~subgoal',PoseStamped, self.cbSubGoal)
# subgoals
self.sub_goal = Vector3()
self.use_clusters = True
# self.use_clusters = False
if self.use_clusters:
self.sub_clusters = rospy.Subscriber('~clusters',Clusters, self.cbClusters)
else:
self.sub_peds = rospy.Subscriber('~peds',PedTrajVec, self.cbPeds)
# control timer
self.control_timer = rospy.Timer(rospy.Duration(0.01),self.cbControl)
self.nn_timer = rospy.Timer(rospy.Duration(0.1),self.cbComputeActionGA3C)
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)