xyz
5 years ago
commit
77c9e6d63d
@ -0,0 +1,13 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<classpath>
|
||||
<classpathentry kind="src" path="src"/>
|
||||
<classpathentry kind="lib" path="lib/commons-codec/commons-codec-1.3.jar"/>
|
||||
<classpathentry kind="lib" path="lib/freemarker.jar"/>
|
||||
<classpathentry kind="lib" path="lib/jrosbridge/javax.json-1.0.4.jar"/>
|
||||
<classpathentry kind="lib" path="lib/jrosbridge/javax.websocket-api-1.0-rc4.jar"/>
|
||||
<classpathentry kind="lib" path="lib/jrosbridge/javax.websocket-api-1.0.jar"/>
|
||||
<classpathentry kind="lib" path="lib/jrosbridge/tyrus-standalone-client-1.9.jar"/>
|
||||
<classpathentry kind="con" path="org.eclipse.jdt.launching.JRE_CONTAINER"/>
|
||||
<classpathentry kind="lib" path="lib/substance5.2.jar"/>
|
||||
<classpathentry kind="output" path="bin"/>
|
||||
</classpath>
|
||||
@ -0,0 +1,5 @@
|
||||
/bin/*
|
||||
!/bin/icon1/*
|
||||
!/bin/Icon/*
|
||||
/null
|
||||
/src/Shells/build
|
||||
@ -0,0 +1,17 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<projectDescription>
|
||||
<name>AutoRobotV1</name>
|
||||
<comment></comment>
|
||||
<projects>
|
||||
</projects>
|
||||
<buildSpec>
|
||||
<buildCommand>
|
||||
<name>org.eclipse.jdt.core.javabuilder</name>
|
||||
<arguments>
|
||||
</arguments>
|
||||
</buildCommand>
|
||||
</buildSpec>
|
||||
<natures>
|
||||
<nature>org.eclipse.jdt.core.javanature</nature>
|
||||
</natures>
|
||||
</projectDescription>
|
||||
@ -0,0 +1,7 @@
|
||||
# AutoRobot-v1
|
||||
|
||||
两台电脑利用Ros通信(从机订阅主机的topic信息,从而观察主机工作状态):
|
||||
https://blog.csdn.net/qq_42037180/article/details/81144561
|
||||
|
||||
ROS多个master消息互通:
|
||||
https://www.cnblogs.com/shhu1993/p/6021396.html
|
||||
@ -0,0 +1,5 @@
|
||||
#Update SSHPath name
|
||||
#Wed Feb 10 15:04:33 CST 2021
|
||||
WorkPath=/home/xyz/workspace/ars_ws
|
||||
RosPath=/opt/ros/melodic
|
||||
SSHPath=huawei@202.197.85.29
|
||||
Binary file not shown.
@ -0,0 +1,130 @@
|
||||
#!/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()
|
||||
@ -0,0 +1,37 @@
|
||||
[env]
|
||||
time_limit = 45
|
||||
time_step = 0.3
|
||||
val_size = 10
|
||||
test_size = 100
|
||||
randomize_attributes = false
|
||||
|
||||
|
||||
[reward]
|
||||
success_reward = 1
|
||||
collision_penalty = -0.25
|
||||
discomfort_dist = 0.1
|
||||
discomfort_penalty_factor = 0.5
|
||||
|
||||
|
||||
[sim]
|
||||
train_val_sim = circle_crossing
|
||||
test_sim = circle_crossing
|
||||
square_width = 5
|
||||
circle_radius = 2
|
||||
human_num = 5
|
||||
|
||||
|
||||
[humans]
|
||||
visible = true
|
||||
policy = orca
|
||||
radius = 0.15
|
||||
v_pref = 0.5
|
||||
sensor = coordinates
|
||||
|
||||
|
||||
[robot]
|
||||
visible = false
|
||||
policy = orca
|
||||
radius = 0.15
|
||||
v_pref = 0.5
|
||||
sensor = coordinates
|
||||
@ -0,0 +1,53 @@
|
||||
#!/usr/bin/env python2
|
||||
import rospy
|
||||
|
||||
from std_msgs.msg import String
|
||||
from std_msgs.msg import Float32MultiArray
|
||||
from std_msgs.msg import MultiArrayLayout
|
||||
import sys
|
||||
sys.path.append ('./python')
|
||||
import sim
|
||||
from sim import *
|
||||
|
||||
actCmd = list()
|
||||
|
||||
def callback(data):
|
||||
if (len(data.data)!=0):
|
||||
rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)
|
||||
actCmd = list(data.data)
|
||||
# print(actCmd)
|
||||
res, retInts, retFloats, retStrings, retBuffer = sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'setRobotVel', [],actCmd, [], '',sim.simx_opmode_blocking)
|
||||
|
||||
|
||||
def listener():
|
||||
sub = rospy.Subscriber('act_cmd', Float32MultiArray, callback)
|
||||
|
||||
rospy.init_node('listener', anonymous=False)
|
||||
|
||||
rate = rospy.Rate(250)
|
||||
|
||||
while not rospy.is_shutdown():
|
||||
rate.sleep()
|
||||
|
||||
if __name__ == '__main__':
|
||||
print('Vrep Sim Program started 2')
|
||||
sim.simxFinish(-1) # just in case, close all opened connections
|
||||
clientID = sim.simxStart('127.0.0.1', 19997, True, True, 5000, 5) # Connect to CoppeliaSim
|
||||
if clientID != -1:
|
||||
print('Connected to remote API server 2')
|
||||
# Now try to retrieve data in a blocking fashion (i.e. a service call):
|
||||
res, objs = sim.simxGetObjects(clientID, sim.sim_handle_all, sim.simx_opmode_blocking)
|
||||
if res == sim.simx_return_ok:
|
||||
print('Number of objects in the scene 2: ', len(objs))
|
||||
else:
|
||||
print('Remote API function call returned with error code: ', res)
|
||||
# time.sleep(2)
|
||||
else:
|
||||
print('Failed connecting to remote API server')
|
||||
listener()
|
||||
|
||||
#
|
||||
# rostopic pub -1 /act_cmd std_msgs/Float32MultiArray -- "{\"data\":[0.30720001459121704,0.00019999999494757503],\"layout\":{\"dim\":[],\"data_offset\":0}}"
|
||||
|
||||
|
||||
# "{\"data\":[0.30720001459121704,0.00019999999494757503],\"layout\":{\"dim\":[],\"data_offset\":0}}"
|
||||
@ -0,0 +1,133 @@
|
||||
import numpy as np
|
||||
import rvo2
|
||||
from policy import Policy
|
||||
import sys
|
||||
from utils.action import ActionXY
|
||||
|
||||
|
||||
class ORCA(Policy):
|
||||
def __init__(self):
|
||||
"""
|
||||
timeStep The time step of the simulation.
|
||||
Must be positive.
|
||||
neighborDist The default maximum distance (center point
|
||||
to center point) to other agents a new agent
|
||||
takes into account in the navigation. The
|
||||
larger this number, the longer the running
|
||||
time of the simulation. If the number is too
|
||||
low, the simulation will not be safe. Must be
|
||||
non-negative.
|
||||
maxNeighbors The default maximum number of other agents a
|
||||
new agent takes into account in the
|
||||
navigation. The larger this number, the
|
||||
longer the running time of the simulation.
|
||||
If the number is too low, the simulation
|
||||
will not be safe.
|
||||
timeHorizon The default minimal amount of time for which
|
||||
a new agent's velocities that are computed
|
||||
by the simulation are safe with respect to
|
||||
other agents. The larger this number, the
|
||||
sooner an agent will respond to the presence
|
||||
of other agents, but the less freedom the
|
||||
agent has in choosing its velocities.
|
||||
Must be positive.
|
||||
timeHorizonObst The default minimal amount of time for which
|
||||
a new agent's velocities that are computed
|
||||
by the simulation are safe with respect to
|
||||
obstacles. The larger this number, the
|
||||
sooner an agent will respond to the presence
|
||||
of obstacles, but the less freedom the agent
|
||||
has in choosing its velocities.
|
||||
Must be positive.
|
||||
radius The default radius of a new agent.
|
||||
Must be non-negative.
|
||||
maxSpeed The default maximum speed of a new agent.
|
||||
Must be non-negative.
|
||||
velocity The default initial two-dimensional linear
|
||||
velocity of a new agent (optional).
|
||||
|
||||
ORCA first uses neighborDist and maxNeighbors to find neighbors that need to be taken into account.
|
||||
Here set them to be large enough so that all agents will be considered as neighbors.
|
||||
Time_horizon should be set that at least it's safe for one time step
|
||||
|
||||
In this work, obstacles are not considered. So the value of time_horizon_obst doesn't matter.
|
||||
|
||||
"""
|
||||
super(ORCA, self).__init__()
|
||||
self.name = 'ORCA'
|
||||
self.trainable = False
|
||||
self.multiagent_training = None
|
||||
self.kinematics = 'holonomic'
|
||||
self.safety_space = 0
|
||||
self.neighbor_dist = 10
|
||||
self.max_neighbors = 10
|
||||
self.time_horizon = 5
|
||||
self.time_horizon_obst = 5
|
||||
self.radius = 0.15
|
||||
self.max_speed = 0.5
|
||||
self.sim = None
|
||||
|
||||
def configure(self, config):
|
||||
# self.time_step = config.getfloat('orca', 'time_step')
|
||||
# self.neighbor_dist = config.getfloat('orca', 'neighbor_dist')
|
||||
# self.max_neighbors = config.getint('orca', 'max_neighbors')
|
||||
# self.time_horizon = config.getfloat('orca', 'time_horizon')
|
||||
# self.time_horizon_obst = config.getfloat('orca', 'time_horizon_obst')
|
||||
# self.radius = config.getfloat('orca', 'radius')
|
||||
# self.max_speed = config.getfloat('orca', 'max_speed')
|
||||
return
|
||||
|
||||
def set_phase(self, phase):
|
||||
return
|
||||
|
||||
def predict(self, state):
|
||||
"""
|
||||
Create a rvo2 simulation at each time step and run one step
|
||||
Python-RVO2 API: https://github.com/sybrenstuvel/Python-RVO2/blob/master/src/rvo2.pyx
|
||||
How simulation is done in RVO2: https://github.com/sybrenstuvel/Python-RVO2/blob/master/src/Agent.cpp
|
||||
|
||||
Agent doesn't stop moving after it reaches the goal, because once it stops moving, the reciprocal rule is broken
|
||||
|
||||
:param state:
|
||||
:return:
|
||||
"""
|
||||
self_state = state.self_state
|
||||
params = self.neighbor_dist, self.max_neighbors, self.time_horizon, self.time_horizon_obst
|
||||
if self.sim is not None and self.sim.getNumAgents() != len(state.human_states) + 1:
|
||||
del self.sim
|
||||
self.sim = None
|
||||
if self.sim is None:
|
||||
self.sim = rvo2.PyRVOSimulator(self.time_step, self.neighbor_dist, self.max_neighbors, self.time_horizon, self.time_horizon_obst, self.radius, self.max_speed)
|
||||
self.sim.addAgent(self_state.position, self.neighbor_dist, self.max_neighbors, self.time_horizon, self.time_horizon_obst, self_state.radius + 0.01 + self.safety_space,
|
||||
self_state.v_pref, self_state.velocity)
|
||||
for human_state in state.human_states:
|
||||
self.sim.addAgent(human_state.position, self.neighbor_dist, self.max_neighbors, self.time_horizon, self.time_horizon_obst, human_state.radius + 0.01 + self.safety_space,
|
||||
self.max_speed, human_state.velocity)
|
||||
else:
|
||||
self.sim.setAgentPosition(0, self_state.position)
|
||||
self.sim.setAgentVelocity(0, self_state.velocity)
|
||||
for i, human_state in enumerate(state.human_states):
|
||||
self.sim.setAgentPosition(i + 1, human_state.position)
|
||||
self.sim.setAgentVelocity(i + 1, human_state.velocity)
|
||||
|
||||
# Set the preferred velocity to be a vector of unit magnitude (speed) in the direction of the goal.
|
||||
velocity = np.array((self_state.gx - self_state.px, self_state.gy - self_state.py))
|
||||
speed = np.linalg.norm(velocity)
|
||||
pref_vel = velocity / speed if speed > 1 else velocity
|
||||
|
||||
# Perturb a little to avoid deadlocks due to perfect symmetry.
|
||||
# perturb_angle = np.random.random() * 2 * np.pi
|
||||
# perturb_dist = np.random.random() * 0.01
|
||||
# perturb_vel = np.array((np.cos(perturb_angle), np.sin(perturb_angle))) * perturb_dist
|
||||
# pref_vel += perturb_vel
|
||||
|
||||
self.sim.setAgentPrefVelocity(0, tuple(pref_vel))
|
||||
for i, human_state in enumerate(state.human_states):
|
||||
# unknown goal position of other humans
|
||||
self.sim.setAgentPrefVelocity(i + 1, (0, 0))
|
||||
|
||||
self.sim.doStep()
|
||||
action = ActionXY(*self.sim.getAgentVelocity(0))
|
||||
self.last_state = state
|
||||
|
||||
return action
|
||||
@ -0,0 +1,53 @@
|
||||
import abc
|
||||
import numpy as np
|
||||
|
||||
|
||||
class Policy(object):
|
||||
def __init__(self):
|
||||
"""
|
||||
Base class for all policies, has an abstract method predict().
|
||||
"""
|
||||
self.trainable = False
|
||||
self.phase = None
|
||||
self.model = None
|
||||
self.modeldl = None
|
||||
self.device = None
|
||||
self.last_state = None
|
||||
self.time_step = None
|
||||
# if agent is assumed to know the dynamics of real world
|
||||
self.env = None
|
||||
|
||||
@abc.abstractmethod
|
||||
def configure(self, config):
|
||||
return
|
||||
|
||||
def set_phase(self, phase):
|
||||
self.phase = phase
|
||||
|
||||
def set_device(self, device):
|
||||
self.device = device
|
||||
|
||||
def set_env(self, env):
|
||||
self.env = env
|
||||
|
||||
def get_model(self):
|
||||
return self.model
|
||||
|
||||
def get_modeldl(self):
|
||||
return self.modeldl
|
||||
|
||||
@abc.abstractmethod
|
||||
def predict(self, state):
|
||||
"""
|
||||
Policy takes state as input and output an action
|
||||
|
||||
"""
|
||||
return
|
||||
|
||||
@staticmethod
|
||||
def reach_destination(state):
|
||||
self_state = state.self_state
|
||||
if np.linalg.norm((self_state.py - self_state.gy, self_state.px - self_state.gx)) < self_state.radius:
|
||||
return True
|
||||
else:
|
||||
return False
|
||||
@ -0,0 +1,9 @@
|
||||
from orca import ORCA
|
||||
|
||||
|
||||
def none_policy():
|
||||
return None
|
||||
|
||||
policy_factory = dict()
|
||||
policy_factory['orca'] = ORCA
|
||||
policy_factory['none'] = none_policy
|
||||
Binary file not shown.
Binary file not shown.
@ -0,0 +1,59 @@
|
||||
# This example illustrates how to execute complex commands from
|
||||
# a remote API client. You can also use a similar construct for
|
||||
# commands that are not directly supported by the remote API.
|
||||
#
|
||||
# Load the demo scene 'remoteApiCommandServerExample.ttt' in CoppeliaSim, then
|
||||
# start the simulation and run this program.
|
||||
#
|
||||
# IMPORTANT: for each successful call to simxStart, there
|
||||
# should be a corresponding call to simxFinish at the end!
|
||||
|
||||
try:
|
||||
import sim
|
||||
except:
|
||||
print ('--------------------------------------------------------------')
|
||||
print ('"sim.py" could not be imported. This means very probably that')
|
||||
print ('either "sim.py" or the remoteApi library could not be found.')
|
||||
print ('Make sure both are in the same folder as this file,')
|
||||
print ('or appropriately adjust the file "sim.py"')
|
||||
print ('--------------------------------------------------------------')
|
||||
print ('')
|
||||
|
||||
import sys
|
||||
import ctypes
|
||||
print ('Program started')
|
||||
sim.simxFinish(-1) # just in case, close all opened connections
|
||||
clientID=sim.simxStart('127.0.0.1',19999,True,True,5000,5) # Connect to CoppeliaSim
|
||||
if clientID!=-1:
|
||||
print ('Connected to remote API server')
|
||||
|
||||
# 1. First send a command to display a specific message in a dialog box:
|
||||
emptyBuff = bytearray()
|
||||
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'displayText_function',[],[],['Hello world!'],emptyBuff,sim.simx_opmode_blocking)
|
||||
if res==sim.simx_return_ok:
|
||||
print ('Return string: ',retStrings[0]) # display the reply from CoppeliaSim (in this case, just a string)
|
||||
else:
|
||||
print ('Remote function call failed')
|
||||
|
||||
# 2. Now create a dummy object at coordinate 0.1,0.2,0.3 with name 'MyDummyName':
|
||||
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'createDummy_function',[],[0.1,0.2,0.3],['MyDummyName'],emptyBuff,sim.simx_opmode_blocking)
|
||||
if res==sim.simx_return_ok:
|
||||
print ('Dummy handle: ',retInts[0]) # display the reply from CoppeliaSim (in this case, the handle of the created dummy)
|
||||
else:
|
||||
print ('Remote function call failed')
|
||||
|
||||
# 3. Now send a code string to execute some random functions:
|
||||
code="local octreeHandle=simCreateOctree(0.5,0,1)\n" \
|
||||
"simInsertVoxelsIntoOctree(octreeHandle,0,{0.1,0.1,0.1},{255,0,255})\n" \
|
||||
"return 'done'"
|
||||
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,"remoteApiCommandServer",sim.sim_scripttype_childscript,'executeCode_function',[],[],[code],emptyBuff,sim.simx_opmode_blocking)
|
||||
if res==sim.simx_return_ok:
|
||||
print ('Code execution returned: ',retStrings[0])
|
||||
else:
|
||||
print ('Remote function call failed')
|
||||
|
||||
# Now close the connection to CoppeliaSim:
|
||||
sim.simxFinish(clientID)
|
||||
else:
|
||||
print ('Failed connecting to remote API server')
|
||||
print ('Program ended')
|
||||
@ -0,0 +1,229 @@
|
||||
"""Creates an image from a numpy array of floating point depth data.
|
||||
|
||||
For details about the encoding see:
|
||||
https://sites.google.com/site/brainrobotdata/home/depth-image-encoding
|
||||
|
||||
Examples:
|
||||
|
||||
depth_array is a 2D numpy array of floating point depth data in meters.
|
||||
|
||||
depth_rgb = FloatArrayToRgbImage(depth_array)
|
||||
depth_rgb is a PIL Image object containing the same data as 24-bit
|
||||
integers encoded in the RGB bytes.
|
||||
depth_rgb.save('image_file.png') - to save to a file.
|
||||
|
||||
depth_array2 = ImageToFloatArray(depth_rgb)
|
||||
depth_array2 is a 2D numpy array containing the same data as
|
||||
depth_array up to the precision of the RGB image (1/256 mm).
|
||||
|
||||
depth_gray = FloatArrayToGrayImage(depth_array)
|
||||
depth_gray is a PIL Image object containing the same data rounded to
|
||||
8-bit integers.
|
||||
depth_gray.save('image_file.jpg', quality=95) - to save to a file.
|
||||
|
||||
depth_array3 = ImageToFloatArray(depth_gray)
|
||||
depth_array3 is a 2D numpy array containing the same data as
|
||||
depth_array up to the precision of the grayscale image (1 cm).
|
||||
|
||||
The image conversions first scale and round the values and then pack
|
||||
them into the desired type in a numpy array before converting the
|
||||
array to a PIL Image object. The Image can be saved in any format.
|
||||
We are using PNG for RGB and high quality JPEG for grayscale images.
|
||||
|
||||
You can use different numeric types (e.g. np.uint16, np.int32), but
|
||||
not all combinations of numeric type and image format are supported by
|
||||
PIL or standard image viewers.
|
||||
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
from PIL import Image
|
||||
from skimage import img_as_ubyte
|
||||
from skimage.color import grey2rgb
|
||||
|
||||
|
||||
def ClipFloatValues(float_array, min_value, max_value):
|
||||
"""Clips values to the range [min_value, max_value].
|
||||
|
||||
First checks if any values are out of range and prints a message.
|
||||
Then clips all values to the given range.
|
||||
|
||||
Args:
|
||||
float_array: 2D array of floating point values to be clipped.
|
||||
min_value: Minimum value of clip range.
|
||||
max_value: Maximum value of clip range.
|
||||
|
||||
Returns:
|
||||
The clipped array.
|
||||
|
||||
"""
|
||||
if float_array.min() < min_value or float_array.max() > max_value:
|
||||
print('Image has out-of-range values [%f,%f] not in [%d,%d]',
|
||||
float_array.min(), float_array.max(), min_value, max_value)
|
||||
float_array = np.clip(float_array, min_value, max_value)
|
||||
return float_array
|
||||
|
||||
|
||||
DEFAULT_RGB_SCALE_FACTOR = 256000.0
|
||||
|
||||
|
||||
def FloatArrayToRgbImage(float_array,
|
||||
scale_factor=DEFAULT_RGB_SCALE_FACTOR,
|
||||
drop_blue=False):
|
||||
"""Convert a floating point array of values to an RGB image.
|
||||
|
||||
Convert floating point values to a fixed point representation where
|
||||
the RGB bytes represent a 24-bit integer.
|
||||
R is the high order byte.
|
||||
B is the low order byte.
|
||||
The precision of the depth image is 1/256 mm.
|
||||
|
||||
Floating point values are scaled so that the integer values cover
|
||||
the representable range of depths.
|
||||
|
||||
This image representation should only use lossless compression.
|
||||
|
||||
Args:
|
||||
float_array: Input array of floating point depth values in meters.
|
||||
scale_factor: Scale value applied to all float values.
|
||||
drop_blue: Zero out the blue channel to improve compression, results in 1mm
|
||||
precision depth values.
|
||||
|
||||
Returns:
|
||||
24-bit RGB PIL Image object representing depth values.
|
||||
"""
|
||||
float_array = np.squeeze(float_array)
|
||||
# Scale the floating point array.
|
||||
scaled_array = np.floor(float_array * scale_factor + 0.5)
|
||||
# Convert the array to integer type and clip to representable range.
|
||||
min_inttype = 0
|
||||
max_inttype = 2**24 - 1
|
||||
scaled_array = ClipFloatValues(scaled_array, min_inttype, max_inttype)
|
||||
int_array = scaled_array.astype(np.uint32)
|
||||
# Calculate:
|
||||
# r = (f / 256) / 256 high byte
|
||||
# g = (f / 256) % 256 middle byte
|
||||
# b = f % 256 low byte
|
||||
rg = np.divide(int_array, 256)
|
||||
r = np.divide(rg, 256)
|
||||
g = np.mod(rg, 256)
|
||||
image_shape = int_array.shape
|
||||
rgb_array = np.zeros((image_shape[0], image_shape[1], 3), dtype=np.uint8)
|
||||
rgb_array[..., 0] = r
|
||||
rgb_array[..., 1] = g
|
||||
if not drop_blue:
|
||||
# Calculate the blue channel and add it to the array.
|
||||
b = np.mod(int_array, 256)
|
||||
rgb_array[..., 2] = b
|
||||
image_mode = 'RGB'
|
||||
image = Image.fromarray(rgb_array, mode=image_mode)
|
||||
return image
|
||||
|
||||
|
||||
DEFAULT_GRAY_SCALE_FACTOR = {np.uint8: 100.0,
|
||||
np.uint16: 1000.0,
|
||||
np.int32: DEFAULT_RGB_SCALE_FACTOR}
|
||||
|
||||
|
||||
def FloatArrayToGrayImage(float_array, scale_factor=None, image_dtype=np.uint8):
|
||||
"""Convert a floating point array of values to an RGB image.
|
||||
|
||||
Convert floating point values to a fixed point representation with
|
||||
the given bit depth.
|
||||
|
||||
The precision of the depth image with default scale_factor is:
|
||||
uint8: 1cm, with a range of [0, 2.55m]
|
||||
uint16: 1mm, with a range of [0, 65.5m]
|
||||
int32: 1/256mm, with a range of [0, 8388m]
|
||||
|
||||
Right now, PIL turns uint16 images into a very strange format and
|
||||
does not decode int32 images properly. Only uint8 works correctly.
|
||||
|
||||
Args:
|
||||
float_array: Input array of floating point depth values in meters.
|
||||
scale_factor: Scale value applied to all float values.
|
||||
image_dtype: Image datatype, which controls the bit depth of the grayscale
|
||||
image.
|
||||
|
||||
Returns:
|
||||
Grayscale PIL Image object representing depth values.
|
||||
|
||||
"""
|
||||
# Ensure that we have a valid numeric type for the image.
|
||||
if image_dtype == np.uint16:
|
||||
image_mode = 'I;16'
|
||||
elif image_dtype == np.int32:
|
||||
image_mode = 'I'
|
||||
else:
|
||||
image_dtype = np.uint8
|
||||
image_mode = 'L'
|
||||
if scale_factor is None:
|
||||
scale_factor = DEFAULT_GRAY_SCALE_FACTOR[image_dtype]
|
||||
# Scale the floating point array.
|
||||
scaled_array = np.floor(float_array * scale_factor + 0.5)
|
||||
# Convert the array to integer type and clip to representable range.
|
||||
min_dtype = np.iinfo(image_dtype).min
|
||||
max_dtype = np.iinfo(image_dtype).max
|
||||
scaled_array = ClipFloatValues(scaled_array, min_dtype, max_dtype)
|
||||
|
||||
image_array = scaled_array.astype(image_dtype)
|
||||
image = Image.fromarray(image_array, mode=image_mode)
|
||||
return image
|
||||
|
||||
|
||||
def ImageToFloatArray(image, scale_factor=None):
|
||||
"""Recovers the depth values from an image.
|
||||
|
||||
Reverses the depth to image conversion performed by FloatArrayToRgbImage or
|
||||
FloatArrayToGrayImage.
|
||||
|
||||
The image is treated as an array of fixed point depth values. Each
|
||||
value is converted to float and scaled by the inverse of the factor
|
||||
that was used to generate the Image object from depth values. If
|
||||
scale_factor is specified, it should be the same value that was
|
||||
specified in the original conversion.
|
||||
|
||||
The result of this function should be equal to the original input
|
||||
within the precision of the conversion.
|
||||
|
||||
For details see https://sites.google.com/site/brainrobotdata/home/depth-image-encoding.
|
||||
|
||||
Args:
|
||||
image: Depth image output of FloatArrayTo[Format]Image.
|
||||
scale_factor: Fixed point scale factor.
|
||||
|
||||
Returns:
|
||||
A 2D floating point numpy array representing a depth image.
|
||||
|
||||
"""
|
||||
image_array = np.array(image)
|
||||
image_dtype = image_array.dtype
|
||||
image_shape = image_array.shape
|
||||
|
||||
channels = image_shape[2] if len(image_shape) > 2 else 1
|
||||
assert 2 <= len(image_shape) <= 3
|
||||
if channels == 3:
|
||||
# RGB image needs to be converted to 24 bit integer.
|
||||
float_array = np.sum(image_array * [65536, 256, 1], axis=2)
|
||||
if scale_factor is None:
|
||||
scale_factor = DEFAULT_RGB_SCALE_FACTOR
|
||||
else:
|
||||
if scale_factor is None:
|
||||
scale_factor = DEFAULT_GRAY_SCALE_FACTOR[image_dtype.type]
|
||||
float_array = image_array.astype(np.float32)
|
||||
scaled_array = float_array / scale_factor
|
||||
return scaled_array
|
||||
|
||||
|
||||
def FloatArrayToRawRGB(im, min_value=0.0, max_value=1.0):
|
||||
"""Convert a grayscale image to rgb, no encoding.
|
||||
|
||||
For proper display try matplotlib's rendering/conversion instead of this version.
|
||||
Please be aware that this does not incorporate a proper color transform.
|
||||
http://pillow.readthedocs.io/en/3.4.x/reference/Image.html#PIL.Image.Image.convert
|
||||
https://en.wikipedia.org/wiki/Rec._601
|
||||
"""
|
||||
im = img_as_ubyte(im)
|
||||
if im.shape[-1] == 1:
|
||||
im = grey2rgb(im)
|
||||
return im
|
||||
@ -0,0 +1,160 @@
|
||||
# This example illustrates how to use the path/motion
|
||||
# planning functionality from a remote API client.
|
||||
#
|
||||
# Load the demo scene 'motionPlanningServerDemo.ttt' in CoppeliaSim
|
||||
# then run this program.
|
||||
#
|
||||
# IMPORTANT: for each successful call to simxStart, there
|
||||
# should be a corresponding call to simxFinish at the end!
|
||||
|
||||
import sim
|
||||
|
||||
print ('Program started')
|
||||
sim.simxFinish(-1) # just in case, close all opened connections
|
||||
clientID=sim.simxStart('127.0.0.1',19997,True,True,-500000,5) # Connect to CoppeliaSim, set a very large time-out for blocking commands
|
||||
if clientID!=-1:
|
||||
print ('Connected to remote API server')
|
||||
|
||||
emptyBuff = bytearray()
|
||||
|
||||
# Start the simulation:
|
||||
sim.simxStartSimulation(clientID,sim.simx_opmode_oneshot_wait)
|
||||
|
||||
# Load a robot instance: res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'loadRobot',[],[0,0,0,0],['d:/coppeliaRobotics/qrelease/release/test.ttm'],emptyBuff,sim.simx_opmode_oneshot_wait)
|
||||
# robotHandle=retInts[0]
|
||||
|
||||
# Retrieve some handles:
|
||||
res,robotHandle=sim.simxGetObjectHandle(clientID,'IRB4600#',sim.simx_opmode_oneshot_wait)
|
||||
res,target1=sim.simxGetObjectHandle(clientID,'testPose1#',sim.simx_opmode_oneshot_wait)
|
||||
res,target2=sim.simxGetObjectHandle(clientID,'testPose2#',sim.simx_opmode_oneshot_wait)
|
||||
res,target3=sim.simxGetObjectHandle(clientID,'testPose3#',sim.simx_opmode_oneshot_wait)
|
||||
res,target4=sim.simxGetObjectHandle(clientID,'testPose4#',sim.simx_opmode_oneshot_wait)
|
||||
|
||||
# Retrieve the poses (i.e. transformation matrices, 12 values, last row is implicit) of some dummies in the scene
|
||||
res,retInts,target1Pose,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'getObjectPose',[target1],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
||||
res,retInts,target2Pose,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'getObjectPose',[target2],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
||||
res,retInts,target3Pose,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'getObjectPose',[target3],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
||||
res,retInts,target4Pose,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'getObjectPose',[target4],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
||||
|
||||
# Get the robot initial state:
|
||||
res,retInts,robotInitialState,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'getRobotState',[robotHandle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
||||
|
||||
# Some parameters:
|
||||
approachVector=[0,0,1] # often a linear approach is required. This should also be part of the calculations when selecting an appropriate state for a given pose
|
||||
maxConfigsForDesiredPose=10 # we will try to find 10 different states corresponding to the goal pose and order them according to distance from initial state
|
||||
maxTrialsForConfigSearch=300 # a parameter needed for finding appropriate goal states
|
||||
searchCount=2 # how many times OMPL will run for a given task
|
||||
minConfigsForPathPlanningPath=400 # interpolation states for the OMPL path
|
||||
minConfigsForIkPath=100 # interpolation states for the linear approach path
|
||||
collisionChecking=1 # whether collision checking is on or off
|
||||
|
||||
# Display a message:
|
||||
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'displayMessage',[],[],['Computing and executing several path planning tasks for a given goal pose.&&nSeveral goal states corresponding to the goal pose are tested.&&nFeasability of a linear approach is also tested. Collision detection is on.'],emptyBuff,sim.simx_opmode_oneshot_wait)
|
||||
|
||||
# Do the path planning here (between a start state and a goal pose, including a linear approach phase):
|
||||
inInts=[robotHandle,collisionChecking,minConfigsForIkPath,minConfigsForPathPlanningPath,maxConfigsForDesiredPose,maxTrialsForConfigSearch,searchCount]
|
||||
inFloats=robotInitialState+target1Pose+approachVector
|
||||
res,retInts,path,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'findPath_goalIsPose',inInts,inFloats,[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
||||
|
||||
if (res==0) and len(path)>0:
|
||||
# The path could be in 2 parts: a path planning path, and a linear approach path:
|
||||
part1StateCnt=retInts[0]
|
||||
part2StateCnt=retInts[1]
|
||||
path1=path[:part1StateCnt*6]
|
||||
|
||||
# Visualize the first path:
|
||||
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'visualizePath',[robotHandle,255,0,255],path1,[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
||||
line1Handle=retInts[0]
|
||||
|
||||
# Make the robot follow the path:
|
||||
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'runThroughPath',[robotHandle],path1,[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
||||
|
||||
# Wait until the end of the movement:
|
||||
runningPath=True
|
||||
while runningPath:
|
||||
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'isRunningThroughPath',[robotHandle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
||||
runningPath=retInts[0]==1
|
||||
|
||||
path2=path[part1StateCnt*6:]
|
||||
|
||||
# Visualize the second path (the linear approach):
|
||||
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'visualizePath',[robotHandle,0,255,0],path2,[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
||||
line2Handle=retInts[0]
|
||||
|
||||
# Make the robot follow the path:
|
||||
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'runThroughPath',[robotHandle],path2,[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
||||
|
||||
# Wait until the end of the movement:
|
||||
runningPath=True
|
||||
while runningPath:
|
||||
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'isRunningThroughPath',[robotHandle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
||||
runningPath=retInts[0]==1
|
||||
|
||||
# Clear the paths visualizations:
|
||||
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'removeLine',[line1Handle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
||||
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'removeLine',[line2Handle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
||||
|
||||
# Get the robot current state:
|
||||
res,retInts,robotCurrentConfig,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'getRobotState',[robotHandle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
||||
|
||||
# Display a message:
|
||||
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'displayMessage',[],[],['Computing and executing several path planning tasks for a given goal state. Collision detection is on.'],emptyBuff,sim.simx_opmode_oneshot_wait)
|
||||
|
||||
# Do the path planning here (between a start state and a goal state):
|
||||
inInts=[robotHandle,collisionChecking,minConfigsForPathPlanningPath,searchCount]
|
||||
inFloats=robotCurrentConfig+robotInitialState
|
||||
res,retInts,path,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'findPath_goalIsState',inInts,inFloats,[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
||||
|
||||
if (res==0) and len(path)>0:
|
||||
# Visualize the path:
|
||||
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'visualizePath',[robotHandle,255,0,255],path,[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
||||
lineHandle=retInts[0]
|
||||
|
||||
# Make the robot follow the path:
|
||||
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'runThroughPath',[robotHandle],path,[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
||||
|
||||
# Wait until the end of the movement:
|
||||
runningPath=True
|
||||
while runningPath:
|
||||
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'isRunningThroughPath',[robotHandle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
||||
runningPath=retInts[0]==1
|
||||
|
||||
# Clear the path visualization:
|
||||
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'removeLine',[lineHandle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
||||
|
||||
# Collision checking off:
|
||||
collisionChecking=0
|
||||
|
||||
# Display a message:
|
||||
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'displayMessage',[],[],['Computing and executing several linear paths, going through several waypoints. Collision detection is OFF.'],emptyBuff,sim.simx_opmode_oneshot_wait)
|
||||
|
||||
# Find a linear path that runs through several poses:
|
||||
inInts=[robotHandle,collisionChecking,minConfigsForIkPath]
|
||||
inFloats=robotInitialState+target2Pose+target1Pose+target3Pose+target4Pose
|
||||
res,retInts,path,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'findIkPath',inInts,inFloats,[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
||||
|
||||
if (res==0) and len(path)>0:
|
||||
# Visualize the path:
|
||||
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'visualizePath',[robotHandle,0,255,255],path,[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
||||
line1Handle=retInts[0]
|
||||
|
||||
# Make the robot follow the path:
|
||||
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'runThroughPath',[robotHandle],path,[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
||||
|
||||
# Wait until the end of the movement:
|
||||
runningPath=True
|
||||
while runningPath:
|
||||
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'isRunningThroughPath',[robotHandle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
||||
runningPath=retInts[0]==1
|
||||
|
||||
# Clear the path visualization:
|
||||
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'removeLine',[line1Handle],[],[],emptyBuff,sim.simx_opmode_oneshot_wait)
|
||||
|
||||
# Stop simulation:
|
||||
sim.simxStopSimulation(clientID,sim.simx_opmode_oneshot_wait)
|
||||
|
||||
# Now close the connection to CoppeliaSim:
|
||||
sim.simxFinish(clientID)
|
||||
else:
|
||||
print ('Failed connecting to remote API server')
|
||||
print ('Program ended')
|
||||
@ -0,0 +1,83 @@
|
||||
# MIT LICENSE
|
||||
# https://github.com/bponsler/kinectToPly/blob/master/kinectToPly.py
|
||||
import numpy as np
|
||||
|
||||
|
||||
class Ply(object):
|
||||
'''The Ply class provides the ability to write a point cloud represented
|
||||
by two arrays: an array of points (num points, 3), and an array of colors
|
||||
(num points, 3) to a PLY file.
|
||||
'''
|
||||
|
||||
def __init__(self, points, colors):
|
||||
'''
|
||||
* points -- The matrix of points (num points, 3)
|
||||
* colors -- The matrix of colors (num points, 3)
|
||||
'''
|
||||
self.__points = points
|
||||
self.__colors = colors
|
||||
|
||||
def write(self, filename):
|
||||
'''Write the point cloud data to a PLY file of the given name.
|
||||
* filename -- The PLY file
|
||||
'''
|
||||
# Write the headers
|
||||
lines = self.__getLinesForHeader()
|
||||
|
||||
fd = open(filename, "w")
|
||||
for line in lines:
|
||||
fd.write("%s\n" % line)
|
||||
|
||||
# Write the points
|
||||
self.__writePoints(fd, self.__points, self.__colors)
|
||||
|
||||
fd.close()
|
||||
|
||||
def __getLinesForHeader(self):
|
||||
'''Get the list of lines for the PLY header.'''
|
||||
lines = [
|
||||
"ply",
|
||||
"format ascii 1.0",
|
||||
"comment generated by: kinectToPly",
|
||||
"element vertex %s" % len(self.__points),
|
||||
"property float x",
|
||||
"property float y",
|
||||
"property float z",
|
||||
"property uchar red",
|
||||
"property uchar green",
|
||||
"property uchar blue",
|
||||
"end_header",
|
||||
]
|
||||
|
||||
return lines
|
||||
|
||||
def __writePoints(self, fd, points, colors):
|
||||
'''Write the point cloud points to a file.
|
||||
* fd -- The file descriptor
|
||||
* points -- The matrix of points (num points, 3)
|
||||
* colors -- The matrix of colors (num points, 3)
|
||||
'''
|
||||
# Stack the two arrays together
|
||||
stacked = np.column_stack((points, colors))
|
||||
|
||||
# Write the array to the file
|
||||
np.savetxt(
|
||||
fd,
|
||||
stacked,
|
||||
delimiter='\n',
|
||||
fmt="%f %f %f %d %d %d")
|
||||
|
||||
|
||||
def write_xyz_rgb_as_ply(point_cloud, rgb_image, path):
|
||||
"""Write a point cloud with associated rgb image to a ply file
|
||||
|
||||
# Arguments
|
||||
|
||||
point_cloud: xyz point cloud in format [height, width, channels]
|
||||
rgb_image: uint8 image in format [height, width, channels]
|
||||
path: Where to save the file, ex: '/path/to/folder/file.ply'
|
||||
"""
|
||||
xyz = point_cloud.reshape([point_cloud.size/3, 3])
|
||||
rgb = np.squeeze(rgb_image).reshape([point_cloud.size/3, 3])
|
||||
ply = Ply(xyz, rgb)
|
||||
ply.write(path)
|
||||
@ -0,0 +1,6 @@
|
||||
Make sure you have following files in your directory, in order to run the various examples:
|
||||
|
||||
1. sim.py
|
||||
2. simConst.py
|
||||
3. the appropriate remote API library: "remoteApi.dll" (Windows), "remoteApi.dylib" (Mac) or "remoteApi.so" (Linux)
|
||||
4. simpleTest.py (or any other example file)
|
||||
Binary file not shown.
File diff suppressed because it is too large
Load Diff
Binary file not shown.
@ -0,0 +1,783 @@
|
||||
#constants
|
||||
#Scene object types. Values are serialized
|
||||
sim_object_shape_type =0
|
||||
sim_object_joint_type =1
|
||||
sim_object_graph_type =2
|
||||
sim_object_camera_type =3
|
||||
sim_object_dummy_type =4
|
||||
sim_object_proximitysensor_type =5
|
||||
sim_object_reserved1 =6
|
||||
sim_object_reserved2 =7
|
||||
sim_object_path_type =8
|
||||
sim_object_visionsensor_type =9
|
||||
sim_object_volume_type =10
|
||||
sim_object_mill_type =11
|
||||
sim_object_forcesensor_type =12
|
||||
sim_object_light_type =13
|
||||
sim_object_mirror_type =14
|
||||
|
||||
#General object types. Values are serialized
|
||||
sim_appobj_object_type =109
|
||||
sim_appobj_collision_type =110
|
||||
sim_appobj_distance_type =111
|
||||
sim_appobj_simulation_type =112
|
||||
sim_appobj_ik_type =113
|
||||
sim_appobj_constraintsolver_type=114
|
||||
sim_appobj_collection_type =115
|
||||
sim_appobj_ui_type =116
|
||||
sim_appobj_script_type =117
|
||||
sim_appobj_pathplanning_type =118
|
||||
sim_appobj_RESERVED_type =119
|
||||
sim_appobj_texture_type =120
|
||||
|
||||
# Ik calculation methods. Values are serialized
|
||||
sim_ik_pseudo_inverse_method =0
|
||||
sim_ik_damped_least_squares_method =1
|
||||
sim_ik_jacobian_transpose_method =2
|
||||
|
||||
# Ik constraints. Values are serialized
|
||||
sim_ik_x_constraint =1
|
||||
sim_ik_y_constraint =2
|
||||
sim_ik_z_constraint =4
|
||||
sim_ik_alpha_beta_constraint=8
|
||||
sim_ik_gamma_constraint =16
|
||||
sim_ik_avoidance_constraint =64
|
||||
|
||||
# Ik calculation results
|
||||
sim_ikresult_not_performed =0
|
||||
sim_ikresult_success =1
|
||||
sim_ikresult_fail =2
|
||||
|
||||
# Scene object sub-types. Values are serialized
|
||||
# Light sub-types
|
||||
sim_light_omnidirectional_subtype =1
|
||||
sim_light_spot_subtype =2
|
||||
sim_light_directional_subtype =3
|
||||
# Joint sub-types
|
||||
sim_joint_revolute_subtype =10
|
||||
sim_joint_prismatic_subtype =11
|
||||
sim_joint_spherical_subtype =12
|
||||
# Shape sub-types
|
||||
sim_shape_simpleshape_subtype =20
|
||||
sim_shape_multishape_subtype =21
|
||||
# Proximity sensor sub-types
|
||||
sim_proximitysensor_pyramid_subtype =30
|
||||
sim_proximitysensor_cylinder_subtype=31
|
||||
sim_proximitysensor_disc_subtype =32
|
||||
sim_proximitysensor_cone_subtype =33
|
||||
sim_proximitysensor_ray_subtype =34
|
||||
# Mill sub-types
|
||||
sim_mill_pyramid_subtype =40
|
||||
sim_mill_cylinder_subtype =41
|
||||
sim_mill_disc_subtype =42
|
||||
sim_mill_cone_subtype =42
|
||||
# No sub-type
|
||||
sim_object_no_subtype =200
|
||||
|
||||
|
||||
#Scene object main properties (serialized)
|
||||
sim_objectspecialproperty_collidable =0x0001
|
||||
sim_objectspecialproperty_measurable =0x0002
|
||||
#reserved =0x0004
|
||||
#reserved =0x0008
|
||||
sim_objectspecialproperty_detectable_ultrasonic =0x0010
|
||||
sim_objectspecialproperty_detectable_infrared =0x0020
|
||||
sim_objectspecialproperty_detectable_laser =0x0040
|
||||
sim_objectspecialproperty_detectable_inductive =0x0080
|
||||
sim_objectspecialproperty_detectable_capacitive =0x0100
|
||||
sim_objectspecialproperty_renderable =0x0200
|
||||
sim_objectspecialproperty_detectable_all =sim_objectspecialproperty_detectable_ultrasonic|sim_objectspecialproperty_detectable_infrared|sim_objectspecialproperty_detectable_laser|sim_objectspecialproperty_detectable_inductive|sim_objectspecialproperty_detectable_capacitive
|
||||
sim_objectspecialproperty_cuttable =0x0400
|
||||
sim_objectspecialproperty_pathplanning_ignored =0x0800
|
||||
|
||||
# Model properties (serialized)
|
||||
sim_modelproperty_not_collidable =0x0001
|
||||
sim_modelproperty_not_measurable =0x0002
|
||||
sim_modelproperty_not_renderable =0x0004
|
||||
sim_modelproperty_not_detectable =0x0008
|
||||
sim_modelproperty_not_cuttable =0x0010
|
||||
sim_modelproperty_not_dynamic =0x0020
|
||||
sim_modelproperty_not_respondable =0x0040 # cannot be selected if sim_modelproperty_not_dynamic is not selected
|
||||
sim_modelproperty_not_reset =0x0080 # Model is not reset at simulation end. This flag is cleared at simulation end
|
||||
sim_modelproperty_not_visible =0x0100 # Whole model is invisible independent of local visibility settings
|
||||
sim_modelproperty_not_model =0xf000 # object is not a model
|
||||
|
||||
|
||||
# Check the documentation instead of comments below!!
|
||||
# Following messages are dispatched to the Lua-message container
|
||||
sim_message_ui_button_state_change =0 # a UI button slider etc. changed (due to a user's action). aux[0]=UI handle aux[1]=button handle aux[2]=button attributes aux[3]=slider position (if slider)
|
||||
sim_message_reserved9 =1 # Do not use
|
||||
sim_message_object_selection_changed=2
|
||||
sim_message_reserved10 =3 # do not use
|
||||
sim_message_model_loaded =4
|
||||
sim_message_reserved11 =5 # do not use
|
||||
sim_message_keypress =6 # a key was pressed while the focus was on a page (aux[0]=key aux[1]=ctrl and shift key state)
|
||||
sim_message_bannerclicked =7 # a banner was clicked (aux[0]=banner ID)
|
||||
|
||||
|
||||
# Following messages are dispatched only to the C-API (not available from Lua)
|
||||
sim_message_for_c_api_only_start =0x100 # Do not use
|
||||
sim_message_reserved1 =0x101 # Do not use
|
||||
sim_message_reserved2 =0x102 # Do not use
|
||||
sim_message_reserved3 =0x103 # Do not use
|
||||
sim_message_eventcallback_scenesave =0x104 # about to save a scene
|
||||
sim_message_eventcallback_modelsave =0x105 # about to save a model (current selection will be saved)
|
||||
sim_message_eventcallback_moduleopen =0x106 # called when simOpenModule in Lua is called
|
||||
sim_message_eventcallback_modulehandle =0x107 # called when simHandleModule in Lua is called with argument false
|
||||
sim_message_eventcallback_moduleclose =0x108 # called when simCloseModule in Lua is called
|
||||
sim_message_reserved4 =0x109 # Do not use
|
||||
sim_message_reserved5 =0x10a # Do not use
|
||||
sim_message_reserved6 =0x10b # Do not use
|
||||
sim_message_reserved7 =0x10c # Do not use
|
||||
sim_message_eventcallback_instancepass =0x10d # Called once every main application loop pass. auxiliaryData[0] contains event flags of events that happened since last time
|
||||
sim_message_eventcallback_broadcast =0x10e
|
||||
sim_message_eventcallback_imagefilter_enumreset =0x10f
|
||||
sim_message_eventcallback_imagefilter_enumerate =0x110
|
||||
sim_message_eventcallback_imagefilter_adjustparams =0x111
|
||||
sim_message_eventcallback_imagefilter_reserved =0x112
|
||||
sim_message_eventcallback_imagefilter_process =0x113
|
||||
sim_message_eventcallback_reserved1 =0x114 # do not use
|
||||
sim_message_eventcallback_reserved2 =0x115 # do not use
|
||||
sim_message_eventcallback_reserved3 =0x116 # do not use
|
||||
sim_message_eventcallback_reserved4 =0x117 # do not use
|
||||
sim_message_eventcallback_abouttoundo =0x118 # the undo button was hit and a previous state is about to be restored
|
||||
sim_message_eventcallback_undoperformed =0x119 # the undo button was hit and a previous state restored
|
||||
sim_message_eventcallback_abouttoredo =0x11a # the redo button was hit and a future state is about to be restored
|
||||
sim_message_eventcallback_redoperformed =0x11b # the redo button was hit and a future state restored
|
||||
sim_message_eventcallback_scripticondblclick =0x11c # scipt icon was double clicked. (aux[0]=object handle associated with script set replyData[0] to 1 if script should not be opened)
|
||||
sim_message_eventcallback_simulationabouttostart =0x11d
|
||||
sim_message_eventcallback_simulationended =0x11e
|
||||
sim_message_eventcallback_reserved5 =0x11f # do not use
|
||||
sim_message_eventcallback_keypress =0x120 # a key was pressed while the focus was on a page (aux[0]=key aux[1]=ctrl and shift key state)
|
||||
sim_message_eventcallback_modulehandleinsensingpart =0x121 # called when simHandleModule in Lua is called with argument true
|
||||
sim_message_eventcallback_renderingpass =0x122 # called just before the scene is rendered
|
||||
sim_message_eventcallback_bannerclicked =0x123 # called when a banner was clicked (aux[0]=banner ID)
|
||||
sim_message_eventcallback_menuitemselected =0x124 # auxiliaryData[0] indicates the handle of the item auxiliaryData[1] indicates the state of the item
|
||||
sim_message_eventcallback_refreshdialogs =0x125 # aux[0]=refresh degree (0=light 1=medium 2=full)
|
||||
sim_message_eventcallback_sceneloaded =0x126
|
||||
sim_message_eventcallback_modelloaded =0x127
|
||||
sim_message_eventcallback_instanceswitch =0x128
|
||||
sim_message_eventcallback_guipass =0x129
|
||||
sim_message_eventcallback_mainscriptabouttobecalled =0x12a
|
||||
sim_message_eventcallback_rmlposition =0x12b #the command simRMLPosition was called. The appropriate plugin should handle the call
|
||||
sim_message_eventcallback_rmlvelocity =0x12c # the command simRMLVelocity was called. The appropriate plugin should handle the call
|
||||
sim_message_simulation_start_resume_request =0x1000
|
||||
sim_message_simulation_pause_request =0x1001
|
||||
sim_message_simulation_stop_request =0x1002
|
||||
|
||||
# Scene object properties. Combine with the | operator
|
||||
sim_objectproperty_reserved1 =0x0000
|
||||
sim_objectproperty_reserved2 =0x0001
|
||||
sim_objectproperty_reserved3 =0x0002
|
||||
sim_objectproperty_reserved4 =0x0003
|
||||
sim_objectproperty_reserved5 =0x0004 # formely sim_objectproperty_visible
|
||||
sim_objectproperty_reserved6 =0x0008 # formely sim_objectproperty_wireframe
|
||||
sim_objectproperty_collapsed =0x0010
|
||||
sim_objectproperty_selectable =0x0020
|
||||
sim_objectproperty_reserved7 =0x0040
|
||||
sim_objectproperty_selectmodelbaseinstead =0x0080
|
||||
sim_objectproperty_dontshowasinsidemodel =0x0100
|
||||
# reserved =0x0200
|
||||
sim_objectproperty_canupdatedna =0x0400
|
||||
sim_objectproperty_selectinvisible =0x0800
|
||||
sim_objectproperty_depthinvisible =0x1000
|
||||
|
||||
|
||||
# type of arguments (input and output) for custom lua commands
|
||||
sim_lua_arg_nil =0
|
||||
sim_lua_arg_bool =1
|
||||
sim_lua_arg_int =2
|
||||
sim_lua_arg_float =3
|
||||
sim_lua_arg_string =4
|
||||
sim_lua_arg_invalid =5
|
||||
sim_lua_arg_table =8
|
||||
|
||||
# custom user interface properties. Values are serialized.
|
||||
sim_ui_property_visible =0x0001
|
||||
sim_ui_property_visibleduringsimulationonly =0x0002
|
||||
sim_ui_property_moveable =0x0004
|
||||
sim_ui_property_relativetoleftborder =0x0008
|
||||
sim_ui_property_relativetotopborder =0x0010
|
||||
sim_ui_property_fixedwidthfont =0x0020
|
||||
sim_ui_property_systemblock =0x0040
|
||||
sim_ui_property_settocenter =0x0080
|
||||
sim_ui_property_rolledup =0x0100
|
||||
sim_ui_property_selectassociatedobject =0x0200
|
||||
sim_ui_property_visiblewhenobjectselected =0x0400
|
||||
|
||||
|
||||
# button properties. Values are serialized.
|
||||
sim_buttonproperty_button =0x0000
|
||||
sim_buttonproperty_label =0x0001
|
||||
sim_buttonproperty_slider =0x0002
|
||||
sim_buttonproperty_editbox =0x0003
|
||||
sim_buttonproperty_staydown =0x0008
|
||||
sim_buttonproperty_enabled =0x0010
|
||||
sim_buttonproperty_borderless =0x0020
|
||||
sim_buttonproperty_horizontallycentered =0x0040
|
||||
sim_buttonproperty_ignoremouse =0x0080
|
||||
sim_buttonproperty_isdown =0x0100
|
||||
sim_buttonproperty_transparent =0x0200
|
||||
sim_buttonproperty_nobackgroundcolor =0x0400
|
||||
sim_buttonproperty_rollupaction =0x0800
|
||||
sim_buttonproperty_closeaction =0x1000
|
||||
sim_buttonproperty_verticallycentered =0x2000
|
||||
sim_buttonproperty_downupevent =0x4000
|
||||
|
||||
|
||||
# Simulation status
|
||||
sim_simulation_stopped =0x00 # Simulation is stopped
|
||||
sim_simulation_paused =0x08 # Simulation is paused
|
||||
sim_simulation_advancing =0x10 # Simulation is advancing
|
||||
sim_simulation_advancing_firstafterstop =sim_simulation_advancing|0x00 # First simulation pass (1x)
|
||||
sim_simulation_advancing_running =sim_simulation_advancing|0x01 # Normal simulation pass (>=1x)
|
||||
# reserved =sim_simulation_advancing|0x02
|
||||
sim_simulation_advancing_lastbeforepause =sim_simulation_advancing|0x03 # Last simulation pass before pause (1x)
|
||||
sim_simulation_advancing_firstafterpause =sim_simulation_advancing|0x04 # First simulation pass after pause (1x)
|
||||
sim_simulation_advancing_abouttostop =sim_simulation_advancing|0x05 # "Trying to stop" simulation pass (>=1x)
|
||||
sim_simulation_advancing_lastbeforestop =sim_simulation_advancing|0x06 # Last simulation pass (1x)
|
||||
|
||||
|
||||
# Script execution result (first return value)
|
||||
sim_script_no_error =0
|
||||
sim_script_main_script_nonexistent =1
|
||||
sim_script_main_script_not_called =2
|
||||
sim_script_reentrance_error =4
|
||||
sim_script_lua_error =8
|
||||
sim_script_call_error =16
|
||||
|
||||
|
||||
# Script types (serialized!)
|
||||
sim_scripttype_mainscript =0
|
||||
sim_scripttype_childscript =1
|
||||
sim_scripttype_jointctrlcallback =4
|
||||
sim_scripttype_contactcallback =5
|
||||
sim_scripttype_customizationscript =6
|
||||
sim_scripttype_generalcallback =7
|
||||
|
||||
# API call error messages
|
||||
sim_api_errormessage_ignore =0 # does not memorize nor output errors
|
||||
sim_api_errormessage_report =1 # memorizes errors (default for C-API calls)
|
||||
sim_api_errormessage_output =2 # memorizes and outputs errors (default for Lua-API calls)
|
||||
|
||||
|
||||
# special argument of some functions
|
||||
sim_handle_all =-2
|
||||
sim_handle_all_except_explicit =-3
|
||||
sim_handle_self =-4
|
||||
sim_handle_main_script =-5
|
||||
sim_handle_tree =-6
|
||||
sim_handle_chain =-7
|
||||
sim_handle_single =-8
|
||||
sim_handle_default =-9
|
||||
sim_handle_all_except_self =-10
|
||||
sim_handle_parent =-11
|
||||
|
||||
|
||||
# special handle flags
|
||||
sim_handleflag_assembly =0x400000
|
||||
sim_handleflag_model =0x800000
|
||||
|
||||
|
||||
# distance calculation methods (serialized)
|
||||
sim_distcalcmethod_dl =0
|
||||
sim_distcalcmethod_dac =1
|
||||
sim_distcalcmethod_max_dl_dac =2
|
||||
sim_distcalcmethod_dl_and_dac =3
|
||||
sim_distcalcmethod_sqrt_dl2_and_dac2=4
|
||||
sim_distcalcmethod_dl_if_nonzero =5
|
||||
sim_distcalcmethod_dac_if_nonzero =6
|
||||
|
||||
|
||||
# Generic dialog styles
|
||||
sim_dlgstyle_message =0
|
||||
sim_dlgstyle_input =1
|
||||
sim_dlgstyle_ok =2
|
||||
sim_dlgstyle_ok_cancel =3
|
||||
sim_dlgstyle_yes_no =4
|
||||
sim_dlgstyle_dont_center =32# can be combined with one of above values. Only with this flag can the position of the related UI be set just after dialog creation
|
||||
|
||||
# Generic dialog return values
|
||||
sim_dlgret_still_open =0
|
||||
sim_dlgret_ok =1
|
||||
sim_dlgret_cancel =2
|
||||
sim_dlgret_yes =3
|
||||
sim_dlgret_no =4
|
||||
|
||||
|
||||
# Path properties
|
||||
sim_pathproperty_show_line =0x0001
|
||||
sim_pathproperty_show_orientation =0x0002
|
||||
sim_pathproperty_closed_path =0x0004
|
||||
sim_pathproperty_automatic_orientation =0x0008
|
||||
sim_pathproperty_invert_velocity =0x0010
|
||||
sim_pathproperty_infinite_acceleration =0x0020
|
||||
sim_pathproperty_flat_path =0x0040
|
||||
sim_pathproperty_show_position =0x0080
|
||||
sim_pathproperty_auto_velocity_profile_translation =0x0100
|
||||
sim_pathproperty_auto_velocity_profile_rotation =0x0200
|
||||
sim_pathproperty_endpoints_at_zero =0x0400
|
||||
sim_pathproperty_keep_x_up =0x0800
|
||||
|
||||
|
||||
# drawing objects
|
||||
# following are mutually exclusive
|
||||
sim_drawing_points =0 # 3 values per point (point size in pixels)
|
||||
sim_drawing_lines =1 # 6 values per line (line size in pixels)
|
||||
sim_drawing_triangles =2 # 9 values per triangle
|
||||
sim_drawing_trianglepoints =3 # 6 values per point (3 for triangle position 3 for triangle normal vector) (triangle size in meters)
|
||||
sim_drawing_quadpoints =4 # 6 values per point (3 for quad position 3 for quad normal vector) (quad size in meters)
|
||||
sim_drawing_discpoints =5 # 6 values per point (3 for disc position 3 for disc normal vector) (disc size in meters)
|
||||
sim_drawing_cubepoints =6 # 6 values per point (3 for cube position 3 for cube normal vector) (cube size in meters)
|
||||
sim_drawing_spherepoints =7 # 3 values per point (sphere size in meters)
|
||||
|
||||
# following can be or-combined
|
||||
sim_drawing_itemcolors =0x00020 # +3 values per item (each item has its own ambient color (rgb values)).
|
||||
# Mutually exclusive with sim_drawing_vertexcolors
|
||||
sim_drawing_vertexcolors =0x00040 # +3 values per vertex (each vertex has its own ambient color (rgb values). Only for sim_drawing_lines (+6) and for sim_drawing_triangles(+9)). Mutually exclusive with sim_drawing_itemcolors
|
||||
sim_drawing_itemsizes =0x00080 # +1 value per item (each item has its own size). Not for sim_drawing_triangles
|
||||
sim_drawing_backfaceculling =0x00100 # back faces are not displayed for all items
|
||||
sim_drawing_wireframe =0x00200 # all items displayed in wireframe
|
||||
sim_drawing_painttag =0x00400 # all items are tagged as paint (for additinal processing at a later stage)
|
||||
sim_drawing_followparentvisibility =0x00800 # if the object is associated with a scene object then it follows that visibility otherwise it is always visible
|
||||
sim_drawing_cyclic =0x01000 # if the max item count was reached then the first items are overwritten.
|
||||
sim_drawing_50percenttransparency =0x02000 # the drawing object will be 50% transparent
|
||||
sim_drawing_25percenttransparency =0x04000 # the drawing object will be 25% transparent
|
||||
sim_drawing_12percenttransparency =0x08000 # the drawing object will be 12.5% transparent
|
||||
sim_drawing_emissioncolor =0x10000 # When used in combination with sim_drawing_itemcolors or sim_drawing_vertexcolors then the specified colors will be for the emissive component
|
||||
sim_drawing_facingcamera =0x20000 # Only for trianglepoints quadpoints discpoints and cubepoints. If specified the normal verctor is calculated to face the camera (each item data requires 3 values less)
|
||||
sim_drawing_overlay =0x40000 # When specified objects are always drawn on top of "regular objects"
|
||||
sim_drawing_itemtransparency =0x80000 # +1 value per item (each item has its own transparency value (0-1)). Not compatible with sim_drawing_vertexcolors
|
||||
|
||||
# banner values
|
||||
# following can be or-combined
|
||||
sim_banner_left =0x00001 # Banners display on the left of the specified point
|
||||
sim_banner_right =0x00002 # Banners display on the right of the specified point
|
||||
sim_banner_nobackground =0x00004 # Banners have no background rectangle
|
||||
sim_banner_overlay =0x00008 # When specified banners are always drawn on top of "regular objects"
|
||||
sim_banner_followparentvisibility =0x00010 # if the object is associated with a scene object then it follows that visibility otherwise it is always visible
|
||||
sim_banner_clickselectsparent =0x00020 # if the object is associated with a scene object then clicking the banner will select the scene object
|
||||
sim_banner_clicktriggersevent =0x00040 # if the banner is clicked an event is triggered (sim_message_eventcallback_bannerclicked and sim_message_bannerclicked are generated)
|
||||
sim_banner_facingcamera =0x00080 # If specified the banner will always face the camera by rotating around the banner's vertical axis (y-axis)
|
||||
sim_banner_fullyfacingcamera =0x00100 # If specified the banner will always fully face the camera (the banner's orientation is same as the camera looking at it)
|
||||
sim_banner_backfaceculling =0x00200 # If specified the banner will only be visible from one side
|
||||
sim_banner_keepsamesize =0x00400 # If specified the banner will always appear in the same size. In that case size represents the character height in pixels
|
||||
sim_banner_bitmapfont =0x00800 # If specified a fixed-size bitmap font is used. The text will also always fully face the camera and be right
|
||||
# to the specified position. Bitmap fonts are not clickable
|
||||
|
||||
|
||||
# particle objects following are mutually exclusive
|
||||
sim_particle_points1 =0 # 6 values per point (pt1 and pt2. Pt1 is start position pt2-pt1 is the initial velocity vector). i
|
||||
#Point is 1 pixel big. Only appearance is a point internally handled as a perfect sphere
|
||||
sim_particle_points2 =1 # 6 values per point. Point is 2 pixel big. Only appearance is a point internally handled as a perfect sphere
|
||||
sim_particle_points4 =2 # 6 values per point. Point is 4 pixel big. Only appearance is a point internally handled as a perfect sphere
|
||||
sim_particle_roughspheres =3 # 6 values per sphere. Only appearance is rough. Internally a perfect sphere
|
||||
sim_particle_spheres =4 # 6 values per sphere. Internally a perfect sphere
|
||||
|
||||
|
||||
|
||||
|
||||
# following can be or-combined
|
||||
sim_particle_respondable1to4 =0x0020 # the particles are respondable against shapes (against all objects that have at least one bit 1-4 activated in the global respondable mask)
|
||||
sim_particle_respondable5to8 =0x0040 # the particles are respondable against shapes (against all objects that have at least one bit 5-8 activated in the global respondable mask)
|
||||
sim_particle_particlerespondable =0x0080 # the particles are respondable against each other
|
||||
sim_particle_ignoresgravity =0x0100 # the particles ignore the effect of gravity. Not compatible with sim_particle_water
|
||||
sim_particle_invisible =0x0200 # the particles are invisible
|
||||
sim_particle_itemsizes =0x0400 # +1 value per particle (each particle can have a different size)
|
||||
sim_particle_itemdensities =0x0800 # +1 value per particle (each particle can have a different density)
|
||||
sim_particle_itemcolors =0x1000 # +3 values per particle (each particle can have a different color)
|
||||
sim_particle_cyclic =0x2000 # if the max item count was reached then the first items are overwritten.
|
||||
sim_particle_emissioncolor =0x4000 # When used in combination with sim_particle_itemcolors then the specified colors will be for the emissive component
|
||||
sim_particle_water =0x8000 # the particles are water particles (no weight in the water (i.e. when z<0)). Not compatible with sim_particle_ignoresgravity
|
||||
sim_particle_painttag =0x10000 # The particles can be seen by vision sensors (sim_particle_invisible must not be set)
|
||||
|
||||
|
||||
|
||||
|
||||
# custom user interface menu attributes
|
||||
sim_ui_menu_title =1
|
||||
sim_ui_menu_minimize =2
|
||||
sim_ui_menu_close =4
|
||||
sim_ui_menu_systemblock =8
|
||||
|
||||
|
||||
|
||||
# Boolean parameters
|
||||
sim_boolparam_hierarchy_visible =0
|
||||
sim_boolparam_console_visible =1
|
||||
sim_boolparam_collision_handling_enabled =2
|
||||
sim_boolparam_distance_handling_enabled =3
|
||||
sim_boolparam_ik_handling_enabled =4
|
||||
sim_boolparam_gcs_handling_enabled =5
|
||||
sim_boolparam_dynamics_handling_enabled =6
|
||||
sim_boolparam_joint_motion_handling_enabled =7
|
||||
sim_boolparam_path_motion_handling_enabled =8
|
||||
sim_boolparam_proximity_sensor_handling_enabled =9
|
||||
sim_boolparam_vision_sensor_handling_enabled =10
|
||||
sim_boolparam_mill_handling_enabled =11
|
||||
sim_boolparam_browser_visible =12
|
||||
sim_boolparam_scene_and_model_load_messages =13
|
||||
sim_reserved0 =14
|
||||
sim_boolparam_shape_textures_are_visible =15
|
||||
sim_boolparam_display_enabled =16
|
||||
sim_boolparam_infotext_visible =17
|
||||
sim_boolparam_statustext_open =18
|
||||
sim_boolparam_fog_enabled =19
|
||||
sim_boolparam_rml2_available =20
|
||||
sim_boolparam_rml4_available =21
|
||||
sim_boolparam_mirrors_enabled =22
|
||||
sim_boolparam_aux_clip_planes_enabled =23
|
||||
sim_boolparam_full_model_copy_from_api =24
|
||||
sim_boolparam_realtime_simulation =25
|
||||
sim_boolparam_force_show_wireless_emission =27
|
||||
sim_boolparam_force_show_wireless_reception =28
|
||||
sim_boolparam_video_recording_triggered =29
|
||||
sim_boolparam_threaded_rendering_enabled =32
|
||||
sim_boolparam_fullscreen =33
|
||||
sim_boolparam_headless =34
|
||||
sim_boolparam_hierarchy_toolbarbutton_enabled =35
|
||||
sim_boolparam_browser_toolbarbutton_enabled =36
|
||||
sim_boolparam_objectshift_toolbarbutton_enabled =37
|
||||
sim_boolparam_objectrotate_toolbarbutton_enabled=38
|
||||
sim_boolparam_force_calcstruct_all_visible =39
|
||||
sim_boolparam_force_calcstruct_all =40
|
||||
sim_boolparam_exit_request =41
|
||||
sim_boolparam_play_toolbarbutton_enabled =42
|
||||
sim_boolparam_pause_toolbarbutton_enabled =43
|
||||
sim_boolparam_stop_toolbarbutton_enabled =44
|
||||
sim_boolparam_waiting_for_trigger =45
|
||||
|
||||
|
||||
# Integer parameters
|
||||
sim_intparam_error_report_mode =0 # Check sim_api_errormessage_... constants above for valid values
|
||||
sim_intparam_program_version =1 # e.g Version 2.1.4 --> 20104. Can only be read
|
||||
sim_intparam_instance_count =2 # do not use anymore (always returns 1 since CoppeliaSim 2.5.11)
|
||||
sim_intparam_custom_cmd_start_id =3 # can only be read
|
||||
sim_intparam_compilation_version =4 # 0=evaluation version 1=full version 2=player version. Can only be read
|
||||
sim_intparam_current_page =5
|
||||
sim_intparam_flymode_camera_handle =6 # can only be read
|
||||
sim_intparam_dynamic_step_divider =7 # can only be read
|
||||
sim_intparam_dynamic_engine =8 # 0=Bullet 1=ODE. 2=Vortex.
|
||||
sim_intparam_server_port_start =9 # can only be read
|
||||
sim_intparam_server_port_range =10 # can only be read
|
||||
sim_intparam_visible_layers =11
|
||||
sim_intparam_infotext_style =12
|
||||
sim_intparam_settings =13
|
||||
sim_intparam_edit_mode_type =14 # can only be read
|
||||
sim_intparam_server_port_next =15 # is initialized at sim_intparam_server_port_start
|
||||
sim_intparam_qt_version =16 # version of the used Qt framework
|
||||
sim_intparam_event_flags_read =17 # can only be read
|
||||
sim_intparam_event_flags_read_clear =18 # can only be read
|
||||
sim_intparam_platform =19 # can only be read
|
||||
sim_intparam_scene_unique_id =20 # can only be read
|
||||
sim_intparam_work_thread_count =21
|
||||
sim_intparam_mouse_x =22
|
||||
sim_intparam_mouse_y =23
|
||||
sim_intparam_core_count =24
|
||||
sim_intparam_work_thread_calc_time_ms =25
|
||||
sim_intparam_idle_fps =26
|
||||
sim_intparam_prox_sensor_select_down =27
|
||||
sim_intparam_prox_sensor_select_up =28
|
||||
sim_intparam_stop_request_counter =29
|
||||
sim_intparam_program_revision =30
|
||||
sim_intparam_mouse_buttons =31
|
||||
sim_intparam_dynamic_warning_disabled_mask =32
|
||||
sim_intparam_simulation_warning_disabled_mask =33
|
||||
sim_intparam_scene_index =34
|
||||
sim_intparam_motionplanning_seed =35
|
||||
sim_intparam_speedmodifier =36
|
||||
|
||||
# Float parameters
|
||||
sim_floatparam_rand=0 # random value (0.0-1.0)
|
||||
sim_floatparam_simulation_time_step =1
|
||||
sim_floatparam_stereo_distance =2
|
||||
|
||||
# String parameters
|
||||
sim_stringparam_application_path=0 # path of CoppeliaSim's executable
|
||||
sim_stringparam_video_filename=1
|
||||
sim_stringparam_app_arg1 =2
|
||||
sim_stringparam_app_arg2 =3
|
||||
sim_stringparam_app_arg3 =4
|
||||
sim_stringparam_app_arg4 =5
|
||||
sim_stringparam_app_arg5 =6
|
||||
sim_stringparam_app_arg6 =7
|
||||
sim_stringparam_app_arg7 =8
|
||||
sim_stringparam_app_arg8 =9
|
||||
sim_stringparam_app_arg9 =10
|
||||
sim_stringparam_scene_path_and_name =13
|
||||
|
||||
# Array parameters
|
||||
sim_arrayparam_gravity =0
|
||||
sim_arrayparam_fog =1
|
||||
sim_arrayparam_fog_color =2
|
||||
sim_arrayparam_background_color1=3
|
||||
sim_arrayparam_background_color2=4
|
||||
sim_arrayparam_ambient_light =5
|
||||
sim_arrayparam_random_euler =6
|
||||
|
||||
sim_objintparam_visibility_layer= 10
|
||||
sim_objfloatparam_abs_x_velocity= 11
|
||||
sim_objfloatparam_abs_y_velocity= 12
|
||||
sim_objfloatparam_abs_z_velocity= 13
|
||||
sim_objfloatparam_abs_rot_velocity= 14
|
||||
sim_objfloatparam_objbbox_min_x= 15
|
||||
sim_objfloatparam_objbbox_min_y= 16
|
||||
sim_objfloatparam_objbbox_min_z= 17
|
||||
sim_objfloatparam_objbbox_max_x= 18
|
||||
sim_objfloatparam_objbbox_max_y= 19
|
||||
sim_objfloatparam_objbbox_max_z= 20
|
||||
sim_objfloatparam_modelbbox_min_x= 21
|
||||
sim_objfloatparam_modelbbox_min_y= 22
|
||||
sim_objfloatparam_modelbbox_min_z= 23
|
||||
sim_objfloatparam_modelbbox_max_x= 24
|
||||
sim_objfloatparam_modelbbox_max_y= 25
|
||||
sim_objfloatparam_modelbbox_max_z= 26
|
||||
sim_objintparam_collection_self_collision_indicator= 27
|
||||
sim_objfloatparam_transparency_offset= 28
|
||||
sim_objintparam_child_role= 29
|
||||
sim_objintparam_parent_role= 30
|
||||
sim_objintparam_manipulation_permissions= 31
|
||||
sim_objintparam_illumination_handle= 32
|
||||
|
||||
sim_visionfloatparam_near_clipping= 1000
|
||||
sim_visionfloatparam_far_clipping= 1001
|
||||
sim_visionintparam_resolution_x= 1002
|
||||
sim_visionintparam_resolution_y= 1003
|
||||
sim_visionfloatparam_perspective_angle= 1004
|
||||
sim_visionfloatparam_ortho_size= 1005
|
||||
sim_visionintparam_disabled_light_components= 1006
|
||||
sim_visionintparam_rendering_attributes= 1007
|
||||
sim_visionintparam_entity_to_render= 1008
|
||||
sim_visionintparam_windowed_size_x= 1009
|
||||
sim_visionintparam_windowed_size_y= 1010
|
||||
sim_visionintparam_windowed_pos_x= 1011
|
||||
sim_visionintparam_windowed_pos_y= 1012
|
||||
sim_visionintparam_pov_focal_blur= 1013
|
||||
sim_visionfloatparam_pov_blur_distance= 1014
|
||||
sim_visionfloatparam_pov_aperture= 1015
|
||||
sim_visionintparam_pov_blur_sampled= 1016
|
||||
sim_visionintparam_render_mode= 1017
|
||||
|
||||
sim_jointintparam_motor_enabled= 2000
|
||||
sim_jointintparam_ctrl_enabled= 2001
|
||||
sim_jointfloatparam_pid_p= 2002
|
||||
sim_jointfloatparam_pid_i= 2003
|
||||
sim_jointfloatparam_pid_d= 2004
|
||||
sim_jointfloatparam_intrinsic_x= 2005
|
||||
sim_jointfloatparam_intrinsic_y= 2006
|
||||
sim_jointfloatparam_intrinsic_z= 2007
|
||||
sim_jointfloatparam_intrinsic_qx= 2008
|
||||
sim_jointfloatparam_intrinsic_qy= 2009
|
||||
sim_jointfloatparam_intrinsic_qz= 2010
|
||||
sim_jointfloatparam_intrinsic_qw= 2011
|
||||
sim_jointfloatparam_velocity= 2012
|
||||
sim_jointfloatparam_spherical_qx= 2013
|
||||
sim_jointfloatparam_spherical_qy= 2014
|
||||
sim_jointfloatparam_spherical_qz= 2015
|
||||
sim_jointfloatparam_spherical_qw= 2016
|
||||
sim_jointfloatparam_upper_limit= 2017
|
||||
sim_jointfloatparam_kc_k= 2018
|
||||
sim_jointfloatparam_kc_c= 2019
|
||||
sim_jointfloatparam_ik_weight= 2021
|
||||
sim_jointfloatparam_error_x= 2022
|
||||
sim_jointfloatparam_error_y= 2023
|
||||
sim_jointfloatparam_error_z= 2024
|
||||
sim_jointfloatparam_error_a= 2025
|
||||
sim_jointfloatparam_error_b= 2026
|
||||
sim_jointfloatparam_error_g= 2027
|
||||
sim_jointfloatparam_error_pos= 2028
|
||||
sim_jointfloatparam_error_angle= 2029
|
||||
sim_jointintparam_velocity_lock= 2030
|
||||
sim_jointintparam_vortex_dep_handle= 2031
|
||||
sim_jointfloatparam_vortex_dep_multiplication= 2032
|
||||
sim_jointfloatparam_vortex_dep_offset= 2033
|
||||
|
||||
sim_shapefloatparam_init_velocity_x= 3000
|
||||
sim_shapefloatparam_init_velocity_y= 3001
|
||||
sim_shapefloatparam_init_velocity_z= 3002
|
||||
sim_shapeintparam_static= 3003
|
||||
sim_shapeintparam_respondable= 3004
|
||||
sim_shapefloatparam_mass= 3005
|
||||
sim_shapefloatparam_texture_x= 3006
|
||||
sim_shapefloatparam_texture_y= 3007
|
||||
sim_shapefloatparam_texture_z= 3008
|
||||
sim_shapefloatparam_texture_a= 3009
|
||||
sim_shapefloatparam_texture_b= 3010
|
||||
sim_shapefloatparam_texture_g= 3011
|
||||
sim_shapefloatparam_texture_scaling_x= 3012
|
||||
sim_shapefloatparam_texture_scaling_y= 3013
|
||||
sim_shapeintparam_culling= 3014
|
||||
sim_shapeintparam_wireframe= 3015
|
||||
sim_shapeintparam_compound= 3016
|
||||
sim_shapeintparam_convex= 3017
|
||||
sim_shapeintparam_convex_check= 3018
|
||||
sim_shapeintparam_respondable_mask= 3019
|
||||
sim_shapefloatparam_init_velocity_a= 3020
|
||||
sim_shapefloatparam_init_velocity_b= 3021
|
||||
sim_shapefloatparam_init_velocity_g= 3022
|
||||
sim_shapestringparam_color_name= 3023
|
||||
sim_shapeintparam_edge_visibility= 3024
|
||||
sim_shapefloatparam_shading_angle= 3025
|
||||
sim_shapefloatparam_edge_angle= 3026
|
||||
sim_shapeintparam_edge_borders_hidden= 3027
|
||||
|
||||
sim_proxintparam_ray_invisibility= 4000
|
||||
|
||||
sim_forcefloatparam_error_x= 5000
|
||||
sim_forcefloatparam_error_y= 5001
|
||||
sim_forcefloatparam_error_z= 5002
|
||||
sim_forcefloatparam_error_a= 5003
|
||||
sim_forcefloatparam_error_b= 5004
|
||||
sim_forcefloatparam_error_g= 5005
|
||||
sim_forcefloatparam_error_pos= 5006
|
||||
sim_forcefloatparam_error_angle= 5007
|
||||
|
||||
sim_lightintparam_pov_casts_shadows= 8000
|
||||
|
||||
sim_cameraintparam_disabled_light_components= 9000
|
||||
sim_camerafloatparam_perspective_angle= 9001
|
||||
sim_camerafloatparam_ortho_size= 9002
|
||||
sim_cameraintparam_rendering_attributes= 9003
|
||||
sim_cameraintparam_pov_focal_blur= 9004
|
||||
sim_camerafloatparam_pov_blur_distance= 9005
|
||||
sim_camerafloatparam_pov_aperture= 9006
|
||||
sim_cameraintparam_pov_blur_samples= 9007
|
||||
|
||||
sim_dummyintparam_link_type= 10000
|
||||
|
||||
sim_mirrorfloatparam_width= 12000
|
||||
sim_mirrorfloatparam_height= 12001
|
||||
sim_mirrorfloatparam_reflectance= 12002
|
||||
sim_mirrorintparam_enable= 12003
|
||||
|
||||
sim_pplanfloatparam_x_min= 20000
|
||||
sim_pplanfloatparam_x_range= 20001
|
||||
sim_pplanfloatparam_y_min= 20002
|
||||
sim_pplanfloatparam_y_range= 20003
|
||||
sim_pplanfloatparam_z_min= 20004
|
||||
sim_pplanfloatparam_z_range= 20005
|
||||
sim_pplanfloatparam_delta_min= 20006
|
||||
sim_pplanfloatparam_delta_range= 20007
|
||||
|
||||
sim_mplanintparam_nodes_computed= 25000
|
||||
sim_mplanintparam_prepare_nodes= 25001
|
||||
sim_mplanintparam_clear_nodes= 25002
|
||||
|
||||
# User interface elements
|
||||
sim_gui_menubar =0x0001
|
||||
sim_gui_popups =0x0002
|
||||
sim_gui_toolbar1 =0x0004
|
||||
sim_gui_toolbar2 =0x0008
|
||||
sim_gui_hierarchy =0x0010
|
||||
sim_gui_infobar =0x0020
|
||||
sim_gui_statusbar =0x0040
|
||||
sim_gui_scripteditor =0x0080
|
||||
sim_gui_scriptsimulationparameters =0x0100
|
||||
sim_gui_dialogs =0x0200
|
||||
sim_gui_browser =0x0400
|
||||
sim_gui_all =0xffff
|
||||
|
||||
|
||||
# Joint modes
|
||||
sim_jointmode_passive =0
|
||||
sim_jointmode_motion =1
|
||||
sim_jointmode_ik =2
|
||||
sim_jointmode_ikdependent =3
|
||||
sim_jointmode_dependent =4
|
||||
sim_jointmode_force =5
|
||||
|
||||
|
||||
# Navigation and selection modes with the mouse. Lower byte values are mutually exclusive upper byte bits can be combined
|
||||
sim_navigation_passive =0x0000
|
||||
sim_navigation_camerashift =0x0001
|
||||
sim_navigation_camerarotate =0x0002
|
||||
sim_navigation_camerazoom =0x0003
|
||||
sim_navigation_cameratilt =0x0004
|
||||
sim_navigation_cameraangle =0x0005
|
||||
sim_navigation_camerafly =0x0006
|
||||
sim_navigation_objectshift =0x0007
|
||||
sim_navigation_objectrotate =0x0008
|
||||
sim_navigation_reserved2 =0x0009
|
||||
sim_navigation_reserved3 =0x000A
|
||||
sim_navigation_jointpathtest =0x000B
|
||||
sim_navigation_ikmanip =0x000C
|
||||
sim_navigation_objectmultipleselection =0x000D
|
||||
# Bit-combine following values and add them to one of above's values for a valid navigation mode
|
||||
sim_navigation_reserved4 =0x0100
|
||||
sim_navigation_clickselection =0x0200
|
||||
sim_navigation_ctrlselection =0x0400
|
||||
sim_navigation_shiftselection =0x0800
|
||||
sim_navigation_camerazoomwheel =0x1000
|
||||
sim_navigation_camerarotaterightbutton =0x2000
|
||||
|
||||
|
||||
|
||||
#Remote API constants
|
||||
SIMX_VERSION =0
|
||||
# Remote API message header structure
|
||||
SIMX_HEADER_SIZE =18
|
||||
simx_headeroffset_crc =0 # 1 simxUShort. Generated by the client or server. The CRC for the message
|
||||
simx_headeroffset_version =2 # 1 byte. Generated by the client or server. The version of the remote API software
|
||||
simx_headeroffset_message_id =3 # 1 simxInt. Generated by the client (and used in a reply by the server)
|
||||
simx_headeroffset_client_time =7 # 1 simxInt. Client time stamp generated by the client (and sent back by the server)
|
||||
simx_headeroffset_server_time =11 # 1 simxInt. Generated by the server when a reply is generated. The server timestamp
|
||||
simx_headeroffset_scene_id =15 # 1 simxUShort. Generated by the server. A unique ID identifying the scene currently displayed
|
||||
simx_headeroffset_server_state =17 # 1 byte. Generated by the server. Bit coded 0 set --> simulation not stopped 1 set --> simulation paused 2 set --> real-time switch on 3-5 edit mode type (0=no edit mode 1=triangle 2=vertex 3=edge 4=path 5=UI)
|
||||
|
||||
# Remote API command header
|
||||
SIMX_SUBHEADER_SIZE =26
|
||||
simx_cmdheaderoffset_mem_size =0 # 1 simxInt. Generated by the client or server. The buffer size of the command.
|
||||
simx_cmdheaderoffset_full_mem_size =4 # 1 simxInt. Generated by the client or server. The full buffer size of the command (applies to split chunks).
|
||||
simx_cmdheaderoffset_pdata_offset0 =8 # 1 simxUShort. Generated by the client or server. The amount of data that is part of the command identification.
|
||||
simx_cmdheaderoffset_pdata_offset1 =10 # 1 simxInt. Generated by the client or server. The amount of shift of the pure data buffer (applies to split chunks).
|
||||
simx_cmdheaderoffset_cmd=14 # 1 simxInt. Generated by the client (and used in a reply by the server). The command combined with the operation mode of the command.
|
||||
simx_cmdheaderoffset_delay_or_split =18 # 1 simxUShort. Generated by the client or server. The amount of delay in ms of a continuous command or the max. pure data size to send at once (applies to split commands).
|
||||
simx_cmdheaderoffset_sim_time =20 # 1 simxInt. Generated by the server. The simulation time (in ms) when the command was executed (or 0 if simulation is not running)
|
||||
simx_cmdheaderoffset_status =24 # 1 byte. Generated by the server. (1 bit 0 is set --> error in function execution on server side). The client writes bit 1 if command cannot be overwritten
|
||||
simx_cmdheaderoffset_reserved =25 # 1 byte. Not yet used
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
# Regular operation modes
|
||||
simx_opmode_oneshot =0x000000 # sends command as one chunk. Reply will also come as one chunk. Doesn't wait for the reply.
|
||||
simx_opmode_blocking =0x010000 # sends command as one chunk. Reply will also come as one chunk. Waits for the reply (_REPLY_WAIT_TIMEOUT_IN_MS is the timeout).
|
||||
simx_opmode_oneshot_wait =0x010000 # sends command as one chunk. Reply will also come as one chunk. Waits for the reply (_REPLY_WAIT_TIMEOUT_IN_MS is the timeout).
|
||||
simx_opmode_continuous =0x020000
|
||||
simx_opmode_streaming =0x020000 # sends command as one chunk. Command will be stored on the server and always executed
|
||||
#(every x ms (as far as possible) where x can be 0-65535. just add x to opmode_continuous).
|
||||
# A reply will be sent continuously each time as one chunk. Doesn't wait for the reply.
|
||||
|
||||
# Operation modes for heavy data
|
||||
simx_opmode_oneshot_split =0x030000 # sends command as several chunks (max chunk size is x bytes where x can be _MIN_SPLIT_AMOUNT_IN_BYTES-65535. Just add x to opmode_oneshot_split). Reply will also come as several chunks. Doesn't wait for the reply.
|
||||
simx_opmode_continuous_split =0x040000
|
||||
simx_opmode_streaming_split =0x040000 # sends command as several chunks (max chunk size is x bytes where x can be _MIN_SPLIT_AMOUNT_IN_BYTES-65535. Just add x to opmode_continuous_split). Command will be stored on the server and always executed. A reply will be sent continuously each time as several chunks. Doesn't wait for the reply.
|
||||
|
||||
# Special operation modes
|
||||
simx_opmode_discontinue =0x050000 # removes and cancels all commands stored on the client or server side (also continuous commands)
|
||||
simx_opmode_buffer =0x060000 # doesn't send anything but checks if a reply for the given command is available in the input buffer (i.e. previously received from the server)
|
||||
simx_opmode_remove =0x070000 # doesn't send anything and doesn't return any specific value. It just erases a similar command reply in the inbox (to free some memory)
|
||||
|
||||
|
||||
# Command return codes
|
||||
simx_return_ok =0x000000
|
||||
simx_return_novalue_flag =0x000001 # input buffer doesn't contain the specified command
|
||||
simx_return_timeout_flag =0x000002 # command reply not received in time for opmode_oneshot_wait operation mode
|
||||
simx_return_illegal_opmode_flag =0x000004 # command doesn't support the specified operation mode
|
||||
simx_return_remote_error_flag =0x000008 # command caused an error on the server side
|
||||
simx_return_split_progress_flag =0x000010 # previous similar command not yet fully processed (applies to opmode_oneshot_split operation modes)
|
||||
simx_return_local_error_flag =0x000020 # command caused an error on the client side
|
||||
simx_return_initialize_error_flag =0x000040 # simxStart was not yet called
|
||||
|
||||
# Following for backward compatibility (same as above)
|
||||
simx_error_noerror =0x000000
|
||||
simx_error_novalue_flag =0x000001 # input buffer doesn't contain the specified command
|
||||
simx_error_timeout_flag =0x000002 # command reply not received in time for opmode_oneshot_wait operation mode
|
||||
simx_error_illegal_opmode_flag =0x000004 # command doesn't support the specified operation mode
|
||||
simx_error_remote_error_flag =0x000008 # command caused an error on the server side
|
||||
simx_error_split_progress_flag =0x000010 # previous similar command not yet fully processed (applies to opmode_oneshot_split operation modes)
|
||||
simx_error_local_error_flag =0x000020 # command caused an error on the client side
|
||||
simx_error_initialize_error_flag =0x000040 # simxStart was not yet called
|
||||
|
||||
|
||||
Binary file not shown.
@ -0,0 +1,58 @@
|
||||
# This small example illustrates how to use the remote API
|
||||
# synchronous mode. The synchronous mode needs to be
|
||||
# pre-enabled on the server side. You would do this by
|
||||
# starting the server (e.g. in a child script) with:
|
||||
#
|
||||
# simRemoteApi.start(19999,1300,false,true)
|
||||
#
|
||||
# But in this example we try to connect on port
|
||||
# 19997 where there should be a continuous remote API
|
||||
# server service already running and pre-enabled for
|
||||
# synchronous mode.
|
||||
#
|
||||
#
|
||||
# IMPORTANT: for each successful call to simxStart, there
|
||||
# should be a corresponding call to simxFinish at the end!
|
||||
|
||||
try:
|
||||
import sim
|
||||
except:
|
||||
print ('--------------------------------------------------------------')
|
||||
print ('"sim.py" could not be imported. This means very probably that')
|
||||
print ('either "sim.py" or the remoteApi library could not be found.')
|
||||
print ('Make sure both are in the same folder as this file,')
|
||||
print ('or appropriately adjust the file "sim.py"')
|
||||
print ('--------------------------------------------------------------')
|
||||
print ('')
|
||||
|
||||
import time
|
||||
import sys
|
||||
|
||||
print ('Program started')
|
||||
sim.simxFinish(-1) # just in case, close all opened connections
|
||||
clientID=sim.simxStart('127.0.0.1',19997,True,True,5000,5) # Connect to CoppeliaSim
|
||||
if clientID!=-1:
|
||||
print ('Connected to remote API server')
|
||||
|
||||
# enable the synchronous mode on the client:
|
||||
sim.simxSynchronous(clientID,True)
|
||||
|
||||
# start the simulation:
|
||||
sim.simxStartSimulation(clientID,sim.simx_opmode_blocking)
|
||||
|
||||
# Now step a few times:
|
||||
for i in range(1,10):
|
||||
if sys.version_info[0] == 3:
|
||||
input('Press <enter> key to step the simulation!')
|
||||
else:
|
||||
raw_input('Press <enter> key to step the simulation!')
|
||||
sim.simxSynchronousTrigger(clientID);
|
||||
|
||||
# stop the simulation:
|
||||
sim.simxStopSimulation(clientID,sim.simx_opmode_blocking)
|
||||
|
||||
# Now close the connection to CoppeliaSim:
|
||||
sim.simxFinish(clientID)
|
||||
else:
|
||||
print ('Failed connecting to remote API server')
|
||||
print ('Program ended')
|
||||
@ -0,0 +1,59 @@
|
||||
# Make sure to have the server side running in CoppeliaSim:
|
||||
# in a child script of a CoppeliaSim scene, add following command
|
||||
# to be executed just once, at simulation start:
|
||||
#
|
||||
# simRemoteApi.start(19999)
|
||||
#
|
||||
# then start simulation, and run this program.
|
||||
#
|
||||
# IMPORTANT: for each successful call to simxStart, there
|
||||
# should be a corresponding call to simxFinish at the end!
|
||||
|
||||
try:
|
||||
import sim
|
||||
except:
|
||||
print ('--------------------------------------------------------------')
|
||||
print ('"sim.py" could not be imported. This means very probably that')
|
||||
print ('either "sim.py" or the remoteApi library could not be found.')
|
||||
print ('Make sure both are in the same folder as this file,')
|
||||
print ('or appropriately adjust the file "sim.py"')
|
||||
print ('--------------------------------------------------------------')
|
||||
print ('')
|
||||
|
||||
import time
|
||||
|
||||
print ('Program started')
|
||||
sim.simxFinish(-1) # just in case, close all opened connections
|
||||
clientID=sim.simxStart('127.0.0.1',19999,True,True,5000,5) # Connect to CoppeliaSim
|
||||
if clientID!=-1:
|
||||
print ('Connected to remote API server')
|
||||
|
||||
# Now try to retrieve data in a blocking fashion (i.e. a service call):
|
||||
res,objs=sim.simxGetObjects(clientID,sim.sim_handle_all,sim.simx_opmode_blocking)
|
||||
if res==sim.simx_return_ok:
|
||||
print ('Number of objects in the scene: ',len(objs))
|
||||
else:
|
||||
print ('Remote API function call returned with error code: ',res)
|
||||
|
||||
time.sleep(2)
|
||||
|
||||
# Now retrieve streaming data (i.e. in a non-blocking fashion):
|
||||
startTime=time.time()
|
||||
sim.simxGetIntegerParameter(clientID,sim.sim_intparam_mouse_x,sim.simx_opmode_streaming) # Initialize streaming
|
||||
while time.time()-startTime < 5:
|
||||
returnCode,data=sim.simxGetIntegerParameter(clientID,sim.sim_intparam_mouse_x,sim.simx_opmode_buffer) # Try to retrieve the streamed data
|
||||
if returnCode==sim.simx_return_ok: # After initialization of streaming, it will take a few ms before the first value arrives, so check the return code
|
||||
print ('Mouse position x: ',data) # Mouse position x is actualized when the cursor is over CoppeliaSim's window
|
||||
time.sleep(0.005)
|
||||
|
||||
# Now send some data to CoppeliaSim in a non-blocking fashion:
|
||||
sim.simxAddStatusbarMessage(clientID,'Hello CoppeliaSim!',sim.simx_opmode_oneshot)
|
||||
|
||||
# Before closing the connection to CoppeliaSim, make sure that the last command sent out had time to arrive. You can guarantee this with (for example):
|
||||
sim.simxGetPingTime(clientID)
|
||||
|
||||
# Now close the connection to CoppeliaSim:
|
||||
sim.simxFinish(clientID)
|
||||
else:
|
||||
print ('Failed connecting to remote API server')
|
||||
print ('Program ended')
|
||||
@ -0,0 +1,286 @@
|
||||
function sysCall_threadmain()
|
||||
-- Get some handles first:
|
||||
-- local leftMotor=sim.getObjectHandle("remoteApiControlledBubbleRobLeftMotor") -- Handle of the left motor
|
||||
-- local rightMotor=sim.getObjectHandle("remoteApiControlledBubbleRobRightMotor") -- Handle of the right motor
|
||||
-- local noseSensor=sim.getObjectHandle("remoteApiControlledBubbleRobSensingNose") -- Handle of the proximity sensor
|
||||
|
||||
-- Choose a port that is probably not used (try to always use a similar code):
|
||||
--[[
|
||||
sim.setThreadAutomaticSwitch(false)
|
||||
local portNb=sim.getInt32Parameter(sim.intparam_server_port_next)
|
||||
local portStart=sim.getInt32Parameter(sim.intparam_server_port_start)
|
||||
local portRange=sim.getInt32Parameter(sim.intparam_server_port_range)
|
||||
local newPortNb=portNb+1
|
||||
if (newPortNb>=portStart+portRange) then
|
||||
newPortNb=portStart
|
||||
end
|
||||
sim.setInt32Parameter(sim.intparam_server_port_next,newPortNb)
|
||||
sim.setThreadAutomaticSwitch(true)
|
||||
--]]
|
||||
|
||||
-- Check what OS we are using:
|
||||
platf=sim.getInt32Parameter(sim.intparam_platform)
|
||||
if (platf==0) then
|
||||
pluginFile='simExtRemoteApi.dll'
|
||||
end
|
||||
if (platf==1) then
|
||||
pluginFile='libsimExtRemoteApi.dylib'
|
||||
end
|
||||
if (platf==2) then
|
||||
pluginFile='libsimExtRemoteApi.so'
|
||||
end
|
||||
|
||||
-- Check if the required legacy remote Api plugin is there:
|
||||
moduleName=0
|
||||
moduleVersion=0
|
||||
index=0
|
||||
pluginNotFound=true
|
||||
while moduleName do
|
||||
moduleName,moduleVersion=sim.getModuleName(index)
|
||||
if (moduleName=='RemoteApi') then
|
||||
pluginNotFound=false
|
||||
end
|
||||
index=index+1
|
||||
end
|
||||
|
||||
if (pluginNotFound) then
|
||||
-- Plugin was not found
|
||||
sim.displayDialog('Error',"Remote Api plugin was not found. ('"..pluginFile.."')&&nSimulation will not run properly",sim.dlgstyle_ok,true,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1})
|
||||
else
|
||||
-- Ok, we found the plugin.
|
||||
-- We first start the remote Api server service (this requires the simExtRemoteApi plugin):
|
||||
-- simRemoteApi.start(portNb) -- this server function will automatically close again at simulation end
|
||||
simRemoteApi.start(19999)
|
||||
-- Now we start the client application:
|
||||
|
||||
|
||||
--result=sim.launchExecutable('bubbleRobClient_remoteApi',portNb.." "..leftMotor.." "..rightMotor.." "..noseSensor,0) -- set the last argument to 1 to see the console of the launched client
|
||||
|
||||
-- if (result==-1) then
|
||||
-- The executable could not be launched!
|
||||
-- sim.displayDialog('Error',"'bubbleRobClient_remoteApi' could not be launched. &&nSimulation will not run properly",sim.dlgstyle_ok,true,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1})
|
||||
-- end
|
||||
end
|
||||
|
||||
-- This thread ends here. The bubbleRob will however still be controlled by
|
||||
-- the client application via the legacy remote Api mechanism!
|
||||
end
|
||||
|
||||
function sysCall_cleanup()
|
||||
-- Put some clean-up code here
|
||||
end
|
||||
|
||||
-- See the user manual or the available code snippets for additional callback functions and details
|
||||
|
||||
|
||||
function getRobotHandle(inInts,inFloats,inStrings,inBuffer)
|
||||
-- inInts, inFloats and inStrings are tables
|
||||
-- inBuffer is a string
|
||||
robotHandle=sim.getObjectAssociatedWithScript(sim.handle_self)
|
||||
-- Perform any type of operation here.
|
||||
|
||||
-- Always return 3 tables and a string, e.g.:
|
||||
return {},{},{},robotHandle
|
||||
end
|
||||
|
||||
-- This is an extremely simple control script for the motorbike. It is meant as a SIMPLE example!
|
||||
getRobotPos=function(inInts,inFloats,inStrings,inBuffer)
|
||||
robotHandle=sim.getObjectAssociatedWithScript(sim.handle_self)
|
||||
position=sim.getObjectPosition(robotHandle,-1)
|
||||
return {},position,{},''
|
||||
end
|
||||
|
||||
function setRobotPos(inInts,inFloats,inStrings,inBuffer)
|
||||
robotHandle=sim.getObjectAssociatedWithScript(sim.handle_self)
|
||||
position=sim.getObjectPosition(robotHandle,-1)
|
||||
return {},position,{},''
|
||||
end
|
||||
|
||||
|
||||
--[[
|
||||
robotHandle=sim.getObjectAssociatedWithScript(sim.handle_self)
|
||||
-- Retrieve some object handles:
|
||||
motor=sim.getObjectHandle('Motorbike_motor')
|
||||
steering=sim.getObjectHandle('Motorbike_steeringMotor')
|
||||
motorbike=sim.getObjectHandle('Motorbike')
|
||||
path=sim.getObjectHandle('Motorbike_path')
|
||||
pathFollower=sim.getObjectHandle('Motorbike_pathFollower')
|
||||
pathFollowerTarget=sim.getObjectHandle('Motorbike_pathFollowerTarget')
|
||||
-- Make the path object orphan (because at the beginning of the simulation it is still built on the motorbike model):
|
||||
sim.setObjectParent(path,-1,true)
|
||||
-- Set a fixed velocity:
|
||||
velocity = 30
|
||||
cStep = 5
|
||||
step = 0.00
|
||||
deviation = 0.0
|
||||
|
||||
t = 0
|
||||
CInfo = {}
|
||||
for i = 1,2,1 do
|
||||
CInfo[i] = 0
|
||||
end
|
||||
|
||||
oldPos = {}
|
||||
nowLA = {}
|
||||
basicPos = {}
|
||||
for i = 1,3,1 do
|
||||
oldPos[i] = 0
|
||||
nowLA[2*i-1] = 0.0
|
||||
nowLA[2*i] = 0.0
|
||||
basicPos[1] = {5.35,14.65,0.7575,-0.020503, -1.486533, -1.590551}
|
||||
basicPos[2] = {-16.175,-1.875,0.7575,1.485270,-0.178061,-0.010568}
|
||||
basicPos[3] = {-9.45,-16.3,0.7575,0.819122, 1.447427, 0.747879}
|
||||
basicPos[4] = {8.975,-12.7,0.7575,-1.451466,0.786589,3.057870}
|
||||
end
|
||||
inclination = 0
|
||||
counter = 0
|
||||
------------------------------------------
|
||||
-- Check if the required ROS plugin is there:
|
||||
moduleName=0
|
||||
moduleVersion=0
|
||||
index=0
|
||||
RosInterface=false
|
||||
while moduleName do
|
||||
moduleName,moduleVersion=sim.getModuleName(index)
|
||||
if (moduleName=='RosInterface') then
|
||||
RosInterface=true
|
||||
end
|
||||
index=index+1
|
||||
end
|
||||
|
||||
if (RosInterface) then
|
||||
sim.addStatusbarMessage('Ros Connected')
|
||||
-- Prepare the sensor publisher and the motor speed subscribers:
|
||||
-- Publisher
|
||||
The_Pos = simExtRosInterface_advertise('/The_P', 'std_msgs/Float32MultiArray')
|
||||
The_Vec = simExtRosInterface_advertise('/The_V', 'std_msgs/Float32MultiArray')
|
||||
The_Inf = simExtRosInterface_advertise('/The_I', 'std_msgs/Float32MultiArray')
|
||||
The_Loc = simExtRosInterface_advertise('/The_L', 'std_msgs/Float32MultiArray')
|
||||
The_Time = simExtRosInterface_advertise('/The_T', 'std_msgs/Float32MultiArray')
|
||||
--Subscriber
|
||||
Controller = simExtRosInterface_subscribe('/Ctrl_I','std_msgs/Float32MultiArray','setControlInfo')
|
||||
ResetRobot=simExtRosInterface_subscribe('/resetRobot','std_msgs/UInt32','resetRobot_cb')
|
||||
end
|
||||
end
|
||||
--]]
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
-- do some initialization here
|
||||
robotHandle=sim.getObjectAssociatedWithScript(sim.handle_self)
|
||||
|
||||
-- Reset robot subscriber callback
|
||||
allModelObjects=sim.getObjectsInTree(robotHandle)
|
||||
sim.setThreadAutomaticSwitch(false)
|
||||
for i=1,#allModelObjects,1 do
|
||||
sim.resetDynamicObject(allModelObjects[i]) -- reset all objects in the model
|
||||
end
|
||||
--basicEle = sim.getObjectOrientation(robotHandle, -1)
|
||||
--basicEle = sim.getObjectQuaternion(robotHandle, -1)
|
||||
--sim.addStatusbarMessage(string.format(" -> %f --> msg: %d",basicEle[1],msg.data))
|
||||
--sim.addStatusbarMessage(string.format("--> %f --> msg: %d",basicEle[2],msg.data))
|
||||
--sim.addStatusbarMessage(string.format("--> %f --> msg: %d",basicEle[3],msg.data))
|
||||
--sim.addStatusbarMessage(string.format("--> %f",basicEle[4]))
|
||||
--sim.addStatusbarMessage(string.format("--> Start Statement: %d", msg.data))
|
||||
oldPos = {}
|
||||
nowLA = {}
|
||||
basicPos = {}
|
||||
for i = 1,3,1 do
|
||||
oldPos[i] = 0
|
||||
nowLA[2*i-1] = 0.0
|
||||
nowLA[2*i] = 0.0
|
||||
basicPos[1] = {5.35,14.65,0.7575,-0.020503, -1.486533, -1.590551}
|
||||
basicPos[2] = {-16.175,-1.875,0.7575,1.485270,-0.178061,-0.010568}
|
||||
basicPos[3] = {-9.45,-16.3,0.7575,0.819122, 1.447427, 0.747879}
|
||||
basicPos[4] = {8.975,-12.7,0.7575,-1.451466,0.786589,3.057870}
|
||||
end
|
||||
|
||||
basicLoc = {}
|
||||
basicAng = {}
|
||||
nowState = msg.data
|
||||
for i = 1,3,1 do
|
||||
basicLoc[i] = basicPos[nowState][i]
|
||||
basicAng[i] = basicPos[nowState][i+3]
|
||||
end
|
||||
sim.setObjectPosition(robotHandle,-1,basicLoc)
|
||||
sim.setObjectOrientation(robotHandle,-1,basicAng)
|
||||
sim.setThreadAutomaticSwitch(true)
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
function sysCall_threadmain()
|
||||
-- Check if the required plugin is there:
|
||||
moduleName=0
|
||||
moduleVersion=0
|
||||
index=0
|
||||
bubbleRobModuleNotFound=true
|
||||
while moduleName do
|
||||
moduleName,moduleVersion=sim.getModuleName(index)
|
||||
if (moduleName=='BubbleRob') then
|
||||
bubbleRobModuleNotFound=false
|
||||
end
|
||||
index=index+1
|
||||
end
|
||||
if (bubbleRobModuleNotFound) then
|
||||
sim.displayDialog('Error','BubbleRob plugin was not found. (v_repExtBubbleRob.dll)&&nSimulation will not run properly',sim.dlgstyle_ok,true,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1})
|
||||
else
|
||||
local jointHandles={sim.getObjectHandle('leftMotor'),sim.getObjectHandle('rightMotor')}
|
||||
local sensorHandle=sim.getObjectHandle('sensingNose')
|
||||
local robHandle=simBubble.create(jointHandles,sensorHandle,{0.5,0.25})
|
||||
if robHandle>=0 then
|
||||
simBubble.start(robHandle,20) -- control happens here
|
||||
simBubble.stop(robHandle)
|
||||
simBubble.destroy(robHandle)
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
function sysCall_init()
|
||||
movingObjects={'bubbleRob'}
|
||||
pos={}
|
||||
orient={}
|
||||
for k, v in pairs(movingObjects) do
|
||||
h=sim.getObjectHandle(v)
|
||||
movingObjects[k]=h
|
||||
pos[h]=sim.getObjectPosition(h,-1)
|
||||
orient[h]=sim.getObjectOrientation(h,-1)
|
||||
end
|
||||
posBase={}
|
||||
h=sim.getObjectHandle('bubbleRob')
|
||||
posBase=sim.getObjectPosition(h,-1)
|
||||
|
||||
end
|
||||
|
||||
function reset()
|
||||
for k, v in pairs(movingObjects) do
|
||||
sim.setObjectPosition(v,-1,pos[v])
|
||||
sim.setObjectOrientation(v,-1,orient[v])
|
||||
sim.resetDynamicObject(v)
|
||||
end
|
||||
end
|
||||
@ -0,0 +1,144 @@
|
||||
# Make sure to have the server side running in CoppeliaSim:
|
||||
# in a child script of a CoppeliaSim scene, add following command
|
||||
# to be executed just once, at simulation start:
|
||||
#
|
||||
# simRemoteApi.start(19999)
|
||||
#
|
||||
# then start simulation, and run this program.
|
||||
#
|
||||
# IMPORTANT: for each successful call to simxStart, there
|
||||
# should be a corresponding call to simxFinish at the end!
|
||||
|
||||
try:
|
||||
import sim
|
||||
from sim import *
|
||||
except:
|
||||
print ('--------------------------------------------------------------')
|
||||
print ('"sim.py" could not be imported. This means very probably that')
|
||||
print ('either "sim.py" or the remoteApi library could not be found.')
|
||||
print ('Make sure both are in the same folder as this file,')
|
||||
print ('or appropriately adjust the file "sim.py"')
|
||||
print ('--------------------------------------------------------------')
|
||||
print ('')
|
||||
|
||||
import time
|
||||
|
||||
print('Program started')
|
||||
sim.simxFinish(-1) # just in case, close all opened connections
|
||||
clientID=sim.simxStart('127.0.0.1',19999,True,True,5000,5) # Connect to CoppeliaSim
|
||||
if clientID!=-1:
|
||||
print ('Connected to remote API server')
|
||||
|
||||
# Now try to retrieve data in a blocking fashion (i.e. a service call):
|
||||
res,objs=sim.simxGetObjects(clientID,sim.sim_handle_all,sim.simx_opmode_blocking)
|
||||
if res==sim.simx_return_ok:
|
||||
print ('Number of objects in the scene: ',len(objs))
|
||||
else:
|
||||
print ('Remote API function call returned with error code: ',res)
|
||||
# time.sleep(2)
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
# # Now retrieve streaming data (i.e. in a non-blocking fashion):
|
||||
# startTime=time.time()
|
||||
# sim.simxGetIntegerParameter(clientID,sim.sim_intparam_mouse_x,sim.simx_opmode_streaming) # Initialize streaming
|
||||
# while time.time()-startTime < 5:
|
||||
# returnCode,data=sim.simxGetIntegerParameter(clientID,sim.sim_intparam_mouse_x,sim.simx_opmode_buffer) # Try to retrieve the streamed data
|
||||
# if returnCode==sim.simx_return_ok: # After initialization of streaming, it will take a few ms before the first value arrives, so check the return code
|
||||
# print ('Mouse position x: ',data) # Mouse position x is actualized when the cursor is over CoppeliaSim's window
|
||||
# time.sleep(0.005)
|
||||
|
||||
# Now send some data to CoppeliaSim in a non-blocking fashion:
|
||||
sim.simxAddStatusbarMessage(clientID,'Hello CoppeliaSim!',sim.simx_opmode_oneshot)
|
||||
|
||||
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'bubbleRob',sim.sim_scripttype_childscript,
|
||||
'getRobotPos',[],[],[],'',sim.simx_opmode_blocking)
|
||||
if res==sim.simx_return_ok:
|
||||
print (retInts)
|
||||
print (retFloats)
|
||||
print (retStrings)
|
||||
print (retBuffer)
|
||||
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'bubbleRob',sim.sim_scripttype_childscript,
|
||||
'setRobotPos',[],[0,-2,0.12,0,0,0],[],'',sim.simx_opmode_blocking)
|
||||
if res==sim.simx_return_ok:
|
||||
print (retInts)
|
||||
print (retFloats)
|
||||
print (retStrings)
|
||||
print (retBuffer)
|
||||
res,retInts,retFloats,retStrings,retBuffer=sim.simxCallScriptFunction(clientID,'bubbleRob',sim.sim_scripttype_childscript,
|
||||
'getRobotPos',[],[],[],'',sim.simx_opmode_blocking)
|
||||
if res==sim.simx_return_ok:
|
||||
print (retInts)
|
||||
print (retFloats)
|
||||
print (retStrings)
|
||||
print (retBuffer)
|
||||
|
||||
|
||||
|
||||
# Retrieve some handles:
|
||||
res,bubbleRob=sim.simxGetObjectHandle(clientID,'bubbleRob#',sim.simx_opmode_oneshot_wait)
|
||||
res,bubbleRob_leftMotor=sim.simxGetObjectHandle(clientID,'bubbleRob_leftMotor#',sim.simx_opmode_oneshot_wait)
|
||||
res,bubbleRob_rightMotor=sim.simxGetObjectHandle(clientID,'bubbleRob_rightMotor#',sim.simx_opmode_oneshot_wait)
|
||||
res,bubbleRob_leftWheel=sim.simxGetObjectHandle(clientID,'bubbleRob_leftWheel#',sim.simx_opmode_oneshot_wait)
|
||||
res,bubbleRob_rightWheel=sim.simxGetObjectHandle(clientID,'bubbleRob_rightWheel#',sim.simx_opmode_oneshot_wait)
|
||||
res,bubbleRob_sensingNose=sim.simxGetObjectHandle(clientID,'bubbleRob_sensingNose#',sim.simx_opmode_oneshot_wait)
|
||||
res,bubbleRob_visionSensor=sim.simxGetObjectHandle(clientID,'bubbleRob_visionSensor#',sim.simx_opmode_oneshot_wait)
|
||||
res,bubbleRob_slider=sim.simxGetObjectHandle(clientID,'bubbleRob_slider#',sim.simx_opmode_oneshot_wait)
|
||||
|
||||
|
||||
# getResult,position=simxGetObjectPosition(clientID,bubbleRob,-1,sim.simx_opmode_streaming)
|
||||
# print(position)
|
||||
# getResult,position2=simxGetObjectPosition(clientID,bubbleRob_leftWheel,-1,sim.simx_opmode_buffer)
|
||||
# print(position2)
|
||||
# getResult,position3=simxGetObjectPosition(clientID,bubbleRob_rightWheel,-1,sim.simx_opmode_buffer)
|
||||
# print(position3)
|
||||
# getResult,position4=simxGetObjectPosition(clientID,bubbleRob_slider,-1,sim.simx_opmode_buffer)
|
||||
# print(position4)
|
||||
|
||||
|
||||
|
||||
# getResult,angle=simxGetObjectOrientation(clientID,bubbleRob,-1,sim.simx_opmode_streaming)
|
||||
# print(angle)
|
||||
# getResult,angle=simxGetObjectOrientation(clientID,bubbleRob_leftWheel,-1,sim.simx_opmode_streaming)
|
||||
# print(angle)
|
||||
# getResult,angle=simxGetObjectOrientation(clientID,bubbleRob_rightWheel,-1,sim.simx_opmode_streaming)
|
||||
# print(angle)
|
||||
# getResult,angle=simxGetObjectOrientation(clientID,bubbleRob_slider,-1,sim.simx_opmode_streaming)
|
||||
# print(angle)
|
||||
|
||||
|
||||
# setResult=simxSetObjectPosition(clientID,bubbleRob,-1,[0,-2,0.12],sim.simx_opmode_oneshot)
|
||||
# print(setResult)
|
||||
# setResult=simxSetObjectPosition(clientID,bubbleRob_leftWheel,-1,[0.05,-1.9,0.04],sim.simx_opmode_oneshot)
|
||||
# print(setResult)
|
||||
# setResult=simxSetObjectPosition(clientID,bubbleRob_rightWheel,-1,[0.05,-2.1,0.04],sim.simx_opmode_oneshot)
|
||||
# print(setResult)
|
||||
# setResult=simxSetObjectPosition(clientID,bubbleRob_slider,-1,[-0.07,-2,0.025],sim.simx_opmode_oneshot)
|
||||
# print(setResult)
|
||||
|
||||
|
||||
# setResult=simxSetObjectOrientation(clientID,bubbleRob,-1,[0,0,90],sim.simx_opmode_oneshot)
|
||||
# print(setResult)
|
||||
# setResult=simxSetObjectOrientation(clientID,bubbleRob_leftWheel,-1,[-90,90,0],sim.simx_opmode_oneshot)
|
||||
# print(setResult)
|
||||
# setResult=simxSetObjectOrientation(clientID,bubbleRob_rightWheel,-1,[-90,90,0],sim.simx_opmode_oneshot)
|
||||
# print(setResult)
|
||||
# setResult=simxSetObjectOrientation(clientID,bubbleRob_slider,-1,[0,0,90],sim.simx_opmode_oneshot)
|
||||
# print(setResult)
|
||||
|
||||
|
||||
|
||||
|
||||
# Before closing the connection to CoppeliaSim, make sure that the last command sent out had time to arrive. You can guarantee this with (for example):
|
||||
sim.simxGetPingTime(clientID)
|
||||
|
||||
# Now close the connection to CoppeliaSim:
|
||||
sim.simxFinish(clientID)
|
||||
else:
|
||||
print ('Failed connecting to remote API server')
|
||||
print ('Program ended')
|
||||
@ -0,0 +1,517 @@
|
||||
# -*- coding: utf-8 -*-
|
||||
"""Code for visualizing data in sim via python.
|
||||
|
||||
Author: Andrew Hundt <ATHundt@gmail.com>
|
||||
|
||||
License: Apache v2 https://www.apache.org/licenses/LICENSE-2.0
|
||||
"""
|
||||
import os
|
||||
import errno
|
||||
import traceback
|
||||
|
||||
import numpy as np
|
||||
import six # compatibility between python 2 + 3 = six
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
try:
|
||||
import sim as sim
|
||||
except Exception as e:
|
||||
print ('--------------------------------------------------------------')
|
||||
print ('"sim.py" could not be imported. This means very probably that')
|
||||
print ('either "sim.py" or the remoteApi library could not be found.')
|
||||
print ('Make sure both are in PYTHONPATH folder relative to this file,')
|
||||
print ('or appropriately adjust the file "sim.py. Also follow the"')
|
||||
print ('ReadMe.txt in the sim remote API folder')
|
||||
print ('--------------------------------------------------------------')
|
||||
print ('')
|
||||
raise e
|
||||
|
||||
import tensorflow as tf
|
||||
|
||||
from tensorflow.python.platform import flags
|
||||
from tensorflow.python.platform import gfile
|
||||
from tensorflow.python.ops import data_flow_ops
|
||||
from ply import write_xyz_rgb_as_ply
|
||||
from PIL import Image
|
||||
|
||||
# progress bars https://github.com/tqdm/tqdm
|
||||
# import tqdm without enforcing it as a dependency
|
||||
try:
|
||||
from tqdm import tqdm
|
||||
except ImportError:
|
||||
|
||||
def tqdm(*args, **kwargs):
|
||||
if args:
|
||||
return args[0]
|
||||
return kwargs.get('iterable', None)
|
||||
|
||||
from depth_image_encoding import ClipFloatValues
|
||||
from depth_image_encoding import FloatArrayToRgbImage
|
||||
from depth_image_encoding import FloatArrayToRawRGB
|
||||
from skimage.transform import resize
|
||||
from skimage import img_as_ubyte
|
||||
from skimage import img_as_uint
|
||||
from skimage.color import grey2rgb
|
||||
|
||||
try:
|
||||
import eigen # https://github.com/jrl-umi3218/Eigen3ToPython
|
||||
import sva # https://github.com/jrl-umi3218/SpaceVecAlg
|
||||
except ImportError:
|
||||
print('eigen and sva python modules are not available. To install run the script at:'
|
||||
'https://github.com/ahundt/robotics_setup/blob/master/robotics_tasks.sh'
|
||||
'or follow the instructions at https://github.com/jrl-umi3218/Eigen3ToPython'
|
||||
'and https://github.com/jrl-umi3218/SpaceVecAlg. '
|
||||
'When you build the modules make sure python bindings are enabled.')
|
||||
|
||||
tf.flags.DEFINE_string('csimVisualizeDepthFormat', 'csim_depth_encoded_rgb',
|
||||
""" Controls how Depth images are displayed. Options are:
|
||||
None: Do not modify the data and display it as-is for rgb input data (not working properly for float depth).
|
||||
'depth_rgb': convert a floating point depth image to a straight 0-255 encoding of depths less than 3m
|
||||
'depth_encoded_rgb': convert a floating point depth image to the rgb encoding used by
|
||||
the google brain robot data grasp dataset's raw png depth image encoding,
|
||||
see https://sites.google.com/site/brainrobotdata/home/depth-image-encoding.
|
||||
'sim': add a sim prefix to any of the above commands to
|
||||
rotate image by 180 degrees, flip left over right, then invert the color channels
|
||||
after the initial conversion step.
|
||||
This is due to a problem where CoppeliaSim seems to display images differently.
|
||||
Examples include 'csim_depth_rgb' and 'csim_depth_encoded_rgb',
|
||||
see http://forum.coppeliarobotics.com/viewtopic.php?f=9&t=737&p=27805#p27805.
|
||||
""")
|
||||
tf.flags.DEFINE_string('csimVisualizeRGBFormat', 'csim_rgb',
|
||||
""" Controls how images are displayed. Options are:
|
||||
None: Do not modify the data and display it as-is for rgb input data (not working properly for float depth).
|
||||
'depth_rgb': convert a floating point depth image to a straight 0-255 encoding of depths less than 3m
|
||||
'depth_encoded_rgb': convert a floating point depth image to the rgb encoding used by
|
||||
the google brain robot data grasp dataset's raw png depth image encoding,
|
||||
see https://sites.google.com/site/brainrobotdata/home/depth-image-encoding.
|
||||
'sim': add a sim prefix to any of the above commands to
|
||||
rotate image by 180 degrees, flip left over right, then invert the color channels
|
||||
after the initial conversion step.
|
||||
This is due to a problem where CoppeliaSim seems to display images differently.
|
||||
Examples include 'csim_depth_rgb' and 'csim_depth_encoded_rgb',
|
||||
see http://forum.coppeliarobotics.com/viewtopic.php?f=9&t=737&p=27805#p27805.
|
||||
""")
|
||||
|
||||
# the following line is needed for tf versions before 1.5
|
||||
# flags.FLAGS._parse_flags()
|
||||
FLAGS = flags.FLAGS
|
||||
|
||||
|
||||
def depth_image_to_point_cloud(depth, intrinsics_matrix, dtype=np.float32, verbose=0):
|
||||
"""Depth images become an XYZ point cloud in the camera frame with shape (depth.shape[0], depth.shape[1], 3).
|
||||
|
||||
Transform a depth image into a point cloud in the camera frame with one point for each
|
||||
pixel in the image, using the camera transform for a camera
|
||||
centred at cx, cy with field of view fx, fy.
|
||||
|
||||
Based on:
|
||||
https://github.com/tensorflow/models/blob/master/research/cognitive_mapping_and_planning/src/depth_utils.py
|
||||
https://codereview.stackexchange.com/a/84990/10101
|
||||
|
||||
also see grasp_geometry_tf.depth_image_to_point_cloud().
|
||||
|
||||
# Arguments
|
||||
|
||||
depth: is a 2-D ndarray with shape (rows, cols) containing
|
||||
32bit floating point depths in meters. The result is a 3-D array with
|
||||
shape (rows, cols, 3). Pixels with invalid depth in the input have
|
||||
NaN or 0 for the z-coordinate in the result.
|
||||
|
||||
intrinsics_matrix: 3x3 matrix for projecting depth values to z values
|
||||
in the point cloud frame. http://ksimek.github.io/2013/08/13/intrinsic/
|
||||
In this case x0, y0 are at index [2, 0] and [2, 1], respectively.
|
||||
|
||||
transform: 4x4 Rt matrix for rotating and translating the point cloud
|
||||
"""
|
||||
fy = intrinsics_matrix[1, 1]
|
||||
fx = intrinsics_matrix[0, 0]
|
||||
# center of image y coordinate
|
||||
center_y = intrinsics_matrix[2, 1]
|
||||
# center of image x coordinate
|
||||
center_x = intrinsics_matrix[2, 0]
|
||||
depth = np.squeeze(depth)
|
||||
y_range, x_range = depth.shape
|
||||
|
||||
y, x = np.meshgrid(np.arange(y_range),
|
||||
np.arange(x_range),
|
||||
indexing='ij')
|
||||
assert y.size == x.size and y.size == depth.size
|
||||
x = x.flatten()
|
||||
y = y.flatten()
|
||||
depth = depth.flatten()
|
||||
|
||||
X = (x - center_x) * depth / fx
|
||||
Y = (y - center_y) * depth / fy
|
||||
|
||||
assert X.size == Y.size and X.size == depth.size
|
||||
assert X.shape == Y.shape and X.shape == depth.shape
|
||||
|
||||
if verbose > 0:
|
||||
print('X np: ', X.shape)
|
||||
print('Y np: ', Y.shape)
|
||||
print('depth np: ', depth.shape)
|
||||
XYZ = np.column_stack([X, Y, depth])
|
||||
assert XYZ.shape == (y_range * x_range, 3)
|
||||
if verbose > 0:
|
||||
print('XYZ pre reshape np: ', XYZ.shape)
|
||||
XYZ = XYZ.reshape((y_range, x_range, 3))
|
||||
|
||||
return XYZ.astype(dtype)
|
||||
|
||||
|
||||
def csimPrint(client_id, message):
|
||||
"""Print a message in both the python command line and on the CoppeliaSim Statusbar.
|
||||
|
||||
The Statusbar is the white command line output on the bottom of the CoppeliaSim GUI window.
|
||||
"""
|
||||
sim.simxAddStatusbarMessage(client_id, message, sim.simx_opmode_oneshot)
|
||||
print(message)
|
||||
|
||||
|
||||
def create_dummy(client_id, display_name, transform=None, parent_handle=-1, debug=FLAGS.csimDebugMode, operation_mode=sim.simx_opmode_blocking):
|
||||
"""Create a dummy object in the simulation
|
||||
|
||||
# Arguments
|
||||
|
||||
transform_display_name: name string to use for the object in the sim scene
|
||||
transform: 3 cartesian (x, y, z) and 4 quaternion (x, y, z, w) elements, same as sim
|
||||
parent_handle: -1 is the world frame, any other int should be a sim object handle
|
||||
"""
|
||||
if transform is None:
|
||||
transform = np.array([0., 0., 0., 0., 0., 0., 1.])
|
||||
# 2. Now create a dummy object at coordinate 0.1,0.2,0.3 with name 'MyDummyName':
|
||||
empty_buffer = bytearray()
|
||||
res, ret_ints, ret_floats, ret_strings, ret_buffer = sim.simxCallScriptFunction(
|
||||
client_id,
|
||||
'remoteApiCommandServer',
|
||||
sim.sim_scripttype_childscript,
|
||||
'createDummy_function',
|
||||
[parent_handle],
|
||||
transform,
|
||||
[display_name],
|
||||
empty_buffer,
|
||||
operation_mode)
|
||||
if res == sim.simx_return_ok:
|
||||
# display the reply from CoppeliaSim (in this case, the handle of the created dummy)
|
||||
if debug is not None and 'print_transform' in debug:
|
||||
print ('Dummy name:', display_name, ' handle: ', ret_ints[0], ' transform: ', transform)
|
||||
else:
|
||||
print('create_dummy remote function call failed.')
|
||||
print(''.join(traceback.format_stack()))
|
||||
return -1
|
||||
return ret_ints[0]
|
||||
|
||||
|
||||
def setPose(client_id, display_name, transform=None, parent_handle=-1):
|
||||
"""Set the pose of an object in the simulation
|
||||
|
||||
# Arguments
|
||||
|
||||
transform_display_name: name string to use for the object in the sim scene
|
||||
transform: 3 cartesian (x, y, z) and 4 quaternion (x, y, z, w) elements, same as sim
|
||||
parent_handle: -1 is the world frame, any other int should be a sim object handle
|
||||
"""
|
||||
if transform is None:
|
||||
transform = np.array([0., 0., 0., 0., 0., 0., 1.])
|
||||
# 2. Now create a dummy object at coordinate 0.1,0.2,0.3 with name 'MyDummyName':
|
||||
empty_buffer = bytearray()
|
||||
res, ret_ints, ret_floats, ret_strings, ret_buffer = sim.simxCallScriptFunction(
|
||||
client_id,
|
||||
'remoteApiCommandServer',
|
||||
sim.sim_scripttype_childscript,
|
||||
'createDummy_function',
|
||||
[parent_handle],
|
||||
transform,
|
||||
[display_name],
|
||||
empty_buffer,
|
||||
sim.simx_opmode_blocking)
|
||||
if res == sim.simx_return_ok:
|
||||
# display the reply from CoppeliaSim (in this case, the handle of the created dummy)
|
||||
print ('SetPose object name:', display_name, ' handle: ', ret_ints[0], ' transform: ', transform)
|
||||
else:
|
||||
print('setPose remote function call failed.')
|
||||
print(''.join(traceback.format_stack()))
|
||||
return -1
|
||||
return ret_ints[0]
|
||||
|
||||
|
||||
def set_vision_sensor_image(client_id, display_name, image, convert=None, scale_factor=256000.0, operation_mode=sim.simx_opmode_oneshot_wait):
|
||||
"""Display vision sensor image data in a CoppeliaSim simulation.
|
||||
|
||||
[CoppeliaSim Vision Sensors](http://www.coppeliarobotics.com/helpFiles/en/visionSensors.htm)
|
||||
[simSetVisionSensorImage](http://www.coppeliarobotics.com/helpFiles/en/apiFunctions.htm#simSetVisionSensorImage)
|
||||
|
||||
# Arguments
|
||||
|
||||
display_name: the string display name of the sensor object in the CoppeliaSim scene
|
||||
image: an rgb char array containing an image
|
||||
convert: Controls how images are displayed. Options are:
|
||||
None: Do not modify the data and display it as-is for rgb input data (not working properly for float depth).
|
||||
'depth_rgb': convert a floating point depth image to a straight 0-255 encoding of depths less than 3m
|
||||
'depth_encoded_rgb': convert a floating point depth image to the rgb encoding used by
|
||||
the google brain robot data grasp dataset's raw png depth image encoding,
|
||||
see https://sites.google.com/site/brainrobotdata/home/depth-image-encoding.
|
||||
'sim': add a sim prefix to any of the above commands to
|
||||
rotate image by 180 degrees, flip left over right, then invert the color channels
|
||||
after the initial conversion step.
|
||||
This is due to a problem where CoppeliaSim seems to display images differently.
|
||||
Examples include 'csim_depth_rgb' and 'csim_depth_encoded_rgb',
|
||||
see http://www.forum.coppeliarobotics.com/viewtopic.php?f=9&t=737&p=27805#p27805.
|
||||
"""
|
||||
strings = [display_name]
|
||||
parent_handle = -1
|
||||
|
||||
# TODO(ahundt) support is_greyscale True again
|
||||
is_greyscale = 0
|
||||
csim_conversion = False
|
||||
if convert is not None:
|
||||
csim_conversion = 'sim' in convert
|
||||
|
||||
if 'depth_encoded_rgb' in convert:
|
||||
image = np.array(FloatArrayToRgbImage(image, scale_factor=scale_factor, drop_blue=False), dtype=np.uint8)
|
||||
elif 'depth_rgb' in convert:
|
||||
|
||||
image = img_as_uint(image)
|
||||
|
||||
elif not csim_conversion:
|
||||
raise ValueError('set_vision_sensor_image() convert parameter must be one of `depth_encoded_rgb`, `depth_rgb`, or None'
|
||||
'with the optional addition of the word `sim` to rotate 180, flip left right, then invert colors.')
|
||||
|
||||
if csim_conversion:
|
||||
# rotate 180 degrees, flip left over right, then invert the colors
|
||||
image = np.array(256 - np.fliplr(np.rot90(image, 2)), dtype=np.uint8)
|
||||
|
||||
if np.issubdtype(image.dtype, np.integer):
|
||||
is_float = 0
|
||||
floats = []
|
||||
color_buffer = bytearray(image.flatten().tobytes())
|
||||
color_size = image.size
|
||||
num_floats = 0
|
||||
else:
|
||||
is_float = 1
|
||||
floats = [image]
|
||||
color_buffer = bytearray()
|
||||
num_floats = image.size
|
||||
color_size = 0
|
||||
|
||||
cloud_handle = -1
|
||||
res, ret_ints, ret_floats, ret_strings, ret_buffer = sim.simxCallScriptFunction(
|
||||
client_id,
|
||||
'remoteApiCommandServer',
|
||||
sim.sim_scripttype_childscript,
|
||||
'setVisionSensorImage_function',
|
||||
[parent_handle, num_floats, is_greyscale, color_size], # int params
|
||||
np.append(floats, []), # float params
|
||||
strings, # string params
|
||||
# byte buffer params
|
||||
color_buffer,
|
||||
operation_mode)
|
||||
if res == sim.simx_return_ok:
|
||||
print ('point cloud handle: ', ret_ints[0]) # display the reply from CoppeliaSim (in this case, the handle of the created dummy)
|
||||
# set the transform for the point cloud
|
||||
return ret_ints[0]
|
||||
else:
|
||||
print('insertPointCloud_function remote function call failed.')
|
||||
print(''.join(traceback.format_stack()))
|
||||
return res
|
||||
|
||||
|
||||
def create_point_cloud(client_id, display_name, transform=None, point_cloud=None, depth_image=None, color_image=None,
|
||||
camera_intrinsics_matrix=None, parent_handle=-1, clear=True,
|
||||
max_voxel_size=0.01, max_point_count_per_voxel=10, point_size=10, options=8,
|
||||
rgb_sensor_display_name=None, depth_sensor_display_name=None, convert_depth=FLAGS.csimVisualizeDepthFormat,
|
||||
convert_rgb=FLAGS.csimVisualizeRGBFormat, save_ply_path=None, rgb_display_mode='vision_sensor'):
|
||||
"""Create a point cloud object in the simulation, plus optionally render the depth and rgb images.
|
||||
|
||||
# Arguments
|
||||
|
||||
display_name: name string to use for the object in the sim scene
|
||||
depth_image: A depth image of size [width, height, 3]
|
||||
transform: [x, y, z, qw, qx, qy, qz] with 3 cartesian (x, y, z) and 4 quaternion (qx, qy, qz, qw) elements, same as sim
|
||||
This transform is from the parent handle to the point cloud base
|
||||
parent_handle: -1 is the world frame, any other int should be a sim object handle
|
||||
clear: clear the point cloud if it already exists with the provided display name
|
||||
maxVoxelSize: the maximum size of the octree voxels containing points
|
||||
maxPtCntPerVoxel: the maximum number of points allowed in a same octree voxel
|
||||
options: bit-coded:
|
||||
bit0 set (1): points have random colors
|
||||
bit1 set (2): show octree structure
|
||||
bit2 set (4): reserved. keep unset
|
||||
bit3 set (8): do not use an octree structure. When enabled, point cloud operations are limited, and point clouds will not be collidable, measurable or detectable anymore, but adding points will be much faster
|
||||
bit4 set (16): color is emissive
|
||||
pointSize: the size of the points, in pixels
|
||||
reserved: reserved for future extensions. Set to NULL
|
||||
save_ply_path: save out a ply file with the point cloud data
|
||||
point_cloud: optional XYZ point cloud of size [width, height, 3], will be generated if not provided.
|
||||
convert_rgb: Controls how images are displayed. Options are:
|
||||
None: Do not modify the data and display it as-is for rgb input data (not working properly for float depth).
|
||||
'depth_rgb': convert a floating point depth image to a straight 0-255 encoding of depths less than 3m
|
||||
'depth_encoded_rgb': convert a floating point depth image to the rgb encoding used by
|
||||
the google brain robot data grasp dataset's raw png depth image encoding,
|
||||
see https://sites.google.com/site/brainrobotdata/home/depth-image-encoding.
|
||||
'sim': add a sim prefix to any of the above commands to
|
||||
rotate image by 180 degrees, flip left over right, then invert the color channels
|
||||
after the initial conversion step.
|
||||
This is due to a problem where CoppeliaSim seems to display images differently.
|
||||
Examples include 'csim_depth_rgb' and 'csim_depth_encoded_rgb',
|
||||
see http://www.forum.coppeliarobotics.com/viewtopic.php?f=9&t=737&p=27805#p27805.
|
||||
rgb_display_mode: Options help with working around quirks in input image data's layout.
|
||||
'point_cloud' to display the image when the point cloud is being generated.
|
||||
'vision_sensor' to make a separate call go the vison sensor display function.
|
||||
"""
|
||||
if transform is None:
|
||||
transform = np.array([0., 0., 0., 0., 0., 0., 1.])
|
||||
|
||||
if point_cloud is None:
|
||||
point_cloud = depth_image_to_point_cloud(depth_image, camera_intrinsics_matrix)
|
||||
point_cloud = point_cloud.reshape([point_cloud.size/3, 3])
|
||||
|
||||
# show the depth sensor image
|
||||
if depth_sensor_display_name is not None and depth_image is not None:
|
||||
# matplotlib.image.imsave(display_name + depth_sensor_display_name + '_norotfliplr.png', depth_image)
|
||||
# rotate 180, flip left over right then invert the image colors for display in CoppeliaSim
|
||||
# depth_image = np.fliplr(np.rot90(depth_image, 2))
|
||||
# matplotlib.image.imsave(display_name + depth_sensor_display_name + '_rot90fliplr.png', depth_image)
|
||||
set_vision_sensor_image(client_id, depth_sensor_display_name, depth_image, convert=convert_depth)
|
||||
|
||||
# show the rgb sensor image, this overwrites the rgb display
|
||||
# done in insertPointCloud_function, which is buggy
|
||||
if rgb_sensor_display_name is not None and color_image is not None and rgb_display_mode == 'vision_sensor':
|
||||
# matplotlib.image.imsave(display_name + rgb_sensor_display_name + '_norotfliplr.png', color_image)
|
||||
# rotate 180, flip left over right then invert the image colors for display in CoppeliaSim
|
||||
# matplotlib.image.imsave(display_name + rgb_sensor_display_name + '_rot90fliplr.png', color_image)
|
||||
set_vision_sensor_image(client_id, rgb_sensor_display_name, color_image, convert=convert_rgb)
|
||||
|
||||
# Save out Point cloud
|
||||
if save_ply_path is not None:
|
||||
write_xyz_rgb_as_ply(point_cloud, color_image, save_ply_path)
|
||||
|
||||
# color_buffer is initially empty
|
||||
color_buffer = bytearray()
|
||||
strings = [display_name]
|
||||
if rgb_sensor_display_name is not None and rgb_display_mode == 'point_cloud':
|
||||
strings = [display_name, rgb_sensor_display_name]
|
||||
|
||||
transform_entries = 7
|
||||
if clear:
|
||||
clear = 1
|
||||
else:
|
||||
clear = 0
|
||||
|
||||
cloud_handle = -1
|
||||
# Create the point cloud if it does not exist, or retrieve the handle if it does
|
||||
res, ret_ints, ret_floats, ret_strings, ret_buffer = sim.simxCallScriptFunction(
|
||||
client_id,
|
||||
'remoteApiCommandServer',
|
||||
sim.sim_scripttype_childscript,
|
||||
'createPointCloud_function',
|
||||
# int params
|
||||
[parent_handle, transform_entries, point_cloud.size, cloud_handle, clear, max_point_count_per_voxel, options, point_size],
|
||||
# float params
|
||||
[max_voxel_size],
|
||||
# string params
|
||||
strings,
|
||||
# byte buffer params
|
||||
color_buffer,
|
||||
sim.simx_opmode_blocking)
|
||||
|
||||
setPose(client_id, display_name, transform, parent_handle)
|
||||
|
||||
if res == sim.simx_return_ok:
|
||||
cloud_handle = ret_ints[0]
|
||||
|
||||
# convert the rgb values to a string
|
||||
color_size = 0
|
||||
if color_image is not None:
|
||||
# see simInsertPointsIntoPointCloud() in sim documentation
|
||||
# 3 indicates the cloud should be in the parent frame, and color is enabled
|
||||
# bit 2 is 1 so each point is colored
|
||||
simInsertPointsIntoPointCloudOptions = 3
|
||||
# color_buffer = bytearray(np.fliplr(np.rot90(color_image, 3)).flatten().tobytes())
|
||||
color_buffer = bytearray(color_image.flatten().tobytes())
|
||||
color_size = color_image.size
|
||||
else:
|
||||
simInsertPointsIntoPointCloudOptions = 1
|
||||
|
||||
# Actually transfer the point cloud
|
||||
res, ret_ints, ret_floats, ret_strings, ret_buffer = sim.simxCallScriptFunction(
|
||||
client_id,
|
||||
'remoteApiCommandServer',
|
||||
sim.sim_scripttype_childscript,
|
||||
'insertPointCloud_function',
|
||||
[parent_handle, transform_entries, point_cloud.size, cloud_handle, color_size, simInsertPointsIntoPointCloudOptions],
|
||||
np.append(point_cloud, []),
|
||||
strings,
|
||||
color_buffer,
|
||||
sim.simx_opmode_blocking)
|
||||
|
||||
if res == sim.simx_return_ok:
|
||||
print ('point cloud handle: ', ret_ints[0]) # display the reply from CoppeliaSim (in this case, the handle of the created dummy)
|
||||
# set the transform for the point cloud
|
||||
return ret_ints[0]
|
||||
else:
|
||||
print('insertPointCloud_function remote function call failed.')
|
||||
print(''.join(traceback.format_stack()))
|
||||
return res
|
||||
|
||||
else:
|
||||
print('createPointCloud_function remote function call failed')
|
||||
print(''.join(traceback.format_stack()))
|
||||
return res
|
||||
|
||||
|
||||
def drawLines(client_id, display_name, lines, parent_handle=-1, transform=None, debug=FLAGS.csimDebugMode, operation_mode=sim.simx_opmode_blocking):
|
||||
"""Create a line in the simulation.
|
||||
|
||||
Note that there are currently some quirks with this function. Only one line is accepted,
|
||||
and sometimes CoppeliaSim fails to delete the object correctly and lines will fail to draw.
|
||||
In that case you need to close and restart CoppeliaSim.
|
||||
|
||||
# Arguments
|
||||
|
||||
transform_display_name: name string to use for the object in the sim scene
|
||||
transform: 3 cartesian (x, y, z) and 4 quaternion (x, y, z, w) elements, same as sim
|
||||
parent_handle: -1 is the world frame, any other int should be a sim object handle
|
||||
lines: array of line definitions using two endpoints (x0, y0, z0, x1, y1, z1).
|
||||
Multiple lines can be defined but there should be 6 entries (two points) per line.
|
||||
"""
|
||||
# 2. Now create a dummy object at coordinate 0.1,0.2,0.3 with name 'MyDummyName':
|
||||
empty_buffer = bytearray()
|
||||
res, ret_ints, ret_floats, ret_strings, ret_buffer = sim.simxCallScriptFunction(
|
||||
client_id,
|
||||
'remoteApiCommandServer',
|
||||
sim.sim_scripttype_childscript,
|
||||
'addDrawingObject_function',
|
||||
[parent_handle, int(lines.size/6)],
|
||||
# np.append(transform, lines),
|
||||
lines,
|
||||
[display_name],
|
||||
empty_buffer,
|
||||
operation_mode)
|
||||
if res == sim.simx_return_ok:
|
||||
# display the reply from CoppeliaSim (in this case, the handle of the created dummy)
|
||||
if debug is not None and 'print_drawLines' in debug:
|
||||
print ('drawLines name:', display_name, ' handle: ', ret_ints[0], ' transform: ', transform)
|
||||
|
||||
if transform is not None:
|
||||
# set the transform for the point cloud
|
||||
setPose(client_id, display_name, transform, parent_handle)
|
||||
else:
|
||||
print('drawLines remote function call failed.')
|
||||
print(''.join(traceback.format_stack()))
|
||||
return -1
|
||||
return ret_ints[0]
|
||||
|
||||
|
||||
def restore_cropped(cropped_image, crop_size, crop_offset, full_size):
|
||||
""" Restore cropped_image to full size image with zero padding
|
||||
First scale image back to crop_size, then padding
|
||||
"""
|
||||
|
||||
cropped_image = np.squeeze(cropped_image)
|
||||
restored = np.zeros((full_size[0], full_size[1]), dtype=cropped_image.dtype)
|
||||
scaled_crop = resize(cropped_image, (crop_size[0], crop_size[1]))
|
||||
restored[crop_offset[0]:crop_offset[0]+crop_size[0],
|
||||
crop_offset[1]:crop_offset[1]+crop_size[1]] = scaled_crop
|
||||
|
||||
return restored
|
||||
@ -0,0 +1,14 @@
|
||||
git clone https://github.com/sybrenstuvel/Python-RVO2.git
|
||||
cd Python-RVO2-master
|
||||
# todo : install conda and create env for py3.6
|
||||
|
||||
conda activate autorobot
|
||||
conda install numpy
|
||||
python -V
|
||||
pip install -r requirements.txt --user
|
||||
python setup.py build
|
||||
python setup.py install --user
|
||||
|
||||
|
||||
|
||||
todo:
|
||||
@ -0,0 +1,161 @@
|
||||
#!/usr/bin/env python2
|
||||
import rospy
|
||||
|
||||
from std_msgs.msg import String
|
||||
from std_msgs.msg import Float32MultiArray
|
||||
from std_msgs.msg import MultiArrayLayout
|
||||
import sys
|
||||
sys.path.append ('./python')
|
||||
import sim
|
||||
from sim import *
|
||||
import numpy as np
|
||||
from utils.human import Human
|
||||
from utils.robot import Robot
|
||||
import logging
|
||||
import argparse
|
||||
import configparser
|
||||
from numpy.linalg import norm
|
||||
import time
|
||||
|
||||
class Sensor(object):
|
||||
"""docstring for ."""
|
||||
|
||||
def __init__(self,clientID, config,robot):
|
||||
self.robot = robot
|
||||
self.config = config
|
||||
self.clientID = clientID
|
||||
self.all_actions = [0 for i in range(12)]
|
||||
sim.simxStopSimulation(clientID, sim.simx_opmode_oneshot)
|
||||
time.sleep(0.5)
|
||||
sim.simxStartSimulation(clientID, sim.simx_opmode_oneshot)
|
||||
time.sleep(0.5)
|
||||
sim.simxPauseSimulation(clientID, sim.simx_opmode_oneshot)
|
||||
time.sleep(0.5)
|
||||
|
||||
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)
|
||||
|
||||
# get current observation
|
||||
ob = [human.get_observable_state() for human in self.humans]
|
||||
|
||||
# collect agents' states in gym env
|
||||
fullstate = self.robot.get_full_state()
|
||||
reset_info = []
|
||||
reset_info.extend([fullstate.px, fullstate.py])
|
||||
for index, human in enumerate(ob):
|
||||
# human_theta = np.arctan2(human.vy, human.vx)
|
||||
reset_info.extend([human.px, human.py])
|
||||
|
||||
res, retInts, retFloats, retStrings, retBuffer = sim.simxCallScriptFunction(self.clientID,
|
||||
'remoteApiCommandServer',
|
||||
sim.sim_scripttype_childscript,
|
||||
'setRobotPos', [], reset_info,
|
||||
[], '',
|
||||
sim.simx_opmode_blocking)
|
||||
|
||||
sim.simxStartSimulation(self.clientID, sim.simx_opmode_oneshot)
|
||||
time.sleep(2)
|
||||
|
||||
rospy.init_node('talker')
|
||||
self.pub = rospy.Publisher('chatter', Float32MultiArray, queue_size=2)
|
||||
self.sub = rospy.Subscriber('act_cmd', Float32MultiArray, self. callback)
|
||||
|
||||
res, retInts, retFloats, retStrings, retBuffer = sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'setVelGetPos', [],self.all_actions, [], '',sim.simx_opmode_blocking)
|
||||
rospy.loginfo(retStrings)
|
||||
data = Float32MultiArray()
|
||||
# retFloats = [round(i,4) for i in retFloats]
|
||||
data.data = retFloats
|
||||
# data.layout = MultiArrayLayout()
|
||||
self.pub.publish(data)
|
||||
time.sleep(0.1)
|
||||
self.pub.publish(data)
|
||||
time.sleep(0.1)
|
||||
self.pub.publish(data)
|
||||
time.sleep(0.1)
|
||||
self.pub.publish(data)
|
||||
|
||||
rate = rospy.Rate(500)
|
||||
|
||||
while not rospy.is_shutdown():
|
||||
# all_actions = [0 for i in range(12)]
|
||||
rate.sleep()
|
||||
|
||||
|
||||
def callback(self, data):
|
||||
if (len(data.data)!=0):
|
||||
rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.data)
|
||||
actCmd = list(data.data)
|
||||
self.all_actions = actCmd
|
||||
# print(actCmd)
|
||||
res, retInts, retFloats, retStrings, retBuffer = sim.simxCallScriptFunction(clientID,'remoteApiCommandServer',sim.sim_scripttype_childscript,'setVelGetPos', [],self.all_actions, [], '',sim.simx_opmode_blocking)
|
||||
rospy.loginfo(retStrings)
|
||||
data = Float32MultiArray()
|
||||
# retFloats = [round(i,4) for i in retFloats]
|
||||
data.data = retFloats
|
||||
# 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')
|
||||
|
||||
sim.simxFinish(-1) # just in case, close all opened connections
|
||||
clientID = sim.simxStart('127.0.0.1', 19997, True, True, 5000, 5) # Connect to CoppeliaSim
|
||||
if clientID != -1:
|
||||
print('Connected to remote API server 2')
|
||||
# Now try to retrieve data in a blocking fashion (i.e. a service call):
|
||||
res, objs = sim.simxGetObjects(clientID, sim.sim_handle_all, sim.simx_opmode_blocking)
|
||||
if res == sim.simx_return_ok:
|
||||
print('Number of objects in the scene 2: ', len(objs))
|
||||
else:
|
||||
print('Remote API function call returned with error code: ', res)
|
||||
# time.sleep(2)
|
||||
else:
|
||||
print('Failed connecting to remote API server')
|
||||
pass
|
||||
|
||||
|
||||
|
||||
try:
|
||||
Sensor(clientID, env_config, robot)
|
||||
except rospy.ROSInterruptException:
|
||||
pass
|
||||
@ -0,0 +1,4 @@
|
||||
from collections import namedtuple
|
||||
|
||||
ActionXY = namedtuple('ActionXY', ['vx', 'vy'])
|
||||
ActionRot = namedtuple('ActionRot', ['v', 'r'])
|
||||
@ -0,0 +1,140 @@
|
||||
import numpy as np
|
||||
from numpy.linalg import norm
|
||||
import abc
|
||||
import logging
|
||||
import sys
|
||||
from policy.policy_factory import policy_factory
|
||||
from action import ActionXY, ActionRot
|
||||
from state import ObservableState, FullState
|
||||
|
||||
|
||||
class Agent(object):
|
||||
def __init__(self, config, section):
|
||||
"""
|
||||
Base class for robot and human. Have the physical attributes of an agent.
|
||||
|
||||
"""
|
||||
self.visible = config.getboolean(section, 'visible')
|
||||
self.v_pref = config.getfloat(section, 'v_pref')
|
||||
self.radius = config.getfloat(section, 'radius')
|
||||
self.policy = policy_factory[config.get(section, 'policy')]()
|
||||
self.sensor = config.get(section, 'sensor')
|
||||
self.kinematics = self.policy.kinematics if self.policy is not None else None
|
||||
self.px = None
|
||||
self.py = None
|
||||
self.gx = None
|
||||
self.gy = None
|
||||
self.vx = None
|
||||
self.vy = None
|
||||
self.theta = None
|
||||
self.time_step = None
|
||||
|
||||
def print_info(self):
|
||||
logging.info('Agent is {} and has {} kinematic constraint'.format(
|
||||
'visible' if self.visible else 'invisible', self.kinematics))
|
||||
|
||||
def set_policy(self, policy):
|
||||
self.policy = policy
|
||||
self.kinematics = policy.kinematics
|
||||
|
||||
def sample_random_attributes(self):
|
||||
"""
|
||||
Sample agent radius and v_pref attribute from certain distribution
|
||||
:return:
|
||||
"""
|
||||
self.v_pref = np.random.uniform(0.5, 1.5)
|
||||
self.radius = np.random.uniform(0.3, 0.5)
|
||||
|
||||
def set(self, px, py, gx, gy, vx, vy, theta, radius=None, v_pref=None):
|
||||
self.px = px
|
||||
self.py = py
|
||||
self.gx = gx
|
||||
self.gy = gy
|
||||
self.vx = vx
|
||||
self.vy = vy
|
||||
self.theta = theta
|
||||
if radius is not None:
|
||||
self.radius = radius
|
||||
if v_pref is not None:
|
||||
self.v_pref = v_pref
|
||||
|
||||
def get_observable_state(self):
|
||||
return ObservableState(self.px, self.py, self.vx, self.vy, self.radius)
|
||||
|
||||
def get_next_observable_state(self, action):
|
||||
self.check_validity(action)
|
||||
pos = self.compute_position(action, self.time_step)
|
||||
next_px, next_py = pos
|
||||
if self.kinematics == 'holonomic':
|
||||
next_vx = action.vx
|
||||
next_vy = action.vy
|
||||
else:
|
||||
next_theta = self.theta + action.r
|
||||
next_vx = action.v * np.cos(next_theta)
|
||||
next_vy = action.v * np.sin(next_theta)
|
||||
return ObservableState(next_px, next_py, next_vx, next_vy, self.radius)
|
||||
|
||||
def get_full_state(self):
|
||||
return FullState(self.px, self.py, self.vx, self.vy, self.radius, self.gx, self.gy, self.v_pref, self.theta)
|
||||
|
||||
def get_position(self):
|
||||
return self.px, self.py
|
||||
|
||||
def set_position(self, position):
|
||||
self.px = position[0]
|
||||
self.py = position[1]
|
||||
|
||||
def get_goal_position(self):
|
||||
return self.gx, self.gy
|
||||
|
||||
def get_velocity(self):
|
||||
return self.vx, self.vy
|
||||
|
||||
def set_velocity(self, velocity):
|
||||
self.vx = velocity[0]
|
||||
self.vy = velocity[1]
|
||||
self.theta = np.arctan2(velocity[1], velocity[0])
|
||||
|
||||
@abc.abstractmethod
|
||||
def act(self, ob):
|
||||
"""
|
||||
Compute state using received observation and pass it to policy
|
||||
|
||||
"""
|
||||
return
|
||||
|
||||
def check_validity(self, action):
|
||||
if self.kinematics == 'holonomic':
|
||||
assert isinstance(action, ActionXY)
|
||||
else:
|
||||
assert isinstance(action, ActionRot)
|
||||
|
||||
def compute_position(self, action, delta_t):
|
||||
self.check_validity(action)
|
||||
if self.kinematics == 'holonomic':
|
||||
px = self.px + action.vx * delta_t
|
||||
py = self.py + action.vy * delta_t
|
||||
else:
|
||||
theta = self.theta + action.r
|
||||
px = self.px + np.cos(theta) * action.v * delta_t
|
||||
py = self.py + np.sin(theta) * action.v * delta_t
|
||||
|
||||
return px, py
|
||||
|
||||
def step(self, action):
|
||||
"""
|
||||
Perform an action and update the state
|
||||
"""
|
||||
self.check_validity(action)
|
||||
pos = self.compute_position(action, self.time_step)
|
||||
self.px, self.py = pos
|
||||
if self.kinematics == 'holonomic':
|
||||
self.vx = action.vx
|
||||
self.vy = action.vy
|
||||
else:
|
||||
self.theta = (self.theta + action.r) % (2 * np.pi)
|
||||
self.vx = action.v * np.cos(self.theta)
|
||||
self.vy = action.v * np.sin(self.theta)
|
||||
|
||||
def reached_destination(self):
|
||||
return norm(np.array(self.get_position()) - np.array(self.get_goal_position())) < self.radius
|
||||
@ -0,0 +1,17 @@
|
||||
from agent import Agent
|
||||
from state import JointState
|
||||
|
||||
|
||||
class Human(Agent):
|
||||
def __init__(self, config, section):
|
||||
super(Human, self).__init__(config, section)
|
||||
|
||||
def act(self, ob):
|
||||
"""
|
||||
The state for human is its full state and all other agents' observable states
|
||||
:param ob:
|
||||
:return:
|
||||
"""
|
||||
state = JointState(self.get_full_state(), ob)
|
||||
action = self.policy.predict(state)
|
||||
return action
|
||||
@ -0,0 +1,14 @@
|
||||
from agent import Agent
|
||||
from state import JointState
|
||||
|
||||
|
||||
class Robot(Agent):
|
||||
def __init__(self, config, section):
|
||||
super(Robot, self).__init__(config, section)
|
||||
|
||||
def act(self, ob):
|
||||
if self.policy is None:
|
||||
raise AttributeError('Policy attribute has to be set!')
|
||||
state = JointState(self.get_full_state(), ob)
|
||||
action = self.policy.predict(state)
|
||||
return action
|
||||
@ -0,0 +1,50 @@
|
||||
class FullState(object):
|
||||
def __init__(self, px, py, vx, vy, radius, gx, gy, v_pref, theta):
|
||||
self.px = px
|
||||
self.py = py
|
||||
self.vx = vx
|
||||
self.vy = vy
|
||||
self.radius = radius
|
||||
self.gx = gx
|
||||
self.gy = gy
|
||||
self.v_pref = v_pref
|
||||
self.theta = theta
|
||||
|
||||
self.position = (self.px, self.py)
|
||||
self.goal_position = (self.gx, self.gy)
|
||||
self.velocity = (self.vx, self.vy)
|
||||
|
||||
def __add__(self, other):
|
||||
return other + (self.px, self.py, self.vx, self.vy, self.radius, self.gx, self.gy, self.v_pref, self.theta)
|
||||
|
||||
def __str__(self):
|
||||
return ' '.join([str(x) for x in [self.px, self.py, self.vx, self.vy, self.radius, self.gx, self.gy,
|
||||
self.v_pref, self.theta]])
|
||||
|
||||
|
||||
class ObservableState(object):
|
||||
def __init__(self, px, py, vx, vy, radius):
|
||||
self.px = px
|
||||
self.py = py
|
||||
self.vx = vx
|
||||
self.vy = vy
|
||||
self.radius = radius
|
||||
|
||||
self.position = (self.px, self.py)
|
||||
self.velocity = (self.vx, self.vy)
|
||||
|
||||
def __add__(self, other):
|
||||
return other + (self.px, self.py, self.vx, self.vy, self.radius)
|
||||
|
||||
def __str__(self):
|
||||
return ' '.join([str(x) for x in [self.px, self.py, self.vx, self.vy, self.radius]])
|
||||
|
||||
|
||||
class JointState(object):
|
||||
def __init__(self, self_state, human_states):
|
||||
assert isinstance(self_state, FullState)
|
||||
for human_state in human_states:
|
||||
assert isinstance(human_state, ObservableState)
|
||||
|
||||
self.self_state = self_state
|
||||
self.human_states = human_states
|
||||
@ -0,0 +1,202 @@
|
||||
|
||||
Apache License
|
||||
Version 2.0, January 2004
|
||||
http://www.apache.org/licenses/
|
||||
|
||||
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
|
||||
|
||||
1. Definitions.
|
||||
|
||||
"License" shall mean the terms and conditions for use, reproduction,
|
||||
and distribution as defined by Sections 1 through 9 of this document.
|
||||
|
||||
"Licensor" shall mean the copyright owner or entity authorized by
|
||||
the copyright owner that is granting the License.
|
||||
|
||||
"Legal Entity" shall mean the union of the acting entity and all
|
||||
other entities that control, are controlled by, or are under common
|
||||
control with that entity. For the purposes of this definition,
|
||||
"control" means (i) the power, direct or indirect, to cause the
|
||||
direction or management of such entity, whether by contract or
|
||||
otherwise, or (ii) ownership of fifty percent (50%) or more of the
|
||||
outstanding shares, or (iii) beneficial ownership of such entity.
|
||||
|
||||
"You" (or "Your") shall mean an individual or Legal Entity
|
||||
exercising permissions granted by this License.
|
||||
|
||||
"Source" form shall mean the preferred form for making modifications,
|
||||
including but not limited to software source code, documentation
|
||||
source, and configuration files.
|
||||
|
||||
"Object" form shall mean any form resulting from mechanical
|
||||
transformation or translation of a Source form, including but
|
||||
not limited to compiled object code, generated documentation,
|
||||
and conversions to other media types.
|
||||
|
||||
"Work" shall mean the work of authorship, whether in Source or
|
||||
Object form, made available under the License, as indicated by a
|
||||
copyright notice that is included in or attached to the work
|
||||
(an example is provided in the Appendix below).
|
||||
|
||||
"Derivative Works" shall mean any work, whether in Source or Object
|
||||
form, that is based on (or derived from) the Work and for which the
|
||||
editorial revisions, annotations, elaborations, or other modifications
|
||||
represent, as a whole, an original work of authorship. For the purposes
|
||||
of this License, Derivative Works shall not include works that remain
|
||||
separable from, or merely link (or bind by name) to the interfaces of,
|
||||
the Work and Derivative Works thereof.
|
||||
|
||||
"Contribution" shall mean any work of authorship, including
|
||||
the original version of the Work and any modifications or additions
|
||||
to that Work or Derivative Works thereof, that is intentionally
|
||||
submitted to Licensor for inclusion in the Work by the copyright owner
|
||||
or by an individual or Legal Entity authorized to submit on behalf of
|
||||
the copyright owner. For the purposes of this definition, "submitted"
|
||||
means any form of electronic, verbal, or written communication sent
|
||||
to the Licensor or its representatives, including but not limited to
|
||||
communication on electronic mailing lists, source code control systems,
|
||||
and issue tracking systems that are managed by, or on behalf of, the
|
||||
Licensor for the purpose of discussing and improving the Work, but
|
||||
excluding communication that is conspicuously marked or otherwise
|
||||
designated in writing by the copyright owner as "Not a Contribution."
|
||||
|
||||
"Contributor" shall mean Licensor and any individual or Legal Entity
|
||||
on behalf of whom a Contribution has been received by Licensor and
|
||||
subsequently incorporated within the Work.
|
||||
|
||||
2. Grant of Copyright License. Subject to the terms and conditions of
|
||||
this License, each Contributor hereby grants to You a perpetual,
|
||||
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||
copyright license to reproduce, prepare Derivative Works of,
|
||||
publicly display, publicly perform, sublicense, and distribute the
|
||||
Work and such Derivative Works in Source or Object form.
|
||||
|
||||
3. Grant of Patent License. Subject to the terms and conditions of
|
||||
this License, each Contributor hereby grants to You a perpetual,
|
||||
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||
(except as stated in this section) patent license to make, have made,
|
||||
use, offer to sell, sell, import, and otherwise transfer the Work,
|
||||
where such license applies only to those patent claims licensable
|
||||
by such Contributor that are necessarily infringed by their
|
||||
Contribution(s) alone or by combination of their Contribution(s)
|
||||
with the Work to which such Contribution(s) was submitted. If You
|
||||
institute patent litigation against any entity (including a
|
||||
cross-claim or counterclaim in a lawsuit) alleging that the Work
|
||||
or a Contribution incorporated within the Work constitutes direct
|
||||
or contributory patent infringement, then any patent licenses
|
||||
granted to You under this License for that Work shall terminate
|
||||
as of the date such litigation is filed.
|
||||
|
||||
4. Redistribution. You may reproduce and distribute copies of the
|
||||
Work or Derivative Works thereof in any medium, with or without
|
||||
modifications, and in Source or Object form, provided that You
|
||||
meet the following conditions:
|
||||
|
||||
(a) You must give any other recipients of the Work or
|
||||
Derivative Works a copy of this License; and
|
||||
|
||||
(b) You must cause any modified files to carry prominent notices
|
||||
stating that You changed the files; and
|
||||
|
||||
(c) You must retain, in the Source form of any Derivative Works
|
||||
that You distribute, all copyright, patent, trademark, and
|
||||
attribution notices from the Source form of the Work,
|
||||
excluding those notices that do not pertain to any part of
|
||||
the Derivative Works; and
|
||||
|
||||
(d) If the Work includes a "NOTICE" text file as part of its
|
||||
distribution, then any Derivative Works that You distribute must
|
||||
include a readable copy of the attribution notices contained
|
||||
within such NOTICE file, excluding those notices that do not
|
||||
pertain to any part of the Derivative Works, in at least one
|
||||
of the following places: within a NOTICE text file distributed
|
||||
as part of the Derivative Works; within the Source form or
|
||||
documentation, if provided along with the Derivative Works; or,
|
||||
within a display generated by the Derivative Works, if and
|
||||
wherever such third-party notices normally appear. The contents
|
||||
of the NOTICE file are for informational purposes only and
|
||||
do not modify the License. You may add Your own attribution
|
||||
notices within Derivative Works that You distribute, alongside
|
||||
or as an addendum to the NOTICE text from the Work, provided
|
||||
that such additional attribution notices cannot be construed
|
||||
as modifying the License.
|
||||
|
||||
You may add Your own copyright statement to Your modifications and
|
||||
may provide additional or different license terms and conditions
|
||||
for use, reproduction, or distribution of Your modifications, or
|
||||
for any such Derivative Works as a whole, provided Your use,
|
||||
reproduction, and distribution of the Work otherwise complies with
|
||||
the conditions stated in this License.
|
||||
|
||||
5. Submission of Contributions. Unless You explicitly state otherwise,
|
||||
any Contribution intentionally submitted for inclusion in the Work
|
||||
by You to the Licensor shall be under the terms and conditions of
|
||||
this License, without any additional terms or conditions.
|
||||
Notwithstanding the above, nothing herein shall supersede or modify
|
||||
the terms of any separate license agreement you may have executed
|
||||
with Licensor regarding such Contributions.
|
||||
|
||||
6. Trademarks. This License does not grant permission to use the trade
|
||||
names, trademarks, service marks, or product names of the Licensor,
|
||||
except as required for reasonable and customary use in describing the
|
||||
origin of the Work and reproducing the content of the NOTICE file.
|
||||
|
||||
7. Disclaimer of Warranty. Unless required by applicable law or
|
||||
agreed to in writing, Licensor provides the Work (and each
|
||||
Contributor provides its Contributions) on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
|
||||
implied, including, without limitation, any warranties or conditions
|
||||
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
|
||||
PARTICULAR PURPOSE. You are solely responsible for determining the
|
||||
appropriateness of using or redistributing the Work and assume any
|
||||
risks associated with Your exercise of permissions under this License.
|
||||
|
||||
8. Limitation of Liability. In no event and under no legal theory,
|
||||
whether in tort (including negligence), contract, or otherwise,
|
||||
unless required by applicable law (such as deliberate and grossly
|
||||
negligent acts) or agreed to in writing, shall any Contributor be
|
||||
liable to You for damages, including any direct, indirect, special,
|
||||
incidental, or consequential damages of any character arising as a
|
||||
result of this License or out of the use or inability to use the
|
||||
Work (including but not limited to damages for loss of goodwill,
|
||||
work stoppage, computer failure or malfunction, or any and all
|
||||
other commercial damages or losses), even if such Contributor
|
||||
has been advised of the possibility of such damages.
|
||||
|
||||
9. Accepting Warranty or Additional Liability. While redistributing
|
||||
the Work or Derivative Works thereof, You may choose to offer,
|
||||
and charge a fee for, acceptance of support, warranty, indemnity,
|
||||
or other liability obligations and/or rights consistent with this
|
||||
License. However, in accepting such obligations, You may act only
|
||||
on Your own behalf and on Your sole responsibility, not on behalf
|
||||
of any other Contributor, and only if You agree to indemnify,
|
||||
defend, and hold each Contributor harmless for any liability
|
||||
incurred by, or claims asserted against, such Contributor by reason
|
||||
of your accepting any such warranty or additional liability.
|
||||
|
||||
END OF TERMS AND CONDITIONS
|
||||
|
||||
APPENDIX: How to apply the Apache License to your work.
|
||||
|
||||
To apply the Apache License to your work, attach the following
|
||||
boilerplate notice, with the fields enclosed by brackets "[]"
|
||||
replaced with your own identifying information. (Don't include
|
||||
the brackets!) The text should be enclosed in the appropriate
|
||||
comment syntax for the file format. We also recommend that a
|
||||
file or class name and description of purpose be included on the
|
||||
same "printed page" as the copyright notice for easier
|
||||
identification within third-party archives.
|
||||
|
||||
Copyright [yyyy] [name of copyright owner]
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
@ -0,0 +1,53 @@
|
||||
The commons-codec team is pleased to announce the Codec 1.3 release!
|
||||
|
||||
http://jakarta.apache.org/commons/codec/
|
||||
|
||||
The codec package contains simple encoder and decoders for various formats
|
||||
such as Base64 and Hexadecimal. In addition to these widely used encoders and
|
||||
decoders, the codec package also maintains a collection of phonetic encoding
|
||||
utilities.
|
||||
|
||||
Changes in this version include:
|
||||
|
||||
New Features:
|
||||
|
||||
o BinaryCodec: Encodes and decodes binary to and from Strings of 0s and 1s.
|
||||
Issue: 27813. Thanks to Alex Karasulu.
|
||||
o QuotedPrintableCodec: Codec for RFC 1521 MIME (Multipurpose Internet Mail
|
||||
Extensions) Part One. Rules #3, #4, and #5 of the quoted-printable spec are
|
||||
not implemented yet. See also issue 27789. Issue: 26617. Thanks to Oleg
|
||||
Kalnichevski.
|
||||
o BCodec: Identical to the Base64 encoding defined by RFC 1521 and allows a
|
||||
character set to be specified. Issue: 26617. Thanks to Oleg Kalnichevski.
|
||||
o QCodec: Similar to the Quoted-Printable content-transfer-encoding defined
|
||||
in RFC 1521 and designed to allow text containing mostly ASCII characters
|
||||
to be decipherable on an ASCII terminal without decoding. Issue: 26617.
|
||||
Thanks to Oleg Kalnichevski.
|
||||
o Soundex: Implemented the DIFFERENCE algorithm. Issue: 25243. Thanks to
|
||||
Matthew Inger.
|
||||
o RefinedSoundex: Implemented the DIFFERENCE algorithm. Issue: 25243. Thanks
|
||||
to Matthew Inger.
|
||||
|
||||
Fixed bugs:
|
||||
|
||||
o The default URL encoding logic was broken. Issue: 25995. Thanks to Oleg
|
||||
Kalnichevski.
|
||||
o Base64 chunked encoding not compliant with RFC 2045 section 2.1 CRLF.
|
||||
Issue: 27781. Thanks to Gary D. Gregory.
|
||||
o Hex converts illegal characters to 255. Issue: 28455.
|
||||
o Metaphone now correctly handles a silent B in a word that ends in MB.
|
||||
"COMB" is encoded as "KM", before this fix "COMB" was encoded as "KMB".
|
||||
Issue: 28457.
|
||||
o Added missing tags in Javadoc comments.
|
||||
o General Javadoc improvements.
|
||||
|
||||
Changes:
|
||||
|
||||
o This version is relesed under the Apache License 2.0 , please see
|
||||
LICENSE.txt. Previous versions were released under the Apache License 1.1.
|
||||
o The Board recommendation to remove Javadoc author tags has been
|
||||
implemented. All author tags are now "Apache Software Foundation".
|
||||
|
||||
Have fun!
|
||||
-The commons-codec team
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@ -0,0 +1,10 @@
|
||||
#Update RosList name
|
||||
#Mon Feb 08 22:15:58 CST 2021
|
||||
ProjectPath=/home/xyz/workspace/ars_ws
|
||||
DefaultPointX=0
|
||||
RosList=WaypointReachedNotifier
|
||||
AgentList=PlannerAgent,PlanDispatchAgent,MoveAgent
|
||||
TaskList=DomainDescription,ProblemDescription
|
||||
ProjectName=IndoorNavigationWaypointTracing
|
||||
ProjectType=Deliberative
|
||||
DefaultPointY=0
|
||||
@ -0,0 +1,34 @@
|
||||
package SampleApplication.Agent;
|
||||
|
||||
import SampleApplication.AgentConfiguration;
|
||||
import agentBehaviourPrototype.ExecutionBehaviour;
|
||||
import agentBehaviourPrototype.ReceiveDataFromAgentBehaviour;
|
||||
import agentBehaviourPrototype.SendDataToAgentBehaviour;
|
||||
import agentBehaviourPrototype.ReceiveDataFromROSBehaviour;
|
||||
import agentEntityPrototype.RobotAgent;
|
||||
import edu.wpi.rail.jrosbridge.services.std.Empty.Request;
|
||||
import jade.core.AID;
|
||||
|
||||
public class ${entity.className}<#if entity.superclass?has_content> extends ${entity.superclass} </#if>
|
||||
{
|
||||
|
||||
<#list entity.allRelatedElements as allReEle>
|
||||
private AID ${allReEle.specificType} = new AID(AgentConfiguration.${allReEle.specificType}ID, false);
|
||||
</#list>
|
||||
|
||||
protected void setup() {
|
||||
//automatically generated
|
||||
this.controller.addTopic(AgentConfiguration.actionDispatchFeedbackTopicName, AgentConfiguration.actionDispatchFeedbackTopicType);
|
||||
|
||||
//agent communication
|
||||
<#list entity.postiveRelatedElementsWithRelation as posReEleWithRela>
|
||||
this.behList.addElement(new SendDataToAgentBehaviour(this, controller, ${posReEleWithRela.specificType}, AgentConfiguration.${posReEleWithRela.relation}DataKey));
|
||||
</#list>
|
||||
<#list entity.negativeRelatedElementsWithRelation as negReEleWithRela>
|
||||
this.behList.addElement(new ReceiveDataFromROSBehaviour(this, controller, AgentConfiguration.actionDispatchFeedbackTopicName, AgentConfiguration.${negReEleWithRela.relation}DataKey));
|
||||
</#list>
|
||||
|
||||
addBehaviour(tbf.wrap(new ExecutionBehaviour(this, ds, this.behList)));
|
||||
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,158 @@
|
||||
package CodeGenerator;
|
||||
import java.io.File;
|
||||
import java.io.FileWriter;
|
||||
import java.io.IOException;
|
||||
import java.io.OutputStreamWriter;
|
||||
import java.io.Writer;
|
||||
import java.util.ArrayList;
|
||||
import java.util.HashMap;
|
||||
import java.util.List;
|
||||
import java.util.Map;
|
||||
|
||||
import freemarker.template.Configuration;
|
||||
import freemarker.template.DefaultObjectWrapper;
|
||||
import freemarker.template.Template;
|
||||
import freemarker.template.TemplateException;
|
||||
|
||||
public class AgentCodeGeneratorClient {
|
||||
|
||||
private static File javaFile = null;
|
||||
|
||||
public static void main(String[] args) {
|
||||
Configuration cfg = new Configuration();
|
||||
try {
|
||||
// 指定模板文件从何处加载的数据源，这里设置一个文件目录
|
||||
cfg.setDirectoryForTemplateLoading(new File("./src/CodeGenerator"));
|
||||
cfg.setObjectWrapper(new DefaultObjectWrapper());
|
||||
|
||||
// 获取模板文件
|
||||
Template template = cfg.getTemplate("agent.ftl");
|
||||
|
||||
// 创建数据模型
|
||||
Map<String, Object> root = createDataModel(null, 0);
|
||||
|
||||
// 合并模板和数据模型
|
||||
// 创建.java类文件
|
||||
if(javaFile != null){
|
||||
Writer javaWriter = new FileWriter(javaFile);
|
||||
template.process(root, javaWriter);
|
||||
javaWriter.flush();
|
||||
System.out.println("文件生成路径：" + javaFile.getCanonicalPath());
|
||||
|
||||
javaWriter.close();
|
||||
}
|
||||
// 输出到Console控制台
|
||||
Writer out = new OutputStreamWriter(System.out);
|
||||
template.process(root, out);
|
||||
out.flush();
|
||||
out.close();
|
||||
|
||||
} catch (IOException e) {
|
||||
e.printStackTrace();
|
||||
} catch (TemplateException e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
//创建数据模型
|
||||
private static Map<String, Object> createDataModel(String agentName, int tbf) {
|
||||
Map<String, Object> root = new HashMap<String, Object>();
|
||||
Entity user = new Entity();
|
||||
TbfEntity user1 = new TbfEntity();
|
||||
ControllerEntity user2 = new ControllerEntity();
|
||||
user.setJavaPackage("agent"); // 创建包名
|
||||
user.setClassName(agentName + "Agent"); // 创建类名
|
||||
user.setConstructors(false); // 是否创建构造函数
|
||||
user.setSuperclass("Agent");//创建父类
|
||||
|
||||
List<Property> propertyList = new ArrayList<Property>();
|
||||
List<TbfEntity> tbfEntityList = new ArrayList<TbfEntity>();
|
||||
List<Entity> entityList = new ArrayList<Entity>();
|
||||
List<ControllerEntity> controllerEntityList = new ArrayList<ControllerEntity>();
|
||||
|
||||
// 创建实体属性
|
||||
Property attribute1 = new Property();
|
||||
attribute1.setJavaType("static final long");
|
||||
attribute1.setPropertyName("serialVersionUID");
|
||||
attribute1.setPropertyType(PropertyType.Long);
|
||||
|
||||
// 创建实体属性(tbf)
|
||||
//int tbfAmount = 4; //这里输入tbf的数量
|
||||
for (int i = 1; i < tbf +1 ; i = i + 1) {
|
||||
String tbfName = "tbf" + i;
|
||||
TbfEntity tbfEntityAttribute = new TbfEntity();
|
||||
tbfEntityAttribute.setClassName(tbfName);
|
||||
tbfEntityAttribute.setSuperclass("ThreadedBehaviourFactory");
|
||||
tbfEntityList.add(tbfEntityAttribute);
|
||||
}
|
||||
|
||||
//TbfEntity tbfEntityAttribute2 = new TbfEntity();
|
||||
//tbfEntityAttribute2.setClassName("tbf2");
|
||||
//tbfEntityAttribute2.setSuperclass("ThreadedBehaviourFactory");
|
||||
|
||||
//创建实体属性(controller)
|
||||
ControllerEntity controllerAttribute = new ControllerEntity();
|
||||
controllerAttribute.setClassName("moveController");
|
||||
controllerAttribute.setSuperclass("Controller");
|
||||
|
||||
//创建实体属性(除tbf和controller以外的属性)
|
||||
Entity entityAttribute1 = new Entity();
|
||||
entityAttribute1.setClassName("cameraAgent");
|
||||
entityAttribute1.setSuperclass("AID");
|
||||
|
||||
Entity entityAttribute2 = new Entity();
|
||||
entityAttribute2.setClassName("bumperAgent");
|
||||
entityAttribute2.setSuperclass("AID");
|
||||
|
||||
Entity entityAttribute3 = new Entity();
|
||||
entityAttribute3.setClassName("ds");
|
||||
entityAttribute3.setSuperclass("DataStore");
|
||||
|
||||
propertyList.add(attribute1);
|
||||
//tbfEntityList.add(tbfEntityAttribute1);
|
||||
//tbfEntityList.add(tbfEntityAttribute2);
|
||||
controllerEntityList.add(controllerAttribute);
|
||||
entityList.add(entityAttribute1);
|
||||
entityList.add(entityAttribute2);
|
||||
entityList.add(entityAttribute3);
|
||||
|
||||
// 将属性集合添加到实体对象中
|
||||
user.setProperties(propertyList);
|
||||
user.setEntities(entityList);
|
||||
user1.setTbfentities(tbfEntityList);
|
||||
user2.setControllerentities(controllerEntityList);
|
||||
|
||||
// 创建.java类文件
|
||||
File outDirFile = new File("./agent-generator");
|
||||
if(!outDirFile.exists()){
|
||||
outDirFile.mkdir();
|
||||
}
|
||||
|
||||
javaFile = toJavaFilename(outDirFile, user.getJavaPackage(), user.getClassName());
|
||||
|
||||
root.put("entity", user);
|
||||
root.put("tbfentity",user1);
|
||||
root.put("controllerentity", user2);
|
||||
return root;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* 创建.java文件所在路径 和 返回.java文件File对象
|
||||
* @param outDirFile 生成文件路径
|
||||
* @param javaPackage java包名
|
||||
* @param javaClassName java类名
|
||||
* @return
|
||||
*/
|
||||
private static File toJavaFilename(File outDirFile, String javaPackage, String javaClassName) {
|
||||
String packageSubPath = javaPackage.replace('.', '/');
|
||||
File packagePath = new File(outDirFile, packageSubPath);
|
||||
File file = new File(packagePath, javaClassName + ".java");
|
||||
if(!packagePath.exists()){
|
||||
packagePath.mkdirs();
|
||||
}
|
||||
return file;
|
||||
}
|
||||
|
||||
}
|
||||
@ -0,0 +1,88 @@
|
||||
package agentApplication2;
|
||||
|
||||
import jade.core.Profile;
|
||||
import jade.core.ProfileImpl;
|
||||
import jade.wrapper.AgentController;
|
||||
import jade.wrapper.StaleProxyException;
|
||||
|
||||
public class AgentConfiguration {
|
||||
|
||||
//-----------------------------------------user inputs------------------------------------------------------//
|
||||
|
||||
//Agent names
|
||||
public static final String moveAgentID = "moveAgent";
|
||||
public static final String taskPlanningAgentID = "taskPlanningAgent";
|
||||
public static final String dispatchAgentID = "dispatchAgent";
|
||||
|
||||
//Planning model paths
|
||||
public static final String domainFilePath = "/home/ys/eclipse-workspace/AutoRobot/PDDLFiles/domain_turtlebot.pddl";
|
||||
public static final String problemFilePath = "/home/ys/eclipse-workspace/AutoRobot/PDDLFiles/problem_turtlebot.pddl";
|
||||
|
||||
//ROS services
|
||||
public static final String problemGenerationServiceName = "/rosplan_problem_interface/problem_generation_server";
|
||||
public static final String problemGenerationServiceType = "std_srvs/Empty";
|
||||
|
||||
public static final String planServiceName = "/rosplan_planner_interface/planning_server";
|
||||
public static final String planServiceType = "std_srvs/Empty";
|
||||
|
||||
public static final String parseServiceName = "/rosplan_parsing_interface/parse_plan";
|
||||
public static final String parseServiceType = "std_srvs/Empty";
|
||||
|
||||
public static final String dispatchServiceName = "/rosplan_plan_dispatcher/dispatch_plan";
|
||||
public static final String dispatchServiceType = "std_srvs/Empty";
|
||||
|
||||
//ROS topics
|
||||
public static final String actionDispatchFeedbackTopicName = "/rosplan_plan_dispatcher/action_feedback";
|
||||
public static final String actionDispatchFeedbackTopicType = "rosplan_dispatch_msgs/ActionFeedback";
|
||||
|
||||
//DataStore keys
|
||||
public static final String planDispatchDataKey = "planDispatch";
|
||||
public static final String planFeedbackDataKey = "planFeedback";
|
||||
public static final String actionDispatchDataKey = "actionDispatch";
|
||||
public static String actionFeedbackDataKey = "actionFeedback";
|
||||
|
||||
|
||||
public static void main(String[] args) throws InterruptedException {
|
||||
|
||||
//automatically generated
|
||||
jade.core.Runtime rt = jade.core.Runtime.instance();
|
||||
Profile pContainer = new ProfileImpl(null, 8888, null);
|
||||
|
||||
rt.setCloseVM(true);
|
||||
|
||||
Profile pMain = new ProfileImpl(null, 1099, null);
|
||||
pMain.setParameter(Profile.SERVICES, "jade.core.messaging.TopicManagementService");
|
||||
|
||||
System.out.println("Launching a whole in-process platform..." + pMain);
|
||||
jade.wrapper.AgentContainer mc = rt.createMainContainer(pMain);
|
||||
jade.wrapper.AgentContainer agentContainer = rt.createAgentContainer(pContainer);
|
||||
System.out.println("continue");
|
||||
|
||||
|
||||
//Create Agents
|
||||
try {
|
||||
AgentController moveAgent = agentContainer.createNewAgent(AgentConfiguration.moveAgentID, "AutoRobot.agentApplication2.MoveAgent", null);
|
||||
moveAgent.start();
|
||||
} catch (StaleProxyException e2) {
|
||||
e2.printStackTrace();
|
||||
}
|
||||
|
||||
try{
|
||||
AgentController dispatchAgent = agentContainer.createNewAgent(AgentConfiguration.dispatchAgentID, "AutoRobot.agentApplication2.DispatchAgent", null);
|
||||
dispatchAgent.start();
|
||||
} catch (StaleProxyException e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
|
||||
try{
|
||||
AgentController taskPlanningAgent = agentContainer.createNewAgent(AgentConfiguration.taskPlanningAgentID, "AutoRobot.agentApplication2.TaskPlanningAgent", null);
|
||||
taskPlanningAgent.start();
|
||||
} catch (StaleProxyException e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
//-------------------------------------------------------------------------------------------------------------------------------------------//
|
||||
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
@ -0,0 +1,174 @@
|
||||
package CodeGenerator;
|
||||
|
||||
import java.awt.Canvas;
|
||||
import java.io.File;
|
||||
import java.io.FileWriter;
|
||||
import java.io.IOException;
|
||||
import java.io.OutputStreamWriter;
|
||||
import java.io.Writer;
|
||||
import java.util.ArrayList;
|
||||
import java.util.HashMap;
|
||||
import java.util.HashSet;
|
||||
import java.util.List;
|
||||
import java.util.Map;
|
||||
|
||||
import freemarker.template.Configuration;
|
||||
import freemarker.template.DefaultObjectWrapper;
|
||||
import freemarker.template.Template;
|
||||
import freemarker.template.TemplateException;
|
||||
|
||||
public class AgentConfigurationCodeGeneratorClient {
|
||||
|
||||
private static File javaFile = null;
|
||||
|
||||
public static void AgentFileGenerator(String path) {
|
||||
Configuration cfg = new Configuration();
|
||||
try {
|
||||
// 指定模板文件从何处加载的数据源，这里设置一个文件目录
|
||||
cfg.setDirectoryForTemplateLoading(new File("./src/CodeGenerator"));
|
||||
cfg.setObjectWrapper(new DefaultObjectWrapper());
|
||||
|
||||
// 获取模板文件
|
||||
Template template = cfg.getTemplate("AgentConfiguration.ftl");
|
||||
|
||||
// 创建数据模型
|
||||
Map<String, Object> root = createDataModel(path);
|
||||
|
||||
// 合并模板和数据模型
|
||||
// 创建.java类文件
|
||||
if(javaFile != null){
|
||||
Writer javaWriter = new FileWriter(javaFile);
|
||||
template.process(root, javaWriter);
|
||||
javaWriter.flush();
|
||||
System.out.println("文件生成路径：" + javaFile.getCanonicalPath());
|
||||
|
||||
javaWriter.close();
|
||||
}
|
||||
|
||||
} catch (IOException e) {
|
||||
e.printStackTrace();
|
||||
} catch (TemplateException e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
//创建数据模型
|
||||
private static Map<String, Object> createDataModel(String path) {
|
||||
Map<String, Object> root = new HashMap<String, Object>();
|
||||
|
||||
Entity user = new Entity();
|
||||
TbfEntity user1 = new TbfEntity();
|
||||
ControllerEntity user2 = new ControllerEntity();
|
||||
user.setClassName("AgentConfiguration");
|
||||
/*
|
||||
|
||||
//user.setJavaPackage("agent"); // 创建包名
|
||||
user.setClassName(drawelement.getName()); // 创建类名
|
||||
user.setConstructors(false); // 是否创建构造函数
|
||||
user.setSuperclass("RobotAgent");//创建父类
|
||||
user.setSpecificType(drawelement.getSpecificType());
|
||||
|
||||
List<Property> propertyList = new ArrayList<Property>();
|
||||
List<TbfEntity> tbfEntityList = new ArrayList<TbfEntity>();
|
||||
List<Entity> entityList = new ArrayList<Entity>();
|
||||
List<ControllerEntity> controllerEntityList = new ArrayList<ControllerEntity>();
|
||||
|
||||
|
||||
|
||||
// 创建实体属性
|
||||
Property attribute1 = new Property();
|
||||
attribute1.setJavaType("static final long");
|
||||
attribute1.setPropertyName("serialVersionUID");
|
||||
attribute1.setPropertyType(PropertyType.Long);
|
||||
|
||||
|
||||
// 创建实体属性(tbf)
|
||||
int tbfAmount = 4; //这里输入tbf的数量
|
||||
for (int i = 1; i < tbfAmount +1 ; i = i + 1) {
|
||||
String tbfName = "tbf" + i;
|
||||
TbfEntity tbfEntityAttribute = new TbfEntity();
|
||||
tbfEntityAttribute.setClassName(tbfName);
|
||||
tbfEntityAttribute.setSuperclass("ThreadedBehaviourFactory");
|
||||
tbfEntityList.add(tbfEntityAttribute);
|
||||
}
|
||||
|
||||
|
||||
|
||||
//创建实体属性(controller)
|
||||
ControllerEntity controllerAttribute = new ControllerEntity();
|
||||
controllerAttribute.setClassName("moveController");
|
||||
controllerAttribute.setSuperclass("Controller");
|
||||
|
||||
//创建实体属性(除tbf和controller以外的属性)
|
||||
Entity entityAttribute1 = new Entity();
|
||||
entityAttribute1.setClassName("cameraAgent");
|
||||
entityAttribute1.setSuperclass("AID");
|
||||
|
||||
Entity entityAttribute2 = new Entity();
|
||||
entityAttribute2.setClassName("bumperAgent");
|
||||
entityAttribute2.setSuperclass("AID");
|
||||
|
||||
Entity entityAttribute3 = new Entity();
|
||||
entityAttribute3.setClassName("ds");
|
||||
entityAttribute3.setSuperclass("DataStore");
|
||||
|
||||
propertyList.add(attribute1);
|
||||
//tbfEntityList.add(tbfEntityAttribute1);
|
||||
//tbfEntityList.add(tbfEntityAttribute2);
|
||||
controllerEntityList.add(controllerAttribute);
|
||||
entityList.add(entityAttribute1);
|
||||
entityList.add(entityAttribute2);
|
||||
entityList.add(entityAttribute3);
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// 将属性集合添加到实体对象中
|
||||
user.setProperties(propertyList);
|
||||
user.setEntities(entityList);
|
||||
user1.setTbfentities(tbfEntityList);
|
||||
user2.setControllerentities(controllerEntityList);
|
||||
*/
|
||||
|
||||
// 创建.java类文件
|
||||
File outDirFile = new File(path); // .java类文件存放的文件夹
|
||||
if(!outDirFile.exists()){
|
||||
outDirFile.mkdir();
|
||||
}
|
||||
|
||||
javaFile = toJavaFilename(outDirFile, user.getClassName());
|
||||
|
||||
root.put("entity", user);
|
||||
root.put("tbfentity",user1);
|
||||
root.put("controllerentity", user2);
|
||||
|
||||
return root;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* 创建.java文件所在路径 和 返回.java文件File对象
|
||||
* @param outDirFile 生成文件路径
|
||||
* @param javaPackage java包名
|
||||
* @param javaClassName java类名
|
||||
* @return
|
||||
*/
|
||||
private static File toJavaFilename(File outDirFile, String javaClassName) {
|
||||
File file = new File(outDirFile, javaClassName + ".java");
|
||||
return file;
|
||||
}
|
||||
|
||||
// 删除ArrayList中重复元素
|
||||
public static void removeDuplicate(List<DrawElement> list) {
|
||||
for (int i = 0; i < list.size() - 1; i++) {
|
||||
for (int j = list.size() - 1; j > i; j--) {
|
||||
if (list.get(j).equals(list.get(i))) {
|
||||
list.remove(j);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@ -0,0 +1,128 @@
|
||||
package CodeGenerator;
|
||||
|
||||
import javax.swing.JLabel;
|
||||
import javax.swing.JTextField;
|
||||
import javax.swing.JButton;
|
||||
import javax.swing.JFrame;
|
||||
|
||||
import java.awt.event.ActionListener;
|
||||
import java.awt.event.ActionEvent;
|
||||
import javax.swing.JComboBox;
|
||||
|
||||
public class AgentGeneratorDialog extends JFrame implements ActionListener {
|
||||
private JTextField textField;
|
||||
private JTextField textField_1;
|
||||
private JTextField textField_2;
|
||||
private JButton createAgentButton;
|
||||
private JComboBox <String> comboBoxAgentType;
|
||||
String mypath;
|
||||
/**
|
||||
* Create the panel.
|
||||
*/
|
||||
public AgentGeneratorDialog(String path) {
|
||||
super("Create Agent");
|
||||
this.mypath = path;
|
||||
System.out.println("sdsd");
|
||||
getContentPane().setLayout(null);
|
||||
this.setBounds(300, 300, 450, 180);
|
||||
|
||||
JLabel lblAgentName = new JLabel("Agent Name:");
|
||||
lblAgentName.setBounds(113, 24, 81, 16);
|
||||
getContentPane().add(lblAgentName);
|
||||
|
||||
/*
|
||||
JLabel lblNumberOfBehavior = new JLabel("Number of Behavior:");
|
||||
lblNumberOfBehavior.setBounds(66, 57, 128, 16);
|
||||
this.add(lblNumberOfBehavior);
|
||||
|
||||
JLabel lblNumberOfConnect = new JLabel("Number of Connect Agent:");
|
||||
lblNumberOfConnect.setBounds(26, 93, 168, 16);
|
||||
this.add(lblNumberOfConnect);
|
||||
*/
|
||||
|
||||
textField = new JTextField();
|
||||
textField.setBounds(206, 19, 217, 26);
|
||||
getContentPane().add(textField);
|
||||
textField.setColumns(10);
|
||||
|
||||
/*
|
||||
textField_1 = new JTextField();
|
||||
textField_1.setBounds(206, 52, 217, 26);
|
||||
this.add(textField_1);
|
||||
textField_1.setColumns(10);
|
||||
|
||||
textField_2 = new JTextField();
|
||||
textField_2.setBounds(206, 88, 217, 26);
|
||||
this.add(textField_2);
|
||||
textField_2.setColumns(10);
|
||||
*/
|
||||
|
||||
createAgentButton = new JButton("Create Agent");
|
||||
//createAgentButton.setBounds(177, 130, 117, 29);
|
||||
createAgentButton.setBounds(306, 98, 117, 29);
|
||||
getContentPane().add(createAgentButton);
|
||||
|
||||
comboBoxAgentType = new JComboBox();
|
||||
comboBoxAgentType.setBounds(206, 59, 217, 27);
|
||||
getContentPane().add(comboBoxAgentType);
|
||||
comboBoxAgentType.addItem("Planner Agent");
|
||||
comboBoxAgentType.addItem("Dispatcher Agent");
|
||||
comboBoxAgentType.addItem("Sensor Agent");
|
||||
comboBoxAgentType.addItem("Actuator Agent");
|
||||
|
||||
JLabel lblAgentType = new JLabel("Agent Type:");
|
||||
lblAgentType.setBounds(123, 63, 75, 16);
|
||||
getContentPane().add(lblAgentType);
|
||||
createAgentButton.addActionListener(this);
|
||||
|
||||
/*
|
||||
JButton cancelButton = new JButton("Cancel");
|
||||
cancelButton.setBounds(306, 130, 117, 29);
|
||||
add(cancelButton);
|
||||
cancelButton.addActionListener(new ActionListener()
|
||||
{
|
||||
public void actionPerformed(ActionEvent event)
|
||||
{
|
||||
createAgentDialog.setVisible(false);
|
||||
}
|
||||
});
|
||||
*/
|
||||
this.setDefaultCloseOperation(JFrame.DISPOSE_ON_CLOSE);
|
||||
this.setLocationRelativeTo(null);
|
||||
this.setVisible(true);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void actionPerformed(ActionEvent e) {
|
||||
if(e.getSource() == this.createAgentButton) {
|
||||
/*
|
||||
String name = this.textField.getText();
|
||||
//String tbfstring = this.textField_1.getText();
|
||||
//int tbf = Integer.parseInt(tbfstring);
|
||||
String type = (String)this.comboBoxAgentType.getSelectedItem();
|
||||
if (type == "Planner Agent")
|
||||
{
|
||||
PlannerAgentCodeGeneratorClient.AgentFileGenerator(mypath, name);
|
||||
//System.out.println("Plan Agent");
|
||||
}
|
||||
else if (type == "Dispatcher Agent")
|
||||
{
|
||||
DispatcherAgentCodeGeneratorClient.AgentFileGenerator(mypath, name);
|
||||
//System.out.println("Dispatch Agent");
|
||||
}
|
||||
else if (type == "Sensor Agent")
|
||||
{
|
||||
SensorAgentCodeGeneratorClient.AgentFileGenerator(mypath, name);
|
||||
//System.out.println("Sensor Agent");
|
||||
}
|
||||
else
|
||||
{
|
||||
SensorAgentCodeGeneratorClient.AgentFileGenerator(mypath, name);
|
||||
//System.out.println("Actuator Agent");
|
||||
}
|
||||
this.dispose();
|
||||
//this.setVisible(false);
|
||||
*/
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,259 @@
|
||||
package CodeGenerator;
|
||||
|
||||
import javax.swing.*;
|
||||
import java.awt.*;
|
||||
|
||||
import java.awt.BorderLayout;
|
||||
import java.awt.EventQueue;
|
||||
|
||||
import javax.swing.JFrame;
|
||||
import javax.swing.JPanel;
|
||||
import javax.swing.border.EmptyBorder;
|
||||
import javax.swing.JRadioButton;
|
||||
import java.awt.Component;
|
||||
import javax.swing.JLabel;
|
||||
import javax.swing.JTextField;
|
||||
import javax.swing.ButtonGroup;
|
||||
import javax.swing.JButton;
|
||||
import javax.swing.JFileChooser;
|
||||
import javax.swing.border.LineBorder;
|
||||
import java.awt.Color;
|
||||
import java.awt.event.ActionListener;
|
||||
import java.io.File;
|
||||
import java.awt.event.ActionEvent;
|
||||
|
||||
public class AutoRobotAssistant extends JFrame {
|
||||
|
||||
private JPanel contentPane;
|
||||
private final JPanel panel = new JPanel();
|
||||
private JTextField newNameText;
|
||||
private JTextField openLocationText;
|
||||
private JTextField textField_3;
|
||||
private JTextField textField_5;
|
||||
private JTextField textField_6;
|
||||
private JRadioButton RadioButtonNew;
|
||||
private JRadioButton RadioButtonOpen;
|
||||
private JTextField newLocationText;
|
||||
private JFileChooser chooser;
|
||||
private Icon icon;
|
||||
|
||||
/**
|
||||
* Launch the application.
|
||||
*/
|
||||
public static void main(String[] args) {
|
||||
EventQueue.invokeLater(new Runnable() {
|
||||
public void run() {
|
||||
//try {
|
||||
// AutoRobotAssistant frame = new AutoRobotAssistant();
|
||||
// frame.setVisible(true);
|
||||
//} catch (Exception e) {
|
||||
// e.printStackTrace();
|
||||
//}
|
||||
AutoRobotAssistant frame = new AutoRobotAssistant();
|
||||
frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
|
||||
frame.setVisible(true);
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
/**
|
||||
* Create the frame.
|
||||
*/
|
||||
public AutoRobotAssistant() {
|
||||
setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
|
||||
setBounds(100, 100, 812, 470);
|
||||
contentPane = new JPanel();
|
||||
contentPane.setBorder(new EmptyBorder(5, 5, 5, 5));
|
||||
setContentPane(contentPane);
|
||||
contentPane.setLayout(null);
|
||||
panel.setBorder(new LineBorder(Color.GRAY));
|
||||
panel.setBounds(0, 0, 450, 188);
|
||||
contentPane.add(panel);
|
||||
panel.setLayout(null);
|
||||
|
||||
chooser = new JFileChooser();
|
||||
|
||||
JLabel lblProject = new JLabel("Application");
|
||||
lblProject.setBounds(6, 7, 72, 16);
|
||||
panel.add(lblProject);
|
||||
|
||||
JLabel newNameLabel = new JLabel("Name");
|
||||
newNameLabel.setBounds(96, 33, 41, 16);
|
||||
panel.add(newNameLabel);
|
||||
|
||||
newNameText = new JTextField();
|
||||
newNameText.setBounds(149, 28, 295, 26);
|
||||
panel.add(newNameText);
|
||||
newNameText.setColumns(10);
|
||||
|
||||
JLabel newLocationLabel = new JLabel("Path");
|
||||
newLocationLabel.setBounds(96, 64, 41, 16);
|
||||
panel.add(newLocationLabel);
|
||||
|
||||
JButton btnNewButton = new JButton("Create and Open");
|
||||
btnNewButton.setBounds(314, 90, 130, 29);
|
||||
panel.add(btnNewButton);
|
||||
|
||||
openLocationText = new JTextField();
|
||||
openLocationText.setColumns(10);
|
||||
openLocationText.setBounds(149, 121, 280, 26);
|
||||
panel.add(openLocationText);
|
||||
|
||||
JButton btnOpen = new JButton("Open");
|
||||
btnOpen.setBounds(314, 152, 130, 29);
|
||||
panel.add(btnOpen);
|
||||
|
||||
JLabel openLocationLabel = new JLabel("Path");
|
||||
openLocationLabel.setBounds(96, 126, 41, 16);
|
||||
panel.add(openLocationLabel);
|
||||
|
||||
JButton pathButton_1 = new JButton("..");
|
||||
pathButton_1.addActionListener(new ActionListener() {
|
||||
public void actionPerformed(ActionEvent e) {
|
||||
chooser.setCurrentDirectory(new File("."));
|
||||
chooser.setFileSelectionMode(JFileChooser.DIRECTORIES_ONLY);
|
||||
chooser.setDialogTitle("Choose Application Location");
|
||||
int result = chooser.showOpenDialog(AutoRobotAssistant.this);
|
||||
if (result == JFileChooser.APPROVE_OPTION)
|
||||
{
|
||||
String path = chooser.getSelectedFile().getPath();
|
||||
newLocationText.setText(path);
|
||||
}
|
||||
}
|
||||
});
|
||||
pathButton_1.setBounds(429, 64, 15, 16);
|
||||
panel.add(pathButton_1);
|
||||
|
||||
ButtonGroup group = new ButtonGroup();
|
||||
|
||||
JRadioButton RadioButtonNew = new JRadioButton("New");
|
||||
RadioButtonNew.addActionListener(new ActionListener() {
|
||||
public void actionPerformed(ActionEvent event) {
|
||||
newNameLabel.setEnabled(true);
|
||||
newNameText.setEnabled(true);
|
||||
newLocationLabel.setEnabled(true);
|
||||
newLocationText.setEnabled(true);
|
||||
pathButton_1.setEnabled(true);
|
||||
openLocationLabel.setEnabled(false);
|
||||
openLocationText.setEnabled(false);
|
||||
}
|
||||
});
|
||||
RadioButtonNew.setBounds(16, 29, 68, 23);
|
||||
panel.add(RadioButtonNew);
|
||||
group.add(RadioButtonNew);
|
||||
|
||||
JRadioButton RadioButtonOpen = new JRadioButton("Open");
|
||||
RadioButtonOpen.addActionListener(new ActionListener() {
|
||||
public void actionPerformed(ActionEvent event) {
|
||||
newNameLabel.setEnabled(false);
|
||||
newNameText.setEnabled(false);
|
||||
newLocationLabel.setEnabled(false);
|
||||
newLocationText.setEnabled(false);
|
||||
pathButton_1.setEnabled(false);
|
||||
openLocationLabel.setEnabled(true);
|
||||
openLocationText.setEnabled(true);
|
||||
}
|
||||
});
|
||||
RadioButtonOpen.setBounds(16, 122, 68, 23);
|
||||
panel.add(RadioButtonOpen);
|
||||
group.add(RadioButtonOpen);
|
||||
|
||||
newLocationText = new JTextField();
|
||||
//LocationText_1.setText();
|
||||
newLocationText.setColumns(10);
|
||||
newLocationText.setBounds(149, 59, 280, 26);
|
||||
panel.add(newLocationText);
|
||||
|
||||
JButton pathButton_2 = new JButton("..");
|
||||
pathButton_2.addActionListener(new ActionListener() {
|
||||
public void actionPerformed(ActionEvent event) {
|
||||
chooser.setCurrentDirectory(new File("."));
|
||||
chooser.setFileSelectionMode(JFileChooser.DIRECTORIES_ONLY);
|
||||
chooser.setDialogTitle("Choose Application Location");
|
||||
int result = chooser.showOpenDialog(AutoRobotAssistant.this);
|
||||
if (result == JFileChooser.APPROVE_OPTION)
|
||||
{
|
||||
String path = chooser.getSelectedFile().getPath();
|
||||
openLocationText.setText(path);
|
||||
}
|
||||
}
|
||||
});
|
||||
pathButton_2.setBounds(429, 127, 15, 16);
|
||||
panel.add(pathButton_2);
|
||||
|
||||
JPanel panel_1 = new JPanel();
|
||||
panel_1.setBorder(new LineBorder(new Color(128, 128, 128)));
|
||||
panel_1.setBounds(0, 188, 450, 188);
|
||||
contentPane.add(panel_1);
|
||||
panel_1.setLayout(null);
|
||||
|
||||
JLabel lblCodegenerator = new JLabel("Agent Generator");
|
||||
lblCodegenerator.setBounds(6, 6, 110, 16);
|
||||
panel_1.add(lblCodegenerator);
|
||||
|
||||
JLabel lblPackageName = new JLabel("Agent Name");
|
||||
lblPackageName.setBounds(95, 34, 77, 16);
|
||||
panel_1.add(lblPackageName);
|
||||
|
||||
textField_3 = new JTextField();
|
||||
textField_3.setBounds(196, 29, 248, 26);
|
||||
panel_1.add(textField_3);
|
||||
textField_3.setColumns(10);
|
||||
|
||||
JLabel lblNumberOfTbf = new JLabel("Number of rbehavior");
|
||||
lblNumberOfTbf.setBounds(43, 85, 130, 21);
|
||||
panel_1.add(lblNumberOfTbf);
|
||||
|
||||
textField_5 = new JTextField();
|
||||
textField_5.setColumns(10);
|
||||
textField_5.setBounds(196, 115, 248, 26);
|
||||
panel_1.add(textField_5);
|
||||
|
||||
JLabel lblNumberOfConnect = new JLabel("Number of Connect Agent");
|
||||
lblNumberOfConnect.setBounds(8, 118, 164, 21);
|
||||
panel_1.add(lblNumberOfConnect);
|
||||
|
||||
textField_6 = new JTextField();
|
||||
textField_6.setColumns(10);
|
||||
textField_6.setBounds(196, 82, 248, 26);
|
||||
panel_1.add(textField_6);
|
||||
|
||||
JButton btnCreateAgent = new JButton("Create Diagram");
|
||||
btnCreateAgent.setBounds(314, 153, 130, 29);
|
||||
panel_1.add(btnCreateAgent);
|
||||
|
||||
JButton button = new JButton("Create Agent");
|
||||
button.setBounds(193, 153, 130, 29);
|
||||
panel_1.add(button);
|
||||
|
||||
JPanel panel_2 = new JPanel();
|
||||
panel_2.setBorder(new LineBorder(Color.GRAY));
|
||||
panel_2.setBounds(0, 375, 450, 73);
|
||||
contentPane.add(panel_2);
|
||||
panel_2.setLayout(null);
|
||||
|
||||
JLabel lblHelp = new JLabel("Help");
|
||||
lblHelp.setBounds(6, 6, 61, 16);
|
||||
panel_2.add(lblHelp);
|
||||
|
||||
JButton btnNewButton_1 = new JButton("Read Me");
|
||||
btnNewButton_1.setBounds(60, 34, 154, 29);
|
||||
panel_2.add(btnNewButton_1);
|
||||
|
||||
JButton btnGithub = new JButton("Github");
|
||||
btnGithub.setBounds(240, 34, 154, 29);
|
||||
panel_2.add(btnGithub);
|
||||
|
||||
JPanel panel_diagram = new JPanel();
|
||||
panel_diagram.setBounds(450, 0, 362, 448);
|
||||
contentPane.add(panel_diagram);
|
||||
panel_diagram.setLayout(null);
|
||||
|
||||
JLabel diagram = new JLabel("New label");
|
||||
icon = new ImageIcon("/Users/mac/Desktop/pic.png");
|
||||
diagram.setIcon(icon);
|
||||
diagram.setBounds(0, 6, 362,442);
|
||||
//diagram.setBounds(0, 6, icon.getIconWidth(),icon.getIconHeight());
|
||||
panel_diagram.add(diagram, new Integer(Integer.MIN_VALUE));
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,74 @@
|
||||
package CodeGenerator;
|
||||
|
||||
import java.awt.Color;
|
||||
import java.awt.GridLayout;
|
||||
import java.io.File;
|
||||
import java.lang.annotation.Annotation;
|
||||
import java.lang.reflect.Field;
|
||||
import java.lang.reflect.Method;
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
|
||||
import javax.swing.JButton;
|
||||
import javax.swing.JFileChooser;
|
||||
import javax.swing.JFrame;
|
||||
import javax.swing.JScrollPane;
|
||||
|
||||
import com.sun.javafx.application.PlatformImpl;
|
||||
|
||||
import javafx.embed.swing.JFXPanel;
|
||||
import javafx.scene.Scene;
|
||||
import javafx.scene.web.WebView;
|
||||
public class CodeEditor extends JFrame{
|
||||
JButton button = new JButton("a");
|
||||
public CodeEditor() {
|
||||
super();
|
||||
this.setName("b");
|
||||
this.setSize(800,500);
|
||||
this.setLocationRelativeTo(null);
|
||||
this.setLayout(new GridLayout(1,1));
|
||||
|
||||
/*JFileChooser fd = new JFileChooser();
|
||||
fd.setFileSelectionMode(JFileChooser.DIRECTORIES_ONLY);
|
||||
fd.showOpenDialog(null);
|
||||
|
||||
File f = fd.getSelectedFile();
|
||||
if(f != null){*/
|
||||
/*
|
||||
File file = new File("/Users/mac/Desktop/autorobot2.0/autorobot2.0/AutoRobot-master/src/agent/");
|
||||
this.add(new JScrollPane(new GramJPanel(file.getAbsolutePath())));
|
||||
*/
|
||||
|
||||
JFXPanel fxPanel = new JFXPanel ();
|
||||
|
||||
PlatformImpl.runLater ( new Runnable () {
|
||||
@Override
|
||||
public void run () {
|
||||
WebView webView = new WebView ();
|
||||
//webView.getEngine().loadContent("<html> Hello World!");
|
||||
//webView.getEngine ().load ( "file:///Users/mac/Desktop/test.html" );
|
||||
webView.getEngine().load("file:////Users/mac/Desktop/test.html");
|
||||
fxPanel.setScene ( new Scene ( webView ) );
|
||||
}
|
||||
});
|
||||
|
||||
JScrollPane jslp = new JScrollPane(fxPanel);
|
||||
jslp.setBounds(254, 26, 544, 525);
|
||||
this.add(jslp);
|
||||
jslp.setHorizontalScrollBarPolicy(JScrollPane.HORIZONTAL_SCROLLBAR_ALWAYS);
|
||||
jslp.setVerticalScrollBarPolicy( JScrollPane.VERTICAL_SCROLLBAR_ALWAYS);
|
||||
//this.add(new JScrollPane(fxPanel));
|
||||
|
||||
//}
|
||||
|
||||
this.setVisible(true);
|
||||
this.setDefaultCloseOperation(HIDE_ON_CLOSE);
|
||||
//this.setDefaultCloseOperation(DISPOSE_ON_CLOSE);
|
||||
//this.setDefaultCloseOperation(DO_NOTHING_ON_CLOSE);
|
||||
}
|
||||
|
||||
// public static void main(String args[]) {
|
||||
// CodeEditor t = new CodeEditor();
|
||||
// }
|
||||
|
||||
}
|
||||
@ -0,0 +1,42 @@
|
||||
package SampleApplication.Agent;
|
||||
|
||||
import SampleApplication.AgentConfiguration;
|
||||
import agentBehaviourPrototype.ExecutionBehaviour;
|
||||
import agentBehaviourPrototype.ReceiveDataFromAgentBehaviour;
|
||||
import agentBehaviourPrototype.SendDataToAgentBehaviour;
|
||||
import agentEntityPrototype.RobotAgent;
|
||||
import edu.wpi.rail.jrosbridge.services.std.Empty.Request;
|
||||
import jade.core.AID;
|
||||
|
||||
|
||||
|
||||
public class ${entity.className}<#if entity.superclass?has_content> extends ${entity.superclass} </#if>
|
||||
{
|
||||
|
||||
Request parseRequest = new Request();
|
||||
Request dispatchRequest = new Request();
|
||||
|
||||
<#list entity.allRelatedElements as allReEle>
|
||||
private AID ${allReEle.specificType} = new AID(AgentConfiguration.${allReEle.specificType}ID, false);
|
||||
</#list>
|
||||
|
||||
protected void setup() {
|
||||
|
||||
this.controller.addService(AgentConfiguration.parseServiceName, AgentConfiguration.parseServiceType);
|
||||
this.controller.addService(AgentConfiguration.dispatchServiceName, AgentConfiguration.dispatchServiceType);
|
||||
|
||||
this.controller.getService(AgentConfiguration.parseServiceName).callServiceAndWait(parseRequest);
|
||||
this.controller.getService(AgentConfiguration.dispatchServiceName).callServiceAndWait(dispatchRequest);
|
||||
|
||||
//agent communication
|
||||
<#list entity.postiveRelatedElementsWithRelation as posReEleWithRela>
|
||||
this.behList.addElement(new SendDataToAgentBehaviour(this, controller, ${posReEleWithRela.specificType}, AgentConfiguration.${posReEleWithRela.relation}DataKey));
|
||||
</#list>
|
||||
<#list entity.negativeRelatedElementsWithRelation as negReEleWithRela>
|
||||
this.behList.addElement(new ReceiveDataFromAgentBehaviour(this, controller, ${negReEleWithRela.specificType}, AgentConfiguration.${negReEleWithRela.relation}DataKey));
|
||||
</#list>
|
||||
|
||||
addBehaviour(tbf.wrap(new ExecutionBehaviour(this, ds, this.behList)));
|
||||
}
|
||||
|
||||
}
|
||||
@ -0,0 +1,18 @@
|
||||
(define (${entity.className})
|
||||
|
||||
(:requirements )
|
||||
|
||||
(:types
|
||||
|
||||
)
|
||||
|
||||
(:predicates
|
||||
|
||||
)
|
||||
|
||||
(:durative-action goto_waypoint
|
||||
:parameters
|
||||
:duration
|
||||
:condition
|
||||
:effect
|
||||
)
|
||||
@ -0,0 +1,110 @@
|
||||
package CodeGenerator;
|
||||
import java.io.File;
|
||||
import java.io.FileWriter;
|
||||
import java.io.IOException;
|
||||
import java.io.OutputStreamWriter;
|
||||
import java.io.Writer;
|
||||
import java.util.ArrayList;
|
||||
import java.util.HashMap;
|
||||
import java.util.List;
|
||||
import java.util.Map;
|
||||
|
||||
import freemarker.template.Configuration;
|
||||
import freemarker.template.DefaultObjectWrapper;
|
||||
import freemarker.template.Template;
|
||||
import freemarker.template.TemplateException;
|
||||
|
||||
public class DomainDescriptionGeneratorClient {
|
||||
|
||||
private static File pddlFile = null;
|
||||
|
||||
public static void PDDLFileGenerator(String path, String pddlName) {
|
||||
Configuration cfg = new Configuration();
|
||||
try {
|
||||
// 指定模板文件从何处加载的数据源，这里设置一个文件目录
|
||||
cfg.setDirectoryForTemplateLoading(new File("./src/CodeGenerator"));
|
||||
cfg.setObjectWrapper(new DefaultObjectWrapper());
|
||||
|
||||
// 获取模板文件
|
||||
Template template = cfg.getTemplate("DomainDescription.ftl");
|
||||
|
||||
// 创建数据模型
|
||||
Map<String, Object> root = createDataModel(path, pddlName);
|
||||
|
||||
// 合并模板和数据模型
|
||||
// 创建.pddl类文件
|
||||
if(pddlFile != null){
|
||||
Writer pddlWriter = new FileWriter(pddlFile);
|
||||
template.process(root, pddlWriter);
|
||||
pddlWriter.flush();
|
||||
System.out.println("文件生成路径：" + pddlFile.getCanonicalPath());
|
||||
|
||||
pddlWriter.close();
|
||||
}
|
||||
// 输出到Console控制台
|
||||
/*
|
||||
Writer out = new OutputStreamWriter(System.out);
|
||||
template.process(root, out);
|
||||
out.flush();
|
||||
out.close();
|
||||
*/
|
||||
|
||||
} catch (IOException e) {
|
||||
e.printStackTrace();
|
||||
} catch (TemplateException e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
//创建数据模型
|
||||
private static Map<String, Object> createDataModel(String path, String pddlName) {
|
||||
Map<String, Object> root = new HashMap<String, Object>();
|
||||
Entity user = new Entity();
|
||||
//TbfEntity user1 = new TbfEntity();
|
||||
//ControllerEntity user2 = new ControllerEntity();
|
||||
//user.setJavaPackage("agent"); // 创建包名
|
||||
user.setClassName(pddlName); // 创建类名
|
||||
user.setConstructors(false); // 是否创建构造函数
|
||||
//user.setSuperclass("Agent");//创建父类
|
||||
|
||||
List<Property> propertyList = new ArrayList<Property>();
|
||||
List<Entity> entityList = new ArrayList<Entity>();
|
||||
|
||||
// 创建实体属性
|
||||
Property attribute1 = new Property();
|
||||
attribute1.setJavaType("static final long");
|
||||
attribute1.setPropertyName("serialVersionUID");
|
||||
attribute1.setPropertyType(PropertyType.Long);
|
||||
|
||||
// 将属性集合添加到实体对象中
|
||||
user.setProperties(propertyList);
|
||||
user.setEntities(entityList);
|
||||
|
||||
// 创建.pddl类文件
|
||||
File outDirFile = new File(path + "/PDDL Files");
|
||||
if(!outDirFile.exists()){
|
||||
outDirFile.mkdir();
|
||||
}
|
||||
|
||||
//javaFile = toJavaFilename(outDirFile, user.getJavaPackage(), user.getClassName());
|
||||
pddlFile = toPddlFilename(outDirFile, user.getClassName());
|
||||
|
||||
root.put("entity", user);
|
||||
return root;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* 创建.pddl文件所在路径 和 返回.pddl文件File对象
|
||||
* @param outDirFile 生成文件路径
|
||||
* @param javaPackage java包名
|
||||
* @param javaClassName java类名
|
||||
* @return
|
||||
*/
|
||||
private static File toPddlFilename(File outDirFile, String pddlName) {
|
||||
File file = new File(outDirFile, pddlName + ".pddl");
|
||||
return file;
|
||||
}
|
||||
|
||||
}
|
||||
@ -0,0 +1,78 @@
|
||||
package CodeGenerator;
|
||||
|
||||
import java.io.BufferedReader;
|
||||
import java.io.File;
|
||||
import java.io.FileNotFoundException;
|
||||
import java.io.FileOutputStream;
|
||||
import java.io.FileReader;
|
||||
import java.io.IOException;
|
||||
import java.io.PrintWriter;
|
||||
import java.util.ArrayList;
|
||||
|
||||
public class FileUtils {
|
||||
public static void Write(String path,String write,int linecount) {
|
||||
int intcount = linecount;
|
||||
File filesave =new File(path);
|
||||
try {
|
||||
FileReader fin = new FileReader(filesave);
|
||||
BufferedReader bin = new BufferedReader(fin);
|
||||
String line = "";
|
||||
ArrayList<String>templist = new ArrayList<String>();
|
||||
while((line = bin.readLine())!=null) {
|
||||
templist.add(line);
|
||||
}
|
||||
FileOutputStream fos = new FileOutputStream(filesave);
|
||||
PrintWriter pw = new PrintWriter(fos);
|
||||
for(int i=0;i<templist.size();i++) {
|
||||
if(i<intcount-1) {
|
||||
pw.println(templist.get(i));
|
||||
}else {
|
||||
pw.println(write);
|
||||
pw.println(templist.get(i));
|
||||
intcount = Integer.MAX_VALUE;
|
||||
}
|
||||
}
|
||||
if(intcount != Integer.MAX_VALUE) {
|
||||
pw.println(write);
|
||||
}
|
||||
pw.close();
|
||||
fos.close();
|
||||
} catch (FileNotFoundException e) {
|
||||
e.printStackTrace();
|
||||
} catch (IOException e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
}
|
||||
|
||||
public static ArrayList<String> reader(String path) {
|
||||
File filesave =new File(path);
|
||||
ArrayList<String>templist = new ArrayList<String>();
|
||||
try {
|
||||
FileReader fin = new FileReader(filesave);
|
||||
BufferedReader bin = new BufferedReader(fin);
|
||||
String line = "";
|
||||
while((line = bin.readLine())!=null) {
|
||||
templist.add(line);
|
||||
}
|
||||
bin.close();
|
||||
fin.close();
|
||||
} catch (FileNotFoundException e) {
|
||||
e.printStackTrace();
|
||||
} catch (IOException e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
return templist;
|
||||
}
|
||||
|
||||
public static void createFile(String path ,String name) {
|
||||
File file = new File(path+"//"+name);
|
||||
if(!file.exists()) {
|
||||
try {
|
||||
file.createNewFile();
|
||||
} catch (IOException e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@ -0,0 +1,77 @@
|
||||
package CodeGenerator;
|
||||
|
||||
import javax.swing.Icon;
|
||||
import javax.swing.tree.DefaultMutableTreeNode;
|
||||
|
||||
|
||||
//定义节点类
|
||||
class IconNode extends DefaultMutableTreeNode
|
||||
{
|
||||
protected Icon icon;
|
||||
protected String txt;
|
||||
protected String filePath; // 文件/文件夹的全路径
|
||||
|
||||
//只包含文本的节点构造
|
||||
public IconNode(String txt)
|
||||
{
|
||||
super();
|
||||
this.txt=txt;
|
||||
}
|
||||
|
||||
//包含文本和图片的节点构造
|
||||
public IconNode(Icon icon,String txt)
|
||||
{
|
||||
super();
|
||||
this.icon = icon;
|
||||
this.txt = txt;
|
||||
}
|
||||
|
||||
// 默认不带参数的构造方法
|
||||
public IconNode()
|
||||
{
|
||||
super();
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* 下面的是自己根据需要添加的构造方法
|
||||
*/
|
||||
public IconNode(Object userObject, boolean allowsChildren)
|
||||
{
|
||||
super(userObject, allowsChildren);
|
||||
this.txt = userObject.toString();
|
||||
}
|
||||
|
||||
public IconNode(IconNode node) {
|
||||
super(node);
|
||||
this.txt = node.toString();
|
||||
}
|
||||
|
||||
public void setIcon(Icon icon)
|
||||
{
|
||||
this.icon = icon;
|
||||
}
|
||||
|
||||
|
||||
public Icon getIcon()
|
||||
{
|
||||
return icon;
|
||||
}
|
||||
|
||||
public void setText(String txt)
|
||||
{
|
||||
this.txt=txt;
|
||||
}
|
||||
|
||||
public String getText()
|
||||
{
|
||||
return txt;
|
||||
}
|
||||
|
||||
public void setFilePath(String filePath) {
|
||||
this.filePath = filePath;
|
||||
}
|
||||
public String getFilePath() {
|
||||
return this.filePath;
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,23 @@
|
||||
package CodeGenerator;
|
||||
|
||||
import java.awt.Component;
|
||||
import java.awt.Font;
|
||||
|
||||
import javax.swing.Icon;
|
||||
import javax.swing.JTree;
|
||||
import javax.swing.tree.DefaultTreeCellRenderer;
|
||||
|
||||
public class IconNodeRenderer extends DefaultTreeCellRenderer//继承该类
|
||||
{
|
||||
//重写该方法
|
||||
public Component getTreeCellRendererComponent(JTree tree, Object value,boolean sel, boolean expanded, boolean leaf, int row,boolean hasFocus)
|
||||
{
|
||||
super.getTreeCellRendererComponent(tree, value, sel, expanded, leaf,row, hasFocus); //调用父类的该方法
|
||||
Icon icon = ((IconNode) value).getIcon();//从节点读取图片
|
||||
String txt=((IconNode) value).getText(); //从节点读取文本
|
||||
setIcon(icon);//设置图片
|
||||
setText(txt);//设置文本
|
||||
setFont(new Font("Default",Font.PLAIN,13));//设置树的整体字体样式Serif
|
||||
return this;
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,50 @@
|
||||
package CodeGenerator;
|
||||
|
||||
import java.awt.Dimension;
|
||||
import java.awt.GridLayout;
|
||||
import java.awt.Toolkit;
|
||||
import java.io.File;
|
||||
import java.io.IOException;
|
||||
|
||||
import javax.swing.JFrame;
|
||||
|
||||
|
||||
/**
|
||||
*画图窗体
|
||||
*/
|
||||
public class ImageFrame extends JFrame {
|
||||
|
||||
//初始化屏幕的尺寸
|
||||
private Dimension screenSize = Toolkit.getDefaultToolkit().getScreenSize();
|
||||
|
||||
public ImageFrame() {
|
||||
super("test");
|
||||
this.setSize((int)screenSize.getWidth()/3,(int)(screenSize.getHeight()/1.5));
|
||||
this.setDefaultCloseOperation(EXIT_ON_CLOSE);
|
||||
this.setLocationRelativeTo(null);
|
||||
this.setLayout(new GridLayout(1,1));
|
||||
|
||||
DrawJPanel panel = new DrawJPanel("src//code");
|
||||
this.add(panel);
|
||||
this.setVisible(true);
|
||||
|
||||
|
||||
}
|
||||
|
||||
public static void main(String args[]) {
|
||||
new ImageFrame();
|
||||
|
||||
/*File file = new File("src//code//123.java");
|
||||
System.out.println(file.getAbsolutePath());
|
||||
if(!file.exists()) {
|
||||
System.out.println(1321);
|
||||
try {
|
||||
file.createNewFile();
|
||||
System.out.println("csafcknj");
|
||||
} catch (IOException e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
}
|
||||
FileUtils.Write(file.getAbsolutePath(), "1321");*/
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,30 @@
|
||||
package CodeGenerator;
|
||||
|
||||
import javax.swing.JFrame;
|
||||
import javax.swing.JScrollPane;
|
||||
|
||||
import javafx.embed.swing.JFXPanel;
|
||||
import javafx.scene.Scene;
|
||||
import javafx.scene.web.WebView;
|
||||
|
||||
|
||||
public class JavaOpenSafari {
|
||||
public static void main(String args[]) {
|
||||
if (java.awt.Desktop.isDesktopSupported()) {
|
||||
try {
|
||||
// 创建一个URI实例
|
||||
java.net.URI uri = java.net.URI.create("http://www.cnblogs.com/lsgwr/");
|
||||
// 获取当前系统桌面扩展
|
||||
java.awt.Desktop dp = java.awt.Desktop.getDesktop();
|
||||
// 判断系统桌面是否支持要执行的功能
|
||||
if (dp.isSupported(java.awt.Desktop.Action.BROWSE)) {
|
||||
// 获取系统默认浏览器打开链接
|
||||
dp.browse(uri);
|
||||
}
|
||||
|
||||
} catch (Exception e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -0,0 +1 @@
|
||||
0
|
||||
@ -0,0 +1,137 @@
|
||||
package CodeGenerator;
|
||||
|
||||
import javax.swing.JPanel;
|
||||
import javax.swing.JLabel;
|
||||
import javax.swing.JTextField;
|
||||
import javax.swing.SwingUtilities;
|
||||
import javax.swing.border.EmptyBorder;
|
||||
import javax.swing.border.LineBorder;
|
||||
|
||||
import java.awt.BorderLayout;
|
||||
import java.awt.Color;
|
||||
import java.awt.Component;
|
||||
import java.awt.Container;
|
||||
import java.awt.Frame;
|
||||
import java.awt.GridLayout;
|
||||
import java.awt.event.ActionEvent;
|
||||
import java.awt.event.ActionListener;
|
||||
import java.io.File;
|
||||
|
||||
import javax.swing.JButton;
|
||||
import javax.swing.JDialog;
|
||||
import javax.swing.JFileChooser;
|
||||
import javax.swing.JFrame;
|
||||
|
||||
public class NewApplication extends JPanel {
|
||||
private JTextField newApplicationName;
|
||||
private JTextField newApplicationPath;
|
||||
private JDialog newApplicationDialog;
|
||||
private JFileChooser newChooser;
|
||||
private boolean ok;
|
||||
private JButton newApplicationButton;
|
||||
private String path;
|
||||
private String name;
|
||||
|
||||
/**
|
||||
* Create the panel.
|
||||
*/
|
||||
public NewApplication() {
|
||||
|
||||
setBounds(100,100,450,130);
|
||||
setLayout(null);
|
||||
|
||||
JLabel lblName = new JLabel("Name:");
|
||||
lblName.setBounds(37, 22, 50, 16);
|
||||
add(lblName);
|
||||
|
||||
newChooser = new JFileChooser();
|
||||
|
||||
newApplicationName = new JTextField();
|
||||
newApplicationName.setBounds(89, 17, 317, 26);
|
||||
add(newApplicationName);
|
||||
newApplicationName.setColumns(10);
|
||||
|
||||
newApplicationPath = new JTextField();
|
||||
newApplicationPath.setBounds(89, 55, 280, 26);
|
||||
add(newApplicationPath);
|
||||
newApplicationPath.setColumns(10);
|
||||
|
||||
JButton openApplicationPathButton = new JButton("...");
|
||||
openApplicationPathButton.addActionListener(new ActionListener() {
|
||||
public void actionPerformed(ActionEvent e) {
|
||||
newChooser.setCurrentDirectory(new File("."));
|
||||
newChooser.setFileSelectionMode(JFileChooser.DIRECTORIES_ONLY);
|
||||
newChooser.setDialogTitle("Choose Application Location");
|
||||
int result = newChooser.showOpenDialog(NewApplication.this);
|
||||
if (result == JFileChooser.APPROVE_OPTION)
|
||||
{
|
||||
path = newChooser.getSelectedFile().getPath();
|
||||
newApplicationPath.setText(path);
|
||||
}
|
||||
}
|
||||
});
|
||||
openApplicationPathButton.setBounds(375, 55, 30, 25);
|
||||
add(openApplicationPathButton);
|
||||
|
||||
JLabel lblPath = new JLabel("Path:");
|
||||
lblPath.setBounds(46, 60, 50, 16);
|
||||
add(lblPath);
|
||||
|
||||
JButton newApplicationButton = new JButton("Create");
|
||||
newApplicationButton.setBounds(180, 93, 110, 29);
|
||||
add(newApplicationButton);
|
||||
newApplicationButton.addActionListener(new ActionListener()
|
||||
{
|
||||
public void actionPerformed(ActionEvent event)
|
||||
{
|
||||
ok = true;
|
||||
newApplicationDialog.setVisible(false);
|
||||
}
|
||||
});
|
||||
|
||||
JButton cancelNewButton = new JButton("Cancel");
|
||||
cancelNewButton.setBounds(295, 93, 110, 29);
|
||||
add(cancelNewButton);
|
||||
cancelNewButton.addActionListener(new ActionListener()
|
||||
{
|
||||
public void actionPerformed(ActionEvent event)
|
||||
{
|
||||
newApplicationDialog.setVisible(false);
|
||||
}
|
||||
});
|
||||
|
||||
}
|
||||
|
||||
public String newApplicationName()
|
||||
{
|
||||
name = newApplicationName.getText();
|
||||
return name;
|
||||
}
|
||||
|
||||
public String newApplicationPath()
|
||||
{
|
||||
return path;
|
||||
}
|
||||
|
||||
public boolean showDialog(Component parent, String title)
|
||||
{
|
||||
ok = false;
|
||||
|
||||
Frame owner = null;
|
||||
if (parent instanceof Frame) owner = (Frame) parent;
|
||||
else owner = (Frame) SwingUtilities.getAncestorOfClass(Frame.class, parent);
|
||||
|
||||
if (newApplicationDialog == null || newApplicationDialog.getOwner() != owner)
|
||||
{
|
||||
newApplicationDialog = new JDialog(owner, true);
|
||||
newApplicationDialog.getContentPane().add(this);
|
||||
newApplicationDialog.getRootPane().setDefaultButton(newApplicationButton);
|
||||
newApplicationDialog.pack();
|
||||
}
|
||||
|
||||
newApplicationDialog.setTitle(title);
|
||||
newApplicationDialog.setBounds(300, 300, 470, 180);
|
||||
newApplicationDialog.setVisible(true);
|
||||
return ok;
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,67 @@
|
||||
package CodeGenerator;
|
||||
|
||||
import javax.swing.JPanel;
|
||||
import javax.swing.JLabel;
|
||||
import javax.swing.JTextField;
|
||||
import javax.swing.SwingUtilities;
|
||||
import javax.swing.UIManager;
|
||||
import javax.swing.border.EmptyBorder;
|
||||
import javax.swing.border.LineBorder;
|
||||
|
||||
import java.awt.BorderLayout;
|
||||
import java.awt.Color;
|
||||
import java.awt.Component;
|
||||
import java.awt.Container;
|
||||
import java.awt.Frame;
|
||||
import java.awt.GridLayout;
|
||||
import java.awt.event.ActionEvent;
|
||||
import java.awt.event.ActionListener;
|
||||
import java.io.File;
|
||||
import java.util.Locale;
|
||||
|
||||
import javax.swing.JButton;
|
||||
import javax.swing.JDialog;
|
||||
import javax.swing.JFileChooser;
|
||||
import javax.swing.JFrame;
|
||||
|
||||
public class OpenApplication extends JPanel {
|
||||
private JFileChooser openChooser;
|
||||
private boolean ok;
|
||||
private String path;
|
||||
|
||||
/**
|
||||
* Create the panel.
|
||||
*/
|
||||
public OpenApplication() {
|
||||
|
||||
setBounds(100,100,450,130);
|
||||
setLayout(null);
|
||||
|
||||
UIManager.put("FileChooser.cancelButtonText", "Cancel"); //修改取消
|
||||
UIManager.put("FileChooser.saveButtonText", "Save"); //修改保存
|
||||
UIManager.put("FileChooser.openButtonText", "Import");//修改打开
|
||||
|
||||
openChooser = new JFileChooser();
|
||||
openChooser.setCurrentDirectory(new File("."));
|
||||
openChooser.setFileSelectionMode(JFileChooser.DIRECTORIES_ONLY);
|
||||
openChooser.setDialogTitle("Choose Application Location");
|
||||
int result = openChooser.showOpenDialog(OpenApplication.this);
|
||||
if (result == JFileChooser.APPROVE_OPTION)
|
||||
{
|
||||
path = openChooser.getSelectedFile().getPath();
|
||||
ok = true;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
public String openApplicationPath()
|
||||
{
|
||||
return path;
|
||||
}
|
||||
|
||||
public boolean showDialog(Component parent, String title)
|
||||
{
|
||||
return ok;
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,39 @@
|
||||
package SampleApplication.Agent;
|
||||
|
||||
import SampleApplication.AgentConfiguration;
|
||||
import agentBehaviourPrototype.ExecutionBehaviour;
|
||||
import agentBehaviourPrototype.ReceiveDataFromAgentBehaviour;
|
||||
import agentBehaviourPrototype.SendDataToAgentBehaviour;
|
||||
import agentEntityPrototype.RobotAgent;
|
||||
import edu.wpi.rail.jrosbridge.services.std.Empty.Request;
|
||||
import jade.core.AID;
|
||||
|
||||
public class ${entity.className}<#if entity.superclass?has_content> extends ${entity.superclass} </#if>
|
||||
{
|
||||
|
||||
Request planRequest = new Request();
|
||||
Request problemGeRequest = new Request();
|
||||
|
||||
<#list entity.allRelatedElements as allReEle>
|
||||
private AID ${allReEle.specificType} = new AID(AgentConfiguration.${allReEle.specificType}ID, false);
|
||||
</#list>
|
||||
|
||||
protected void setup() {
|
||||
//automatically generated
|
||||
this.controller.addService(AgentConfiguration.problemGenerationServiceName, AgentConfiguration.problemGenerationServiceType);
|
||||
this.controller.addService(AgentConfiguration.problemGenerationServiceName, AgentConfiguration.problemGenerationServiceType);
|
||||
this.controller.getService(AgentConfiguration.problemGenerationServiceName).callServiceAndWait(problemGeRequest);
|
||||
this.controller.getService(AgentConfiguration.problemGenerationServiceName).callServiceAndWait(planRequest);
|
||||
|
||||
//agent communication
|
||||
<#list entity.postiveRelatedElementsWithRelation as posReEleWithRela>
|
||||
this.behList.addElement(new SendDataToAgentBehaviour(this, controller, ${posReEleWithRela.specificType}, AgentConfiguration.${posReEleWithRela.relation}DataKey));
|
||||
</#list>
|
||||
<#list entity.negativeRelatedElementsWithRelation as negReEleWithRela>
|
||||
this.behList.addElement(new ReceiveDataFromAgentBehaviour(this, controller, ${negReEleWithRela.specificType}, AgentConfiguration.${negReEleWithRela.relation}DataKey));
|
||||
</#list>
|
||||
|
||||
addBehaviour(tbf.wrap(new ExecutionBehaviour(this, ds, this.behList)));
|
||||
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,36 @@
|
||||
(define (${entity.className})
|
||||
|
||||
(:domain
|
||||
)
|
||||
|
||||
(:requirements
|
||||
)
|
||||
|
||||
(:types
|
||||
)
|
||||
|
||||
(:predicates
|
||||
)
|
||||
|
||||
(:objects
|
||||
)
|
||||
|
||||
(:init
|
||||
)
|
||||
|
||||
(:goal
|
||||
)
|
||||
|
||||
(:durative-action
|
||||
:parameters ()
|
||||
:duration ()
|
||||
:condition ()
|
||||
:effect ()
|
||||
)
|
||||
|
||||
(:action
|
||||
:parameters ()
|
||||
:precondition ()
|
||||
:effect ()
|
||||
:observe ()
|
||||
)
|
||||
@ -0,0 +1,110 @@
|
||||
package CodeGenerator;
|
||||
import java.io.File;
|
||||
import java.io.FileWriter;
|
||||
import java.io.IOException;
|
||||
import java.io.OutputStreamWriter;
|
||||
import java.io.Writer;
|
||||
import java.util.ArrayList;
|
||||
import java.util.HashMap;
|
||||
import java.util.List;
|
||||
import java.util.Map;
|
||||
|
||||
import freemarker.template.Configuration;
|
||||
import freemarker.template.DefaultObjectWrapper;
|
||||
import freemarker.template.Template;
|
||||
import freemarker.template.TemplateException;
|
||||
|
||||
public class PlanningNodeCodeGeneratorClient {
|
||||
|
||||
private static File pddlFile = null;
|
||||
|
||||
public static void PDDLFileGenerator(String path, String pddlName) {
|
||||
Configuration cfg = new Configuration();
|
||||
try {
|
||||
// 指定模板文件从何处加载的数据源，这里设置一个文件目录
|
||||
cfg.setDirectoryForTemplateLoading(new File("./src/CodeGenerator"));
|
||||
cfg.setObjectWrapper(new DefaultObjectWrapper());
|
||||
|
||||
// 获取模板文件
|
||||
Template template = cfg.getTemplate("PlanningNode.ftl");
|
||||
|
||||
// 创建数据模型
|
||||
Map<String, Object> root = createDataModel(path, pddlName);
|
||||
|
||||
// 合并模板和数据模型
|
||||
// 创建.pddl类文件
|
||||
if(pddlFile != null){
|
||||
Writer pddlWriter = new FileWriter(pddlFile);
|
||||
template.process(root, pddlWriter);
|
||||
pddlWriter.flush();
|
||||
System.out.println("文件生成路径：" + pddlFile.getCanonicalPath());
|
||||
|
||||
pddlWriter.close();
|
||||
}
|
||||
// 输出到Console控制台
|
||||
/*
|
||||
Writer out = new OutputStreamWriter(System.out);
|
||||
template.process(root, out);
|
||||
out.flush();
|
||||
out.close();
|
||||
*/
|
||||
|
||||
} catch (IOException e) {
|
||||
e.printStackTrace();
|
||||
} catch (TemplateException e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
//创建数据模型
|
||||
private static Map<String, Object> createDataModel(String path, String pddlName) {
|
||||
Map<String, Object> root = new HashMap<String, Object>();
|
||||
Entity user = new Entity();
|
||||
//TbfEntity user1 = new TbfEntity();
|
||||
//ControllerEntity user2 = new ControllerEntity();
|
||||
//user.setJavaPackage("agent"); // 创建包名
|
||||
user.setClassName(pddlName); // 创建类名
|
||||
user.setConstructors(false); // 是否创建构造函数
|
||||
//user.setSuperclass("Agent");//创建父类
|
||||
|
||||
List<Property> propertyList = new ArrayList<Property>();
|
||||
List<Entity> entityList = new ArrayList<Entity>();
|
||||
|
||||
// 创建实体属性
|
||||
Property attribute1 = new Property();
|
||||
attribute1.setJavaType("static final long");
|
||||
attribute1.setPropertyName("serialVersionUID");
|
||||
attribute1.setPropertyType(PropertyType.Long);
|
||||
|
||||
// 将属性集合添加到实体对象中
|
||||
user.setProperties(propertyList);
|
||||
user.setEntities(entityList);
|
||||
|
||||
// 创建.pddl类文件
|
||||
File outDirFile = new File(path + "/PDDL Files");
|
||||
if(!outDirFile.exists()){
|
||||
outDirFile.mkdir();
|
||||
}
|
||||
|
||||
//javaFile = toJavaFilename(outDirFile, user.getJavaPackage(), user.getClassName());
|
||||
pddlFile = toPddlFilename(outDirFile, user.getClassName());
|
||||
|
||||
root.put("entity", user);
|
||||
return root;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* 创建.pddl文件所在路径 和 返回.pddl文件File对象
|
||||
* @param outDirFile 生成文件路径
|
||||
* @param javaPackage java包名
|
||||
* @param javaClassName java类名
|
||||
* @return
|
||||
*/
|
||||
private static File toPddlFilename(File outDirFile, String pddlName) {
|
||||
File file = new File(outDirFile, pddlName + ".pddl");
|
||||
return file;
|
||||
}
|
||||
|
||||
}
|
||||
@ -0,0 +1,75 @@
|
||||
package CodeGenerator;
|
||||
|
||||
import javax.swing.JLabel;
|
||||
import javax.swing.JTextField;
|
||||
import javax.swing.JButton;
|
||||
import javax.swing.JFrame;
|
||||
|
||||
import java.awt.event.ActionListener;
|
||||
import java.awt.event.ActionEvent;
|
||||
import javax.swing.JComboBox;
|
||||
|
||||
public class PlanningNodeGeneratorDialog extends JFrame implements ActionListener {
|
||||
public JTextField textfield;
|
||||
private JButton createPlanningNodeButton;
|
||||
String mypath;
|
||||
private JComboBox <String> comboBoxPlanningType;
|
||||
|
||||
/**
|
||||
* Create the panel.
|
||||
*/
|
||||
public PlanningNodeGeneratorDialog(String path) {
|
||||
super("Planning Create");
|
||||
this.mypath = path;
|
||||
|
||||
this.setLayout(null);
|
||||
this.setBounds(300, 300, 450, 180);
|
||||
|
||||
JLabel lblPlanningNodeName = new JLabel("Planning Node name:");
|
||||
lblPlanningNodeName.setBounds(60, 24, 134, 16);
|
||||
this.add(lblPlanningNodeName);
|
||||
|
||||
textfield = new JTextField();
|
||||
textfield.setBounds(206, 19, 217, 26);
|
||||
this.add(textfield);
|
||||
textfield.setColumns(10);
|
||||
|
||||
JLabel lblPlanningType = new JLabel("Planning Node Type:");
|
||||
lblPlanningType.setBounds(123, 63, 75, 16);
|
||||
this.add(lblPlanningType);
|
||||
|
||||
comboBoxPlanningType = new JComboBox();
|
||||
comboBoxPlanningType.setBounds(206, 59, 217, 27);
|
||||
this.add(comboBoxPlanningType);
|
||||
comboBoxPlanningType.addItem("Domain Description");
|
||||
comboBoxPlanningType.addItem("Problem Description");
|
||||
|
||||
createPlanningNodeButton = new JButton("Create Planning Node");
|
||||
//createAgentButton.setBounds(177, 130, 117, 29);
|
||||
createPlanningNodeButton.setBounds(260, 98, 163, 29);
|
||||
this.add(createPlanningNodeButton);
|
||||
createPlanningNodeButton.addActionListener(this);
|
||||
createPlanningNodeButton.addActionListener(this);
|
||||
|
||||
this.setDefaultCloseOperation(JFrame.DISPOSE_ON_CLOSE);
|
||||
this.setLocationRelativeTo(null);
|
||||
this.setVisible(true);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void actionPerformed(ActionEvent e) {
|
||||
if(e.getSource() == this.createPlanningNodeButton) {
|
||||
String name = this.textfield.getText();
|
||||
String type = (String)this.comboBoxPlanningType.getSelectedItem();
|
||||
if (type == "Domain Description")
|
||||
{
|
||||
DomainDescriptionGeneratorClient.PDDLFileGenerator(mypath, name);
|
||||
}
|
||||
else
|
||||
{
|
||||
ProblemDescriptionGeneratorClient.PDDLFileGenerator(mypath, name);
|
||||
}
|
||||
this.dispose();
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,15 @@
|
||||
(define (${entity.className})
|
||||
|
||||
(:domain )
|
||||
|
||||
(:objects
|
||||
|
||||
)
|
||||
|
||||
(:init
|
||||
|
||||
)
|
||||
|
||||
(:goal
|
||||
|
||||
)
|
||||
@ -0,0 +1,110 @@
|
||||
package CodeGenerator;
|
||||
import java.io.File;
|
||||
import java.io.FileWriter;
|
||||
import java.io.IOException;
|
||||
import java.io.OutputStreamWriter;
|
||||
import java.io.Writer;
|
||||
import java.util.ArrayList;
|
||||
import java.util.HashMap;
|
||||
import java.util.List;
|
||||
import java.util.Map;
|
||||
|
||||
import freemarker.template.Configuration;
|
||||
import freemarker.template.DefaultObjectWrapper;
|
||||
import freemarker.template.Template;
|
||||
import freemarker.template.TemplateException;
|
||||
|
||||
public class ProblemDescriptionGeneratorClient {
|
||||
|
||||
private static File pddlFile = null;
|
||||
|
||||
public static void PDDLFileGenerator(String path, String pddlName) {
|
||||
Configuration cfg = new Configuration();
|
||||
try {
|
||||
// 指定模板文件从何处加载的数据源，这里设置一个文件目录
|
||||
cfg.setDirectoryForTemplateLoading(new File("./src/CodeGenerator"));
|
||||
cfg.setObjectWrapper(new DefaultObjectWrapper());
|
||||
|
||||
// 获取模板文件
|
||||
Template template = cfg.getTemplate("ProblemDescription.ftl");
|
||||
|
||||
// 创建数据模型
|
||||
Map<String, Object> root = createDataModel(path, pddlName);
|
||||
|
||||
// 合并模板和数据模型
|
||||
// 创建.pddl类文件
|
||||
if(pddlFile != null){
|
||||
Writer pddlWriter = new FileWriter(pddlFile);
|
||||
template.process(root, pddlWriter);
|
||||
pddlWriter.flush();
|
||||
System.out.println("文件生成路径：" + pddlFile.getCanonicalPath());
|
||||
|
||||
pddlWriter.close();
|
||||
}
|
||||
// 输出到Console控制台
|
||||
/*
|
||||
Writer out = new OutputStreamWriter(System.out);
|
||||
template.process(root, out);
|
||||
out.flush();
|
||||
out.close();
|
||||
*/
|
||||
|
||||
} catch (IOException e) {
|
||||
e.printStackTrace();
|
||||
} catch (TemplateException e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
//创建数据模型
|
||||
private static Map<String, Object> createDataModel(String path, String pddlName) {
|
||||
Map<String, Object> root = new HashMap<String, Object>();
|
||||
Entity user = new Entity();
|
||||
//TbfEntity user1 = new TbfEntity();
|
||||
//ControllerEntity user2 = new ControllerEntity();
|
||||
//user.setJavaPackage("agent"); // 创建包名
|
||||
user.setClassName(pddlName); // 创建类名
|
||||
user.setConstructors(false); // 是否创建构造函数
|
||||
//user.setSuperclass("Agent");//创建父类
|
||||
|
||||
List<Property> propertyList = new ArrayList<Property>();
|
||||
List<Entity> entityList = new ArrayList<Entity>();
|
||||
|
||||
// 创建实体属性
|
||||
Property attribute1 = new Property();
|
||||
attribute1.setJavaType("static final long");
|
||||
attribute1.setPropertyName("serialVersionUID");
|
||||
attribute1.setPropertyType(PropertyType.Long);
|
||||
|
||||
// 将属性集合添加到实体对象中
|
||||
user.setProperties(propertyList);
|
||||
user.setEntities(entityList);
|
||||
|
||||
// 创建.pddl类文件
|
||||
File outDirFile = new File(path + "/PDDL Files");
|
||||
if(!outDirFile.exists()){
|
||||
outDirFile.mkdir();
|
||||
}
|
||||
|
||||
//javaFile = toJavaFilename(outDirFile, user.getJavaPackage(), user.getClassName());
|
||||
pddlFile = toPddlFilename(outDirFile, user.getClassName());
|
||||
|
||||
root.put("entity", user);
|
||||
return root;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* 创建.pddl文件所在路径 和 返回.pddl文件File对象
|
||||
* @param outDirFile 生成文件路径
|
||||
* @param javaPackage java包名
|
||||
* @param javaClassName java类名
|
||||
* @return
|
||||
*/
|
||||
private static File toPddlFilename(File outDirFile, String pddlName) {
|
||||
File file = new File(outDirFile, pddlName + ".pddl");
|
||||
return file;
|
||||
}
|
||||
|
||||
}
|
||||
@ -0,0 +1,40 @@
|
||||
package CodeGenerator;
|
||||
/**
|
||||
* 实体对应的属性类
|
||||
* @author lvzb.software@qq.com
|
||||
*
|
||||
*/
|
||||
public class Property {
|
||||
// 属性数据类型
|
||||
private String javaType;
|
||||
// 属性名称
|
||||
private String propertyName;
|
||||
|
||||
private PropertyType propertyType;
|
||||
|
||||
public String getJavaType() {
|
||||
return javaType;
|
||||
}
|
||||
|
||||
public void setJavaType(String javaType) {
|
||||
this.javaType = javaType;
|
||||
}
|
||||
|
||||
public String getPropertyName() {
|
||||
return propertyName;
|
||||
}
|
||||
|
||||
public void setPropertyName(String propertyName) {
|
||||
this.propertyName = propertyName;
|
||||
}
|
||||
|
||||
public PropertyType getPropertyType() {
|
||||
return propertyType;
|
||||
}
|
||||
|
||||
public void setPropertyType(PropertyType propertyType) {
|
||||
this.propertyType = propertyType;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
@ -0,0 +1,9 @@
|
||||
package CodeGenerator;
|
||||
/**
|
||||
* 属性类型枚举类
|
||||
* @author lvzb.software@qq.com
|
||||
*
|
||||
*/
|
||||
public enum PropertyType {
|
||||
Byte, Short, Int, Long, Boolean, Float, Double, String, ByteArray, Date
|
||||
}
|
||||
@ -0,0 +1,142 @@
|
||||
import rospy
|
||||
from roslib import message
|
||||
from sensor_msgs import point_cloud2
|
||||
from sensor_msgs.msg import PointCloud2
|
||||
from geometry_msgs.msg import Twist
|
||||
from math import copysign
|
||||
|
||||
class ${entity.className}():
|
||||
def __init__(self):
|
||||
rospy.init_node("${entity.className}")
|
||||
|
||||
# Set the shutdown function (stop the robot)
|
||||
rospy.on_shutdown(self.shutdown)
|
||||
|
||||
# The dimensions (in meters) of the box in which we will search
|
||||
# for the person (blob). These are given in camera coordinates
|
||||
# where x is left/right,y is up/down and z is depth (forward/backward)
|
||||
self.min_x = rospy.get_param("~min_x", -0.2)
|
||||
self.max_x = rospy.get_param("~max_x", 0.2)
|
||||
self.min_y = rospy.get_param("~min_y", -0.3)
|
||||
self.max_y = rospy.get_param("~max_y", 0.5)
|
||||
self.max_z = rospy.get_param("~max_z", 1.2)
|
||||
|
||||
# The goal distance (in meters) to keep between the robot and the person
|
||||
self.goal_z = rospy.get_param("~goal_z", 0.6)
|
||||
|
||||
# How far away from the goal distance (in meters) before the robot reacts
|
||||
self.z_threshold = rospy.get_param("~z_threshold", 0.05)
|
||||
|
||||
# How far away from being centered (x displacement) on the person
|
||||
# before the robot reacts
|
||||
self.x_threshold = rospy.get_param("~x_threshold", 0.05)
|
||||
|
||||
# How much do we weight the goal distance (z) when making a movement
|
||||
self.z_scale = rospy.get_param("~z_scale", 1.0)
|
||||
|
||||
# How much do we weight left/right displacement of the person when making a movement
|
||||
self.x_scale = rospy.get_param("~x_scale", 2.5)
|
||||
|
||||
# The maximum rotation speed in radians per second
|
||||
self.max_angular_speed = rospy.get_param("~max_angular_speed", 2.0)
|
||||
|
||||
# The minimum rotation speed in radians per second
|
||||
self.min_angular_speed = rospy.get_param("~min_angular_speed", 0.0)
|
||||
|
||||
# The max linear speed in meters per second
|
||||
self.max_linear_speed = rospy.get_param("~max_linear_speed", 0.3)
|
||||
|
||||
# The minimum linear speed in meters per second
|
||||
self.min_linear_speed = rospy.get_param("~min_linear_speed", 0.1)
|
||||
|
||||
# Slow down factor when stopping
|
||||
self.slow_down_factor = rospy.get_param("~slow_down_factor", 0.8)
|
||||
|
||||
# Initialize the movement command
|
||||
self.move_cmd = Twist()
|
||||
|
||||
# Publisher to control the robot's movement
|
||||
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
|
||||
|
||||
# Subscribe to the point cloud
|
||||
self.depth_subscriber = rospy.Subscriber('point_cloud', PointCloud2, self.set_cmd_vel, queue_size=1)
|
||||
|
||||
rospy.loginfo("Subscribing to point cloud...")
|
||||
|
||||
# Wait for the pointcloud topic to become available
|
||||
rospy.wait_for_message('point_cloud', PointCloud2)
|
||||
|
||||
rospy.loginfo("Ready to follow!")
|
||||
|
||||
def set_cmd_vel(self, msg):
|
||||
# Initialize the centroid coordinates point count
|
||||
x = y = z = n = 0
|
||||
|
||||
# Read in the x, y, z coordinates of all points in the cloud
|
||||
for point in point_cloud2.read_points(msg, skip_nans=True):
|
||||
pt_x = point[0]
|
||||
pt_y = point[1]
|
||||
pt_z = point[2]
|
||||
|
||||
# Keep only those points within our designated boundaries and sum them up
|
||||
if -pt_y > self.min_y and -pt_y < self.max_y and pt_x < self.max_x and pt_x > self.min_x and pt_z < self.max_z:
|
||||
x += pt_x
|
||||
y += pt_y
|
||||
z += pt_z
|
||||
n += 1
|
||||
|
||||
# If we have points, compute the centroid coordinates
|
||||
if n:
|
||||
x /= n
|
||||
y /= n
|
||||
z /= n
|
||||
|
||||
# Check our movement thresholds
|
||||
if (abs(z - self.goal_z) > self.z_threshold):
|
||||
# Compute the angular component of the movement
|
||||
linear_speed = (z - self.goal_z) * self.z_scale
|
||||
|
||||
# Make sure we meet our min/max specifications
|
||||
self.move_cmd.linear.x = copysign(max(self.min_linear_speed,
|
||||
min(self.max_linear_speed, abs(linear_speed))), linear_speed)
|
||||
else:
|
||||
self.move_cmd.linear.x *= self.slow_down_factor
|
||||
|
||||
if (abs(x) > self.x_threshold):
|
||||
# Compute the linear component of the movement
|
||||
angular_speed = -x * self.x_scale
|
||||
|
||||
# Make sure we meet our min/max specifications
|
||||
self.move_cmd.angular.z = copysign(max(self.min_angular_speed,
|
||||
min(self.max_angular_speed, abs(angular_speed))), angular_speed)
|
||||
else:
|
||||
# Stop the rotation smoothly
|
||||
self.move_cmd.angular.z *= self.slow_down_factor
|
||||
|
||||
else:
|
||||
# Stop the robot smoothly
|
||||
self.move_cmd.linear.x *= self.slow_down_factor
|
||||
self.move_cmd.angular.z *= self.slow_down_factor
|
||||
|
||||
# Publish the movement command
|
||||
self.cmd_vel_pub.publish(self.move_cmd)
|
||||
|
||||
|
||||
def shutdown(self):
|
||||
rospy.loginfo("Stopping the robot...")
|
||||
|
||||
# Unregister the subscriber to stop cmd_vel publishing
|
||||
self.depth_subscriber.unregister()
|
||||
rospy.sleep(1)
|
||||
|
||||
# Send an emtpy Twist message to stop the robot
|
||||
self.cmd_vel_pub.publish(Twist())
|
||||
rospy.sleep(1)
|
||||
|
||||
if __name__ == '__main__':
|
||||
try:
|
||||
${entity.className}()
|
||||
rospy.spin()
|
||||
except rospy.ROSInterruptException:
|
||||
rospy.loginfo("Follower node terminated.")
|
||||
|
||||
@ -0,0 +1,110 @@
|
||||
package CodeGenerator;
|
||||
import java.io.File;
|
||||
import java.io.FileWriter;
|
||||
import java.io.IOException;
|
||||
import java.io.OutputStreamWriter;
|
||||
import java.io.Writer;
|
||||
import java.util.ArrayList;
|
||||
import java.util.HashMap;
|
||||
import java.util.List;
|
||||
import java.util.Map;
|
||||
|
||||
import freemarker.template.Configuration;
|
||||
import freemarker.template.DefaultObjectWrapper;
|
||||
import freemarker.template.Template;
|
||||
import freemarker.template.TemplateException;
|
||||
|
||||
public class ROSNodeCodeGeneratorClient {
|
||||
|
||||
private static File ROSNodeFile = null;
|
||||
|
||||
public static void ROSNodeFileGenerator(String path, String ROSNodeName) {
|
||||
Configuration cfg = new Configuration();
|
||||
try {
|
||||
// 指定模板文件从何处加载的数据源，这里设置一个文件目录
|
||||
cfg.setDirectoryForTemplateLoading(new File("./src/CodeGenerator"));
|
||||
cfg.setObjectWrapper(new DefaultObjectWrapper());
|
||||
|
||||
// 获取模板文件
|
||||
Template template = cfg.getTemplate("ROSNode.ftl");
|
||||
|
||||
// 创建数据模型
|
||||
Map<String, Object> root = createDataModel(path, ROSNodeName);
|
||||
|
||||
// 合并模板和数据模型
|
||||
// 创建.pddl类文件
|
||||
if(ROSNodeFile != null){
|
||||
Writer ROSNodeWriter = new FileWriter(ROSNodeFile);
|
||||
template.process(root, ROSNodeWriter);
|
||||
ROSNodeWriter.flush();
|
||||
System.out.println("文件生成路径：" + ROSNodeFile.getCanonicalPath());
|
||||
|
||||
ROSNodeWriter.close();
|
||||
}
|
||||
// 输出到Console控制台
|
||||
/*
|
||||
Writer out = new OutputStreamWriter(System.out);
|
||||
template.process(root, out);
|
||||
out.flush();
|
||||
out.close();
|
||||
*/
|
||||
|
||||
} catch (IOException e) {
|
||||
e.printStackTrace();
|
||||
} catch (TemplateException e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
//创建数据模型
|
||||
private static Map<String, Object> createDataModel(String path, String ROSNodeName) {
|
||||
Map<String, Object> root = new HashMap<String, Object>();
|
||||
Entity user = new Entity();
|
||||
//TbfEntity user1 = new TbfEntity();
|
||||
//ControllerEntity user2 = new ControllerEntity();
|
||||
//user.setJavaPackage("agent"); // 创建包名
|
||||
user.setClassName(ROSNodeName); // 创建类名
|
||||
user.setConstructors(false); // 是否创建构造函数
|
||||
//user.setSuperclass("Agent");//创建父类
|
||||
|
||||
List<Property> propertyList = new ArrayList<Property>();
|
||||
List<Entity> entityList = new ArrayList<Entity>();
|
||||
|
||||
// 创建实体属性
|
||||
Property attribute1 = new Property();
|
||||
attribute1.setJavaType("static final long");
|
||||
attribute1.setPropertyName("serialVersionUID");
|
||||
attribute1.setPropertyType(PropertyType.Long);
|
||||
|
||||
// 将属性集合添加到实体对象中
|
||||
user.setProperties(propertyList);
|
||||
user.setEntities(entityList);
|
||||
|
||||
// 创建.python类文件
|
||||
File outDirFile = new File(path + "/Ros Nodes");
|
||||
if(!outDirFile.exists()){
|
||||
outDirFile.mkdir();
|
||||
}
|
||||
|
||||
//javaFile = toJavaFilename(outDirFile, user.getJavaPackage(), user.getClassName());
|
||||
ROSNodeFile = toROSNodeFilename(outDirFile, user.getClassName());
|
||||
|
||||
root.put("entity", user);
|
||||
return root;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* 创建.python文件所在路径 和 返回.python文件File对象
|
||||
* @param outDirFile 生成文件路径
|
||||
* @param javaPackage java包名
|
||||
* @param javaClassName java类名
|
||||
* @return
|
||||
*/
|
||||
private static File toROSNodeFilename(File outDirFile, String ROSNodeName) {
|
||||
File file = new File(outDirFile, ROSNodeName + ".python");
|
||||
return file;
|
||||
}
|
||||
|
||||
}
|
||||
@ -0,0 +1,54 @@
|
||||
package CodeGenerator;
|
||||
|
||||
import javax.swing.JLabel;
|
||||
import javax.swing.JTextField;
|
||||
import javax.swing.JButton;
|
||||
import javax.swing.JFrame;
|
||||
|
||||
import java.awt.event.ActionListener;
|
||||
import java.awt.event.ActionEvent;
|
||||
import javax.swing.JComboBox;
|
||||
|
||||
public class ROSNodeGeneratorDialog extends JFrame implements ActionListener {
|
||||
public JTextField textfield;
|
||||
private JButton createROSNodeButton;
|
||||
String mypath;
|
||||
/**
|
||||
* Create the panel.
|
||||
*/
|
||||
public ROSNodeGeneratorDialog(String path) {
|
||||
super("Create ROS Node");
|
||||
this.mypath = path;
|
||||
|
||||
this.setLayout(null);
|
||||
this.setBounds(300, 300, 450, 141);
|
||||
|
||||
JLabel lblROSNodeName = new JLabel("ROS Node name:");
|
||||
lblROSNodeName.setBounds(90, 24, 104, 16);
|
||||
this.add(lblROSNodeName);
|
||||
|
||||
textfield = new JTextField();
|
||||
textfield.setBounds(206, 19, 217, 26);
|
||||
this.add(textfield);
|
||||
textfield.setColumns(10);
|
||||
|
||||
createROSNodeButton = new JButton("Create ROS Node");
|
||||
//createAgentButton.setBounds(177, 130, 117, 29);
|
||||
createROSNodeButton.setBounds(290, 59, 133, 29);
|
||||
this.add(createROSNodeButton);
|
||||
createROSNodeButton.addActionListener(this);
|
||||
|
||||
this.setDefaultCloseOperation(JFrame.DISPOSE_ON_CLOSE);
|
||||
this.setLocationRelativeTo(null);
|
||||
this.setVisible(true);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void actionPerformed(ActionEvent e) {
|
||||
if(e.getSource() == this.createROSNodeButton) {
|
||||
String name = this.textfield.getText();
|
||||
ROSNodeCodeGeneratorClient.ROSNodeFileGenerator(mypath, name);
|
||||
this.dispose();
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,45 @@
|
||||
package SampleApplication.Agent;
|
||||
|
||||
import jade.core.AID;
|
||||
import jade.core.behaviours.DataStore;
|
||||
import jade.core.behaviours.ThreadedBehaviourFactory;
|
||||
|
||||
public class ${entity.className}<#if entity.superclass?has_content> extends ${entity.superclass} </#if>
|
||||
{
|
||||
<#list entity.properties as property>
|
||||
private ${property.javaType} ${property.propertyName} = ;
|
||||
</#list>
|
||||
|
||||
<#list tbfentity.tbfentities as tbfentity>
|
||||
private ${tbfentity.superclass} ${tbfentity.className} = new ${tbfentity.superclass} ();
|
||||
</#list>
|
||||
|
||||
<#list controllerentity.controllerentities as controllerentity>
|
||||
private ${controllerentity.superclass} ${controllerentity.className} = new ${controllerentity.superclass}(this);
|
||||
|
||||
</#list>
|
||||
/**
|
||||
*jade.core.AID.AID(String name, boolean isGUID)
|
||||
*/
|
||||
<#list entity.entities as entity1>
|
||||
private ${entity1.superclass} ${entity1.className} = new ${entity1.superclass} ();
|
||||
</#list>
|
||||
|
||||
protect void setup() {
|
||||
System.out.println("Agent " + getLocalName() + " waiting for operation...");
|
||||
|
||||
/**
|
||||
*void rbehaviours.Controller.addTopic(String topicName, String topicType)
|
||||
*/
|
||||
<#list controllerentity.controllerentities as controllerentity>
|
||||
${controllerentity.className}.addTopic();
|
||||
</#list>
|
||||
|
||||
/**
|
||||
*Behaviour jade.core.behaviours.ThreadedBehaviourFactory.wrap(Behaviour b)
|
||||
*/
|
||||
<#list tbfentity.tbfentities as tbfentity>
|
||||
addBehaviour(${tbfentity.className}.wrap());
|
||||
</#list>
|
||||
}
|
||||
}
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in new issue