diff --git a/crowdbotsim/CMakeLists.txt b/crowdbotsim/CMakeLists.txt
index e6f7ad2cdd67cd54c64127276be5b44113a12b7d..2ed3b0e289b94d5c5c8ccb0c66395d2c2393544c 100644
--- a/crowdbotsim/CMakeLists.txt
+++ b/crowdbotsim/CMakeLists.txt
@@ -11,6 +11,7 @@ find_package(catkin REQUIRED COMPONENTS
rospy
roscpp
std_msgs
+ sensor_msgs
geometry_msgs
message_generation
)
diff --git a/crowdbotsim/launch/.vscode/c_cpp_properties.json b/crowdbotsim/launch/.vscode/c_cpp_properties.json
new file mode 100644
index 0000000000000000000000000000000000000000..8f852a25842a985b88d4f590e4f44b9491e7c83f
--- /dev/null
+++ b/crowdbotsim/launch/.vscode/c_cpp_properties.json
@@ -0,0 +1,17 @@
+{
+ "configurations": [
+ {
+ "browse": {
+ "databaseFilename": "",
+ "limitSymbolsToIncludedHeaders": true
+ },
+ "includePath": [
+ "/home/fabien/git/cb_catkin/devel/include",
+ "/home/fabien/git/virtual-real-pepper/catkin_ws/devel/include",
+ "/opt/ros/kinetic/include",
+ "/usr/include"
+ ],
+ "name": "Linux"
+ }
+ ]
+}
\ No newline at end of file
diff --git a/crowdbotsim/launch/.vscode/settings.json b/crowdbotsim/launch/.vscode/settings.json
new file mode 100644
index 0000000000000000000000000000000000000000..968c8afb9c782def62c98ace08ad00ffaec94806
--- /dev/null
+++ b/crowdbotsim/launch/.vscode/settings.json
@@ -0,0 +1,8 @@
+{
+ "python.autoComplete.extraPaths": [
+ "/home/fabien/git/cb_catkin/devel/lib/python2.7/dist-packages",
+ "/home/fabien/git/virtual-real-pepper/catkin_ws/devel/lib/python2.7/dist-packages",
+ "/home/fabien/git/catkin_ws/devel/lib/python2.7/dist-packages",
+ "/opt/ros/kinetic/lib/python2.7/dist-packages"
+ ]
+}
\ No newline at end of file
diff --git a/crowdbotsim/launch/joint_state_publisher.launch b/crowdbotsim/launch/joint_state_publisher.launch
index a3be7a5202d34bd31bcc8e59bfd7b3aae21b495f..fc1521acb963f3c919837e676e9528fa22fd02f9 100644
--- a/crowdbotsim/launch/joint_state_publisher.launch
+++ b/crowdbotsim/launch/joint_state_publisher.launch
@@ -1,6 +1,9 @@
<launch>
<arg name="gui" default="true" />
+ <arg name="model" default="$(find crowdbotsim)/JulietteY20MP_crowdbotsim1.urdf"/>
+
<param name="use_gui" value="$(arg gui)"/>
+ <param name="robot_description" textfile="$(arg model)" />
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node pkg="robot_state_publisher" type="state_publisher" name="robot_state_publisher" />
</launch>
\ No newline at end of file
diff --git a/crowdbotsim/launch/leg_detector.launch b/crowdbotsim/launch/leg_detector.launch
new file mode 100644
index 0000000000000000000000000000000000000000..ea6b69fab812dd012ecbf0b926e23d236a111358
--- /dev/null
+++ b/crowdbotsim/launch/leg_detector.launch
@@ -0,0 +1,12 @@
+<launch>
+
+ <node pkg="leg_detector" type="leg_detector" name="leg_detector_front" args="scan:=sick_laser_front/scan $(find leg_detector)/config/trained_leg_detector.yaml" output="screen">
+ <param name="fixed_frame" type="string" value="sick_laser_front" />
+ </node>
+
+ <node pkg="leg_detector" type="leg_detector" name="leg_detector_rear" args="scan:=sick_laser_rear/scan $(find leg_detector)/config/trained_leg_detector.yaml" output="screen">
+ <param name="fixed_frame" type="string" value="sick_laser_rear" />
+ </node>
+
+</launch>
+
diff --git a/crowdbotsim/launch/upload_robot_description.launch b/crowdbotsim/launch/upload_robot_description.launch
index 2ce1cd4e6c005711cf2ce8788592622362fa188e..46328b96090e27225d02d6c3334f9280c325d50b 100644
--- a/crowdbotsim/launch/upload_robot_description.launch
+++ b/crowdbotsim/launch/upload_robot_description.launch
@@ -1,6 +1,6 @@
<?xml version="1.0"?>
<launch>
- <arg name="model" default="$(find crowdbotsim)/pepper.urdf"/>
+ <arg name="model" default="$(find crowdbotsim)/JulietteY20MP_crowdbotsim1.urdf"/>
<param name="robot_description" textfile="$(arg model)" />
<node name="file_server" pkg="file_server" type="file_server" output="screen"/>
</launch>
diff --git a/crowdbotsim/scripts/.D.swp b/crowdbotsim/scripts/.D.swp
new file mode 100644
index 0000000000000000000000000000000000000000..4acf6097102d31f54dda3ae3c00eeccaf3e5cd55
Binary files /dev/null and b/crowdbotsim/scripts/.D.swp differ
diff --git a/crowdbotsim/scripts/.vscode/c_cpp_properties.json b/crowdbotsim/scripts/.vscode/c_cpp_properties.json
new file mode 100644
index 0000000000000000000000000000000000000000..3d271e0bd5770acfcd77a9273624c1fbd5083842
--- /dev/null
+++ b/crowdbotsim/scripts/.vscode/c_cpp_properties.json
@@ -0,0 +1,22 @@
+{
+ "configurations": [
+ {
+ "browse": {
+ "databaseFilename": "",
+ "limitSymbolsToIncludedHeaders": true
+ },
+ "includePath": [
+ "/home/fabien/git/cb_catkin/devel/include",
+ "/home/fabien/git/virtual-real-pepper/catkin_ws/devel/include",
+ "/opt/ros/kinetic/include",
+ "/usr/include"
+ ],
+ "name": "Linux",
+ "intelliSenseMode": "gcc-x64",
+ "compilerPath": "/usr/bin/gcc",
+ "cStandard": "c11",
+ "cppStandard": "c++17"
+ }
+ ],
+ "version": 4
+}
\ No newline at end of file
diff --git a/crowdbotsim/scripts/.vscode/settings.json b/crowdbotsim/scripts/.vscode/settings.json
new file mode 100644
index 0000000000000000000000000000000000000000..88a8320e9dbf4427d4e4cdcd2828d17ba025afe0
--- /dev/null
+++ b/crowdbotsim/scripts/.vscode/settings.json
@@ -0,0 +1,9 @@
+{
+ "python.autoComplete.extraPaths": [
+ "/home/fabien/git/cb_catkin/devel/lib/python2.7/dist-packages",
+ "/home/fabien/git/virtual-real-pepper/catkin_ws/devel/lib/python2.7/dist-packages",
+ "/home/fabien/git/catkin_ws/devel/lib/python2.7/dist-packages",
+ "/opt/ros/kinetic/lib/python2.7/dist-packages",
+ "/home/fabien/NaoqiSDK/pynaoqi-python2.7-2.4.3.28-linux64"
+ ]
+}
\ No newline at end of file
diff --git a/crowdbotsim/scripts/dataGen.py b/crowdbotsim/scripts/dataGen.py
new file mode 100755
index 0000000000000000000000000000000000000000..d62d6c45359289efcf4962e313b5da7e63382c03
--- /dev/null
+++ b/crowdbotsim/scripts/dataGen.py
@@ -0,0 +1,70 @@
+#!/usr/bin/env python
+import rospy
+import sys
+import numpy as np
+
+from std_msgs.msg import Float64
+from sensor_msgs.msg import LaserScan
+from geometry_msgs.msg import Twist
+
+myargv = rospy.myargv(argv=sys.argv)
+script, scenarios_count, last_scenario_duration, directory = myargv
+
+global front_scan, rear_scan, twist, timestamps, scenario, data
+
+front_scan = np.zeros(818)
+rear_scan = np.zeros(818)
+twist = np.zeros(3) #x y thetaZ
+timestamps = np.zeros(1)
+scenario = np.zeros(1)
+
+data = np.concatenate((scenario, timestamps, twist, front_scan, rear_scan),axis=0)
+
+def on_front_scan(scan):
+ global front_scan, timestamps
+ front_scan = np.array(scan.ranges)
+ timestamps = np.array([scan.header.stamp.secs + float(scan.header.stamp.nsecs) / 1000000000])
+
+def on_rear_scan(scan):
+ global rear_scan
+ rear_scan = np.array(scan.ranges)
+
+def on_twist(t):
+ global twist
+ twist = np.array([t.linear.x, t.linear.y, t.angular.z])
+
+def on_scenario(val):
+ global scenario
+ scenario = np.array([val.data])
+
+def data_gen():
+ global front_scan, rear_scan, twist, timestamps, scenario, data
+ # initialize node
+ rospy.init_node('data_gen', anonymous = False)
+
+ ### Set up subscribers
+ front_scan_sub = rospy.Subscriber('/sick_laser_front/scan', LaserScan, on_front_scan, queue_size = 1)
+ rear_scan_sub = rospy.Subscriber('/sick_laser_rear/scan', LaserScan, on_rear_scan, queue_size = 1)
+ scenario_sub = rospy.Subscriber('/scenario', Float64, on_scenario, queue_size = 1)
+ robot_twist_sub = rospy.Subscriber('/robot/twist', Twist, on_twist, queue_size = 1)
+
+ rate = rospy.Rate(2)
+ _now = rospy.Time.now()
+
+ current_scenario = 0
+ f = open(directory+"/scenario{}".format(current_scenario),"a+")
+
+ while not rospy.is_shutdown() and (scenario[0] < float(scenarios_count) or timestamps[0] < float(last_scenario_duration)):
+ data = np.concatenate((scenario, timestamps, twist, front_scan, rear_scan),axis=0)
+ #write data
+ if(current_scenario < scenario[0]):
+ f.close()
+ current_scenario = scenario[0]
+ f = open(directory+"/scenario{}.csv".format(current_scenario+366),"a+")
+ f.write(",".join([str(x) for x in data.tolist()])+'\n')
+ rate.sleep()
+
+ f.close()
+
+if __name__ == '__main__':
+ data_gen()
diff --git a/crowdbotsim/scripts/dataGen2.py b/crowdbotsim/scripts/dataGen2.py
new file mode 100755
index 0000000000000000000000000000000000000000..f8ba1d174e807342990dcd671f075a2c45526736
--- /dev/null
+++ b/crowdbotsim/scripts/dataGen2.py
@@ -0,0 +1,190 @@
+#!/usr/bin/env python
+import rospy
+import sys
+import numpy as np
+from time import sleep, time
+
+from rosgraph_msgs.msg import Clock
+from std_msgs.msg import Float64
+from sensor_msgs.msg import LaserScan
+from geometry_msgs.msg import Twist
+from crowdbotsim.msg import TwistArrayStamped
+
+myargv = rospy.myargv(argv=sys.argv)
+script, crowd_size, time_step, scenario_number = myargv
+time_step = float(time_step)
+crowd_size = int(crowd_size)
+scenario_number = int(scenario_number)
+
+
+np.set_printoptions(formatter={'float': lambda x: "{0:0.3f}".format(x)})
+
+global scenario, scan_seq, scan, scanMask, robotPos, crowdX, crowdY, crowdT
+global dataHeader, dataLidar, dataLidarMask, dataCrowdX, dataCrowdY, dataCrowdT, dataOdom, dataFull
+
+scenario = np.zeros(1)
+scan_seq = np.zeros(1)
+simTime = np.zeros(1)
+scan = np.zeros(450)
+scanMask = np.zeros(450)
+robotPos = np.zeros(3)
+crowdX = np.zeros(crowd_size)
+crowdY = np.zeros(crowd_size)
+crowdT = np.zeros(crowd_size)
+
+
+dataHeader = np.concatenate((scenario, scan_seq, simTime))
+print(dataHeader)
+
+# Data format - lidar:
+# - Each scenario is a .csv file, with each line corresponds to a scan, separated by comma.
+# - The first number of each line should be a scan sequence number (unique within scenario)
+# - The second number should be simulation time
+# - The rest should be lidar reading, arranged from left to right
+
+dataLidar = np.concatenate((dataHeader, scan))
+
+# Data format - lidar segmentation mask:
+# - Use .csv.mask for file ending, but in essence is a csv file
+# - Mostly same as data format for lidar
+# - Instead of having lidar reading for each entry, have corresponding pedestrian id (unique within scenario)
+
+dataLidarMask = np.concatenate((dataHeader, scanMask))
+
+# Data format - pedestrian trajectories:
+# - Three csv files with ending .csv.traj.x, .csv.traj.y, and .csv.traj.phi
+# - Use the first line to store ids of all pedestrian (unique within scenario)
+# - Then for the rest of the lines:
+# - First two numbers of each line are scan sequence and time (same as for lidar)
+# - The rest of the line should be x/y/phi (depending on the file) of each pedestrian at that time
+
+dataCrowdX = np.concatenate((dataHeader, crowdX))
+dataCrowdY = np.concatenate((dataHeader, crowdY))
+dataCrowdT = np.concatenate((dataHeader, crowdT))
+
+# Data format - robot odometry:
+# - A csv file with ending .csv.odom
+# - For each line:
+# - First and second number: scan id and time
+# - Third, fouth, and fifth number: x, y, and phi of robot at that time
+
+dataOdom = np.concatenate((scenario,simTime,robotPos))
+
+
+# Data format - Full
+# - A csv file with ending csv.full
+# - For each line:
+# - scan id, time, scan, scan asks, odom, crowdX, crowdY, crowdT
+
+dataFull = np.concatenate((scenario, simTime, scan, scanMask, robotPos, crowdX, crowdY, crowdT))
+
+global scan_updated, twist_updated, crowd_updated
+
+scan_updated = False
+twist_updated = False
+crowd_updated = False
+
+def on_scan(rcv_scan):
+ global scan, scanMask, scan_updated
+ scan = np.array(rcv_scan.ranges)
+ # ad hoc trick, masks on intensities
+ scanMask = np.array(rcv_scan.intensities)
+ scan_updated = True
+
+def on_twist(t):
+ global robotPos, twist_updated
+ robotPos = np.array([t.linear.x, t.linear.y, np.deg2rad(t.angular.z)])
+ twist_updated = True
+
+def on_crowd(t_arr):
+ global crowdX, crowdY, crowdT, crowd_updated
+ for i,t in enumerate(t_arr.twist):
+ crowdX[i] = t.linear.x
+ crowdY[i] = t.linear.y
+ crowdT[i] = np.deg2rad(t.angular.y)
+ crowd_updated = True
+
+
+def publish_clock(current_time, clock_pub, msg):
+ ### Publish clock
+ secs = int(current_time)
+ nsecs = 1e9 * (current_time - secs)
+ msg.clock = rospy.Time(secs,nsecs)
+ clock_pub.publish(msg)
+
+
+def write_data(path,name,scenNb,ext,data):
+ f = open(path+"/"+name+"_{}.".format(scenNb)+ext,"a+")
+ f.write(",".join([str(x) for x in data.tolist()])+'\n')
+ f.close()
+
+def data_gen():
+ global scan_updated, twist_updated, crowd_updated
+ global scenario, scan_seq, scan, scanMask, robotPos, crowdX, crowdY, crowdT
+ global dataHeader, dataLidar, dataLidarMask, dataCrowdX, dataCrowdY, dataCrowdT, dataOdom, dataFull
+
+ # initialize node
+ rospy.init_node('data_gen', anonymous = False)
+
+ ### Set up clock publisher
+ clock_pub = rospy.Publisher('clock', Clock, queue_size = 1)
+
+ msg = Clock()
+ msg.clock = rospy.Time()
+
+ current_time = 0
+ scenario = np.array([0])
+ scan_seq = np.array([0])
+
+ write_data("./RVO","RVO",scenario[0],"csv.traj.x",np.arange(crowd_size))
+ write_data("./RVO","RVO",scenario[0],"csv.traj.y",np.arange(crowd_size))
+ write_data("./RVO","RVO",scenario[0],"csv.traj.t",np.arange(crowd_size))
+
+ ### Set up subscribers
+ scan_sub = rospy.Subscriber('/laser/scan', LaserScan, on_scan, queue_size = 1)
+ robot_twist_sub = rospy.Subscriber('/robot/odom', Twist, on_twist, queue_size = 1)
+ crowd_sub = rospy.Subscriber('/crowd', TwistArrayStamped, on_crowd, queue_size = 1)
+
+ start = time()
+
+ while not rospy.is_shutdown() and scenario[0] < scenario_number:
+ while not rospy.is_shutdown() and scan_seq[0] < (60/time_step - 1):
+ publish_clock(current_time, clock_pub, msg)
+
+ if scan_updated and crowd_updated and twist_updated:
+
+ simTime = np.array([current_time])
+
+ dataHeader = np.concatenate((scenario, scan_seq, simTime))
+ dataLidar = np.concatenate((dataHeader, scan))
+ dataLidarMask = np.concatenate((dataHeader, scanMask))
+ dataCrowdX = np.concatenate((dataHeader, crowdX))
+ dataCrowdY = np.concatenate((dataHeader, crowdY))
+ dataCrowdT = np.concatenate((dataHeader, crowdT))
+ dataOdom = np.concatenate((scenario,simTime,robotPos))
+ dataFull = np.concatenate((scenario, simTime, scan, scanMask, robotPos, crowdX, crowdY, crowdT))
+
+ write_data("./RVO","RVO",scenario[0],"csv",dataLidar)
+ write_data("./RVO","RVO",scenario[0],"csv.mask",dataLidarMask)
+ write_data("./RVO","RVO",scenario[0],"csv.traj.x",dataCrowdX)
+ write_data("./RVO","RVO",scenario[0],"csv.traj.y",dataCrowdY)
+ write_data("./RVO","RVO",scenario[0],"csv.traj.t",dataCrowdT)
+ write_data("./RVO","RVO",scenario[0],"csv.odom",dataOdom)
+ write_data("./RVO","RVO",scenario[0],"csv.full",dataFull)
+
+ scan_seq[0] = scan_seq[0] + 1
+ scan_updated = False
+ crowd_updated = False
+ twist_updated = False
+
+ current_time += time_step
+ sleep(6*time_step)
+ print(scenario[0], "{0:.2f}".format(current_time),"{0:.2f}".format(time() - start))
+ scenario[0] = scenario[0] + 1
+ current_time = 0
+ scan_seq = np.array([0])
+ print("New scenario")
+ sleep(2)
+
+if __name__ == '__main__':
+ data_gen()
diff --git a/crowdbotsim/scripts/footstep_predictor.py b/crowdbotsim/scripts/footstep_predictor.py
new file mode 100755
index 0000000000000000000000000000000000000000..91f8c20b287529464c69d42f703d185cad51fc69
--- /dev/null
+++ b/crowdbotsim/scripts/footstep_predictor.py
@@ -0,0 +1,48 @@
+#!/usr/bin/env python
+import rospy
+import sys
+import math
+import people_msgs
+from people_msgs.msg import PositionMeasurementArray
+
+myargv = rospy.myargv(argv=sys.argv)
+script = myargv
+
+legs = {}
+previous_legs = {}
+previous_people = {}
+
+def leg_tracker_callback(leg):
+ for i in range(0, len(leg.people)):
+ # if(leg.people[i].header.frame_id == 'sick_laser_front'):
+ # legs[leg.people[i].object_id] =
+ print( "{} - {} - x : {} y : {}".format(leg.people[i].header.frame_id, leg.people[i].object_id,leg.people[i].pos.x,leg.people[i].pos.y))
+ print("\n\n------------------------\n\n")
+
+
+def people_tracker_callback(people):
+ return
+
+def clean_old_id():
+ return
+
+def footstep_predictor_setup():
+ #init node
+ rospy.init_node('footstep_predictor', anonymous=True)
+
+ #init subscribers
+ leg_sub = rospy.Subscriber('leg_tracker_measurements', PositionMeasurementArray, leg_tracker_callback)
+ people_sub = rospy.Subscriber('people_tracker_measurements', PositionMeasurementArray, people_tracker_callback)
+
+ rate = rospy.Rate(1)
+
+ while not rospy.is_shutdown():
+ clean_old_id()
+ rate.sleep()
+
+
+if __name__ == '__main__':
+ try:
+ footstep_predictor_setup()
+ except Exception as e:
+ print(e)
\ No newline at end of file