def run():
print 'hello world from cadrl_node.py'
file_dir = os.path.dirname(os.path.realpath(__file__))
plt.rcParams.update({'font.size': 18})
rospack = rospkg.RosPack()
a = network.Actions()
actions = a.actions
num_actions = a.num_actions
nn = network.NetworkVP_rnn(network.Config.DEVICE, 'network', num_actions)
nn.simple_load(
rospack.get_path('cadrl_ros') + '/checkpoints/network_01900000')
rospy.init_node('nn_jackal', anonymous=False)
veh_name = 'JA01'
pref_speed = rospy.get_param("~jackal_speed")
veh_data = {
'goal': np.zeros((2, )),
'radius': 0.5,
'pref_speed': pref_speed,
'kw': 10.0,
'kp': 1.0,
'name': 'JA01'
}
print "********\n*******\n*********\nJackal speed:", pref_speed, "\n**********\n******"
nn_jackal = NN_jackal(veh_name, veh_data, nn, actions)
rospy.on_shutdown(nn_jackal.on_shutdown)
rospy.spin()
def run():
rospack = rospkg.RosPack()
a = network.Actions()
actions = a.actions
num_actions = a.num_actions
nn = network.NetworkVP_rnn(network.Config.DEVICE, 'network', num_actions)
nn.simple_load(
rospack.get_path('cadrl_ros') + '/checkpoints/network_01900000')
rospy.init_node('cadrl_tb3')
tb3_nav = NN_tb3(nn, actions)
rospy.spin()
def control(self, event):
possible_actions = network.Actions()
num_actions = possible_actions.num_actions
nn = network.NetworkVP_rnn(network.Config.DEVICE, 'network',
num_actions)
nn.simple_load(
'/home/yeguo/turtlebot3_ws/src/cadrl_ros/checkpoints/network_01900000'
)
host_agent = agent.Agent(self.host_pos_x, self.host_pos_y,
self.host_goal_x, self.host_goal_y,
self.radius, self.pref_speed,
self.host_heading_angle, 0)
host_agent.vel_global_frame = np.array([self.host_vx, self.host_vy])
other_agents = []
#other_agents.append(agent.Agent(self.tb3_0_pos_x, self.tb3_0_pos_y, self.tb3_0_goal_x, self.tb3_0_goal_y, self.radius, self.pref_speed, self.other_agents_heading_angle, 1))
other_agents.append(
agent.Agent(self.tb3_1_pos_x, self.tb3_1_pos_y, self.tb3_1_goal_x,
self.tb3_1_goal_y, self.radius, self.pref_speed,
self.other_agents_heading_angle, 2))
#other_agents.append(agent.Agent(self.tb3_2_pos_x, self.tb3_2_pos_y, self.tb3_2_goal_x, self.tb3_2_goal_y, self.radius, self.pref_speed, self.other_agents_heading_angle, 3))
obs = host_agent.observe(other_agents)[1:]
obs = np.expand_dims(obs, axis=0)
predictions = nn.predict_p(obs, None)[0]
raw_action = possible_actions.actions[np.argmax(predictions)]
action = np.array([
host_agent.pref_speed * raw_action[0],
util.wrap(raw_action[1] + host_agent.heading_global_frame)
])
print "action:", action
command = Twist()
command.linear.x = action[0]
yaw_error = action[1] - self.psi
if yaw_error > np.pi:
yaw_error -= 2 * np.pi
if yaw_error < -np.pi:
yaw_error += 2 * np.pi
command.angular.z = 2 * yaw_error
print "host command:", command
self.host_cmd_vel_pub.publish(command)
#control agents
tb3_0_command = Twist()
tb3_0_command.linear.x = 0.05
self.tb3_0_cmd_vel_pub.publish(tb3_0_command)
tb3_1_command = Twist()
tb3_1_command.linear.x = 0.05
self.tb3_1_cmd_vel_pub.publish(tb3_1_command)
tb3_2_command = Twist()
tb3_2_command.linear.x = 0.05
self.tb3_2_cmd_vel_pub.publish(tb3_2_command)
def __init__(self, nn, actions):
# tb3 -------------------------------------
# radius
self.radius = 0.5
# goal
self.global_goal = PoseStamped()
self.sub_goal = PoseStamped()
self.pref_speed = 0.14
#pose
self.pose = PoseStamped().pose
# heading_angle
self.psi = 0.0
#vel
self.vel = Vector3()
self.t_past = 0
#
self.new_global_goal_received = False
# obstacles
self.other_agents_state = []
self.possible_actions = network.Actions()
self.num_actions = self.possible_actions.num_actions
# load nn
self.nn = nn
self.actions = actions
# subscribe
self.odom_sub = rospy.Subscriber('/odom', Odometry, self.updatePose)
self.sub_global_goal = rospy.Subscriber('/move_base_simple/goal',
PoseStamped,
self.updateGlobalGoal)
# publisher
self.pub_twist = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
# print self.global_goal
# self.global_goal.pose = self.pose
# loop
self.updateOtherAgents()
self.nn_timer = rospy.Timer(rospy.Duration(0.1), self.cbComputeAction)
def run(self):
#Load network weights and structure
possible_actions = network.Actions()
actions = possible_actions.actions
num_actions = possible_actions.num_actions
nn = network.NetworkVP_rnn(network.Config.DEVICE, 'network',
num_actions)
nn.simple_load(
'/home/smile/smile-mobile/smile_mobile_sim_ws/src/smile_mobile_dynamic/scripts/cadrl/checkpoints/network_01900000'
)
#Load parameters from json file
with open('../parameters/parameters_configurations.json'
) as parameter_file:
parameters = json.load(parameter_file)
#Load parameters of host agent
host_goal = parameters['host_goal']
host_radius = parameters['host_radius']
host_pref_speed = parameters['host_pref_speed']
host_initial = parameters['host_initial']
path_name = '/home/smile/smile-mobile/smile_mobile_sim_ws/src/smile_mobile_dynamic/scripts/query_correction/final_path.txt'
#Load paramters of other agent
other_robot_name = parameters['other_name']
other_initial = parameters['other_initial']
other_goal = parameters['other_goal']
other_radius = parameters['other_radius']
other_pref_speed = parameters['other_pref_speed']
index = 0
husky_robot = HuskyRosSimulator(host_initial, host_goal, host_radius,
host_pref_speed, index, nn, actions,
path_name, other_robot_name,
other_initial, other_goal,
other_radius, other_pref_speed)
rospy.spin()
def run():
file_dir = os.path.dirname(os.path.realpath(__file__))
plt.rcParams.update({'font.size': 18})
rospack = rospkg.RosPack()
a = network.Actions()
actions = a.actions
num_actions = a.num_actions
nn = network.NetworkVP_rnn(network.Config.DEVICE, 'network', num_actions)
nn.simple_load(rospack.get_path('cadrl_ros')+'/checkpoints/network_01900000')
rospy.init_node('nn_tb3',anonymous=False)
veh_name = 'tb3_01'
pref_speed = rospy.get_param("~tb3_speed")
veh_data = {'goal':np.zeros((2,)),'radius':0.5,'pref_speed':pref_speed,'kw':10.0,'kp':1.0,'name':'tb3_01'}
print('==================================\ncadrl node started')
print("tb3 speed:", pref_speed, "\n==================================")
nn_tb3 = NN_tb3(veh_name, veh_data, nn, actions)
rospy.on_shutdown(nn_tb3.on_shutdown)
rospy.spin()
def __init__(self, radius=0.5):
rospack = rospkg.RosPack()
self.a = network.Actions()
self.num_actions = self.a.num_actions
self.nn = network.NetworkVP_rnn(network.Config.DEVICE, 'network',
self.num_actions)
self.nn.simple_load(
rospack.get_path('obstacle_avoid') +
'/checkpoints/network_01900000')
#Publisher
self.pub_twist = rospy.Publisher('mitsumi_twist', Twist, queue_size=1)
self.pub_pose_marker = rospy.Publisher('mitsumi_pose_marker',
Marker,
queue_size=1)
self.pub_agent_markers = rospy.Publisher('mitsumi_agent_markers',
MarkerArray,
queue_size=1)
#Subscriber
self.sub_pose = rospy.Subscriber('mitsumi_pose', PoseStamped,
self.cbPose)
self.sub_global_goal = rospy.Subscriber('mitsumi_goal', PoseStamped,
self.cbGoal)
self.sub_vel = rospy.Subscriber('mitsumi_velocity', Vector3,
self.cbVel)
self.sub_obstacles = rospy.Subscriber('mitsumi_obstacles', AgentList,
self.cbObstacles)
self.heading_angle = 0
self.start_x = 0
self.start_y = 0
self.goal_x = 10
self.goal_y = 3
self.radius = radius
self.pref_speed = 1.2
self.velocity_x = 0
self.velocity_y = 0
self.nn_timer = rospy.Timer(rospy.Duration(0.1), self.start)
self.num_poses = 1
self.other_agents_x = []
self.other_agents_y = []
self.other_agents_r = []
self.other_agents_vx = []
self.other_agents_vy = []
agentList = AgentList()
sample = Agent()
sample.position.x = 4
sample.position.y = 2
sample.radius = 0.5
sample.velocity.x = 0
sample.velocity.y = 0
agentList.list.append(sample)
sample2 = Agent()
sample2.position.x = 7
sample2.position.y = -1
sample2.radius = 0.5
sample2.velocity.x = 0
sample2.velocity.y = 0
agentList.list.append(sample2)
sample3 = Agent()
sample3.position.x = 6
sample3.position.y = 1.5
sample3.radius = 0.5
sample3.velocity.x = 0
sample3.velocity.y = 0
agentList.list.append(sample3)
self.cbObstacles(agentList)
def __init__(self, ros_port, envIndex):
#load trained network
self.possible_actions = network.Actions()
self.num_actions = self.possible_actions.num_actions
self.nn = network.NetworkVP_rnn(network.Config.DEVICE, 'network',
self.num_actions)
self.nn.simple_load('../checkpoints/network_01900000')
os.environ["ROS_MASTER_URI"] = "http://localhost:%d" % (ros_port)
rospy.init_node("cadrl_stage", anonymous=None)
rospy.sleep(1.)
#-----------------subscriber and publisher----------#
roboIndex = 0
self.state = [0.0, 0.0, 1.0]
self.speed = [0.0, 0.0]
odom_topic = 'robot_' + str(roboIndex) + '/odom'
self.odom_sub = rospy.Subscriber(odom_topic, Odometry,
self.odometry_callback)
cmd_vel_topic = 'robot_' + str(roboIndex) + '/cmd_vel'
self.cmd_vel = rospy.Publisher(cmd_vel_topic, Twist, queue_size=10)
self.reach_flag = Int8()
self.reach_flag.data = 8
reach_topic = 'robot_' + str(roboIndex) + '/is_reached'
self.reach_publisher = rospy.Publisher(reach_topic, Int8, queue_size=2)
crash_topic = 'robot_' + str(roboIndex) + '/is_crashed'
self.check_crash = rospy.Subscriber(crash_topic, Int8,
self.crash_callback)
goal_topic = 'robot_' + str(roboIndex) + '/goal_pose'
self.goal_pub = rospy.Publisher(goal_topic, Pose, queue_size=10)
self.state_ad1 = [0.0, 0.0, 1.0]
self.speed_ad1 = [0.0, 0.0]
odom_topic_ad1 = 'robot_' + str(1) + '/odom'
self.odom_sub_ad1 = rospy.Subscriber(odom_topic_ad1, Odometry,
self.odometry_callback_ad1)
self.state_ad2 = [0.0, 0.0, 1.0]
self.speed_ad2 = [0.0, 0.0]
odom_topic_ad2 = 'robot_' + str(2) + '/odom'
self.odom_sub_ad2 = rospy.Subscriber(odom_topic_ad2, Odometry,
self.odometry_callback_ad2)
self.state_ad3 = [0.0, 0.0, 1.0]
self.speed_ad3 = [0.0, 0.0]
odom_topic_ad3 = 'robot_' + str(3) + '/odom'
self.odom_sub_ad3 = rospy.Subscriber(odom_topic_ad3, Odometry,
self.odometry_callback_ad3)
self.reset_stage = rospy.ServiceProxy('reset_positions', Empty)
self.reset_stage()
rospy.sleep(1.)
self.goal_x = 0.0
self.goal_y = 0.0
self.envIndex = envIndex
#!/usr/bin/env python
import agent
import network
import util
import numpy as np
possible_actions = network.Actions()
num_actions = possible_actions.num_actions
print 'Num Actions:', num_actions
nn = network.NetworkVP_rnn(network.Config.DEVICE, 'network', num_actions)
nn.simple_load('../checkpoints/network_01900000')
start_x = 2
start_y = 5
goal_x = 3
goal_y = 2
radius = 0.5
pref_speed = 1.2
heading_angle = 0
index = 0
v_x = 0
v_y = 0
host_agent = agent.Agent(start_x, start_y, goal_x, goal_y, radius, pref_speed,
heading_angle, index)
host_agent.vel_global_frame = np.array([v_x, v_y])
# Sample observation data in a format easily generated from sensors``
other_agents_x = [-1, -2, -3]
other_agents_y = [2, 3, 4]