From 4e78055d103e1b253e8f08ac432b2bc5dea06b67 Mon Sep 17 00:00:00 2001 From: Daniel Parra Date: Mon, 20 Feb 2023 10:17:11 -0500 Subject: [PATCH 1/3] Removed IndyAV file --- .../command/indyav_launch/CMakeLists.txt | 5 - .../indyav_launch/config/actuators.yaml | 15 -- .../indyav_launch/launch/controls.launch | 17 -- .../indyav_launch/launch/gazebo.launch | 8 - .../indyav_launch/launch/joydrive.launch | 21 -- .../indyav_launch/launch/master.launch | 11 - .../indyav_launch/launch/odometry.launch | 10 - .../indyav_launch/launch/path_player.launch | 11 - .../indyav_launch/launch/path_recorder.launch | 15 -- .../command/indyav_launch/launch/tf.launch | 11 - .../indyav_launch/launch/upload_urdf.launch | 6 - .../IndyAV/command/indyav_launch/package.xml | 22 -- deprecated/IndyAV/control/.gitkeep | 0 .../control/indyav_control/CMakeLists.txt | 98 --------- .../IndyAV/control/indyav_control/README.txt | 1 - .../simulated_steering_driver.hpp | 16 -- .../indyav_control/msg/RevsStamped.msg | 2 - .../indyav_control/msg/SteeringStamped.msg | 2 - .../IndyAV/control/indyav_control/package.xml | 59 ----- .../control/indyav_control/src/joy_teleop.cpp | 112 ---------- .../src/simulated_steering_driver.cpp | 50 ----- deprecated/IndyAV/indyav.rviz | 205 ------------------ deprecated/IndyAV/localization/.gitkeep | 0 .../indyav_localization/CMakeLists.txt | 173 --------------- .../indyav_localization/package.xml | 69 ------ .../indyav_localization/src/dumb_truth_tf.cpp | 28 --- .../indyav_localization/test/test_sylphase.py | 49 ----- .../test/test_sylphase.test | 6 - deprecated/IndyAV/perception/.gitkeep | 0 .../planning/indyav_path/CMakeLists.txt | 110 ---------- .../include/indyav_path/path_recorder.hpp | 46 ---- .../IndyAV/planning/indyav_path/package.xml | 73 ------- .../planning/indyav_path/src/path_player.cpp | 17 -- .../indyav_path/src/path_recorder.cpp | 39 ---- deprecated/IndyAV/scripts/bash_aliases.sh | 3 - deprecated/IndyAV/simulation/.gitkeep | 0 .../simulation/indyav_gazebo/CMakeLists.txt | 118 ---------- .../indyav_gazebo/backwheel_plugin.hpp | 27 --- .../include/indyav_gazebo/wheel_plugin.hpp | 35 --- .../indyav_gazebo/launch/spawn.launch | 10 - .../indyav_gazebo/launch/swan.launch | 20 -- .../simulation/indyav_gazebo/package.xml | 61 ------ .../scripts/generate_racetrack.py | 94 -------- .../indyav_gazebo/src/backwheel_plugin.cpp | 49 ----- .../indyav_gazebo/src/wheel_plugin.cpp | 33 --- .../indyav_gazebo/urdf/indyav_car.urdf.xacro | 47 ---- .../worlds/new_smyrna.world.xacro | 9 - .../indyav_gazebo/worlds/new_smyrna.xacro | 56 ----- .../indyav_gazebo/worlds/new_smyrna.yaml | 18 -- .../indyav_gazebo/worlds/speedways/.gitkeep | 0 .../indyav_gazebo/xacro/axles.xacro | 56 ----- .../indyav_gazebo/xacro/back_wheel.xacro | 48 ---- .../indyav_gazebo/xacro/go_kart.xacro | 27 --- .../indyav_gazebo/xacro/wheels.xacro | 102 --------- 54 files changed, 2120 deletions(-) delete mode 100644 deprecated/IndyAV/command/indyav_launch/CMakeLists.txt delete mode 100644 deprecated/IndyAV/command/indyav_launch/config/actuators.yaml delete mode 100644 deprecated/IndyAV/command/indyav_launch/launch/controls.launch delete mode 100644 deprecated/IndyAV/command/indyav_launch/launch/gazebo.launch delete mode 100644 deprecated/IndyAV/command/indyav_launch/launch/joydrive.launch delete mode 100644 deprecated/IndyAV/command/indyav_launch/launch/master.launch delete mode 100644 deprecated/IndyAV/command/indyav_launch/launch/odometry.launch delete mode 100644 deprecated/IndyAV/command/indyav_launch/launch/path_player.launch delete mode 100644 deprecated/IndyAV/command/indyav_launch/launch/path_recorder.launch delete mode 100644 deprecated/IndyAV/command/indyav_launch/launch/tf.launch delete mode 100644 deprecated/IndyAV/command/indyav_launch/launch/upload_urdf.launch delete mode 100644 deprecated/IndyAV/command/indyav_launch/package.xml delete mode 100644 deprecated/IndyAV/control/.gitkeep delete mode 100644 deprecated/IndyAV/control/indyav_control/CMakeLists.txt delete mode 100644 deprecated/IndyAV/control/indyav_control/README.txt delete mode 100644 deprecated/IndyAV/control/indyav_control/include/indyav_control/simulated_steering_driver.hpp delete mode 100644 deprecated/IndyAV/control/indyav_control/msg/RevsStamped.msg delete mode 100644 deprecated/IndyAV/control/indyav_control/msg/SteeringStamped.msg delete mode 100644 deprecated/IndyAV/control/indyav_control/package.xml delete mode 100644 deprecated/IndyAV/control/indyav_control/src/joy_teleop.cpp delete mode 100644 deprecated/IndyAV/control/indyav_control/src/simulated_steering_driver.cpp delete mode 100644 deprecated/IndyAV/indyav.rviz delete mode 100644 deprecated/IndyAV/localization/.gitkeep delete mode 100644 deprecated/IndyAV/localization/indyav_localization/CMakeLists.txt delete mode 100644 deprecated/IndyAV/localization/indyav_localization/package.xml delete mode 100644 deprecated/IndyAV/localization/indyav_localization/src/dumb_truth_tf.cpp delete mode 100755 deprecated/IndyAV/localization/indyav_localization/test/test_sylphase.py delete mode 100644 deprecated/IndyAV/localization/indyav_localization/test/test_sylphase.test delete mode 100644 deprecated/IndyAV/perception/.gitkeep delete mode 100644 deprecated/IndyAV/planning/indyav_path/CMakeLists.txt delete mode 100644 deprecated/IndyAV/planning/indyav_path/include/indyav_path/path_recorder.hpp delete mode 100644 deprecated/IndyAV/planning/indyav_path/package.xml delete mode 100644 deprecated/IndyAV/planning/indyav_path/src/path_player.cpp delete mode 100644 deprecated/IndyAV/planning/indyav_path/src/path_recorder.cpp delete mode 100755 deprecated/IndyAV/scripts/bash_aliases.sh delete mode 100644 deprecated/IndyAV/simulation/.gitkeep delete mode 100644 deprecated/IndyAV/simulation/indyav_gazebo/CMakeLists.txt delete mode 100644 deprecated/IndyAV/simulation/indyav_gazebo/include/indyav_gazebo/backwheel_plugin.hpp delete mode 100644 deprecated/IndyAV/simulation/indyav_gazebo/include/indyav_gazebo/wheel_plugin.hpp delete mode 100644 deprecated/IndyAV/simulation/indyav_gazebo/launch/spawn.launch delete mode 100644 deprecated/IndyAV/simulation/indyav_gazebo/launch/swan.launch delete mode 100644 deprecated/IndyAV/simulation/indyav_gazebo/package.xml delete mode 100755 deprecated/IndyAV/simulation/indyav_gazebo/scripts/generate_racetrack.py delete mode 100644 deprecated/IndyAV/simulation/indyav_gazebo/src/backwheel_plugin.cpp delete mode 100644 deprecated/IndyAV/simulation/indyav_gazebo/src/wheel_plugin.cpp delete mode 100644 deprecated/IndyAV/simulation/indyav_gazebo/urdf/indyav_car.urdf.xacro delete mode 100644 deprecated/IndyAV/simulation/indyav_gazebo/worlds/new_smyrna.world.xacro delete mode 100644 deprecated/IndyAV/simulation/indyav_gazebo/worlds/new_smyrna.xacro delete mode 100644 deprecated/IndyAV/simulation/indyav_gazebo/worlds/new_smyrna.yaml delete mode 100644 deprecated/IndyAV/simulation/indyav_gazebo/worlds/speedways/.gitkeep delete mode 100644 deprecated/IndyAV/simulation/indyav_gazebo/xacro/axles.xacro delete mode 100644 deprecated/IndyAV/simulation/indyav_gazebo/xacro/back_wheel.xacro delete mode 100644 deprecated/IndyAV/simulation/indyav_gazebo/xacro/go_kart.xacro delete mode 100644 deprecated/IndyAV/simulation/indyav_gazebo/xacro/wheels.xacro diff --git a/deprecated/IndyAV/command/indyav_launch/CMakeLists.txt b/deprecated/IndyAV/command/indyav_launch/CMakeLists.txt deleted file mode 100644 index 862961e37..000000000 --- a/deprecated/IndyAV/command/indyav_launch/CMakeLists.txt +++ /dev/null @@ -1,5 +0,0 @@ -# cmake_minimum_required(VERSION 2.8.3) -# project(indyav_launch) -# find_package(catkin REQUIRED roslaunch) -# roslaunch_add_file_check(launch) -# catkin_package() diff --git a/deprecated/IndyAV/command/indyav_launch/config/actuators.yaml b/deprecated/IndyAV/command/indyav_launch/config/actuators.yaml deleted file mode 100644 index 300f1d13d..000000000 --- a/deprecated/IndyAV/command/indyav_launch/config/actuators.yaml +++ /dev/null @@ -1,15 +0,0 @@ -#TODO: fill out with actual params in the smart motor -joint_state_publisher: - type: joint_state_controller/JointStateController - publish_rate: 50 -car: - simulated_hardware_controllers: - steering: - left: - type: effort_controllers/JointPositionController - joint: base_link_to_front_left_axle_link_joint - pid: {p: 1000.0, i: 0.01, d: 10.0} - right: - type: effort_controllers/JointPositionController - joint: base_link_to_front_right_axle_link_joint - pid: {p: 1000.0, i: 0.01, d: 10.0} diff --git a/deprecated/IndyAV/command/indyav_launch/launch/controls.launch b/deprecated/IndyAV/command/indyav_launch/launch/controls.launch deleted file mode 100644 index 95a235110..000000000 --- a/deprecated/IndyAV/command/indyav_launch/launch/controls.launch +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - - - - diff --git a/deprecated/IndyAV/command/indyav_launch/launch/gazebo.launch b/deprecated/IndyAV/command/indyav_launch/launch/gazebo.launch deleted file mode 100644 index d7689244b..000000000 --- a/deprecated/IndyAV/command/indyav_launch/launch/gazebo.launch +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - diff --git a/deprecated/IndyAV/command/indyav_launch/launch/joydrive.launch b/deprecated/IndyAV/command/indyav_launch/launch/joydrive.launch deleted file mode 100644 index 7b628b2cb..000000000 --- a/deprecated/IndyAV/command/indyav_launch/launch/joydrive.launch +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - diff --git a/deprecated/IndyAV/command/indyav_launch/launch/master.launch b/deprecated/IndyAV/command/indyav_launch/launch/master.launch deleted file mode 100644 index de548dbd0..000000000 --- a/deprecated/IndyAV/command/indyav_launch/launch/master.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - diff --git a/deprecated/IndyAV/command/indyav_launch/launch/odometry.launch b/deprecated/IndyAV/command/indyav_launch/launch/odometry.launch deleted file mode 100644 index a1b72c007..000000000 --- a/deprecated/IndyAV/command/indyav_launch/launch/odometry.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - diff --git a/deprecated/IndyAV/command/indyav_launch/launch/path_player.launch b/deprecated/IndyAV/command/indyav_launch/launch/path_player.launch deleted file mode 100644 index 2e47c8fca..000000000 --- a/deprecated/IndyAV/command/indyav_launch/launch/path_player.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - diff --git a/deprecated/IndyAV/command/indyav_launch/launch/path_recorder.launch b/deprecated/IndyAV/command/indyav_launch/launch/path_recorder.launch deleted file mode 100644 index 8f689512b..000000000 --- a/deprecated/IndyAV/command/indyav_launch/launch/path_recorder.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - diff --git a/deprecated/IndyAV/command/indyav_launch/launch/tf.launch b/deprecated/IndyAV/command/indyav_launch/launch/tf.launch deleted file mode 100644 index da924dc81..000000000 --- a/deprecated/IndyAV/command/indyav_launch/launch/tf.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - diff --git a/deprecated/IndyAV/command/indyav_launch/launch/upload_urdf.launch b/deprecated/IndyAV/command/indyav_launch/launch/upload_urdf.launch deleted file mode 100644 index 78e47ead8..000000000 --- a/deprecated/IndyAV/command/indyav_launch/launch/upload_urdf.launch +++ /dev/null @@ -1,6 +0,0 @@ - - - - - diff --git a/deprecated/IndyAV/command/indyav_launch/package.xml b/deprecated/IndyAV/command/indyav_launch/package.xml deleted file mode 100644 index 9eb1597d2..000000000 --- a/deprecated/IndyAV/command/indyav_launch/package.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - indyav_launch - 0.0.0 - The indyav_launch package - Marshall Rawson - MIT - roslaunch - tf - mil_tools - indyav_path - robot_state_publisher - joint_state_publisher - indyav_localization - indyav_control - joy - controller_manager - indyav_gazebo - catkin - - - diff --git a/deprecated/IndyAV/control/.gitkeep b/deprecated/IndyAV/control/.gitkeep deleted file mode 100644 index e69de29bb..000000000 diff --git a/deprecated/IndyAV/control/indyav_control/CMakeLists.txt b/deprecated/IndyAV/control/indyav_control/CMakeLists.txt deleted file mode 100644 index da587e161..000000000 --- a/deprecated/IndyAV/control/indyav_control/CMakeLists.txt +++ /dev/null @@ -1,98 +0,0 @@ -# cmake_minimum_required(VERSION 2.8.3) -# project(indyav_control) -# -# find_package(catkin REQUIRED -# roscpp -# message_generation -# std_msgs) -# -# add_message_files( -# DIRECTORY msg -# FILES -# SteeringStamped.msg -# RevsStamped.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 -# ) -# -# -# catkin_package( -# CATKIN_DEPENDS std_msgs message_generation -# ) -# -# ########### -# ## 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}/indyav_control.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(simulated_steering_driver -# src/simulated_steering_driver.cpp) -# add_dependencies(simulated_steering_driver -# std_msgs -# ${catkin_EXPORTED_TARGETS}) -# target_link_libraries(simulated_steering_driver -# ${catkin_LIBRARIES}) -# add_executable(joy_teleop -# src/joy_teleop.cpp) -# add_dependencies(joy_teleop -# ${catkin_EXPORTED_TARGETS}) -# target_link_libraries(joy_teleop -# ${catkin_LIBRARIES}) -# -# ## 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 "") -# -# -# -# ############# -# ## Testing ## -# ############# -# -# ## Add gtest based cpp test target and link libraries -# # catkin_add_gtest(${PROJECT_NAME}-test test/test_indyav_control.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/deprecated/IndyAV/control/indyav_control/README.txt b/deprecated/IndyAV/control/indyav_control/README.txt deleted file mode 100644 index d871e0ede..000000000 --- a/deprecated/IndyAV/control/indyav_control/README.txt +++ /dev/null @@ -1 +0,0 @@ -TODO: impliment key / joy drive diff --git a/deprecated/IndyAV/control/indyav_control/include/indyav_control/simulated_steering_driver.hpp b/deprecated/IndyAV/control/indyav_control/include/indyav_control/simulated_steering_driver.hpp deleted file mode 100644 index c2d3f6ce4..000000000 --- a/deprecated/IndyAV/control/indyav_control/include/indyav_control/simulated_steering_driver.hpp +++ /dev/null @@ -1,16 +0,0 @@ -#include -#include - -template -class SimulatedSteeringDriver -{ -public: - SimulatedSteeringDriver(ros::NodeHandle* _nh, const std::string& sub_topic); - -protected: - ros::NodeHandle* nh_; - ros::Subscriber sub_; - std::map pubs_; - - void Callback(const MSG& _msg); -}; diff --git a/deprecated/IndyAV/control/indyav_control/msg/RevsStamped.msg b/deprecated/IndyAV/control/indyav_control/msg/RevsStamped.msg deleted file mode 100644 index 0db7d0c60..000000000 --- a/deprecated/IndyAV/control/indyav_control/msg/RevsStamped.msg +++ /dev/null @@ -1,2 +0,0 @@ -Header header -float64 radians_per_second diff --git a/deprecated/IndyAV/control/indyav_control/msg/SteeringStamped.msg b/deprecated/IndyAV/control/indyav_control/msg/SteeringStamped.msg deleted file mode 100644 index 2652d9020..000000000 --- a/deprecated/IndyAV/control/indyav_control/msg/SteeringStamped.msg +++ /dev/null @@ -1,2 +0,0 @@ -Header header -float64 steering_angle diff --git a/deprecated/IndyAV/control/indyav_control/package.xml b/deprecated/IndyAV/control/indyav_control/package.xml deleted file mode 100644 index 316375fc7..000000000 --- a/deprecated/IndyAV/control/indyav_control/package.xml +++ /dev/null @@ -1,59 +0,0 @@ - - - indyav_control - 0.0.0 - The indyav_control package - - - - - spaceduck - - - - - - TODO - - - - - - - - - - - - - - - - - - - roscpp - - - - - message_generation - - message_generation - - - - message_runtime - - - - - std_msgs - catkin - - - - - - - diff --git a/deprecated/IndyAV/control/indyav_control/src/joy_teleop.cpp b/deprecated/IndyAV/control/indyav_control/src/joy_teleop.cpp deleted file mode 100644 index 04e758890..000000000 --- a/deprecated/IndyAV/control/indyav_control/src/joy_teleop.cpp +++ /dev/null @@ -1,112 +0,0 @@ -#include -#include -#include - -#include -#include - -#include "ros/ros.h" -#include "sensor_msgs/Joy.h" - -class JoyTeleop -{ -public: - JoyTeleop(ros::NodeHandle* _nh); - -private: - void Callback(const sensor_msgs::Joy::ConstPtr& joy); - - ros::NodeHandle* nh_; - - int steer_axis_, throttle_axis_; - double s_scale_, r_scale_; - std::string t_advertise_, s_advertise_; - ros::Subscriber joy_; - ros::Publisher steering_pub_, throttle_pub_; -}; - -JoyTeleop::JoyTeleop(ros::NodeHandle* _nh) -{ - nh_ = _nh; - - if (nh_->getParam("axis_steer", steer_axis_)) - { - ROS_INFO("Got param 'axis_steer': %d", steer_axis_); - } - else - { - ROS_ERROR("Failed to get param 'axis_steer'"); - } - if (nh_->getParam("axis_throttle", throttle_axis_)) - { - ROS_INFO("Got param 'axis_throttle': %d", throttle_axis_); - } - else - { - ROS_ERROR("Failed to get param 'axis_throttle'"); - } - if (nh_->getParam("scale_steer", s_scale_)) - { - ROS_INFO("Got param 'scale_steer': %f", s_scale_); - } - else - { - ROS_ERROR("Failed to get param 'scale_steer'"); - } - if (nh_->getParam("scale_trigger", r_scale_)) - { - ROS_INFO("Got param 'scale_trigger': %f", r_scale_); - } - else - { - ROS_ERROR("Failed to get param 'scale_trigger'"); - } - if (nh_->getParam("throttle_topic", t_advertise_)) - { - ROS_INFO("Got param 'throttle_topic': %s", t_advertise_.c_str()); - } - else - { - ROS_ERROR("Failed to get param 'throttle_topic'"); - } - if (nh_->getParam("steering_topic", s_advertise_)) - { - ROS_INFO("Got param 'steering_topic': %s", s_advertise_.c_str()); - } - else - { - ROS_ERROR("Failed to get param 'steering_topic'"); - } - - throttle_pub_ = nh_->advertise(t_advertise_, 1); - steering_pub_ = nh_->advertise(s_advertise_, 1); - joy_ = nh_->subscribe("/joy", 100, &JoyTeleop::Callback, this); -} - -void JoyTeleop::Callback(const sensor_msgs::Joy::ConstPtr& _joy) -{ - static indyav_control::RevsStamped radians_per_second; - static indyav_control::SteeringStamped steering_angle; - - steering_angle.steering_angle = s_scale_ * _joy->axes.at(steer_axis_); - - // Gamepad trigger incorrectly defaults to 0 when resting position is 1. Moving the trigger updates the value, - // correcting the error. To seamlessly work around the problem, the angular velocity is not assigned until this change - // is detected. - static bool gamepad_init = false; - if (_joy->axes.at(throttle_axis_) != 0) - gamepad_init = true; - if (gamepad_init) - radians_per_second.radians_per_second = r_scale_ * (abs(_joy->axes.at(throttle_axis_) - 1)) / 2; - - steering_pub_.publish(steering_angle); - throttle_pub_.publish(radians_per_second); -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "joy_teleop"); - ros::NodeHandle nh("~"); - JoyTeleop joy_teleop(&nh); - ros::spin(); -} \ No newline at end of file diff --git a/deprecated/IndyAV/control/indyav_control/src/simulated_steering_driver.cpp b/deprecated/IndyAV/control/indyav_control/src/simulated_steering_driver.cpp deleted file mode 100644 index 1b03ead24..000000000 --- a/deprecated/IndyAV/control/indyav_control/src/simulated_steering_driver.cpp +++ /dev/null @@ -1,50 +0,0 @@ -#include -#include - -template -SimulatedSteeringDriver::SimulatedSteeringDriver(ros::NodeHandle* _nh, const std::string& sub_topic) -{ - nh_ = _nh; - - sub_ = nh_->subscribe(sub_topic, 1, &SimulatedSteeringDriver::Callback, this); - - // get all the controllers from the ros params - std::string name = "/car"; - ROS_ASSERT_MSG(ros::param::has(name), "no car param namspace"); - name += "/simulated_hardware_controllers"; - ROS_ASSERT_MSG(nh_->hasParam(name), "no simulated hardware controllers param namspace"); - name += "/steering"; - ROS_ASSERT_MSG(nh_->hasParam(name), "no steering param namspace"); - - auto left_name = name + "/left"; - ROS_ASSERT_MSG(ros::param::has(left_name), "no left controller"); - pubs_[left_name] = nh_->advertise(left_name + "/command", 5); - - auto right_name = name + "/right"; - ROS_ASSERT_MSG(ros::param::has(right_name), "no right controller"); - pubs_[right_name] = nh_->advertise(right_name + "/command", 5); -} - -template -void SimulatedSteeringDriver::Callback(const MSG& _msg) -{ - double cmd_angle = _msg.steering_angle; - for (auto i = pubs_.begin(); i != pubs_.end(); ++i) - { - // TODO: impliment ackerman steering angles - std_msgs::Float64 msg; - msg.data = cmd_angle; - i->second.publish(msg); - } -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "simulated_steering_driver"); - ros::NodeHandle nh("~"); - std::string input_topic; - ROS_ASSERT_MSG(nh.getParam("input_topic", input_topic), "No input topic specified"); - auto a = SimulatedSteeringDriver(&nh, input_topic); - ros::spin(); - return 0; -} diff --git a/deprecated/IndyAV/indyav.rviz b/deprecated/IndyAV/indyav.rviz deleted file mode 100644 index ff7e74d4a..000000000 --- a/deprecated/IndyAV/indyav.rviz +++ /dev/null @@ -1,205 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /TF1 - - /TF1/Frames1 - Splitter Ratio: 0.5 - Tree Height: 663 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Class: rviz/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: true - back_left_wheel_link: - Value: true - back_right_wheel_link: - Value: true - base_link: - Value: true - enu: - Value: true - front_left_wheel_link: - Value: true - front_right_wheel_link: - Value: true - ins: - Value: true - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - enu: - ins: - base_link: - back_left_wheel_link: - {} - back_right_wheel_link: - {} - front_left_wheel_link: - {} - front_right_wheel_link: - {} - Update Interval: 0 - Value: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - back_left_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - back_right_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - front_left_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - front_right_wheel_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ins: - Alpha: 1 - Show Axes: false - Show Trail: false - Name: RobotModel - Robot Description: robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: enu - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 10 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0 - Y: 0 - Z: 0 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.5953980684280396 - Target Frame: - Value: Orbit (rviz) - Yaw: 1.3003981113433838 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 960 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000015600000322fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000322000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000322fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000322000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000050f0000032200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1920 - X: 0 - Y: 27 diff --git a/deprecated/IndyAV/localization/.gitkeep b/deprecated/IndyAV/localization/.gitkeep deleted file mode 100644 index e69de29bb..000000000 diff --git a/deprecated/IndyAV/localization/indyav_localization/CMakeLists.txt b/deprecated/IndyAV/localization/indyav_localization/CMakeLists.txt deleted file mode 100644 index b2a37c938..000000000 --- a/deprecated/IndyAV/localization/indyav_localization/CMakeLists.txt +++ /dev/null @@ -1,173 +0,0 @@ -# cmake_minimum_required(VERSION 2.8.3) -# project(indyav_localization) -# -# ## 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 -# std_msgs -# nav_msgs -# tf -# rostest -# ) -# -# ## 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 indyav_localization -# CATKIN_DEPENDS roscpp std_msgs nav_msgs tf -# # DEPENDS system_lib -# ) -# -# ########### -# ## Build ## -# ########### -# -# ## Specify additional locations of header files -# ## Your package locations should be listed before other locations -# include_directories( -# ${catkin_INCLUDE_DIRS} -# ) -# -# ## Declare a C++ library -# # add_library(${PROJECT_NAME} -# # src/${PROJECT_NAME}/indyav_localization.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(dumb_truth_tf src/dumb_truth_tf.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(dumb_truth_tf ${catkin_EXPORTED_TARGETS}) -# -# ## Specify libraries to link a library or executable target against -# target_link_libraries(dumb_truth_tf -# ${catkin_LIBRARIES} -# ) -# -# -# ############# -# ## Testing ## -# ############# -# -# if(CATKIN_ENABLE_TESTING) -# foreach(T -# test/test_sylphase.test) -# add_rostest(${T}) -# endforeach() -# endif() -# -# ## Add gtest based cpp test target and link libraries -# # catkin_add_gtest(${PROJECT_NAME}-test test/test_indyav_localization.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/deprecated/IndyAV/localization/indyav_localization/package.xml b/deprecated/IndyAV/localization/indyav_localization/package.xml deleted file mode 100644 index 07d2a9e03..000000000 --- a/deprecated/IndyAV/localization/indyav_localization/package.xml +++ /dev/null @@ -1,69 +0,0 @@ - - - indyav_localization - 0.0.0 - The indyav_localization package - - - - - spaceduck - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - roscpp - std_msgs - roscpp - std_msgs - roscpp - std_msgs - - nav_msgs - tf - rosunit - - - - - - - - diff --git a/deprecated/IndyAV/localization/indyav_localization/src/dumb_truth_tf.cpp b/deprecated/IndyAV/localization/indyav_localization/src/dumb_truth_tf.cpp deleted file mode 100644 index 8801ce678..000000000 --- a/deprecated/IndyAV/localization/indyav_localization/src/dumb_truth_tf.cpp +++ /dev/null @@ -1,28 +0,0 @@ -#include -#include -#include - -// TODO: remove this code when p3d in gazebo is replaced with the sylphase xacro - -void cb(const nav_msgs::Odometry& _msg) -{ - static tf::TransformBroadcaster br; - tf::Transform transform; - transform.setOrigin(tf::Vector3(_msg.pose.pose.position.x, _msg.pose.pose.position.y, _msg.pose.pose.position.z)); - transform.setRotation(tf::Quaternion(_msg.pose.pose.orientation.x, _msg.pose.pose.orientation.y, - _msg.pose.pose.orientation.z, _msg.pose.pose.orientation.w)); - br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), _msg.header.frame_id, _msg.child_frame_id)); -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "dumb_truth_odom_tf"); - ros::NodeHandle nh("~"); - std::string topic_name; - ROS_ASSERT_MSG(nh.getParam("topic_name", topic_name), "no topic name provided"); - ros::Subscriber sub = nh.subscribe(topic_name, 1, cb); - - ros::spin(); - - return 0; -} diff --git a/deprecated/IndyAV/localization/indyav_localization/test/test_sylphase.py b/deprecated/IndyAV/localization/indyav_localization/test/test_sylphase.py deleted file mode 100755 index 6807ba976..000000000 --- a/deprecated/IndyAV/localization/indyav_localization/test/test_sylphase.py +++ /dev/null @@ -1,49 +0,0 @@ -#!/usr/bin/env python -import os -import unittest -from time import sleep - -import rospy -import rostest -import tf -from nav_msgs.msg import Odometry - - -class SylphaseTestCase(unittest.TestCase): - sylphase_pub = False - sylphase_hz_rate = 0 - high_hz = False - - def callback(self, data): - self.sylphase_hz_rate += 1 - - def test_ins_odom_frequency(self): - rospy.wait_for_service("/gazebo/spawn_urdf_model") - rospy.Subscriber("/ins_odom", Odometry, self.callback) - sleep(1) - - # tests if ins_odom is being published - if self.sylphase_hz_rate > 0: - self.sylphase_pub = True - self.assertTrue(self.sylphase_pub) - - # tests if ins_odom is being published at a rate above 25 hz - if self.sylphase_hz_rate > 25: - self.high_hz = True - self.assertTrue(self.high_hz) - - def test_base_link_enu_tf(self): - # wait till the robot is spawned in gazebo - rospy.wait_for_service("/gazebo/spawn_urdf_model") - tf_listener = tf.TransformListener() - tf_listener.waitForTransform( - "base_link", "enu", rospy.Time(0), rospy.Duration(5.0) - ) - - -if __name__ == "__main__": - rospy.init_node("test_sylphase") - import rostest - - rostest.rosrun("test_package", "test_name", SylphaseTestCase) - os.system("killgazebo") diff --git a/deprecated/IndyAV/localization/indyav_localization/test/test_sylphase.test b/deprecated/IndyAV/localization/indyav_localization/test/test_sylphase.test deleted file mode 100644 index 0d0c8c87f..000000000 --- a/deprecated/IndyAV/localization/indyav_localization/test/test_sylphase.test +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - diff --git a/deprecated/IndyAV/perception/.gitkeep b/deprecated/IndyAV/perception/.gitkeep deleted file mode 100644 index e69de29bb..000000000 diff --git a/deprecated/IndyAV/planning/indyav_path/CMakeLists.txt b/deprecated/IndyAV/planning/indyav_path/CMakeLists.txt deleted file mode 100644 index a268597fd..000000000 --- a/deprecated/IndyAV/planning/indyav_path/CMakeLists.txt +++ /dev/null @@ -1,110 +0,0 @@ -# cmake_minimum_required(VERSION 2.8.3) -# project(indyav_path) -# -# ## 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 -# std_msgs -# nav_msgs -# mil_tools -# ) -# -# set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -g") -# -# # System dependencies are found with CMake's conventions -# #find_package(REQUIRED COMPONENTS system) -# -# ## 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 -# CATKIN_DEPENDS roscpp std_msgs nav_msgs mil_tools -# # LIBRARIES -# ) -# -# ########### -# ## 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}/planning.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}) -# -# # Path Player -# add_executable(path_player src/path_player.cpp) -# target_link_libraries(path_player ${catkin_LIBRARIES}) -# add_dependencies (path_player ${catkin_EXPORTED_TARGETS}) -# -# # Path Recorder -# add_executable(path_recorder src/path_recorder.cpp) -# target_link_libraries(path_recorder ${catkin_LIBRARIES}) -# add_dependencies (path_recorder ${catkin_EXPORTED_TARGETS}) diff --git a/deprecated/IndyAV/planning/indyav_path/include/indyav_path/path_recorder.hpp b/deprecated/IndyAV/planning/indyav_path/include/indyav_path/path_recorder.hpp deleted file mode 100644 index 1480e2f1b..000000000 --- a/deprecated/IndyAV/planning/indyav_path/include/indyav_path/path_recorder.hpp +++ /dev/null @@ -1,46 +0,0 @@ -#include - -#include -#include - -#include - -template -class PathRecorder : protected mil_tools::TopicRecorder -{ -private: - ROS_DURATION period_; - ROS_TIME last_save_; - -public: - PathRecorder(ros::NodeHandle* _nh) : mil_tools::TopicRecorder(_nh) - { - double rate; - if (!_nh->getParam("record_rate", rate)) - { - ROS_FATAL("path_recorder, no rate specified"); - } - period_ = ROS_DURATION(1.0 / rate); - last_save_ = ROS_TIME::now(); - } - // TODO: findout what the controller actually needs and place it here - void CallBack(const nav_msgs::Odometry& _msg) - { - if (!enabled_) - { - // check frame id from before be start - if (_msg.header.frame_id != "/ecef") - ROS_WARN("path_recorder frame id is not in ECEF"); - } - else - { - auto now = ROS_TIME::now(); - auto next = last_save_ + period_; - if (next >= now) - return; - ++message_count_; - bag_.write(topic_, ros::Time(now.sec, now.nsec), _msg); - last_save_ = now; - } - } -}; diff --git a/deprecated/IndyAV/planning/indyav_path/package.xml b/deprecated/IndyAV/planning/indyav_path/package.xml deleted file mode 100644 index dc2f28942..000000000 --- a/deprecated/IndyAV/planning/indyav_path/package.xml +++ /dev/null @@ -1,73 +0,0 @@ - - - indyav_path - 0.0.0 - The path package - - - - - MarshallRawson - - - - - - MIT - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - - roscpp - std_msgs - nav_msgs - mil_tools - - - roscpp - std_msgs - - roscpp - std_msgs - nav_msgs - mil_tools - - - - - - - - diff --git a/deprecated/IndyAV/planning/indyav_path/src/path_player.cpp b/deprecated/IndyAV/planning/indyav_path/src/path_player.cpp deleted file mode 100644 index 8f33f10c6..000000000 --- a/deprecated/IndyAV/planning/indyav_path/src/path_player.cpp +++ /dev/null @@ -1,17 +0,0 @@ -#include - -#include -#include - -#include - -int main(int argc, char** argv) -{ - // TODO: findout exactly what the controler would like instead of pulbshing - // every message on the topic that we happened to record - ros::init(argc, argv, "path_recorder"); - ros::NodeHandle nh("~"); - mil_tools::TopicPlayer path_player(&nh); - ros::spin(); - return 0; -} diff --git a/deprecated/IndyAV/planning/indyav_path/src/path_recorder.cpp b/deprecated/IndyAV/planning/indyav_path/src/path_recorder.cpp deleted file mode 100644 index ef0c38029..000000000 --- a/deprecated/IndyAV/planning/indyav_path/src/path_recorder.cpp +++ /dev/null @@ -1,39 +0,0 @@ -#include - -#include - -#include - -/* -periodic odom recorder meant for prerecording and playing back pathes later -*/ -int main(int argc, char** argv) -{ - ros::init(argc, argv, "path_recorder"); - ros::NodeHandle nh("~"); - std::string env = ""; - if (argc > 1) - env = std::string(argv[1]); - else - { - ROS_FATAL("environment arg required"); - return 1; - } - - if (env == "real") - { - PathRecorder path_recorder(&nh); - ros::spin(); - } - else if (env == "gazebo") - { - PathRecorder path_recorder(&nh); - ros::spin(); - } - else - { - ROS_FATAL("environemnt of %s not supported", env.c_str()); - return 1; - } - return 0; -} diff --git a/deprecated/IndyAV/scripts/bash_aliases.sh b/deprecated/IndyAV/scripts/bash_aliases.sh deleted file mode 100755 index 4d48fa51f..000000000 --- a/deprecated/IndyAV/scripts/bash_aliases.sh +++ /dev/null @@ -1,3 +0,0 @@ -#!/bin/bash - -alias indyviz="rviz -d \$MIL_REPO/IndyAV/indyav.rviz" diff --git a/deprecated/IndyAV/simulation/.gitkeep b/deprecated/IndyAV/simulation/.gitkeep deleted file mode 100644 index e69de29bb..000000000 diff --git a/deprecated/IndyAV/simulation/indyav_gazebo/CMakeLists.txt b/deprecated/IndyAV/simulation/indyav_gazebo/CMakeLists.txt deleted file mode 100644 index 4129b7fe2..000000000 --- a/deprecated/IndyAV/simulation/indyav_gazebo/CMakeLists.txt +++ /dev/null @@ -1,118 +0,0 @@ -# cmake_minimum_required(VERSION 2.8.3) -# project(indyav_gazebo) -# -# ## 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 -# indyav_control -# gazebo_ros -# xacro -# ) -# -# ## System dependencies are found with CMake's conventions -# find_package(gazebo REQUIRED) -# link_directories(${GAZEBO_LIBRARY_DIRS}) -# include_directories( -# ${catkin_INCLUDE_DIRS} -# ${GAZEBO_INCLUDE_DIRS} -# include -# ) -# -# -# -# ## 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() -# -# -# ################################### -# ## 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 backwheel_plugin -# CATKIN_DEPENDS indyav_control_generate_message gazebo_ros roscpp -# ) -# -# ########### -# ## Build ## -# ########### -# -# -# add_library( -# backwheel_plugin -# src/backwheel_plugin.cpp -# ) -# target_link_libraries( -# backwheel_plugin -# ${catkin_LIBRARIES} -# ) -# add_dependencies(backwheel_plugin -# include/wheel_plugin.hpp -# indyav_control_generate_message -# ${catkin_EXPORTED_TARGETS} -# ) -# -# ## 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/indyav_gazebo_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} -# # ) -# -# -# # Add new_smyrna speedway generation -# add_custom_target(new_smyrna -# ${CMAKE_CURRENT_SOURCE_DIR}/scripts/generate_racetrack.py ${CMAKE_CURRENT_SOURCE_DIR}/worlds/new_smyrna.yaml ${CMAKE_CURRENT_SOURCE_DIR}/worlds/speedways/new_smyrna_speedway.xacro -# BYPRODUCTS -# ${CMAKE_CURRENT_SOURCE_DIR}/worlds/speedways/new_smyrna_speedway.xacro -# DEPENDS -# ${CMAKE_CURRENT_SOURCE_DIR}/scripts/generate_racetrack.py -# ${CMAKE_CURRENT_SOURCE_DIR}/worlds/new_smyrna.yaml -# USES_TERMINAL -# ) -# xacro_add_files( -# worlds/new_smyrna.world.xacro -# INORDER INSTALL DESTINATION ${CMAKE_CURRENT_DEVEL_DIR}/worlds -# DEPENDS new_smyrna -# ) -# -# -# # add car urdf -# xacro_add_files( -# urdf/indyav_car.urdf.xacro -# INORDER INSTALL DESTINATION urdf -# TARGET xacro_urdf -# ) -# diff --git a/deprecated/IndyAV/simulation/indyav_gazebo/include/indyav_gazebo/backwheel_plugin.hpp b/deprecated/IndyAV/simulation/indyav_gazebo/include/indyav_gazebo/backwheel_plugin.hpp deleted file mode 100644 index 26a8a79b3..000000000 --- a/deprecated/IndyAV/simulation/indyav_gazebo/include/indyav_gazebo/backwheel_plugin.hpp +++ /dev/null @@ -1,27 +0,0 @@ -#pragma once - -#include -#include - -namespace gazebo -{ -class BackWheelPlugin : public WheelPlugin -{ -public: - BackWheelPlugin(); - virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); - virtual void Init(); - virtual void Callback(const indyav_control::RevsStamped& _msg); - virtual void OnUpdate(); - -protected: - std::string back_axle_joint_name_; - physics::JointPtr back_axle_joint_; - - double wheel_rotational_vel_ = 0.0; - double max_wheel_rotational_vel_ = 0.0; - double max_velocity_ = 0.0; -}; - -GZ_REGISTER_MODEL_PLUGIN(BackWheelPlugin) -} diff --git a/deprecated/IndyAV/simulation/indyav_gazebo/include/indyav_gazebo/wheel_plugin.hpp b/deprecated/IndyAV/simulation/indyav_gazebo/include/indyav_gazebo/wheel_plugin.hpp deleted file mode 100644 index 3680551d0..000000000 --- a/deprecated/IndyAV/simulation/indyav_gazebo/include/indyav_gazebo/wheel_plugin.hpp +++ /dev/null @@ -1,35 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include - -namespace gazebo -{ -template -class WheelPlugin : public ModelPlugin -{ -public: - WheelPlugin(); - virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); - virtual void Init() = 0; - virtual void Callback(const MSG& _msg) = 0; - virtual void OnUpdate() = 0; - -protected: - std::vector wheel_names_; - - physics::ModelPtr model_; - - ros::NodeHandle nh_; - - ros::Subscriber sub_; - - event::ConnectionPtr updateConnection_; -}; -} -#include "../src/wheel_plugin.cpp" diff --git a/deprecated/IndyAV/simulation/indyav_gazebo/launch/spawn.launch b/deprecated/IndyAV/simulation/indyav_gazebo/launch/spawn.launch deleted file mode 100644 index 1cf868828..000000000 --- a/deprecated/IndyAV/simulation/indyav_gazebo/launch/spawn.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - diff --git a/deprecated/IndyAV/simulation/indyav_gazebo/launch/swan.launch b/deprecated/IndyAV/simulation/indyav_gazebo/launch/swan.launch deleted file mode 100644 index 89420a8ab..000000000 --- a/deprecated/IndyAV/simulation/indyav_gazebo/launch/swan.launch +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - - - - - - - diff --git a/deprecated/IndyAV/simulation/indyav_gazebo/package.xml b/deprecated/IndyAV/simulation/indyav_gazebo/package.xml deleted file mode 100644 index 0cb0e319a..000000000 --- a/deprecated/IndyAV/simulation/indyav_gazebo/package.xml +++ /dev/null @@ -1,61 +0,0 @@ - - - indyav_gazebo - 0.0.0 - The indyav_gazebo package - - - - - spaceduck - - - - - - TODO - - - - - - - - - - - - - - - - - - - roscpp - - - - - - - - - - - - - - - - indyav_control_generate_message - gazebo_ros - catkin - - - - - - - - diff --git a/deprecated/IndyAV/simulation/indyav_gazebo/scripts/generate_racetrack.py b/deprecated/IndyAV/simulation/indyav_gazebo/scripts/generate_racetrack.py deleted file mode 100755 index 7bbeacd08..000000000 --- a/deprecated/IndyAV/simulation/indyav_gazebo/scripts/generate_racetrack.py +++ /dev/null @@ -1,94 +0,0 @@ -#!/usr/bin/env python3 -import argparse - -import matplotlib.pyplot as plt -import numpy as np -import yaml - -parser = argparse.ArgumentParser( - description="Generates the points down the centerline of a reace track given specified by a yaml file" -) -# get input yaml specifying racetrack -parser.add_argument( - "input_file", help="name of the input yaml file specifying the racetrack" -) -# get output file name -parser.add_argument("output_file", help="location of output xacro file") -# get debug plot or not -parser.add_argument( - "--debug", action="store_true", help="if present, will plot the points of the track" -) - -args = parser.parse_args() - - -def points_as_complex(pose, radius, center_dist, resolution): - azimuth = pose["orientation"]["azimuth"] - orientation = ( - complex( - np.cos((90 - azimuth) * np.pi / 180), np.sin((90 - azimuth) * np.pi / 180) - ) - * 1j - ) - center_of_rotation = complex( - pose["orientation"]["center_of_rotation"]["x"], - pose["orientation"]["center_of_rotation"]["y"], - ) - translation = complex(pose["translation"]["x"], pose["translation"]["y"]) - - points = np.zeros((int(2 * np.pi * radius * resolution),), dtype=np.complex) - steps = np.linspace(0, 2 * np.pi, points.size) - points = np.cos(steps) + np.sin(steps) * 1j - points *= radius - points[: points.size // 2] += center_dist * 1j / 2 - points[points.size // 2 :] -= center_dist * 1j / 2 - points = np.hstack( - ( - points, - points[0], - ) - ) - points += center_of_rotation # set the center of rotation - points *= orientation # rotate all the points - points += translation # translate all the points to be at the right spot in gazebo - if args.debug: - plt.plot(points.real, points.imag) - plt.show() - return points - - -def points_as_str(*args): - points = points_as_complex(*args) - points = np.array([points.real, points.imag]).transpose() - points = np.hstack((points, np.zeros((points.shape[0], 1)))) - ret = "" - for i in range(points.shape[0]): - ret += ( - " " - + str(points[i, 0]) - + " " - + str(points[i, 1]) - + " " - + str(points[i, 2]) - + "\n" - ) - return ret - - -if __name__ == "__main__": - with open(args.input_file) as input_file: - track = yaml.safe_load(input_file) - out = "\n" - out += '\n' - out += ' \n' - out += ' \n' - out += " " + str(track["width"]) + "\n" - out += points_as_str( - track["pose"], track["radius"], track["center_dist"], track["resolution"] - ) - out += " \n" - out += " " - out += "\n" - output_file = open(args.output_file, "w+") - output_file.write(out) - output_file.close() diff --git a/deprecated/IndyAV/simulation/indyav_gazebo/src/backwheel_plugin.cpp b/deprecated/IndyAV/simulation/indyav_gazebo/src/backwheel_plugin.cpp deleted file mode 100644 index 89d3131e3..000000000 --- a/deprecated/IndyAV/simulation/indyav_gazebo/src/backwheel_plugin.cpp +++ /dev/null @@ -1,49 +0,0 @@ -#include - -namespace gazebo -{ -BackWheelPlugin::BackWheelPlugin() : WheelPlugin() -{ -} - -void BackWheelPlugin::Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr _sdf) -{ - WheelPlugin::Load(_model, _sdf); - - GZ_ASSERT(_sdf->HasElement("max_wheel_rotational_vel"), "no max wheel velocity provided"); - max_wheel_rotational_vel_ = _sdf->Get("max_wheel_rotational_vel"); - - GZ_ASSERT(_sdf->HasElement("max_velocity"), "no max velocity provided"); - max_velocity_ = _sdf->Get("max_velocity"); - - GZ_ASSERT(_sdf->HasElement("back_axle_joint"), "no back axle provided"); - back_axle_joint_name_ = _sdf->Get("back_axle_joint"); - back_axle_joint_ = model_->GetJoint(back_axle_joint_name_); -} - -void BackWheelPlugin::Callback(const indyav_control::RevsStamped& _msg) -{ - // TODO: simulate engine input delay - if (abs(_msg.radians_per_second) <= max_wheel_rotational_vel_) - wheel_rotational_vel_ = _msg.radians_per_second; -} - -void BackWheelPlugin::OnUpdate() -{ - // TODO: apply forces on base_link at the offsets of the wheels in varrying amounts - // to replicate a differential torque thingy - if (model_->RelativeLinearVel().X() < max_velocity_) - { - back_axle_joint_->SetVelocity(0, wheel_rotational_vel_); - } - else - { - back_axle_joint_->SetVelocity(0, max_wheel_rotational_vel_); - } -} - -void BackWheelPlugin::Init() -{ - updateConnection_ = event::Events::ConnectWorldUpdateBegin(std::bind(&BackWheelPlugin::OnUpdate, this)); -} -} diff --git a/deprecated/IndyAV/simulation/indyav_gazebo/src/wheel_plugin.cpp b/deprecated/IndyAV/simulation/indyav_gazebo/src/wheel_plugin.cpp deleted file mode 100644 index 184a7bb1f..000000000 --- a/deprecated/IndyAV/simulation/indyav_gazebo/src/wheel_plugin.cpp +++ /dev/null @@ -1,33 +0,0 @@ - -namespace gazebo -{ -template -WheelPlugin::WheelPlugin() -{ -} - -template -void WheelPlugin::Load(gazebo::physics::ModelPtr _model, sdf::ElementPtr _sdf) -{ - GZ_ASSERT(_model != NULL, "Model is NULL"); - - model_ = _model; - - GZ_ASSERT(_sdf != NULL, "SDF is NULL"); - - GZ_ASSERT(ros::isInitialized(), "ROS not initialized"); - - GZ_ASSERT(_sdf->HasElement("wheels"), "No Wheels specified"); - unsigned int i = 0; - auto wheels = _sdf->GetElement("wheels"); - while (wheels->HasElement("wheel_" + std::to_string(i))) - { - wheel_names_.push_back(wheels->Get("wheel_" + std::to_string(i))); - ++i; - } - - GZ_ASSERT(_sdf->HasElement("topic_name"), "WheelPlugin: no topic_name provided"); - std::string topic_name = _sdf->Get("topic_name"); - sub_ = nh_.subscribe(topic_name, 1, &WheelPlugin::Callback, this); -} -} diff --git a/deprecated/IndyAV/simulation/indyav_gazebo/urdf/indyav_car.urdf.xacro b/deprecated/IndyAV/simulation/indyav_gazebo/urdf/indyav_car.urdf.xacro deleted file mode 100644 index 9d6e12cc3..000000000 --- a/deprecated/IndyAV/simulation/indyav_gazebo/urdf/indyav_car.urdf.xacro +++ /dev/null @@ -1,47 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - back_axle_link_to_back_right_wheel_link_joint - back_axle_link_to_back_left_wheel_link_joint - - base_link_to_back_axle_link_joint - 1000 - 20 - throttle - - - - - - - - diff --git a/deprecated/IndyAV/simulation/indyav_gazebo/worlds/new_smyrna.world.xacro b/deprecated/IndyAV/simulation/indyav_gazebo/worlds/new_smyrna.world.xacro deleted file mode 100644 index 9e9e92e19..000000000 --- a/deprecated/IndyAV/simulation/indyav_gazebo/worlds/new_smyrna.world.xacro +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - diff --git a/deprecated/IndyAV/simulation/indyav_gazebo/worlds/new_smyrna.xacro b/deprecated/IndyAV/simulation/indyav_gazebo/worlds/new_smyrna.xacro deleted file mode 100644 index aac8ecaa9..000000000 --- a/deprecated/IndyAV/simulation/indyav_gazebo/worlds/new_smyrna.xacro +++ /dev/null @@ -1,56 +0,0 @@ - - - - - - - - - - 12 - - - 0 - 0 - - - - EARTH_WGS84 - ENU - 29.013809 - -81.070471 - - 11.70432 - - - - - 0 0 0 0 0 0 - - - - - 0 0 1 - - - - - - 100 - - - - - - true - - - - - - - model://sun - - - diff --git a/deprecated/IndyAV/simulation/indyav_gazebo/worlds/new_smyrna.yaml b/deprecated/IndyAV/simulation/indyav_gazebo/worlds/new_smyrna.yaml deleted file mode 100644 index f16b15066..000000000 --- a/deprecated/IndyAV/simulation/indyav_gazebo/worlds/new_smyrna.yaml +++ /dev/null @@ -1,18 +0,0 @@ -track_name: 'new_smyrna_speedway' -width: 16.46 -radius: 64.0 -center_dist: 200.0 -resolution: 1 -# deterimined with https://www.acscdg.com -pose: - orientation: - azimuth: 187.0 # the heading of the first straight away in degrees deflected to the right from North - center_of_rotation: # the point around which the track will be rotated relative to the track's center - x: 64.0 - y: 0.0 - translation: # where the center of rotation of the track is in the frame of the gazebo simulation - x: 0.0 - y: 0.0 - - - diff --git a/deprecated/IndyAV/simulation/indyav_gazebo/worlds/speedways/.gitkeep b/deprecated/IndyAV/simulation/indyav_gazebo/worlds/speedways/.gitkeep deleted file mode 100644 index e69de29bb..000000000 diff --git a/deprecated/IndyAV/simulation/indyav_gazebo/xacro/axles.xacro b/deprecated/IndyAV/simulation/indyav_gazebo/xacro/axles.xacro deleted file mode 100644 index 9726b314c..000000000 --- a/deprecated/IndyAV/simulation/indyav_gazebo/xacro/axles.xacro +++ /dev/null @@ -1,56 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/deprecated/IndyAV/simulation/indyav_gazebo/xacro/back_wheel.xacro b/deprecated/IndyAV/simulation/indyav_gazebo/xacro/back_wheel.xacro deleted file mode 100644 index 9ec2ae62a..000000000 --- a/deprecated/IndyAV/simulation/indyav_gazebo/xacro/back_wheel.xacro +++ /dev/null @@ -1,48 +0,0 @@ - - - - - - - - - - - - - - 1000 - 1.0 - 0 1 0 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/deprecated/IndyAV/simulation/indyav_gazebo/xacro/go_kart.xacro b/deprecated/IndyAV/simulation/indyav_gazebo/xacro/go_kart.xacro deleted file mode 100644 index 7bd9ea01a..000000000 --- a/deprecated/IndyAV/simulation/indyav_gazebo/xacro/go_kart.xacro +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/deprecated/IndyAV/simulation/indyav_gazebo/xacro/wheels.xacro b/deprecated/IndyAV/simulation/indyav_gazebo/xacro/wheels.xacro deleted file mode 100644 index eb10b75fa..000000000 --- a/deprecated/IndyAV/simulation/indyav_gazebo/xacro/wheels.xacro +++ /dev/null @@ -1,102 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - 500 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - From 36d9e1b563a652ba730ae707c795d717c70d3246 Mon Sep 17 00:00:00 2001 From: Daniel Parra Date: Tue, 24 Oct 2023 14:27:59 -0400 Subject: [PATCH 2/3] Reduced error by 50 percent --- .../vrx_missions/vrx_wayfinding_2.py | 24 +++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/NaviGator/mission_control/navigator_missions/vrx_missions/vrx_wayfinding_2.py b/NaviGator/mission_control/navigator_missions/vrx_missions/vrx_wayfinding_2.py index 60496d0b8..2b0eb0b86 100644 --- a/NaviGator/mission_control/navigator_missions/vrx_missions/vrx_wayfinding_2.py +++ b/NaviGator/mission_control/navigator_missions/vrx_missions/vrx_wayfinding_2.py @@ -5,6 +5,7 @@ from .vrx import Vrx ___author___ = "Alex Perez" +# Optimized by Daniel Parra class VrxWayfinding2(Vrx): @@ -37,11 +38,17 @@ async def run(self, parameters): for j in range(array_size): dist_matrix[i][j] = np.linalg.norm(poses[i][0] - poses[j][0]) + #self.send_feedback(f"Distance Matrix:\n{dist_matrix}") + # solve tsp algorithm (ensure start point is where boat is located) & remove current position from pose list path = solve_tsp(dist_matrix, endpoints=(start_pose_index, None)) poses = poses[:start_pose_index] path = path[1:] + #self.send_feedback(f"Poses:\n{poses}") + + #self.send_feedback(f"Path:\n{path}") + # self.send_feedback('Sorted poses' + str(poses)) await self.wait_for_task_such_that(lambda task: task.state in ["running"]) @@ -49,5 +56,22 @@ async def run(self, parameters): for index in path: self.send_feedback(f"Going to {poses[index]}") + #path_msg = await self.get_latching_msg(self.wayfinding_path_sub) + #self.send_feedback(f"Message:\n{path_msg}") # Go to goal + + # Percent Radius of Adjustment + P = 0.85 + part_way_point = [x * P for x in poses[index][0][:-1]] + # Partial Axis Approach + # part_way_point[1] = poses[index][0][1] + + # Constant Radius of Adjustment + # R = 10 + # part_way_point = [(x - R if x > 0 else (x + R if x < 0 else x)) for x in poses[index][0][:-1]] + + part_way_point.append(poses[index][0][-1]) + self.send_feedback(f"\nPartway:\n{part_way_point}\nEndPoint:\n{poses[index][0]}") + + await self.send_trajectory_without_path([part_way_point, poses[index][1]]) await self.send_trajectory_without_path(poses[index]) From 6b2e076a1571234bcd308499eaa282dc741f888a Mon Sep 17 00:00:00 2001 From: Daniel Parra Date: Sat, 28 Oct 2023 17:20:01 -0400 Subject: [PATCH 3/3] reformatted file --- .../vrx_missions/vrx_wayfinding_2.py | 25 +++---------------- 1 file changed, 4 insertions(+), 21 deletions(-) diff --git a/NaviGator/mission_control/navigator_missions/vrx_missions/vrx_wayfinding_2.py b/NaviGator/mission_control/navigator_missions/vrx_missions/vrx_wayfinding_2.py index 2b0eb0b86..966f38be0 100644 --- a/NaviGator/mission_control/navigator_missions/vrx_missions/vrx_wayfinding_2.py +++ b/NaviGator/mission_control/navigator_missions/vrx_missions/vrx_wayfinding_2.py @@ -38,40 +38,23 @@ async def run(self, parameters): for j in range(array_size): dist_matrix[i][j] = np.linalg.norm(poses[i][0] - poses[j][0]) - #self.send_feedback(f"Distance Matrix:\n{dist_matrix}") - # solve tsp algorithm (ensure start point is where boat is located) & remove current position from pose list path = solve_tsp(dist_matrix, endpoints=(start_pose_index, None)) poses = poses[:start_pose_index] path = path[1:] - #self.send_feedback(f"Poses:\n{poses}") - - #self.send_feedback(f"Path:\n{path}") - - # self.send_feedback('Sorted poses' + str(poses)) await self.wait_for_task_such_that(lambda task: task.state in ["running"]) # do movements for index in path: self.send_feedback(f"Going to {poses[index]}") - - #path_msg = await self.get_latching_msg(self.wayfinding_path_sub) - #self.send_feedback(f"Message:\n{path_msg}") # Go to goal - - # Percent Radius of Adjustment - P = 0.85 + P = 0.85 part_way_point = [x * P for x in poses[index][0][:-1]] - # Partial Axis Approach - # part_way_point[1] = poses[index][0][1] - - # Constant Radius of Adjustment - # R = 10 - # part_way_point = [(x - R if x > 0 else (x + R if x < 0 else x)) for x in poses[index][0][:-1]] - part_way_point.append(poses[index][0][-1]) - self.send_feedback(f"\nPartway:\n{part_way_point}\nEndPoint:\n{poses[index][0]}") + self.send_feedback( + f"\nPartway:\n{part_way_point}\nEndPoint:\n{poses[index][0]}", + ) await self.send_trajectory_without_path([part_way_point, poses[index][1]]) await self.send_trajectory_without_path(poses[index])