You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
131 lines
4.3 KiB
131 lines
4.3 KiB
#!/usr/bin/env python2
|
|
# coding:utf-8
|
|
import rospy
|
|
|
|
from std_msgs.msg import String
|
|
from std_msgs.msg import Float32MultiArray
|
|
from std_msgs.msg import MultiArrayLayout
|
|
import sys
|
|
from python import sim
|
|
from python.sim import *
|
|
from policy.orca import ORCA
|
|
import numpy as np
|
|
import time
|
|
# TODO:
|
|
from utils.human import Human
|
|
from utils.robot import Robot
|
|
import logging
|
|
import argparse
|
|
import configparser
|
|
from numpy.linalg import norm
|
|
|
|
actCmd = list()
|
|
|
|
class Interpreter():
|
|
def __init__(self, config, robot):
|
|
self.robot = robot
|
|
self.config = config
|
|
# self.clientID = clientID
|
|
sub = rospy.Subscriber('chatter', Float32MultiArray, self.callback)
|
|
self.pub = rospy.Publisher('act_cmd', Float32MultiArray, queue_size=2)
|
|
# self.policy = ORCA()
|
|
# TODO: initialize RVO2-Vrep sim
|
|
self.time_step = 0.25
|
|
|
|
# set Robot
|
|
self.circle_radius = config.getfloat('sim', 'circle_radius')
|
|
self.discomfort_dist = config.getfloat('reward', 'discomfort_dist')
|
|
self.robot.set(0, -self.circle_radius, 0, self.circle_radius, 0, 0, np.pi / 2)
|
|
np.random.seed(1)
|
|
self.generate_random_human_position(human_num=5)
|
|
for agent in [self.robot] + self.humans:
|
|
agent.time_step = self.time_step
|
|
agent.policy.time_step = self.time_step
|
|
|
|
# 把后边可能用到的 sub, pub 在初始化函数中定义好
|
|
|
|
def callback(self, data):
|
|
if (len(data.data)!=0):
|
|
rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)
|
|
envState = list(data.data)
|
|
# TODO: procedure to transfer envState to actCmd, just the decision-making procedure
|
|
# prepare state info
|
|
retFloats = envState
|
|
for index, human in enumerate(self.humans):
|
|
human.set_position([retFloats[4 * (index + 1) + 0], retFloats[4 * (index + 1) + 1]])
|
|
human.set_velocity([retFloats[4 * (index + 1) + 2], retFloats[4 * (index + 1) + 3]])
|
|
pass
|
|
|
|
self.robot.set_position([retFloats[0], retFloats[1]])
|
|
self.robot.set_velocity([retFloats[2], retFloats[3]])
|
|
ob = [human.get_observable_state() for human in self.humans]
|
|
|
|
action = self.robot.act(ob)
|
|
# get act for each agent, generate actCmd
|
|
human_actions = []
|
|
for human in self.humans:
|
|
# observation for humans is always coordinates
|
|
ob = [other_human.get_observable_state() for other_human in self.humans if other_human != human]
|
|
human_actions.append(human.act(ob))
|
|
theta = np.arctan2(action.vy, action.vx)
|
|
velocity = np.hypot(action.vx, action.vy)
|
|
all_actions = [velocity, theta]
|
|
for human_action in human_actions:
|
|
theta = np.arctan2(human_action.vy, human_action.vx)
|
|
velocity = np.hypot(human_action.vx, human_action.vy)
|
|
all_actions.extend([velocity, theta])
|
|
pass
|
|
actCmd = all_actions #[1.0 for i in range(12)]
|
|
data = Float32MultiArray()
|
|
# retFloats = [round(i,4) for i in retFloats]
|
|
data.data = actCmd
|
|
# data.layout = MultiArrayLayout()
|
|
self.pub.publish(data)
|
|
|
|
def generate_random_human_position(self, human_num):
|
|
self.humans = []
|
|
for i in range(human_num):
|
|
self.humans.append(self.generate_circle_crossing_human())
|
|
|
|
def generate_circle_crossing_human(self):
|
|
human = Human(self.config, 'humans')
|
|
while True:
|
|
angle = np.random.random() * np.pi * 2
|
|
# add some noise to simulate all the possible cases robot could meet with human
|
|
px_noise = (np.random.random() - 0.5) * human.v_pref
|
|
py_noise = (np.random.random() - 0.5) * human.v_pref
|
|
px = self.circle_radius * np.cos(angle) + px_noise
|
|
py = self.circle_radius * np.sin(angle) + py_noise
|
|
collide = False
|
|
for agent in [self.robot] + self.humans:
|
|
min_dist = human.radius + agent.radius + self.discomfort_dist
|
|
if norm((px - agent.px, py - agent.py)) < min_dist or \
|
|
norm((px - agent.gx, py - agent.gy)) < min_dist:
|
|
collide = True
|
|
break
|
|
if not collide:
|
|
break
|
|
human.set(px, py, -px, -py, 0, 0, 0)
|
|
return human
|
|
|
|
if __name__ == '__main__':
|
|
print('Vrep Sim Program started 2')
|
|
# configure environment
|
|
parser = argparse.ArgumentParser('Parse configuration file')
|
|
parser.add_argument('--env_config', type=str, default='env.config')
|
|
args = parser.parse_args()
|
|
env_config = configparser.RawConfigParser()
|
|
env_config.read(args.env_config)
|
|
|
|
robot = Robot(env_config, 'robot')
|
|
|
|
rospy.init_node('interpreter')
|
|
try:
|
|
Interpreter(env_config, robot)
|
|
except rospy.ROSInterruptException:
|
|
pass
|
|
# rate = rospy.Rate(250)
|
|
# while not rospy.is_shutdown():
|
|
# rate.sleep()
|
|
rospy.spin()
|