diff --git a/CoLo-AT/localization_algos/__pycache__/ekf_gs_ci.cpython-36.pyc b/CoLo-AT/localization_algos/__pycache__/ekf_gs_ci.cpython-36.pyc
index 69150a44fca989b0b0e3843b3ddad2a91f1f44f4..9cd47b95990e6f1502abf27e71ccb08da1441c47 100644
Binary files a/CoLo-AT/localization_algos/__pycache__/ekf_gs_ci.cpython-36.pyc and b/CoLo-AT/localization_algos/__pycache__/ekf_gs_ci.cpython-36.pyc differ
diff --git a/CoLo-AT/localization_algos/under_developing_algos/ekf_gs_ci2.py b/CoLo-AT/localization_algos/ekf_gs_ci2.py
similarity index 100%
rename from CoLo-AT/localization_algos/under_developing_algos/ekf_gs_ci2.py
rename to CoLo-AT/localization_algos/ekf_gs_ci2.py
diff --git a/CoLo-AT/localization_algos/ekf_gs_ci.py b/CoLo-AT/localization_algos/under_developing_algos/ekf_gs_ci.py
similarity index 99%
rename from CoLo-AT/localization_algos/ekf_gs_ci.py
rename to CoLo-AT/localization_algos/under_developing_algos/ekf_gs_ci.py
index 9933159ce6137d758867cd75bdd29519b9234ff9..d726d6b1885a780efe2f692dc6e556d8b8228a82 100644
--- a/CoLo-AT/localization_algos/ekf_gs_ci.py
+++ b/CoLo-AT/localization_algos/under_developing_algos/ekf_gs_ci.py
@@ -85,7 +85,7 @@ class EKF_GS_CI(ekf_algo_framework):
return [s, orinetations, sigma_s]
- '''
+
def relative_obser_update(self, robot_data, sensor_data):
#when robot i observes robot j
@@ -127,8 +127,7 @@ class EKF_GS_CI(ekf_algo_framework):
sigma_s = sigma_s - kalman_gain*H*sigma_s
return [s, orinetations, sigma_s]
- '''
- '''
+
def communication(self, robot_data, sensor_data):
[s, orinetations, sigma_s, index] = robot_data
@@ -145,4 +144,3 @@ class EKF_GS_CI(ekf_algo_framework):
sigma_s = sig_inv.getI()
return [s, orinetations, sigma_s]
- '''
\ No newline at end of file
diff --git a/CoLo-AT/robots/__pycache__/robot_system.cpython-36.pyc b/CoLo-AT/robots/__pycache__/robot_system.cpython-36.pyc
index f804a9e8365eebc5c47729bdfb6824eaa49232ac..7aadf383202797e3ccda9258a470de1557b1c484 100644
Binary files a/CoLo-AT/robots/__pycache__/robot_system.cpython-36.pyc and b/CoLo-AT/robots/__pycache__/robot_system.cpython-36.pyc differ
diff --git a/CoLo-AT/robots/robot_system.py b/CoLo-AT/robots/robot_system.py
index d1c9daf6b4dba9726b07a2ce830730088dcd7c8e..14d20e3f029026353b72040a69145796622dcc26 100644
--- a/CoLo-AT/robots/robot_system.py
+++ b/CoLo-AT/robots/robot_system.py
@@ -75,17 +75,17 @@ class RobotSystem:
if rsp_type == 'odometry': #propogation update
update_type = 'propagation'
v = message_data['velocity']
+ a_v = message_data ['angular velocity']
#sigma_odo = np.matrix([[5.075*v, 0], [0, 0.1]]) #with respect to velocity and orientation for utias datasets
sigma_odo = np.matrix([[0.01, 0], [0, 0.5]]) #with respect to velocity and orientation
sensor_covariance = sigma_odo
- #delta_t = message_data['time'] - self.prev_prop_times[robot_index]
delta_t = message_data['delta_t']
- '''
- if delta_t <= 0:
+
+ if delta_t <0:
print('current time: ', message_data['time'] )
print('prev_prop_time: ', self.prev_prop_times[robot_index])
raise Exception("Error incorrect delta_t!")
- '''
+
sensor_input = [delta_t, v, message_data['orientation']]
self.prev_prop_times[robot_index] = message_data['time']
diff --git a/CoLo-AT/test_simulation.py b/CoLo-AT/test_simulation.py
index a990e7d84b25084961e612d7ebc9c0fb8ac2a97a..97c4c37388a5056df0276c6eea66605f3e943709 100644
--- a/CoLo-AT/test_simulation.py
+++ b/CoLo-AT/test_simulation.py
@@ -24,7 +24,7 @@ from simple_ekf import Simple_EKF
from ekf_ls_bda import EKF_LS_BDA
from ekf_ls_ci import EKF_LS_CI
from ekf_gs_bound import EKF_GS_BOUND
-from ekf_gs_ci import EKF_GS_CI
+from ekf_gs_ci2 import EKF_GS_CI2
dataset_path = '/home/william/UTIAS-dataset/MRCLAM_Dataset3/'
@@ -37,10 +37,10 @@ start_time, starting_states, dataset_data, time_arr = testing_dataset.load_MRCLA
freqs0 = [[10, 10, 10],[1, 1, 1],[0.5, 0.5, 0.5]]
freqs1 = [[10, 10, 10],[4, 4, 4],[0.5, 0.5, 0.5]]
-loc_algo = Centralized_EKF('algo')
+loc_algo = EKF_GS_CI2('algo')
robot = RobotSystem('robot', dataset_labels, loc_algo, distr_sys = False)
-sim = SimulationManager('sim Centralized_EKF')
+sim = SimulationManager('sim EKF_GS_CI2')
state_recorder = StatesRecorder('recorder',dataset_labels)
sim.sim_process_schedule(dataset_labels, testing_dataset, robot, state_recorder, freqs0, simple_plot = True)
#sim.sim_process_naive(dataset_labels, testing_dataset, robot, state_recorder, simple_plot = True)
diff --git a/CoLo-PE/Other ROS packages/create_autonomy-indigo-devel.zip b/CoLo-PE/Other ROS packages/create_autonomy-indigo-devel.zip
new file mode 100644
index 0000000000000000000000000000000000000000..f6b3cb351a421da8814cae7ff000c1a560c969f7
Binary files /dev/null and b/CoLo-PE/Other ROS packages/create_autonomy-indigo-devel.zip differ
diff --git a/CoLo-PE/Other ROS packages/fiducials-kinetic-devel.zip b/CoLo-PE/Other ROS packages/fiducials-kinetic-devel.zip
new file mode 100644
index 0000000000000000000000000000000000000000..7cc27167cd342d70a6e3040e530195f1386c1899
Binary files /dev/null and b/CoLo-PE/Other ROS packages/fiducials-kinetic-devel.zip differ
diff --git a/CoLo-PE/README.md b/CoLo-PE/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/CoLo-PE/aruco_marker_finder.launch b/CoLo-PE/aruco_marker_finder.launch
new file mode 100644
index 0000000000000000000000000000000000000000..04a7173a2b720b1755b3bf69880cd45f100aecb8
--- /dev/null
+++ b/CoLo-PE/aruco_marker_finder.launch
@@ -0,0 +1,23 @@
+<launch>
+
+<arg name="markerId" default="701"/>
+<arg name="markerSize" default="0.05"/> <!-- in meter -->
+<arg name="eye" default="left"/>
+<arg name="marker_frame" default="marker_frame"/>
+<arg name="ref_frame" default=""/> <!-- leave empty and the pose will be published wrt param parent_name -->
+<arg name="corner_refinement" default="LINES" /> <!-- NONE, HARRIS, LINES, SUBPIX -->
+
+
+<node pkg="aruco_ros" type="single" name="aruco_single">
+<remap from="/camera_info" to="/usb_cam/camera_info" />
+<remap from="/image" to="/usb_cam/image_raw" />
+<param name="image_is_rectified" value="True"/>
+<param name="marker_size" value="$(arg markerSize)"/>
+<param name="marker_id" value="$(arg markerId)"/>
+<param name="reference_frame" value="$(arg ref_frame)"/> <!-- frame in which the marker pose will be refered -->
+<param name="camera_frame" value="base_link"/>
+<param name="marker_frame" value="$(arg marker_frame)" />
+<param name="corner_refinement" value="$(arg corner_refinement)" />
+</node>
+
+</launch>
diff --git a/CoLo-PE/aruco_markers_detect.launch b/CoLo-PE/aruco_markers_detect.launch
new file mode 100644
index 0000000000000000000000000000000000000000..2389eeb98f9991951aebceb9854e284bdd6723af
--- /dev/null
+++ b/CoLo-PE/aruco_markers_detect.launch
@@ -0,0 +1,21 @@
+<!-- Run the aruco_detect node -->
+<launch>
+ <!-- namespace for camera input -->
+ <arg name="camera" default="/usb_cam"/>
+ <arg name="image" default="image"/>
+ <arg name="transport" default="raw"/>
+ <arg name="fiducial_len" default="0.14"/>
+ <arg name="dictionary" default="7"/>
+ <arg name="do_pose_estimation" default="true"/>
+
+ <node pkg="aruco_detect" name="aruco_detect"
+ type="aruco_detect" output="screen" respawn="false">
+ <param name="image_transport" value="$(arg transport)"/>
+ <param name="publish_images" value="true" />
+ <param name="fiducial_len" value="$(arg fiducial_len)"/>
+ <param name="dictionary" value="$(arg dictionary)"/>
+ <param name="do_pose_estimation" value="$(arg do_pose_estimation)"/>
+ <remap from="/camera" to="/usb_cam/image_raw" />
+ <remap from="/camera_info" to="$(arg camera)/camera_info"/>
+ </node>
+</launch>
diff --git a/CoLo-PE/aruco_record/CMakeLists.txt b/CoLo-PE/aruco_record/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..7b488f425cee4f04818fdbc85b039a9a8a9814ba
--- /dev/null
+++ b/CoLo-PE/aruco_record/CMakeLists.txt
@@ -0,0 +1,204 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(aruco_record)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+ fiducial_msgs
+ roscpp
+ rospy
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+## * add a build_depend tag for "message_generation"
+## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+## but can be declared for certainty nonetheless:
+## * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+## * add "message_generation" and every package in MSG_DEP_SET to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * add "message_runtime" and every package in MSG_DEP_SET to
+## catkin_package(CATKIN_DEPENDS ...)
+## * uncomment the add_*_files sections below as needed
+## and list every .msg/.srv/.action file to be processed
+## * uncomment the generate_messages entry below
+## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# Service1.srv
+# Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# fiducial_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+## * add "dynamic_reconfigure" to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * uncomment the "generate_dynamic_reconfigure_options" section below
+## and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+# cfg/DynReconf1.cfg
+# cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+# INCLUDE_DIRS include
+# LIBRARIES aruco_record
+# CATKIN_DEPENDS fiducial_msgs roscpp rospy
+# DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+ ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+# src/${PROJECT_NAME}/aruco_record.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/aruco_record_node.cpp)
+
+add_executable(meas_record src/measurement_record.cpp)
+target_link_libraries(meas_record ${catkin_LIBRARIES})
+add_dependencies(meas_record aruco_record_cpp)
+
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+# ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+# FILES_MATCHING PATTERN "*.h"
+# PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+# # myfile1
+# # myfile2
+# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_aruco_record.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/CoLo-PE/aruco_record/package.xml b/CoLo-PE/aruco_record/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..04db1a85f2a9f0b8736cb21ea89b1038c511bba1
--- /dev/null
+++ b/CoLo-PE/aruco_record/package.xml
@@ -0,0 +1,61 @@
+<?xml version="1.0"?>
+<package format="2">
+ <name>aruco_record</name>
+ <version>0.0.0</version>
+ <description>The aruco_record package</description>
+
+ <maintainer email="billyskc@g.ucla.edu">Shengkang 'William' Chen</maintainer>
+
+ <license>BSD</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/aruco_record</url> -->
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+ <author email="billyskc@g.ucla.edu">Shengkang 'William' Chen</author>
+
+ <!-- The *depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+ <!-- <depend>roscpp</depend> -->
+ <!-- Note that this is equivalent to the following: -->
+ <!-- <build_depend>roscpp</build_depend> -->
+ <!-- <exec_depend>roscpp</exec_depend> -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use build_export_depend for packages you need in order to build against this package: -->
+ <!-- <build_export_depend>message_generation</build_export_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use exec_depend for packages you need at runtime: -->
+ <!-- <exec_depend>message_runtime</exec_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <!-- Use doc_depend for packages you need only for building documentation: -->
+ <!-- <doc_depend>doxygen</doc_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>fiducial_msgs</build_depend>
+ <build_depend>roscpp</build_depend>
+ <build_depend>rospy</build_depend>
+ <build_export_depend>fiducial_msgs</build_export_depend>
+ <build_export_depend>roscpp</build_export_depend>
+ <build_export_depend>rospy</build_export_depend>
+ <exec_depend>fiducial_msgs</exec_depend>
+ <exec_depend>roscpp</exec_depend>
+ <exec_depend>rospy</exec_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
diff --git a/CoLo-PE/aruco_record/src/measurement_record.cpp b/CoLo-PE/aruco_record/src/measurement_record.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fec37de8ad38de3762e92484f2c95e1bf99257d8
--- /dev/null
+++ b/CoLo-PE/aruco_record/src/measurement_record.cpp
@@ -0,0 +1,80 @@
+#include "ros/ros.h"
+#include "std_msgs/String.h"
+#include <math.h>
+#include "fiducial_msgs/FiducialTransform.h"
+#include "fiducial_msgs/FiducialTransformArray.h"
+#include <iomanip>
+
+#include <iostream>
+#include <fstream>
+
+using namespace std;
+#define PI 3.14159265
+
+class ArucoRecorder{
+ private:
+ ros::Subscriber * pose_sub;
+ void record_data(const fiducial_msgs::FiducialTransformArray::ConstPtr& msg);
+ public:
+ ArucoRecorder(ros::NodeHandle &nh);
+ ~ArucoRecorder()
+ {
+ cout << "Destructor called" <<endl;
+ }
+};
+
+
+void ArucoRecorder::record_data(const fiducial_msgs::FiducialTransformArray::ConstPtr& msg)
+{
+ ofstream meas_file;
+ ofstream odo_file, gt_file;
+ meas_file.open("/home/lemur-robot2/catkin_ws/ros_colo_dataset/Robot2_Measurement_x.dat", ofstream::app);
+ for (int i=0; i<msg->transforms.size(); i++) {
+ const fiducial_msgs::FiducialTransform &ft = msg->transforms[i];
+
+ int id = ft.fiducial_id;
+ float x_pos = ft.transform.translation.x;
+ float y_pos = ft.transform.translation.y;
+ float z_pos = ft.transform.translation.z;
+
+ float range = sqrt(pow(x_pos, 2) + pow(z_pos, 2));
+ float bearing = atan2(z_pos, x_pos)-PI/2;
+
+ ROS_INFO("##Object ID: [%d]##", id);
+ ROS_INFO("Range: [%f]", range);
+ ROS_INFO("Bearing: [%f]", bearing);
+ ROS_INFO("***************");
+
+ meas_file << fixed << setprecision(4) << ros::Time::now() << "\t" << id << "\t\t" << range << "\t\t" << bearing << "\n";
+
+ }
+ meas_file.close();
+ odo_file.close();
+ gt_file.close();
+}
+
+
+ArucoRecorder::ArucoRecorder(ros::NodeHandle & nh){
+ ROS_INFO("Aruco Record Started");
+ ofstream meas_file;
+ ofstream odo_file, gt_file;
+ meas_file.open("/home/lemur-robot2/catkin_ws/ros_colo_dataset/Robot2_Measurement_x.dat");
+ meas_file << "# time [sec]" << "\t\t" << "object id" << "\t" << "range [m]" << "\t" << "bearing [rad]" << "\n";
+ meas_file.close();
+
+ pose_sub = new ros::Subscriber(nh.subscribe<fiducial_msgs::FiducialTransformArray>("/fiducial_transforms_throttle", 1, &ArucoRecorder::record_data, this));
+}
+
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "meas_record");
+ ros::NodeHandle nh("~");
+
+ ArucoRecorder * node = new ArucoRecorder(nh);
+
+ ros::spin();
+
+ return 0;
+}
+
diff --git a/CoLo-PE/aruco_sensing.sh b/CoLo-PE/aruco_sensing.sh
new file mode 100644
index 0000000000000000000000000000000000000000..cef3536396af29706ff8e5104bf992cc8b56b463
--- /dev/null
+++ b/CoLo-PE/aruco_sensing.sh
@@ -0,0 +1,32 @@
+#!/bin/bash
+
+cd $HOME/catkin_ws
+xterm -hold -e "roscore" &
+xterm -hold -e "roslaunch usb_cam_stream_publisher.launch" &
+sleep 5
+xterm -hold -e "roslaunch aruco_markers_detect.launch" &
+sleep 3
+xterm -hold -e "rosrun image_view image_view image:=/fiducial_images" &
+xterm -hold -e "rostopic echo /fiducial_transforms" &
+sleep 2
+xterm -hold -e "rosrun topic_tools throttle messages /fiducial_transforms 5.0" &
+xterm -hold -e "rostopic hz fiducial_transforms_throttle"
+
+cd $HOME/
+
+exit 0
+
+<<COMMENT
+ For the throttle frequencies:
+ Issues: throttle rates lower than expected(default rate = 30Hz)
+ Expected rate [Hz]: Real rate [Hz]:
+ 40 ~30
+ 30 19.5~20
+ 20 ~15
+ 15 ~10.5
+ 10 ~8.4
+ 8 ~7.5
+ 4 ~3.75
+ 1 0.98
+
+COMMENT
diff --git a/CoLo-PE/cal_1080p.yml b/CoLo-PE/cal_1080p.yml
new file mode 100644
index 0000000000000000000000000000000000000000..b53f2f7d6e1a89aff8e303d161a36273a679e370
--- /dev/null
+++ b/CoLo-PE/cal_1080p.yml
@@ -0,0 +1,20 @@
+image_width: 1920
+image_height: 1080
+camera_name: narrow_stereo
+camera_matrix:
+ rows: 3
+ cols: 3
+ data: [1465.728433, 0.000000, 960.661035, 0.000000, 1476.926252, 525.618322, 0.000000, 0.000000, 1.000000]
+distortion_model: plumb_bob
+distortion_coefficients:
+ rows: 1
+ cols: 5
+ data: [0.037891, -0.079731, -0.006196, -0.000102, 0.000000]
+rectification_matrix:
+ rows: 3
+ cols: 3
+ data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000]
+projection_matrix:
+ rows: 3
+ cols: 4
+ data: [1469.868652, 0.000000, 960.479293, 0.000000, 0.000000, 1484.509399, 520.966236, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]
diff --git a/CoLo-PE/cal_480p.yml b/CoLo-PE/cal_480p.yml
new file mode 100644
index 0000000000000000000000000000000000000000..3f496ad8e28df9c50551a61cd4ec75ffca7beae4
--- /dev/null
+++ b/CoLo-PE/cal_480p.yml
@@ -0,0 +1,20 @@
+image_width: 640
+image_height: 480
+camera_name: narrow_stereo
+camera_matrix:
+ rows: 3
+ cols: 3
+ data: [654.029002, 0, 318.310629, 0, 651.6204770000001, 210.721894, 0, 0, 1]
+distortion_model: plumb_bob
+distortion_coefficients:
+ rows: 1
+ cols: 5
+ data: [-0.006783, -0.178923, 0.000926, 0.007013999999999999, 0]
+rectification_matrix:
+ rows: 3
+ cols: 3
+ data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
+projection_matrix:
+ rows: 3
+ cols: 4
+ data: [645.730774, 0, 321.784936, 0, 0, 649.168945, 210.480637, 0, 0, 0, 1, 0]
diff --git a/CoLo-PE/cal_720p.yml b/CoLo-PE/cal_720p.yml
new file mode 100644
index 0000000000000000000000000000000000000000..4f6b000196e50507f600f411f3d5640ef8f7e533
--- /dev/null
+++ b/CoLo-PE/cal_720p.yml
@@ -0,0 +1,20 @@
+image_width: 1280
+image_height: 720
+camera_name: narrow_stereo
+camera_matrix:
+ rows: 3
+ cols: 3
+ data: [1030.629874, 0.000000, 656.550843, 0.000000, 1039.786278, 377.591933, 0.000000, 0.000000, 1.000000]
+distortion_model: plumb_bob
+distortion_coefficients:
+ rows: 1
+ cols: 5
+ data: [0.035110, -0.042717, 0.001123, -0.000234, 0.000000]
+rectification_matrix:
+ rows: 3
+ cols: 3
+ data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000]
+projection_matrix:
+ rows: 3
+ cols: 4
+ data: [1038.616211, 0.000000, 656.304259, 0.000000, 0.000000, 1047.470825, 378.341316, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]
diff --git a/CoLo-PE/get_sensor_variances.py b/CoLo-PE/get_sensor_variances.py
new file mode 100644
index 0000000000000000000000000000000000000000..620e9bcf2047e07039198b25c9e1559af240a7d1
--- /dev/null
+++ b/CoLo-PE/get_sensor_variances.py
@@ -0,0 +1,261 @@
+# -*- coding: utf-8 -*-
+
+import numpy as np
+import sys
+import getpass
+from math import atan2, sqrt, pi
+import statistics
+
+
+def find_nearest_time_idx(l, value):
+ if len(l) != 0:
+ array = np.asarray(l)
+ idx = (np.abs(array-value)).argmin()
+ return idx
+ else:
+ return None
+
+def find_next_time_idx(array, start_idx, value):
+ i = start_idx
+ try:
+ array[i]
+ except IndexError:
+ i = -1
+ return i
+ while array[i] < value:
+ i = i+1
+ try:
+ array[i]
+ except IndexError:
+ i = -1
+ break
+ return i
+
+class DatasetAnalyzer:
+
+ def __init__(self, name):
+ self.name = name
+
+ def create_landmark_map(self):
+ ### Build landmark map ###
+ self.landmark_map = {}
+ path=self.dataset_path + "Landmark_Groundtruth.dat"
+ landmark_file=open(path,'r+');
+ s = landmark_file.readline()
+ while(s):
+ if(s[0]!='#'):
+ landmark_location = [float(s.split( )[1]), float(s.split( )[2])]
+ self.landmark_map.update({int(s.split( )[0]): landmark_location})
+ s = landmark_file.readline()
+ landmark_file.close()
+ print(self.landmark_map)
+ #print "lm ", self.landmark_map, " lm"
+ return self.landmark_map
+
+
+ def getting_sensor_variances(self, dataset_path, dataset_labels, duration):
+ self.dataset_path = dataset_path
+ print("Absolute datapath: ")
+ print(self.dataset_path)
+ self.dataset_labels = dataset_labels
+ self.create_landmark_map()
+
+ self.num_robots = len(self.dataset_labels)
+ self.diff_range_data = [[] for i in range(self.num_robots)]
+ self.diff_bearing_data = [[] for i in range(self.num_robots)]
+ self.diff_vel_data = [[] for i in range(self.num_robots)]
+ self.diff_a_v_data = [[] for i in range(self.num_robots)]
+ self.groundtruth_data = [[] for i in range(self.num_robots)]
+
+ #self.groundtruth_time= [[] for i in range(self.num_robots)]
+ self.time_arr = {'odometry': [[] for i in range(self.num_robots)], 'measurement': [[] for i in range(self.num_robots)], 'groundtruth': [[] for i in range(self.num_robots)]}
+
+ #initialization for MRCLAMDatasets: put files into dictionaries:
+ self.duration = duration # some more time to avoid index error
+ self.start_time_arr = []
+ #finding the starting time:
+ for i, label in enumerate(self.dataset_labels):
+ robot_num = str(label)
+ groundtruth_path = self.dataset_path+"Robot"+robot_num+"_Groundtruth.dat"
+ with open(groundtruth_path,'r+') as groundtruth_file:
+ for line in groundtruth_file:
+ if str(line.split()[0]) != '#':
+ time = float(line.split()[0])
+ self.start_time_arr.append(time)
+ break
+ self.start_time = max(self.start_time_arr)
+ self.end_time = self.start_time + self.duration
+ print('Staring time: ', self.start_time)
+ #finding starting states:
+ self.starting_states = {}
+ for i, label in enumerate(self.dataset_labels):
+ robot_num = str(label)
+ groundtruth_path = self.dataset_path+"Robot"+robot_num+"_Groundtruth.dat"
+ with open(groundtruth_path,'r+') as groundtruth_file:
+ for line in groundtruth_file:
+ if line[0] != '#' and float(line.split()[0]) >= self.start_time:
+ time = round(float(line.split()[0]), 3)
+ x_pos = float(line.split()[1])
+ y_pos = float(line.split()[2])
+ orientation = float(line.split()[3])
+ self.starting_states[label] = [time, x_pos, y_pos, orientation]
+ break
+
+
+ for i, label in enumerate(self.dataset_labels):
+ robot_num = str(label)
+
+ groundtruth_path = self.dataset_path+"Robot"+robot_num+"_Groundtruth.dat"
+ with open(groundtruth_path,'r+') as groundtruth_file:
+ for line in groundtruth_file:
+ if line[0] != '#' and (self.end_time >= float(line.split()[0]) >= self.start_time):
+ time = round(float(line.split()[0]), 3)
+ x_pos = float(line.split()[1])
+ y_pos = float(line.split()[2])
+ orientation = float(line.split()[3])
+ groundtruth_info = {'time':time, 'x_pos':x_pos, 'y_pos':y_pos, 'orientation':orientation}
+ self.groundtruth_data[i].append(groundtruth_info)
+ self.time_arr['groundtruth'][i].append(time)
+
+
+ meas_path = self.dataset_path+"Robot"+robot_num+"_Measurement_x.dat"
+ with open(meas_path,'r+') as measure_file:
+ for line in measure_file:
+ if line[0] != '#' and (self.end_time>= float(line.split()[0]) >= self.start_time):
+ time = round(float(line.split()[0]), 3)
+ subject_ID = int(line.split()[1])
+ measurment_range = float(line.split()[2])
+ bearing = float(line.split()[3])
+
+ g_idx = find_nearest_time_idx(self.time_arr['groundtruth'][i],time)
+ gt_x = self.groundtruth_data[i][g_idx]['x_pos']
+ gt_y = self.groundtruth_data[i][g_idx]['y_pos']
+ orientation = self.groundtruth_data[i][g_idx]['orientation']
+ landmark_loc = self.landmark_map.get(subject_ID)
+ if landmark_loc != None:
+ [lx, ly] = landmark_loc
+ range_hat = sqrt((lx-gt_x)*(lx-gt_x)+(ly-gt_y)*(ly-gt_y))
+ bearing_hat = (atan2((ly-gt_y), (lx-gt_x))-orientation)%pi
+ if abs(bearing_hat-pi) < abs(bearing_hat):
+ bearing_hat = bearing_hat-pi
+
+ diff_bearing = bearing - bearing_hat
+ diff_range = measurment_range- range_hat
+ self.diff_range_data[i].append(diff_range)
+ self.diff_bearing_data[i].append(diff_bearing)
+ self.time_arr['measurement'][i].append(time)
+
+
+ odo_path = self.dataset_path+"Robot"+robot_num+"_Odometry.dat"
+ with open(odo_path,'r+') as odometry_file:
+ lines = odometry_file.readlines()
+ for line_idx in range(0, len(lines)):
+ line = lines[line_idx]
+ if line[0] != '#' and (self.end_time >= float(line.split()[0]) >= self.start_time):
+ t = float(line.split()[0])
+ time = round(float(line.split()[0]), 3)
+ g_idx = find_nearest_time_idx(self.time_arr['groundtruth'][i],time)
+ velocity = float(line.split()[1])
+ a_v = float(line.split()[2])
+ orientation = self.groundtruth_data[i][g_idx]['orientation']
+ try:
+ next_line = lines[line_idx+1]
+ next_time = float(next_line.split()[0])
+ delta_t = next_time - time
+ except IndexError:
+ delta_t = 0
+ if delta_t < 0:
+ sys.exit('incorrect delta_t: '+ str(delta_t))
+
+ gt_t = self.groundtruth_data[i][g_idx]['time']
+ gt_x = self.groundtruth_data[i][g_idx]['x_pos']
+ gt_y = self.groundtruth_data[i][g_idx]['y_pos']
+
+ try:
+ gt_t_next = self.groundtruth_data[i][g_idx+5]['time']
+ gt_x_next = self.groundtruth_data[i][g_idx+5]['x_pos']
+ gt_y_next = self.groundtruth_data[i][g_idx+5]['y_pos']
+ distance = sqrt((gt_x_next-gt_x)*(gt_x_next-gt_x)+(gt_y_next-gt_y)*(gt_y_next-gt_y))
+ change_bearing = (atan2((gt_y_next-gt_y), (gt_x_next-gt_x))-orientation)%pi
+ if abs(change_bearing-pi) < abs(change_bearing):
+ change_bearing = change_bearing-pi
+ duration = gt_t_next - gt_t
+
+ except IndexError:
+ gt_t_prev = self.groundtruth_data[i][g_idx-5]['time']
+ gt_x_prev = self.groundtruth_data[i][g_idx-5]['x_pos']
+ gt_y_prev = self.groundtruth_data[i][g_idx-5]['y_pos']
+ distance = sqrt((gt_x_prev-gt_x)*(gt_x_prev-gt_x)+(gt_y_prev-gt_y)*(gt_y_prev-gt_y))
+ change_bearing = (atan2((gt_y-gt_y_prev), (gt_x-gt_x_prev))-orientation)%pi
+ if abs(change_bearing-pi) < abs(change_bearing):
+ change_bearing = change_bearing-pi
+ duration = gt_t - gt_t_prev
+ diff_vel = velocity - distance/duration
+
+ try:
+ gt_t_next = self.groundtruth_data[i][g_idx+15]['time']
+ gt_x_next = self.groundtruth_data[i][g_idx+15]['x_pos']
+ gt_y_next = self.groundtruth_data[i][g_idx+15]['y_pos']
+ distance = sqrt((gt_x_next-gt_x)*(gt_x_next-gt_x)+(gt_y_next-gt_y)*(gt_y_next-gt_y))
+ change_bearing = (atan2((gt_y_next-gt_y), (gt_x_next-gt_x))-orientation)%pi
+ if abs(change_bearing-pi) < abs(change_bearing):
+ change_bearing = change_bearing-pi
+ duration = gt_t_next - gt_t
+
+ except IndexError:
+ gt_t_prev = self.groundtruth_data[i][g_idx-15]['time']
+ gt_x_prev = self.groundtruth_data[i][g_idx-15]['x_pos']
+ gt_y_prev = self.groundtruth_data[i][g_idx-15]['y_pos']
+ distance = sqrt((gt_x_prev-gt_x)*(gt_x_prev-gt_x)+(gt_y_prev-gt_y)*(gt_y_prev-gt_y))
+ change_bearing = (atan2((gt_y-gt_y_prev), (gt_x-gt_x_prev))-orientation)%pi
+ if abs(change_bearing-pi) < abs(change_bearing):
+ change_bearing = change_bearing-pi
+ duration = gt_t - gt_t_prev
+
+ diff_a_v = a_v - change_bearing/duration
+ self.diff_vel_data[i].append(diff_vel)
+ self.diff_a_v_data[i].append(diff_a_v)
+
+
+ self.time_arr['odometry'][i].append(time)
+
+
+ range_vars = []
+ bearing_vars = []
+ velocity_vars = []
+ angular_vel_vars = []
+ for i, label in enumerate(self.dataset_labels):
+ print("*****************")
+ print('robot label: ', label)
+ print('range mean: ', statistics.mean(self.diff_range_data[i]))
+ range_var = statistics.variance(self.diff_range_data[i])
+ range_vars.append(range_var)
+ print('range variance: ', range_var)
+
+ print('bearing mean:', statistics.mean(self.diff_bearing_data[i]))
+ bearing_var = statistics.variance(self.diff_bearing_data[i])
+ bearing_vars.append(bearing_var)
+ print('bearing variance: ', bearing_var)
+
+ print('velocity mean: ', statistics.mean(self.diff_vel_data[i]))
+ velocity_var = statistics.variance(self.diff_vel_data[i])
+ velocity_vars.append(velocity_var)
+ print('velocity variance: ', velocity_var)
+
+ print('angular velocity mean:', statistics.mean(self.diff_a_v_data[i]))
+ angular_vel_var = statistics.variance(self.diff_a_v_data[i])
+ angular_vel_vars.append(angular_vel_var)
+ print('angular velocity variance: ', angular_vel_var)
+
+ print("*******Sensor Variances**********")
+ print("range \t\t bearing \t\t velocity \t\t angular velocity ")
+ print(statistics.mean(bearing_vars), statistics.mean(range_vars), statistics.mean(velocity_vars), statistics.mean(angular_vel_vars))
+ return statistics.mean(bearing_vars), statistics.mean(range_vars), statistics.mean(velocity_vars), statistics.mean(angular_vel_vars)
+
+
+compname = getpass.getuser()
+dataset_path = "/home/"+ compname +"/full_tests/full_test_v3_2/"
+dataset_labels = [1,2,3]
+duration = 200 # duration for the simulation in sec
+dataset = DatasetAnalyzer('DatasetAnalyzer')
+dataset.getting_sensor_variances(dataset_path, dataset_labels, duration)
\ No newline at end of file
diff --git a/CoLo-PE/meas_transform.py b/CoLo-PE/meas_transform.py
new file mode 100644
index 0000000000000000000000000000000000000000..44d96109108e67031654e78b9a50f4336f655bb8
--- /dev/null
+++ b/CoLo-PE/meas_transform.py
@@ -0,0 +1,54 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+Created on Thu Aug 23 15:45:39 2018
+
+@author: william
+"""
+
+import getpass
+import math
+
+compname = getpass.getuser()
+
+dataset_path = "/home/"+ compname +"/full_tests/full_test_v3_6/"
+
+def meas_to_meas_x(robot_label):
+ meas_x_file_name = "Robot"+str(robot_label)+"_Measurement_x.dat"
+ print(meas_x_file_name)
+ f = open(dataset_path+meas_x_file_name, "w+")
+ f.write("# Time [sec] \t\t object id \t\t range [m] \t\t bearing [rad]\n")
+ robot_r = 0.16 #distance between label and the center of the robot
+ l = 0.1 #distance between cam and the center of the robot
+
+ lable_dict = {}
+ path=dataset_path + "robot_labels.dat"
+ robot_labels=open(path,'r+');
+ s = robot_labels.readline()
+ while(s):
+ if(s[0]!='#'):
+ lable_dict.update({s.split( )[0]: s.split( )[1]})
+ s = robot_labels.readline()
+ robot_labels.close()
+
+ meas_file_name = "Robot"+str(robot_label)+"_Measurement.dat"
+ with open(dataset_path+meas_file_name,'r+') as measure_file:
+ for line in measure_file:
+ if line[0] != '#':
+ subject_ID = line.split()[1]
+ range_cam = float(line.split()[2])
+ bearing = float(line.split()[3])
+ robot_id = lable_dict.get(subject_ID)
+ if robot_id != None:
+ subject_ID = robot_id
+ range_cam = range_cam+robot_r
+
+ phi = math.atan2((range_cam*math.sin(bearing)), (range_cam*math.cos(bearing)+l))
+ if phi != 0:
+ r_cen = range_cam*math.sin(bearing)/math.sin(phi)
+ f.write(line.split()[0] + '\t' + subject_ID + '\t\t\t' + str(r_cen) + '\t\t\t'+ str(phi) +'\n')
+
+ f.close()
+
+for robot_label in [1,2,3]:
+ meas_to_meas_x(robot_label)
\ No newline at end of file
diff --git a/CoLo-PE/read_csv.py b/CoLo-PE/read_csv.py
new file mode 100644
index 0000000000000000000000000000000000000000..1ca0d5bb44b2174e24ccd1cf9621d855a3d6d9c1
--- /dev/null
+++ b/CoLo-PE/read_csv.py
@@ -0,0 +1,102 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+Created on Thu Aug 23 14:36:28 2018
+
+@author: william
+"""
+
+import getpass
+import csv
+from datetime import datetime
+import math
+
+compname = getpass.getuser()
+datapath = "/home/"+ compname +"/full_tests/full_test_v3_6/"
+
+
+
+def generate_robot_gt_file(robot_label):
+ gt_file_name = "Robot"+ str(robot_label) + "_Groundtruth.dat"
+ print(gt_file_name)
+ f_r = open(datapath+gt_file_name, "w+")
+ f_r.write("# Time [sec] \t\t\t x [m] \t\t y [m] \t\t orientation [rad]\n")
+
+ input_data = list(csv.reader(open(datapath+"gt.csv", "r+")))
+ num_rows = len(input_data)
+ start_time = input_data[0][9]
+
+ print("Experiment start time: ", start_time)
+ dt_obj = datetime.strptime(start_time, "%Y-%m-%d %I.%M.%S.%f %p")
+ time_tuple = dt_obj.timetuple()
+ timestamp = dt_obj.timestamp()
+ print(repr(timestamp))
+
+ col_idx = 2
+ while input_data[3][col_idx] != '':
+ if input_data[3][col_idx] == 'irobot'+str(robot_label):
+ break
+ col_idx+=1
+
+ row_idx = 7
+
+ while row_idx < num_rows:
+ cur_time = input_data[row_idx][1]
+ x = input_data[row_idx][col_idx+3]
+ tmp = input_data[row_idx][col_idx+5] #z-positon(csv) -> y-position(dat)
+ y_rot = input_data[row_idx][col_idx]
+ if x!='':
+ y = float(tmp)*-1
+ x = float(x)
+ if float(cur_time) >= 10:
+ pitch = math.radians(float(y_rot))
+ f_r.write(str(float(cur_time)+timestamp) + '\t\t' + str(x) + '\t\t' + str(y) + '\t\t'+ str(pitch) + '\n')
+
+ row_idx+=1
+
+ f_r.close()
+
+def generate_landmarks_gt_file(landmark_names, landmarks_dict):
+ gt_file_name = "Landmark_Groundtruth.dat"
+ print(gt_file_name)
+ f_r = open(datapath+gt_file_name, "w+")
+ f_r.write("# Landmark_tags [sec] \t x [m] \t y [m] \n")
+
+ input_data = list(csv.reader(open(datapath+"gt.csv", "r+")))
+ num_rows = len(input_data)
+
+ for landmark_name in landmark_names:
+ col_idx = 2
+ while input_data[3][col_idx] != '':
+ if input_data[3][col_idx] == landmark_name:
+ break
+ col_idx+=1
+
+ row_idx = 7
+
+ while row_idx < num_rows:
+ cur_time = input_data[row_idx][1]
+ x = input_data[row_idx][col_idx+3]
+ tmp = input_data[row_idx][col_idx+5] #z-positon(csv) -> y-position(dat)
+ if x!='' and float(cur_time) >= 10:
+ tags = landmarks_dict.get(landmark_name)
+ for tag_id in tags:
+ y = float(tmp)*-1
+ x = float(x)
+ print(str(landmark_name)+"\t"+str(tag_id) + "\t" + str(x) + "\t" + str(y))
+ f_r.write(str(tag_id) + "\t" + str(x) + "\t" + str(y) + '\n')
+
+ break
+ row_idx+=1
+
+ f_r.close()
+
+
+
+for robot_label in [1,2,3]:
+ generate_robot_gt_file(robot_label)
+ pass
+
+landmark_names = ["landmark0", "landmark1", "landmark2", "landmark3", "landmark4", "landmark5", "landmark6", "landmark7"]
+landmarks_dict = {"landmark0": [113, 114, 115, 116], "landmark1":[103, 104, 105, 106], "landmark2":[107], "landmark3":[110], "landmark4":[109], "landmark5":[108], "landmark6":[120],"landmark7":[124]}
+generate_landmarks_gt_file(landmark_names, landmarks_dict)
\ No newline at end of file
diff --git a/CoLo-PE/robot_controls/CMakeLists.txt b/CoLo-PE/robot_controls/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..8ccc0b4d2ca97f91785a520d2ee41f261fa32c46
--- /dev/null
+++ b/CoLo-PE/robot_controls/CMakeLists.txt
@@ -0,0 +1,199 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(robot_controls)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+ rospy
+ sensor_msgs
+ std_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+## * add a build_depend tag for "message_generation"
+## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+## but can be declared for certainty nonetheless:
+## * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+## * add "message_generation" and every package in MSG_DEP_SET to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * add "message_runtime" and every package in MSG_DEP_SET to
+## catkin_package(CATKIN_DEPENDS ...)
+## * uncomment the add_*_files sections below as needed
+## and list every .msg/.srv/.action file to be processed
+## * uncomment the generate_messages entry below
+## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# Service1.srv
+# Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# ca_msgs# sensor_msgs# std_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+## * add "dynamic_reconfigure" to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * uncomment the "generate_dynamic_reconfigure_options" section below
+## and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+# cfg/DynReconf1.cfg
+# cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+# INCLUDE_DIRS include
+# LIBRARIES robot_controls
+# CATKIN_DEPENDS ca_msgs rospy sensor_msgs std_msgs
+# DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+ ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+# src/${PROJECT_NAME}/robot_controls.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/robot_controls_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+#add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+# ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+# FILES_MATCHING PATTERN "*.h"
+# PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+# # myfile1
+# # myfile2
+# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_robot_controls.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/CoLo-PE/robot_controls/controls.py b/CoLo-PE/robot_controls/controls.py
new file mode 100644
index 0000000000000000000000000000000000000000..6af60f8636487944e5de62cdbead4624270f7c63
--- /dev/null
+++ b/CoLo-PE/robot_controls/controls.py
@@ -0,0 +1,153 @@
+#!/usr/bin/env python
+from __future__ import print_function
+
+import rospy
+import sys, termios, tty
+import time
+import random
+from math import radians, degrees
+from ca_msgs.msg import Bumper
+import getpass
+from geometry_msgs.msg import Twist
+
+movement_bindings = {
+ 'i':(1,0,0,0),
+ 'o':(1,0,0,-1),
+ 'j':(0,0,0,1),
+ 'l':(0,0,0,-1),
+ 'u':(1,0,0,1),
+ ',':(-1,0,0,0),
+ '.':(-1,0,0,1),
+ 'm':(-1,0,0,-1),
+ }
+
+welcomemsg = '''
+Welcome!
+If you wish to access autonomous control of the robot, then enter one.
+If you wish to access manual control, enter two.
+If you wish to leave, enter break.
+'''
+automsg = '''
+If you want the robot to move in a square, type square.
+If you want to see the robot move randomly, type random.
+If you want to leave, type break.
+'''
+manualmsg = '''
+The controls are like so, and all are lowercase.
+u i o
+j l
+m , .
+'''
+
+class main(object):
+ def __init__(self):
+ self.lin = rospy.get_param('~linVel', .1)
+ self.ang = rospy.get_param('~angVel', 1.0)
+ self.compname = getpass.getuser()
+ self.f = open("/home/"+ self.compname +"/catkin_ws/ros_colo_dataset/command_vel.dat", "w+")
+ self.twist = Twist()
+
+ def twist_msgs(self, lin_vel, ang_vel):
+ linear = ang_vel
+ angular = lin_vel
+ rtime = time.time()
+ self.f.write(str(rtime) + '\t\t' +str(linear) + '\t\t' + str(angular) + '\n')
+
+ def publisher(self):
+ pub = rospy.Publisher("cmd_vel", Twist, queue_size = 30)
+ return pub
+
+ def listener(self):
+ rospy.Subscriber("Bumper", Bumper, self.callback)
+
+ def key(self):
+ fd = sys.stdin.fileno()
+ old_settings = termios.tcgetattr(fd)
+ tty.setraw(sys.stdin.fileno())
+ ch = sys.stdin.read(1)
+ termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
+ return ch
+
+ def movements(self, key, linear, angular):
+ x = movement_bindings[key][0]
+ y = movement_bindings[key][1]
+ z = movement_bindings[key][2]
+ th = movement_bindings[key][3]
+
+ self.twist.linear.x = x*linear; self.twist.linear.y = y*linear; self.twist.linear.z = z*linear;
+ self.twist.angular.x = 0; self.twist.angular.y = 0; self.twist.angular.z = th*angular
+ pub = self.publisher()
+ pub.publish(self.twist)
+ self.twist_msgs(self.twist.linear.x, self.twist.angular.z)
+
+ def callback(self, data):
+ pass
+
+ def square(self):
+ sides = 4
+ turns = 55
+ forwards = 100
+ for s in range(sides):
+ for t in range(turns):
+ self.movements('j', self.lin, 1.5)
+ time.sleep(.1)
+ for f in range(forwards):
+ self.movements('i', self.lin, self.ang)
+ time.sleep(.1)
+
+
+ def random(self, start, endTime):
+ while True:
+ keylist = ['u','i','o','u','i','o','j','l']
+ rankey = random.choice(keylist)
+
+ if rankey in ['j','l']:
+ ranint = random.randint(2,30)
+ else:
+ ranint = random.randint(15,25)
+ i = 0
+ while i < ranint:
+ movements(rankey, self.lin, self.ang)
+ time.sleep(.1)
+ i += 1
+ if time.time() > start + endTime:
+ break
+
+ def manual(self):
+ print(manualmsg)
+ while True:
+ k = self.key()
+ if k in movement_bindings.keys():
+ self.movements(k, self.lin, self.ang)
+ if k == "b":
+ break
+
+if __name__ =="__main__":
+ m = main()
+ rospy.init_node('controls')
+ m.publisher()
+ m.square()
+ rospy.spin()
+
+ '''while True:
+ input1 = raw_input(welcomemsg + "\n")
+
+ if input1 == "one":
+ input2 = raw_input("\n"+automsg+"\n")
+
+ if input2 == "square":
+ m.square()
+
+ if input2 == "random":
+ start = time.time()
+ endTime = 15
+ m.random(start, endTime)
+
+ if input2 == "break":
+ break
+
+ if input1 == "two":
+ m.manual()
+
+ if input1 == "break":
+ break'''
diff --git a/CoLo-PE/robot_controls/create2_motion_control.py b/CoLo-PE/robot_controls/create2_motion_control.py
new file mode 100644
index 0000000000000000000000000000000000000000..8f7b06674b530050e19075a6aa420a8d1d9774d0
--- /dev/null
+++ b/CoLo-PE/robot_controls/create2_motion_control.py
@@ -0,0 +1,176 @@
+#!/usr/bin/env python
+# license removed for brevity
+import numpy as np
+import rospy
+import sys, termios, tty
+import random
+from math import radians, degrees
+from ca_msgs.msg import Bumper
+import getpass
+import time
+from geometry_msgs.msg import Twist
+
+
+class create2_motion_controller(object):
+ """
+ docstring for create2_motion_controller
+
+ note: improve evasion to be smarter
+ make manual (optional)
+
+
+ """
+ def __init__(self):
+ self.control_pub = rospy.Publisher("cmd_vel", Twist, queue_size = 30)
+ self.state_sub = rospy.Subscriber("bumper", Bumper, self.robot_motion) #original_freq = 10hz
+ self.rate = rospy.Rate(10)
+ self.eva = False
+ #not sure for get_param
+ #self.lin = rospy.get_param('~linVel', .2)
+ #self.ang = rospy.get_param('~angVel', 1.0)
+ self.lin = 0.1
+ self.ang = 0.4
+ self.evasion_time = 0
+ self.end_node = time.time() + (60 * 5)
+ self.compname = getpass.getuser()
+ self.f = open("/home/"+ self.compname +"/catkin_ws/ros_colo_dataset/Robot1_Odometry_c"+str(time.time())+".dat", "w+")
+ self.f.write("# Time [sec] \t Velocity [m/s] \t Angular Velocity [rad/s] \n")
+ self.movement_bindings = {
+ 'i':(1,0,0,0),
+ 'o':(1,0,0,-1),
+ 'j':(0,0,0,1),
+ 'l':(0,0,0,-1),
+ 'u':(1,0,0,1),
+ ',':(-1,0,0,0),
+ '.':(-1,0,0,1),
+ 'm':(-1,0,0,-1),
+ }
+ self.twist = Twist()
+
+ def twist_msgs(self, lin_vel, ang_vel):
+ rtime = time.time()
+ self.f.write(str(rtime) + '\t\t' +str(lin_vel) + '\t\t' + str(ang_vel) + '\n')
+ print(str(rtime) + '\t\t' +str(lin_vel) + '\t\t' + str(ang_vel) + '\n')
+
+ def movements(self, key, linear, angular):
+ x = self.movement_bindings[key][0]
+ y = self.movement_bindings[key][1]
+ z = self.movement_bindings[key][2]
+ th = self.movement_bindings[key][3]
+ twist = Twist()
+ twist.linear.x = x*linear; twist.linear.y = y*linear; twist.linear.z = z*linear;
+ twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = th*angular
+ self.control_pub.publish(twist)
+
+ def smooth_movements(self, linear_vel, angular_vel):
+
+ '''
+ control_input = [linear_vel, angular_vel]
+ self.twist.linear.x = 0 # [pos: forward velocity
+ self.twist.linear.y = 0 # no effects
+ self.twist.linear.z = 0 # no effects
+ self.twist.angular.x = 0 # no effects
+ self.twist.angular.y = 0 # no effects
+ self.twist.angular.z = 0.5 # angular velocity pos: counter-clock
+ '''
+ if 0.2 > linear_vel > 0:
+ self.twist.linear.x = linear_vel
+ elif linear_vel <= 0:
+ self.twist.linear.x = 0
+ else:
+ self.twist.linear.x = 0.2
+
+ if 0.1 > angular_vel > -0.1:
+ self.twist.angular.z = angular_vel
+ elif angular_vel > 0.1:
+ self.twist.angular.z = 0.1
+ else:
+ self.twist.angular.z = -0.1
+
+ self.twist.linear.x = 0.1
+ self.twist.angular.z = 0
+
+
+ self.twist_msgs(self.twist.linear.x, self.twist.angular.z)
+ self.control_pub.publish(self.twist)
+
+
+ def manual_control(self, manual_input):
+ pass
+
+ def backoff(self, barriers):
+ print("backoff")
+ linear_vel = self.twist.linear.x
+ angular_vel = self.twist.angular.z
+
+ linear_vel = (linear_vel-0.1)/2
+ angular_vel = (angular_vel - 0.2)/2
+
+ self.twist.linear.x = linear_vel
+ self.twist.angular.z = angular_vel
+ print("back 0: ", self.twist)
+ self.control_pub.publish(self.twist)
+ self.twist_msgs(self.twist.linear.x, self.twist.angular.z)
+
+ '''
+ linear_vel = -0.1
+ if barriers[0]: # barrier on the left:
+ angular_vel = -0.2
+ elif barriers[1]:
+ angular_vel = -0.1
+ elif barriers[2]:
+ angular_vel = 0.2
+ self.twist.linear.x = linear_vel
+ self.twist.angular.z = angular_vel
+ print("back 1: ", self.twist)
+ self.control_pub.publish(self.twist)
+ self.twist_msgs(self.twist.linear.x, self.twist.angular.z)
+ '''
+ def evasion(self, barriers):
+ print("evasion")
+ linear_vel = -0.1
+ angular_vel = -0.2
+ self.twist.linear.x = linear_vel
+ self.twist.angular.z = angular_vel
+ print("back 0: ", self.twist)
+ self.control_pub.publish(self.twist)
+ self.twist_msgs(self.twist.linear.x, self.twist.angular.z)
+
+ def random_movement(self):
+ print("Movement")
+ delta_vel = random.uniform(-0.02, 0.02)
+ delta_ang_vel = random.uniform(-0.07, 0.07)
+ self.smooth_movements(self.twist.linear.x+delta_vel, self.twist.angular.z+delta_ang_vel)
+
+ def robot_motion(self, data):
+ if time.time() > self.end_node:
+ exit()
+ lef = data.is_light_left
+ lefron = data.is_light_front_left
+ lefcen = data.is_light_center_left
+ rigcen = data.is_light_center_right
+ rigfron = data.is_light_front_right
+ rig = data.is_light_right
+
+ bumper_l = data.is_left_pressed
+ bumper_r = data.is_right_pressed
+
+ left_barrier = bumper_l | lefron
+ front_barrier = lefcen | rigcen
+ right_barrier = rigfron | bumper_r
+ barriers = [left_barrier, front_barrier, right_barrier]
+ #disabled rig and lef
+ barriers_detected = lefron | lefcen | rigcen | rigfron | bumper_l | bumper_r
+ if barriers_detected:
+ self.backoff(barriers)
+ self.evasion_time = time.time()+3
+ elif self.evasion_time >= time.time():
+ self.evasion(barriers)
+ else:
+ self.random_movement()
+
+if __name__ == '__main__':
+ rospy.init_node('create2_motion_controller', anonymous=True)
+ c = create2_motion_controller()
+ c.rate.sleep()
+ rospy.spin()
diff --git a/CoLo-PE/robot_controls/odometry_record.py b/CoLo-PE/robot_controls/odometry_record.py
new file mode 100644
index 0000000000000000000000000000000000000000..b404af49f39a18f4543f90246b9aa6fef3d5f993
--- /dev/null
+++ b/CoLo-PE/robot_controls/odometry_record.py
@@ -0,0 +1,40 @@
+#!/usr/bin/env python
+import rospy
+import getpass
+from sensor_msgs.msg import JointState
+import time
+import math
+
+
+def ang_vel(lvel, rvel):
+ w = 0.0
+ if ((lvel - rvel) != 0):
+ v1 = float(lvel)
+ v2 = float(rvel)
+ r1 = (v1 * .25)/(v2-v1)
+ w = v1/r1
+ return w
+
+def callback(data):
+ t_sec = data.header.stamp.secs
+ t_nsec = data.header.stamp.nsecs
+ lvel = data.velocity[0]
+ rvel = data.velocity[1]
+ v = (lvel + rvel)/2
+ w = ang_vel(lvel, rvel)
+ first = f.read(1)
+ print(str(t_sec) + '.' + str(t_nsec) + '\t\t' + str(v) +'\t\t'+ str(w) + '\t\t' + str(lvel) + '\t\t' + str(rvel) + '\n')
+ f.write(str(t_sec) + '.' + str(t_nsec) + '\t\t' + str(v) + '\t\t' + str(w) + '\t\t' + str(lvel) +'\t\t'+ str(rvel) +'\n')
+ #time.sleep(2/60)
+
+def listener():
+ rospy.init_node('listenprint', anonymous=True)
+ rospy.Subscriber("joint_states", JointState, callback)
+ rospy.spin()
+
+if __name__== '__main__':
+ compname = getpass.getuser()
+ f = open("/home/"+ compname +"/catkin_ws/ros_colo_dataset/Robot1_Odometry.dat", "w+")
+ f.write("# Time [sec] \t\t\t Velocity [m/s] \t\t Angular Velocity [rad/s]\t\tLeft Wheel Velocity\t\tRIght Wheel Velocity\n")
+ listener()
+
diff --git a/CoLo-PE/robot_controls/package.xml b/CoLo-PE/robot_controls/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..1c0050b52f460754a21b042fb39326fe10673d07
--- /dev/null
+++ b/CoLo-PE/robot_controls/package.xml
@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<package format="2">
+ <name>robot_controls</name>
+ <version>0.1.0</version>
+ <description>The robot_controls package</description>
+
+ <maintainer email="you@yourdomain.tld">Your Name</maintainer>
+ <license>BSD</license>
+ <url type="website">http://wiki.ros.org/robot_controls</url>
+ <author email="you@yourdomain.tld">Jane Doe</author>
+
+ <buildtool_depend>catkin</buildtool_depend>
+
+ <exec_depend>sensor_msgs</exec_depend>
+ <exec_depend>std_msgs</exec_depend>
+ <exec_depend>rospy</exec_depend>
+ <exec_depend>geometry_msgs</exec_depend>
+ <exec_depend>ca_msgs</exec_depend>
+
+</package>
diff --git a/CoLo-PE/robot_controls/publish.py b/CoLo-PE/robot_controls/publish.py
new file mode 100644
index 0000000000000000000000000000000000000000..a277c17e268fc59940941ffd53e52ca4d0c7099e
--- /dev/null
+++ b/CoLo-PE/robot_controls/publish.py
@@ -0,0 +1,37 @@
+#!/usr/bin/env python
+import rospy
+import getpass
+from sensor_msgs.msg import JointState
+import time
+import math
+
+def velocity(lvel, rvel):
+ w = 0.0
+ if ((lvel - rvel) != 0):
+ v1 = float(lvel)
+ v2 = float(rvel)
+ r1 = (v2 * .24)/(v2-v1)
+ w = v2/r1
+ return w
+
+def callback(data):
+ t_sec = data.header.stamp.secs
+ t_nsec = data.header.stamp.nsecs
+ lvel = data.velocity[0]
+ rvel = data.velocity[1]
+ v = (lvel + rvel)/2
+ w = velocity(lvel, rvel)
+ first = f.read(1)
+ f.write(str(t_sec) + '.' + str(t_nsec) + '\t\t' + str(v) +'\t\t\t'+ str(w) +'\n')
+ time.sleep(2/60)
+
+def listener():
+ rospy.init_node('listenprint', anonymous=True)
+ rospy.Subscriber("joint_states", JointState, callback)
+ rospy.spin()
+
+if __name__== '__main__':
+ compname = getpass.getuser()
+ f = open("/home/"+ compname +"/catkin_ws/src/robot_controls/moveData.dat", "w+")
+ f.write("Time(seconds)\t\t\tVelocity(m/s)\t\tAngular Velocity(ra/s)\n")
+ listener()
diff --git a/CoLo-PE/robot_full_run.sh b/CoLo-PE/robot_full_run.sh
new file mode 100644
index 0000000000000000000000000000000000000000..99d1c89cedbcebe0fc686ed38983142dad733913
--- /dev/null
+++ b/CoLo-PE/robot_full_run.sh
@@ -0,0 +1,37 @@
+#!/bin/bash
+
+cd $HOME/catkin_ws
+xterm -hold -e "roslaunch usb_cam_stream_publisher.launch" &
+sleep 5
+xterm -hold -e "roslaunch aruco_markers_detect.launch" &
+sleep 3
+xterm -hold -e "rosrun image_view image_view image:=/fiducial_images" &
+xterm -hold -e "rostopic echo /fiducial_transforms" &
+sleep 2
+xterm -hold -e "rosrun topic_tools throttle messages /fiducial_transforms 5.0" &
+xterm -hold -e "rostopic hz fiducial_transforms_throttle" &
+sleep 1
+xterm -hold -e "rosrun aruco_record meas_record" &
+sleep 1
+xterm -hold -e "roslaunch ca_driver create_2.launch" &
+sleep 2
+xterm -hold -e "rosrun robot_controls create2_motion_control.py"
+
+cd $HOME/
+
+exit 0
+
+<<COMMENT
+ For the throttle frequencies:
+ Issues: throttle rates lower than expected(default rate = 30Hz)
+ Expected rate [Hz]: Real rate [Hz]:
+ 40 ~30
+ 30 19.5~20
+ 20 ~15
+ 15 ~10.5
+ 10 ~8.4
+ 8 ~7.5
+ 4 ~3.75
+ 1 0.98
+
+COMMENT
diff --git a/CoLo-PE/usb_cam_stream_publisher.launch b/CoLo-PE/usb_cam_stream_publisher.launch
new file mode 100644
index 0000000000000000000000000000000000000000..b11a9a74d7bdfdc046fd2deae8f6dffa32d4e110
--- /dev/null
+++ b/CoLo-PE/usb_cam_stream_publisher.launch
@@ -0,0 +1,30 @@
+<!--
+Example of run:
+roslaunch usb_cam_stream_publisher.launch video_device:=/dev/video0 image_width:=640 image_height:=480
+-->
+
+<launch>
+<arg name="video_device" default="/dev/video0" />
+<arg name="image_width" default="1280" />
+<arg name="image_height" default="720" />
+<arg name="framerate" default="20" />
+<arg name = "contrast" default="100"/>
+<arg name="image_topic_name" default="/usb_cam/image_raw" />
+<arg name="camera_info_topic_name" default="/usb_cam/camera_info" />
+
+
+<node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen" >
+<param name="video_device" value="$(arg video_device)" />
+<param name="image_width" value="$(arg image_width)" />
+<param name="image_height" value="$(arg image_height)"/>
+<param name="pixel_format" value="mjpeg" />
+<param name="io_method" value="mmap"/>
+
+<!-- update these line accroding to your setting, here are some examples:-->
+<!-- <param name="camera_info_url" value="package://your_cameras/info/camera.yaml"/> -->
+<param name="camera_info_url" value="file:///home/lemur-robot2/catkin_ws/cal_720p.yml"/>
+<!-- <param name="camera_info_url" value="file://${ROS_HOME}/camera_info/head_camera.yaml"/> -->
+<remap from="/usb_cam/camera_info" to="$(arg camera_info_topic_name)"/>
+<remap from="/usb_cam/image_raw" to="$(arg image_topic_name)"/>
+</node>
+</launch>