diff --git a/controllers/19Jul21_RL_accel_output/19Jul21_RL_accel_output.onnx b/controllers/19Jul21_RL_accel_output/19Jul21_RL_accel_output.onnx new file mode 100644 index 0000000..03bbbcc Binary files /dev/null and b/controllers/19Jul21_RL_accel_output/19Jul21_RL_accel_output.onnx differ diff --git a/observers/gaussian_observer/CMakeLists.txt b/observers/gaussian_observer/CMakeLists.txt deleted file mode 100755 index c4e1407..0000000 --- a/observers/gaussian_observer/CMakeLists.txt +++ /dev/null @@ -1,206 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(gaussian_observer) - -## 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 - roscpp - rospy - 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 -# 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 gaussian_observer -# CATKIN_DEPENDS roscpp rospy 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}/gaussian_observer.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/gaussian_observer_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 -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_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_gaussian_observer.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/observers/gaussian_observer/package.xml b/observers/gaussian_observer/package.xml deleted file mode 100755 index 1b9c4c6..0000000 --- a/observers/gaussian_observer/package.xml +++ /dev/null @@ -1,68 +0,0 @@ - - - gaussian_observer - 0.0.0 - The gaussian_observer package - - - - - fangyu - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - roscpp - rospy - std_msgs - roscpp - rospy - std_msgs - roscpp - rospy - std_msgs - - - - - - - - diff --git a/observers/ideal_observer/CMakeLists.txt b/observers/ideal_observer/CMakeLists.txt deleted file mode 100755 index 13afc1c..0000000 --- a/observers/ideal_observer/CMakeLists.txt +++ /dev/null @@ -1,206 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(ideal_observer) - -## 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 - roscpp - rospy - 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 -# 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 ideal_observer -# CATKIN_DEPENDS roscpp rospy 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}/ideal_observer.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/ideal_observer_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 -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_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_ideal_observer.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/observers/ideal_observer/package.xml b/observers/ideal_observer/package.xml deleted file mode 100755 index 0fd8e4f..0000000 --- a/observers/ideal_observer/package.xml +++ /dev/null @@ -1,68 +0,0 @@ - - - ideal_observer - 0.0.0 - The ideal_observer package - - - - - fangyu - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - roscpp - rospy - std_msgs - roscpp - rospy - std_msgs - roscpp - rospy - std_msgs - - - - - - - - diff --git a/onnx2ros/include/NetworkReader.h b/onnx2ros/include/NetworkReader.h index a6368e4..ee2327e 100644 --- a/onnx2ros/include/NetworkReader.h +++ b/onnx2ros/include/NetworkReader.h @@ -25,6 +25,15 @@ class BaseReader{ Ort::Experimental::Session session; std::vector input_names, output_names; std::vector> input_shapes; + double HEADWAY_SCALE; + double SPEED_SCALE; + double T_param; + std::string ego_vel_topic; + std::string headway_topic; + std::string relative_vel_topic; + + bool use_leadvel; + bool use_accel_predict; public: BaseReader(ros::NodeHandle *nh, std::string onnx_model); @@ -51,8 +60,8 @@ class SynchronousReader : BaseReader{ class PromptReader : BaseReader{ protected: - ros::Subscriber sub_v, sub_lv, sub_h; - geometry_msgs::Twist state_v, state_lv; + ros::Subscriber sub_v, sub_lv, sub_h, sub_relative_vel; + geometry_msgs::Twist state_v, state_lv, state_relative_vel; std_msgs::Float64 state_h; public: @@ -61,6 +70,8 @@ class PromptReader : BaseReader{ void callback_v(const geometry_msgs::Twist& v_msg); void callback_lv(const geometry_msgs::Twist& lv_msg); + + void callback_relative_vel(const geometry_msgs::Twist& rv_msg); void callback_h(const std_msgs::Float64& h_msg); diff --git a/onnx2ros/launch/control.launch b/onnx2ros/launch/control.launch new file mode 100644 index 0000000..eec7d6f --- /dev/null +++ b/onnx2ros/launch/control.launch @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/onnx2ros/src/NetworkReader.cpp b/onnx2ros/src/NetworkReader.cpp index bcef278..78e352c 100644 --- a/onnx2ros/src/NetworkReader.cpp +++ b/onnx2ros/src/NetworkReader.cpp @@ -11,8 +11,17 @@ BaseReader::BaseReader(ros::NodeHandle *nh, std::string onnx_model): output_names = session.GetOutputNames(); input_shapes = session.GetInputShapes(); input_shapes[0][0] = 1; - nh->setParam("SPEED_SCALE", 1.0); - nh->setParam("HEADWAY_SCALE", 1.0); + //nh->setParam("SPEED_SCALE", 1.0); + //nh->setParam("HEADWAY_SCALE", 1.0); + + nh->param("SPEED_SCALE", SPEED_SCALE, 1.0); + nh->param("HEADWAY_SCALE", HEADWAY_SCALE, 1.0); + nh->param("ego_vel_topic", ego_vel_topic, std::string("/vel")); + nh->param("relative_vel_topic", relative_vel_topic, std::string("/rel_vel")); + nh->param("headway_topic", headway_topic, std::string("/lead_dist")); + nh->param("use_lead_vel", use_leadvel, false); + nh->param("T", T_param, 0.6); + nh->param("use_accel_predict", use_accel_predict, false); } double BaseReader::forward(std::vector input_values) { @@ -21,7 +30,7 @@ double BaseReader::forward(std::vector input_values) { input_values.data(), input_values.size(), input_shapes[0])); auto output_tensors = session.Run(input_names, input_tensors, output_names); const auto *output_values = output_tensors[0].GetTensorData(); - ROS_INFO("%.8f %.8f %.8f > %.8f", input_values[0], input_values[1], input_values[2], output_values[0]); + //ROS_INFO("%.8f %.8f %.8f > %.8f", input_values[0], input_values[1], input_values[2], output_values[0]); return output_values[0]; } @@ -57,31 +66,72 @@ void SynchronousReader::callback(const geometry_msgs::TwistStampedConstPtr& v_ms PromptReader::PromptReader(ros::NodeHandle *nh, std::string onnx_model): BaseReader(nh, std::move(onnx_model)){ - pub = nh->advertise("v_des", 10); - sub_v = nh->subscribe("vel", 10, &PromptReader::callback_v, this); - sub_lv = nh->subscribe("leader_vel", 10, &PromptReader::callback_lv, this); - sub_h = nh->subscribe("headway_est", 10, &PromptReader::callback_h, this); + pub = nh->advertise("v_des", 1); + sub_v = nh->subscribe(ego_vel_topic, 1, &PromptReader::callback_v, this); + sub_lv = nh->subscribe("leader_vel", 1, &PromptReader::callback_lv, this); + sub_relative_vel = nh->subscribe(relative_vel_topic, 1, &PromptReader::callback_relative_vel, this); + sub_h = nh->subscribe(headway_topic, 1, &PromptReader::callback_h, this); + + + ROS_INFO_STREAM("ego vel topic: "<< ego_vel_topic); + ROS_INFO_STREAM("relative vel topic: "<< relative_vel_topic); + ROS_INFO_STREAM("headway topic: "<< headway_topic); + + ROS_INFO_STREAM("use lead vel: "<< use_leadvel); + ROS_INFO_STREAM("headyway scale: "<< HEADWAY_SCALE); + ROS_INFO_STREAM("speed scale: "<< SPEED_SCALE); + + if (use_accel_predict) + { + ROS_INFO_STREAM("We will predict acceleration first. Acceleration will be on linear.z component"); + } + ROS_INFO_STREAM("T Parameter is :"<getParam("SPEED_SCALE", speed_scale); - nh->getParam("HEADWAY_SCALE", headway_scale); - float v = (float) state_v.linear.x / speed_scale; - float lv = (float) state_lv.linear.x / speed_scale; - float h = (float) state_h.data / headway_scale; + //float speed_scale, headway_scale; + //nh->getParam("SPEED_SCALE", speed_scale); + //nh->getParam("HEADWAY_SCALE", headway_scale); + + //ROS_INFO_STREAM("Current velocity of Ego: "< input_values(input_shapes[0][1]); input_values[0] = v; input_values[1] = lv; input_values[2] = h; - std_msgs::Float64 delta_v; - delta_v.data = PromptReader::forward(input_values); + geometry_msgs::Twist delta_v; + delta_v.linear.x = PromptReader::forward(input_values); + + if (use_accel_predict) + { + delta_v.linear.z = delta_v.linear.x; + delta_v.linear.x = T_param*delta_v.linear.x + state_v.linear.x; + } pub.publish(delta_v); }