From 5fffe48d83375e4e3cfc47b672f5168a3abe3a86 Mon Sep 17 00:00:00 2001 From: matthewhannay567 Date: Sun, 23 Jan 2022 17:37:15 -0500 Subject: [PATCH 01/27] Modified values for teb --- .../config/teb_local_planner_params.yaml | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/igvc_navigation/config/teb_local_planner_params.yaml b/igvc_navigation/config/teb_local_planner_params.yaml index 9c8d23696..78ebdacde 100644 --- a/igvc_navigation/config/teb_local_planner_params.yaml +++ b/igvc_navigation/config/teb_local_planner_params.yaml @@ -20,16 +20,19 @@ TebLocalPlannerROS: # Robot - max_vel_x: 1.6 - max_vel_x_backwards: 0.8 - max_vel_y: 0.0 - max_vel_theta: 2.5 + max_vel_x: 2.0 + max_vel_x_backwards: 2.0 + max_vel_y: 2.0 + max_vel_theta: 4.0 acc_lim_x: 0.4 + acc_lim_y: 0.4 acc_lim_theta: 2.0 min_turning_radius: 0.0 # diff-drive robot (can turn on place!) footprint_model: - type: "point" + type: "line" + line_start: [-0.2, 0] + line_end: [0.2, 0] # GoalTolerance @@ -40,7 +43,7 @@ TebLocalPlannerROS: # Obstacles - min_obstacle_dist: 0.7 # This value must also include our robot radius, since footprint_model is set to "point". + min_obstacle_dist: 0.45 inflation_dist: 0.0 include_costmap_obstacles: True costmap_obstacles_behind_robot_dist: 1.5 @@ -71,7 +74,7 @@ TebLocalPlannerROS: weight_max_vel_theta: 1 weight_acc_lim_x: 100 weight_acc_lim_theta: 1 - weight_kinematics_nh: 1000 + weight_kinematics_nh: 1 weight_kinematics_forward_drive: 100 weight_kinematics_turning_radius: 1 weight_optimaltime: 1 # must be > 0 From beb6be6a7ba4753536e6e5eda06dc19feff5ec3e Mon Sep 17 00:00:00 2001 From: matthewhannay567 Date: Sun, 23 Jan 2022 18:33:02 -0500 Subject: [PATCH 02/27] Split teb parameters into two files --- .../teb_local_planner_params_jessi.yaml | 117 ++++++++++++++++++ ...l => teb_local_planner_params_swervi.yaml} | 0 igvc_navigation/launch/mbf_navigation.launch | 59 ++++++--- .../launch/navigation_simulation.launch | 6 +- 4 files changed, 164 insertions(+), 18 deletions(-) create mode 100644 igvc_navigation/config/teb_local_planner_params_jessi.yaml rename igvc_navigation/config/{teb_local_planner_params.yaml => teb_local_planner_params_swervi.yaml} (100%) diff --git a/igvc_navigation/config/teb_local_planner_params_jessi.yaml b/igvc_navigation/config/teb_local_planner_params_jessi.yaml new file mode 100644 index 000000000..9c8d23696 --- /dev/null +++ b/igvc_navigation/config/teb_local_planner_params_jessi.yaml @@ -0,0 +1,117 @@ +TebLocalPlannerROS: + odom_topic: /odometry/filtered + map_frame: /odom + + # Trajectory + + teb_autosize: True + dt_ref: 0.2 + dt_hysteresis: 0.02 + max_samples: 500 + global_plan_overwrite_orientation: True + allow_init_with_backwards_motion: False + max_global_plan_lookahead_dist: 5.0 + global_plan_viapoint_sep: -1 + global_plan_prune_distance: 1 + exact_arc_length: False + feasibility_check_no_poses: 4 + publish_feedback: True + control_look_ahead_poses: 2 + + # Robot + + max_vel_x: 1.6 + max_vel_x_backwards: 0.8 + max_vel_y: 0.0 + max_vel_theta: 2.5 + acc_lim_x: 0.4 + acc_lim_theta: 2.0 + min_turning_radius: 0.0 # diff-drive robot (can turn on place!) + + footprint_model: + type: "point" + + # GoalTolerance + + xy_goal_tolerance: 0.2 + yaw_goal_tolerance: 1.57 + free_goal_vel: False + complete_global_plan: True + + # Obstacles + + min_obstacle_dist: 0.7 # This value must also include our robot radius, since footprint_model is set to "point". + inflation_dist: 0.0 + include_costmap_obstacles: True + costmap_obstacles_behind_robot_dist: 1.5 + obstacle_poses_affected: 15 + + dynamic_obstacle_inflation_dist: 0.0 + include_dynamic_obstacles: False + + costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH" + costmap_converter_spin_thread: True + costmap_converter_rate: 2 + + costmap_converter/CostmapToPolygonsDBSMCCH: + cluster_max_distance: 0.4 + cluster_min_pts: 3 + cluster_max_pts: 30 + convex_hull_min_pt_separation: 0.1 + + # Optimization + + no_inner_iterations: 5 + no_outer_iterations: 5 + optimization_activate: True + optimization_verbose: False + penalty_epsilon: 0.1 + obstacle_cost_exponent: 2 + weight_max_vel_x: 100 + weight_max_vel_theta: 1 + weight_acc_lim_x: 100 + weight_acc_lim_theta: 1 + weight_kinematics_nh: 1000 + weight_kinematics_forward_drive: 100 + weight_kinematics_turning_radius: 1 + weight_optimaltime: 1 # must be > 0 + weight_shortest_path: 0 + weight_obstacle: 100 + weight_inflation: 90 + weight_dynamic_obstacle: 10 + weight_dynamic_obstacle_inflation: 0.2 + weight_viapoint: 0.01 + weight_adapt_factor: 2 + + # Homotopy Class Planner + + enable_homotopy_class_planning: False +# enable_multithreading: True +# max_number_classes: 4 +# selection_cost_hysteresis: 1.0 +# selection_prefer_initial_plan: 0.9 +# selection_obst_cost_scale: 100.0 +# selection_alternative_time_cost: False +# +# roadmap_graph_no_samples: 15 +# roadmap_graph_area_width: 5 +# roadmap_graph_area_length_scale: 1.0 +# h_signature_prescaler: 0.5 +# h_signature_threshold: 0.1 +# obstacle_heading_threshold: 0.45 +# switching_blocking_period: 0.0 +# viapoints_all_candidates: True +# delete_detours_backwards: True +# max_ratio_detours_duration_best_duration: 3.0 +# visualize_hc_graph: False +# visualize_with_time_as_z_axis_scale: False + + # Recovery + + shrink_horizon_backup: True + shrink_horizon_min_duration: 10 + oscillation_recovery: True + oscillation_v_eps: 0.1 + oscillation_omega_eps: 0.1 + oscillation_recovery_min_duration: 10 + oscillation_filter_duration: 10 diff --git a/igvc_navigation/config/teb_local_planner_params.yaml b/igvc_navigation/config/teb_local_planner_params_swervi.yaml similarity index 100% rename from igvc_navigation/config/teb_local_planner_params.yaml rename to igvc_navigation/config/teb_local_planner_params_swervi.yaml diff --git a/igvc_navigation/launch/mbf_navigation.launch b/igvc_navigation/launch/mbf_navigation.launch index 07de5ba94..84818387b 100644 --- a/igvc_navigation/launch/mbf_navigation.launch +++ b/igvc_navigation/launch/mbf_navigation.launch @@ -1,20 +1,45 @@ + + + - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/igvc_navigation/launch/navigation_simulation.launch b/igvc_navigation/launch/navigation_simulation.launch index 9283ef056..0192786ec 100644 --- a/igvc_navigation/launch/navigation_simulation.launch +++ b/igvc_navigation/launch/navigation_simulation.launch @@ -1,11 +1,15 @@ + + - + + + From 2f1bcd7914849a1c3686980180b1f719edf5d759 Mon Sep 17 00:00:00 2001 From: matthewhannay567 Date: Mon, 24 Jan 2022 13:05:36 -0500 Subject: [PATCH 03/27] Fixed footprint in costmap_params --- igvc_navigation/config/global_costmap_params.yaml | 2 +- igvc_navigation/config/local_costmap_params.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/igvc_navigation/config/global_costmap_params.yaml b/igvc_navigation/config/global_costmap_params.yaml index 275af011e..0ccbc96a5 100644 --- a/igvc_navigation/config/global_costmap_params.yaml +++ b/igvc_navigation/config/global_costmap_params.yaml @@ -7,7 +7,7 @@ global_costmap: - {name: back_circle_layer, type: "back_circle_layer::BackCircleLayer"} - {name: inflation_layer, type: "costmap_2d::InflationLayer"} publish_frequency: 5.0 - footprint: [[-0.24,0.32],[0.72,0.32],[0.72,-0.32],[-0.24,-0.32]] + footprint: [[-0.4318,0.3048],[0.4318,0.3048],[0.4318,-0.3048],[-0.4318,-0.3048]] width: 200 height: 200 origin_x: -100 diff --git a/igvc_navigation/config/local_costmap_params.yaml b/igvc_navigation/config/local_costmap_params.yaml index aff313593..d62853d46 100644 --- a/igvc_navigation/config/local_costmap_params.yaml +++ b/igvc_navigation/config/local_costmap_params.yaml @@ -4,7 +4,7 @@ local_costmap: plugins: - {name: rolling_layer, type: "rolling_layer::RollingLayer"} publish_frequency: 5.0 - footprint: [[-0.24,0.32],[0.72,0.32],[0.72,-0.32],[-0.24,-0.32]] + footprint: [[-0.4318,0.3048],[0.4318,0.3048],[0.4318,-0.3048],[-0.4318,-0.3048]] width: 20 height: 20 origin_x: -10 From 10a476214bf98b45a7daffe58b96dafe0283bde0 Mon Sep 17 00:00:00 2001 From: matthewhannay567 Date: Mon, 24 Jan 2022 13:10:20 -0500 Subject: [PATCH 04/27] Split global and local costmap params --- .../config/global_costmap_params_jessi.yaml | 103 ++++++++++++++++++ ...yaml => global_costmap_params_swervi.yaml} | 0 .../config/local_costmap_params_jessi.yaml | 19 ++++ ....yaml => local_costmap_params_swervi.yaml} | 0 igvc_navigation/launch/mbf_navigation.launch | 8 +- 5 files changed, 126 insertions(+), 4 deletions(-) create mode 100644 igvc_navigation/config/global_costmap_params_jessi.yaml rename igvc_navigation/config/{global_costmap_params.yaml => global_costmap_params_swervi.yaml} (100%) create mode 100644 igvc_navigation/config/local_costmap_params_jessi.yaml rename igvc_navigation/config/{local_costmap_params.yaml => local_costmap_params_swervi.yaml} (100%) diff --git a/igvc_navigation/config/global_costmap_params_jessi.yaml b/igvc_navigation/config/global_costmap_params_jessi.yaml new file mode 100644 index 000000000..275af011e --- /dev/null +++ b/igvc_navigation/config/global_costmap_params_jessi.yaml @@ -0,0 +1,103 @@ +global_costmap: + global_frame: odom + robot_base_frame: base_link + plugins: + - {name: traversability_layer, type: "traversability_layer::TraversabilityLayer"} + - {name: line_layer, type: "line_layer::LineLayer"} + - {name: back_circle_layer, type: "back_circle_layer::BackCircleLayer"} + - {name: inflation_layer, type: "costmap_2d::InflationLayer"} + publish_frequency: 5.0 + footprint: [[-0.24,0.32],[0.72,0.32],[0.72,-0.32],[-0.24,-0.32]] + width: 200 + height: 200 + origin_x: -100 + origin_y: -100 + update_frequency: 10 + track_unknown_space: false + unknown_cost_value: 30 + rolling_window: false + resolution: 0.1 + always_send_full_costmap: true + inflation_layer: + inflation_radius: 5.0 +line_layer: + map: + frame_id: "odom" + resolution: 0.1 + length_x: 200 + length_y: 200 + occupied_threshold: 0.5 + max_occupancy: 0.99 + min_occupancy: 0.01 + costmap_topic: "/line_layer/costmap" + debug: + map_topic: "/mapper/debug/lines/gridmap" + enabled: false + /cam/center: + topics: + raw_image_ns: "/raw" + raw_image: "/image" + segmented_image_ns: "/segmented" + segmented_image: "/image" + max_distance: 10 + probabilities: + miss: 0.7 + hit: 0.7 + miss_exponential_coeff: 0.1 + miss_angle_exponential_coeff: 5 + hit_exponential_coeff: 0.1 + debug: + line_topic: "/line_layer/debug/center/line" + nonline_topic: "/line_layer/debug/center/nonline" + /cam/left: + topics: + raw_image_ns: "/raw" + raw_image: "/image" + segmented_image_ns: "/segmented" + segmented_image: "/image" + max_distance: 10 + probabilities: + miss: 0.7 + hit: 0.7 + miss_exponential_coeff: 0.05 + miss_angle_exponential_coeff: 5 + hit_exponential_coeff: 0.05 + debug: + line_topic: "/line_layer/debug/left/line" + nonline_topic: "/line_layer/debug/left/nonline" + /cam/right: + topics: + raw_image_ns: "/raw" + raw_image: "/image" + segmented_image_ns: "/segmented" + segmented_image: "/image" + max_distance: 10 + probabilities: + miss: 0.7 + hit: 0.7 + miss_exponential_coeff: 0.05 + miss_angle_exponential_coeff: 5 + hit_exponential_coeff: 0.05 + debug: + line_topic: "/line_layer/debug/right/line" + nonline_topic: "/line_layer/debug/right/nonline" + projection: + length_x: 30 + length_y: 30 + line_closing_kernel_size: 5 + freespace_closing_kernel_size: 5 +traversability_layer: + map: + frame_id: "odom" + resolution: 0.1 + length_x: 200 + length_y: 200 + occupied_threshold: 0.5 + max_occupancy: 0.99 + min_occupancy: 0.01 + costmap_topic: "/slope/costmap" + debug: + map_topic: "/slope/debug" + enabled: false + untraversable_probability: 0.8 + slope_threshold: 0.45 diff --git a/igvc_navigation/config/global_costmap_params.yaml b/igvc_navigation/config/global_costmap_params_swervi.yaml similarity index 100% rename from igvc_navigation/config/global_costmap_params.yaml rename to igvc_navigation/config/global_costmap_params_swervi.yaml diff --git a/igvc_navigation/config/local_costmap_params_jessi.yaml b/igvc_navigation/config/local_costmap_params_jessi.yaml new file mode 100644 index 000000000..aff313593 --- /dev/null +++ b/igvc_navigation/config/local_costmap_params_jessi.yaml @@ -0,0 +1,19 @@ +local_costmap: + global_frame: odom + robot_base_frame: base_link + plugins: + - {name: rolling_layer, type: "rolling_layer::RollingLayer"} + publish_frequency: 5.0 + footprint: [[-0.24,0.32],[0.72,0.32],[0.72,-0.32],[-0.24,-0.32]] + width: 20 + height: 20 + origin_x: -10 + origin_y: -10 + update_frequency: 10 + track_unknown_space: false + unknown_cost_value: 30 + static_map: false + rolling_window: true + resolution: 0.1 +rolling_layer: + topic: "/move_base_flex/global_costmap/costmap" diff --git a/igvc_navigation/config/local_costmap_params.yaml b/igvc_navigation/config/local_costmap_params_swervi.yaml similarity index 100% rename from igvc_navigation/config/local_costmap_params.yaml rename to igvc_navigation/config/local_costmap_params_swervi.yaml diff --git a/igvc_navigation/launch/mbf_navigation.launch b/igvc_navigation/launch/mbf_navigation.launch index 84818387b..e57ee8675 100644 --- a/igvc_navigation/launch/mbf_navigation.launch +++ b/igvc_navigation/launch/mbf_navigation.launch @@ -13,8 +13,8 @@ - - + + @@ -33,8 +33,8 @@ - - + + From c6e9d8a24e7604f21b1bf59669e623549107357d Mon Sep 17 00:00:00 2001 From: matthewhannay567 Date: Wed, 26 Jan 2022 19:42:14 -0500 Subject: [PATCH 05/27] Fixed nav_sim not using swerve drive --- igvc_navigation/launch/navigation_simulation.launch | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/igvc_navigation/launch/navigation_simulation.launch b/igvc_navigation/launch/navigation_simulation.launch index 0192786ec..cbf11b026 100644 --- a/igvc_navigation/launch/navigation_simulation.launch +++ b/igvc_navigation/launch/navigation_simulation.launch @@ -22,8 +22,13 @@ - - + + + + + + + From 8e949a5c6aab44442387c8e4dc35c26906e9e8b5 Mon Sep 17 00:00:00 2001 From: matthewhannay567 Date: Sun, 30 Jan 2022 16:42:38 -0500 Subject: [PATCH 06/27] Tuned TEB params --- .../config/teb_local_planner_params_swervi.yaml | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/igvc_navigation/config/teb_local_planner_params_swervi.yaml b/igvc_navigation/config/teb_local_planner_params_swervi.yaml index 78ebdacde..8a3319793 100644 --- a/igvc_navigation/config/teb_local_planner_params_swervi.yaml +++ b/igvc_navigation/config/teb_local_planner_params_swervi.yaml @@ -27,7 +27,7 @@ TebLocalPlannerROS: acc_lim_x: 0.4 acc_lim_y: 0.4 acc_lim_theta: 2.0 - min_turning_radius: 0.0 # diff-drive robot (can turn on place!) + min_turning_radius: 0.0 # swerve-drive robot (can turn on place!) footprint_model: type: "line" @@ -70,12 +70,12 @@ TebLocalPlannerROS: optimization_verbose: False penalty_epsilon: 0.1 obstacle_cost_exponent: 2 - weight_max_vel_x: 100 + weight_max_vel_x: 1 weight_max_vel_theta: 1 - weight_acc_lim_x: 100 + weight_acc_lim_x: 1 weight_acc_lim_theta: 1 - weight_kinematics_nh: 1 - weight_kinematics_forward_drive: 100 + weight_kinematics_nh: 0.01 + weight_kinematics_forward_drive: 1 weight_kinematics_turning_radius: 1 weight_optimaltime: 1 # must be > 0 weight_shortest_path: 0 From f283bf749dff4ac356c4e1276b2491a8a07a7286 Mon Sep 17 00:00:00 2001 From: matthewhannay567 Date: Sun, 6 Feb 2022 17:39:33 -0500 Subject: [PATCH 07/27] Removed Jessiii files --- igvc_description/launch/spawn_jessii.launch | 34 ---- igvc_gazebo/CMakeLists.txt | 1 - igvc_gazebo/launch/igvc_control.launch | 25 --- igvc_gazebo/launch/simulation.launch | 46 ++--- igvc_gazebo/launch/simulation_low.launch | 46 ++--- igvc_gazebo/nodes/control/CMakeLists.txt | 10 - igvc_gazebo/nodes/control/main.cpp | 176 ------------------ igvc_msgs/CMakeLists.txt | 2 - igvc_msgs/msg/action_path.msg | 2 - igvc_msgs/msg/velocity_pair.msg | 4 - igvc_navigation/CMakeLists.txt | 1 - ...swervi.yaml => global_costmap_params.yaml} | 0 .../config/global_costmap_params_jessi.yaml | 103 ---------- ..._swervi.yaml => local_costmap_params.yaml} | 0 .../config/local_costmap_params_jessi.yaml | 19 -- ...rvi.yaml => teb_local_planner_params.yaml} | 0 .../teb_local_planner_params_jessi.yaml | 117 ------------ igvc_navigation/launch/mbf_navigation.launch | 60 ++---- .../launch/navigation_simulation.launch | 7 +- .../src/differential_drive/CMakeLists.txt | 10 - .../differential_drive/differential_drive.cpp | 43 ----- .../differential_drive/differential_drive.h | 27 --- 22 files changed, 42 insertions(+), 691 deletions(-) delete mode 100644 igvc_description/launch/spawn_jessii.launch delete mode 100644 igvc_gazebo/launch/igvc_control.launch delete mode 100644 igvc_gazebo/nodes/control/CMakeLists.txt delete mode 100644 igvc_gazebo/nodes/control/main.cpp delete mode 100644 igvc_msgs/msg/action_path.msg delete mode 100644 igvc_msgs/msg/velocity_pair.msg rename igvc_navigation/config/{global_costmap_params_swervi.yaml => global_costmap_params.yaml} (100%) delete mode 100644 igvc_navigation/config/global_costmap_params_jessi.yaml rename igvc_navigation/config/{local_costmap_params_swervi.yaml => local_costmap_params.yaml} (100%) delete mode 100644 igvc_navigation/config/local_costmap_params_jessi.yaml rename igvc_navigation/config/{teb_local_planner_params_swervi.yaml => teb_local_planner_params.yaml} (100%) delete mode 100644 igvc_navigation/config/teb_local_planner_params_jessi.yaml delete mode 100644 igvc_navigation/src/differential_drive/CMakeLists.txt delete mode 100644 igvc_navigation/src/differential_drive/differential_drive.cpp delete mode 100644 igvc_navigation/src/differential_drive/differential_drive.h diff --git a/igvc_description/launch/spawn_jessii.launch b/igvc_description/launch/spawn_jessii.launch deleted file mode 100644 index 34b1b0311..000000000 --- a/igvc_description/launch/spawn_jessii.launch +++ /dev/null @@ -1,34 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/igvc_gazebo/CMakeLists.txt b/igvc_gazebo/CMakeLists.txt index 6be28971a..8a0b91ee1 100644 --- a/igvc_gazebo/CMakeLists.txt +++ b/igvc_gazebo/CMakeLists.txt @@ -47,7 +47,6 @@ install(DIRECTORY config/ ) add_subdirectory(nodes/magnetometer) -add_subdirectory(nodes/control) add_subdirectory(nodes/swerve_control) add_subdirectory(nodes/scan_to_pointcloud) add_subdirectory(nodes/ground_truth) diff --git a/igvc_gazebo/launch/igvc_control.launch b/igvc_gazebo/launch/igvc_control.launch deleted file mode 100644 index 1f9af7338..000000000 --- a/igvc_gazebo/launch/igvc_control.launch +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/igvc_gazebo/launch/simulation.launch b/igvc_gazebo/launch/simulation.launch index eac1002ac..ef220086b 100644 --- a/igvc_gazebo/launch/simulation.launch +++ b/igvc_gazebo/launch/simulation.launch @@ -37,41 +37,19 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + - - - + diff --git a/igvc_gazebo/launch/simulation_low.launch b/igvc_gazebo/launch/simulation_low.launch index 9357a7802..297cc8d8e 100644 --- a/igvc_gazebo/launch/simulation_low.launch +++ b/igvc_gazebo/launch/simulation_low.launch @@ -37,41 +37,19 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + - - - + diff --git a/igvc_gazebo/nodes/control/CMakeLists.txt b/igvc_gazebo/nodes/control/CMakeLists.txt deleted file mode 100644 index e22cf4a9d..000000000 --- a/igvc_gazebo/nodes/control/CMakeLists.txt +++ /dev/null @@ -1,10 +0,0 @@ -add_executable(control main.cpp) -add_dependencies(control ${catkin_EXPORTED_TARGETS}) -target_link_libraries(control ${catkin_LIBRARIES}) - -install( - TARGETS control - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/igvc_gazebo/nodes/control/main.cpp b/igvc_gazebo/nodes/control/main.cpp deleted file mode 100644 index 0ea0b64ad..000000000 --- a/igvc_gazebo/nodes/control/main.cpp +++ /dev/null @@ -1,176 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include - -double speed_set_point_left = 0.0; -double speed_set_point_right = 0.0; -double speed_measured_left = 0.0; -double speed_measured_right = 0.0; -double wheel_radius; - -void speedCallback(const igvc_msgs::velocity_pair::ConstPtr &msg) -{ - if (msg->left_velocity == msg->left_velocity) - { - speed_set_point_left = msg->left_velocity; - } - if (msg->right_velocity == msg->right_velocity) - { - speed_set_point_right = msg->right_velocity; - } -} - -void jointStateCallback(const sensor_msgs::JointStateConstPtr &msg) -{ - auto iter = std::find(msg->name.begin(), msg->name.end(), std::string{ "left_axle" }); - - if (iter != msg->name.end()) - { - auto index = std::distance(msg->name.begin(), iter); - - speed_measured_left = (msg->velocity[index]) * (wheel_radius); - } - - iter = std::find(msg->name.begin(), msg->name.end(), std::string{ "right_axle" }); - - if (iter != msg->name.end()) - { - auto index = std::distance(msg->name.begin(), iter); - - speed_measured_right = (msg->velocity[index]) * (wheel_radius); - } - - // ROS_INFO_STREAM("right speed: " << speed_measured_right << " left speed: " << speed_measured_left); -} - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "jessii_controller"); - - ros::NodeHandle handle; - ros::NodeHandle pNh("~"); - - ros::Publisher left_wheel_effort_publisher = - handle.advertise("/left_wheel_effort_controller/command", 1); - ros::Publisher right_wheel_effort_publisher = - handle.advertise("/right_wheel_effort_controller/command", 1); - ros::Publisher wheel_speed_publisher = handle.advertise("/encoders", 1); - - ros::Publisher right_wheel_shock_publisher = - handle.advertise("right_wheel_shock_controller/command", 1, true); - ros::Publisher left_wheel_shock_publisher = - handle.advertise("left_wheel_shock_controller/command", 1, true); - - std_msgs::Float64 shock_set_point; - shock_set_point.data = 0.0; - right_wheel_shock_publisher.publish(shock_set_point); - left_wheel_shock_publisher.publish(shock_set_point); - - double speed_P_left, speed_P_right, speed_D_left, speed_D_right, speed_I_left, speed_I_right; - double rate_var; - double max_effort = 0.0; - - assertions::param(pNh, "speed_P_left", speed_P_left, 5.0); - assertions::param(pNh, "speed_P_right", speed_P_right, 5.0); - assertions::param(pNh, "speed_D_left", speed_D_left, 1.0); - assertions::param(pNh, "speed_D_right", speed_D_right, 1.0); - assertions::param(pNh, "speed_I_left", speed_I_left, 0.0); - assertions::param(pNh, "speed_I_right", speed_I_right, 0.0); - assertions::param(pNh, "wheel_radius", wheel_radius, 0.3); - assertions::param(pNh, "max_effort", max_effort, 4.0); - assertions::param(pNh, "rate", rate_var, 60.0); - - // Publish that the robot is enabled - ros::Publisher enabled_pub = handle.advertise("/robot_enabled", 1, /* latch = */ true); - std_msgs::Bool enabled_msg; - enabled_msg.data = true; - enabled_pub.publish(enabled_msg); - - ros::Subscriber speed_sub = handle.subscribe("/motors", 1, speedCallback); - - ros::Subscriber state_sub = handle.subscribe("/joint_states", 1, jointStateCallback); - - ros::Time prev, now; - prev = ros::Time::now(); - - ros::Rate rate{ rate_var }; - - double speed_last_error_left = 0.0; - double speed_last_error_right = 0.0; - double speed_left_error_accum = 0.0; - double speed_right_error_accum = 0.0; - double effort_right = 0.0; - double effort_left = 0.0; - ros::Time prev_time = ros::Time::now(); - while (ros::ok()) - { - ros::spinOnce(); - - ros::Time cur_time = ros::Time::now(); - double dt = cur_time.toSec() - prev_time.toSec(); - prev_time = cur_time; - - double error_left = speed_set_point_left - speed_measured_left; - double dError_left = (error_left - speed_last_error_left) / dt; - speed_left_error_accum += error_left; - speed_last_error_left = error_left; - - // ROS_INFO_STREAM("error left = " << error_left); - // ROS_INFO_STREAM("effort_left = " << effort_left); - - effort_left += speed_P_left * error_left + speed_D_left * dError_left + speed_I_left * speed_left_error_accum; - - double error_right = speed_set_point_right - speed_measured_right; - double dError_right = (error_right - speed_last_error_right) / dt; - speed_right_error_accum += error_right; - speed_last_error_right = error_right; - - effort_right += - speed_P_right * error_right + speed_D_right * dError_right + speed_I_right * speed_right_error_accum; - - effort_right = std::min(max_effort, std::max(-max_effort, effort_right)); - effort_left = std::min(max_effort, std::max(-max_effort, effort_left)); - - // ROS_INFO_STREAM("Publishing effort: " << effort_right); - // ROS_INFO_STREAM("left: " << effort_left << " right: " << effort_right); - std_msgs::Float64 left_wheel_message; - std_msgs::Float64 right_wheel_message; - - left_wheel_message.data = effort_left; - right_wheel_message.data = effort_right; - /* - if(speed_set_point_left == 0) { - effort_left = 0.0; - left_wheel_message.data = 0.0; - } else { - left_wheel_message.data = effort_left; - } - if(speed_set_point_right == 0) { - effort_right = 0; - right_wheel_message.data = 0.0; - } else { - right_wheel_message.data = effort_right; - } - */ - - left_wheel_effort_publisher.publish(left_wheel_message); - right_wheel_effort_publisher.publish(right_wheel_message); - igvc_msgs::velocity_pair speed_measured; - speed_measured.left_velocity = speed_measured_left; - speed_measured.right_velocity = speed_measured_right; - now = ros::Time::now(); - ros::Duration duration = now - prev; - speed_measured.duration = duration.toSec(); - speed_measured.header.stamp = ros::Time::now(); - wheel_speed_publisher.publish(speed_measured); - rate.sleep(); - prev = now; - } - - return 0; -} diff --git a/igvc_msgs/CMakeLists.txt b/igvc_msgs/CMakeLists.txt index a59f1c354..6677ed573 100644 --- a/igvc_msgs/CMakeLists.txt +++ b/igvc_msgs/CMakeLists.txt @@ -14,10 +14,8 @@ find_package(catkin REQUIRED COMPONENTS add_message_files( DIRECTORY msg FILES - velocity_pair.msg velocity_quad.msg lights.msg - action_path.msg system_stats.msg map.msg igvc_path.msg diff --git a/igvc_msgs/msg/action_path.msg b/igvc_msgs/msg/action_path.msg deleted file mode 100644 index bd98ea5c1..000000000 --- a/igvc_msgs/msg/action_path.msg +++ /dev/null @@ -1,2 +0,0 @@ -Header header -velocity_pair[] actions diff --git a/igvc_msgs/msg/velocity_pair.msg b/igvc_msgs/msg/velocity_pair.msg deleted file mode 100644 index 5f36f7c6b..000000000 --- a/igvc_msgs/msg/velocity_pair.msg +++ /dev/null @@ -1,4 +0,0 @@ -Header header -float64 left_velocity -float64 right_velocity -float64 duration diff --git a/igvc_navigation/CMakeLists.txt b/igvc_navigation/CMakeLists.txt index 3403c1338..40fd90908 100644 --- a/igvc_navigation/CMakeLists.txt +++ b/igvc_navigation/CMakeLists.txt @@ -210,7 +210,6 @@ target_link_libraries(GraphSearch ${OpenCV_LIBRARIES} ${catkin_LIBRARIES}) add_subdirectory(src/back_circle) add_subdirectory(src/back_up_recovery) -add_subdirectory(src/differential_drive) add_subdirectory(src/swerve_drive) add_subdirectory(src/mapper) add_subdirectory(src/navigation_client) diff --git a/igvc_navigation/config/global_costmap_params_swervi.yaml b/igvc_navigation/config/global_costmap_params.yaml similarity index 100% rename from igvc_navigation/config/global_costmap_params_swervi.yaml rename to igvc_navigation/config/global_costmap_params.yaml diff --git a/igvc_navigation/config/global_costmap_params_jessi.yaml b/igvc_navigation/config/global_costmap_params_jessi.yaml deleted file mode 100644 index 275af011e..000000000 --- a/igvc_navigation/config/global_costmap_params_jessi.yaml +++ /dev/null @@ -1,103 +0,0 @@ -global_costmap: - global_frame: odom - robot_base_frame: base_link - plugins: - - {name: traversability_layer, type: "traversability_layer::TraversabilityLayer"} - - {name: line_layer, type: "line_layer::LineLayer"} - - {name: back_circle_layer, type: "back_circle_layer::BackCircleLayer"} - - {name: inflation_layer, type: "costmap_2d::InflationLayer"} - publish_frequency: 5.0 - footprint: [[-0.24,0.32],[0.72,0.32],[0.72,-0.32],[-0.24,-0.32]] - width: 200 - height: 200 - origin_x: -100 - origin_y: -100 - update_frequency: 10 - track_unknown_space: false - unknown_cost_value: 30 - rolling_window: false - resolution: 0.1 - always_send_full_costmap: true - inflation_layer: - inflation_radius: 5.0 -line_layer: - map: - frame_id: "odom" - resolution: 0.1 - length_x: 200 - length_y: 200 - occupied_threshold: 0.5 - max_occupancy: 0.99 - min_occupancy: 0.01 - costmap_topic: "/line_layer/costmap" - debug: - map_topic: "/mapper/debug/lines/gridmap" - enabled: false - /cam/center: - topics: - raw_image_ns: "/raw" - raw_image: "/image" - segmented_image_ns: "/segmented" - segmented_image: "/image" - max_distance: 10 - probabilities: - miss: 0.7 - hit: 0.7 - miss_exponential_coeff: 0.1 - miss_angle_exponential_coeff: 5 - hit_exponential_coeff: 0.1 - debug: - line_topic: "/line_layer/debug/center/line" - nonline_topic: "/line_layer/debug/center/nonline" - /cam/left: - topics: - raw_image_ns: "/raw" - raw_image: "/image" - segmented_image_ns: "/segmented" - segmented_image: "/image" - max_distance: 10 - probabilities: - miss: 0.7 - hit: 0.7 - miss_exponential_coeff: 0.05 - miss_angle_exponential_coeff: 5 - hit_exponential_coeff: 0.05 - debug: - line_topic: "/line_layer/debug/left/line" - nonline_topic: "/line_layer/debug/left/nonline" - /cam/right: - topics: - raw_image_ns: "/raw" - raw_image: "/image" - segmented_image_ns: "/segmented" - segmented_image: "/image" - max_distance: 10 - probabilities: - miss: 0.7 - hit: 0.7 - miss_exponential_coeff: 0.05 - miss_angle_exponential_coeff: 5 - hit_exponential_coeff: 0.05 - debug: - line_topic: "/line_layer/debug/right/line" - nonline_topic: "/line_layer/debug/right/nonline" - projection: - length_x: 30 - length_y: 30 - line_closing_kernel_size: 5 - freespace_closing_kernel_size: 5 -traversability_layer: - map: - frame_id: "odom" - resolution: 0.1 - length_x: 200 - length_y: 200 - occupied_threshold: 0.5 - max_occupancy: 0.99 - min_occupancy: 0.01 - costmap_topic: "/slope/costmap" - debug: - map_topic: "/slope/debug" - enabled: false - untraversable_probability: 0.8 - slope_threshold: 0.45 diff --git a/igvc_navigation/config/local_costmap_params_swervi.yaml b/igvc_navigation/config/local_costmap_params.yaml similarity index 100% rename from igvc_navigation/config/local_costmap_params_swervi.yaml rename to igvc_navigation/config/local_costmap_params.yaml diff --git a/igvc_navigation/config/local_costmap_params_jessi.yaml b/igvc_navigation/config/local_costmap_params_jessi.yaml deleted file mode 100644 index aff313593..000000000 --- a/igvc_navigation/config/local_costmap_params_jessi.yaml +++ /dev/null @@ -1,19 +0,0 @@ -local_costmap: - global_frame: odom - robot_base_frame: base_link - plugins: - - {name: rolling_layer, type: "rolling_layer::RollingLayer"} - publish_frequency: 5.0 - footprint: [[-0.24,0.32],[0.72,0.32],[0.72,-0.32],[-0.24,-0.32]] - width: 20 - height: 20 - origin_x: -10 - origin_y: -10 - update_frequency: 10 - track_unknown_space: false - unknown_cost_value: 30 - static_map: false - rolling_window: true - resolution: 0.1 -rolling_layer: - topic: "/move_base_flex/global_costmap/costmap" diff --git a/igvc_navigation/config/teb_local_planner_params_swervi.yaml b/igvc_navigation/config/teb_local_planner_params.yaml similarity index 100% rename from igvc_navigation/config/teb_local_planner_params_swervi.yaml rename to igvc_navigation/config/teb_local_planner_params.yaml diff --git a/igvc_navigation/config/teb_local_planner_params_jessi.yaml b/igvc_navigation/config/teb_local_planner_params_jessi.yaml deleted file mode 100644 index 9c8d23696..000000000 --- a/igvc_navigation/config/teb_local_planner_params_jessi.yaml +++ /dev/null @@ -1,117 +0,0 @@ -TebLocalPlannerROS: - odom_topic: /odometry/filtered - map_frame: /odom - - # Trajectory - - teb_autosize: True - dt_ref: 0.2 - dt_hysteresis: 0.02 - max_samples: 500 - global_plan_overwrite_orientation: True - allow_init_with_backwards_motion: False - max_global_plan_lookahead_dist: 5.0 - global_plan_viapoint_sep: -1 - global_plan_prune_distance: 1 - exact_arc_length: False - feasibility_check_no_poses: 4 - publish_feedback: True - control_look_ahead_poses: 2 - - # Robot - - max_vel_x: 1.6 - max_vel_x_backwards: 0.8 - max_vel_y: 0.0 - max_vel_theta: 2.5 - acc_lim_x: 0.4 - acc_lim_theta: 2.0 - min_turning_radius: 0.0 # diff-drive robot (can turn on place!) - - footprint_model: - type: "point" - - # GoalTolerance - - xy_goal_tolerance: 0.2 - yaw_goal_tolerance: 1.57 - free_goal_vel: False - complete_global_plan: True - - # Obstacles - - min_obstacle_dist: 0.7 # This value must also include our robot radius, since footprint_model is set to "point". - inflation_dist: 0.0 - include_costmap_obstacles: True - costmap_obstacles_behind_robot_dist: 1.5 - obstacle_poses_affected: 15 - - dynamic_obstacle_inflation_dist: 0.0 - include_dynamic_obstacles: False - - costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH" - costmap_converter_spin_thread: True - costmap_converter_rate: 2 - - costmap_converter/CostmapToPolygonsDBSMCCH: - cluster_max_distance: 0.4 - cluster_min_pts: 3 - cluster_max_pts: 30 - convex_hull_min_pt_separation: 0.1 - - # Optimization - - no_inner_iterations: 5 - no_outer_iterations: 5 - optimization_activate: True - optimization_verbose: False - penalty_epsilon: 0.1 - obstacle_cost_exponent: 2 - weight_max_vel_x: 100 - weight_max_vel_theta: 1 - weight_acc_lim_x: 100 - weight_acc_lim_theta: 1 - weight_kinematics_nh: 1000 - weight_kinematics_forward_drive: 100 - weight_kinematics_turning_radius: 1 - weight_optimaltime: 1 # must be > 0 - weight_shortest_path: 0 - weight_obstacle: 100 - weight_inflation: 90 - weight_dynamic_obstacle: 10 - weight_dynamic_obstacle_inflation: 0.2 - weight_viapoint: 0.01 - weight_adapt_factor: 2 - - # Homotopy Class Planner - - enable_homotopy_class_planning: False -# enable_multithreading: True -# max_number_classes: 4 -# selection_cost_hysteresis: 1.0 -# selection_prefer_initial_plan: 0.9 -# selection_obst_cost_scale: 100.0 -# selection_alternative_time_cost: False -# -# roadmap_graph_no_samples: 15 -# roadmap_graph_area_width: 5 -# roadmap_graph_area_length_scale: 1.0 -# h_signature_prescaler: 0.5 -# h_signature_threshold: 0.1 -# obstacle_heading_threshold: 0.45 -# switching_blocking_period: 0.0 -# viapoints_all_candidates: True -# delete_detours_backwards: True -# max_ratio_detours_duration_best_duration: 3.0 -# visualize_hc_graph: False -# visualize_with_time_as_z_axis_scale: False - - # Recovery - - shrink_horizon_backup: True - shrink_horizon_min_duration: 10 - oscillation_recovery: True - oscillation_v_eps: 0.1 - oscillation_omega_eps: 0.1 - oscillation_recovery_min_duration: 10 - oscillation_filter_duration: 10 diff --git a/igvc_navigation/launch/mbf_navigation.launch b/igvc_navigation/launch/mbf_navigation.launch index e57ee8675..1f05e1ef9 100644 --- a/igvc_navigation/launch/mbf_navigation.launch +++ b/igvc_navigation/launch/mbf_navigation.launch @@ -1,45 +1,19 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + diff --git a/igvc_navigation/launch/navigation_simulation.launch b/igvc_navigation/launch/navigation_simulation.launch index cbf11b026..fcf9c1efa 100644 --- a/igvc_navigation/launch/navigation_simulation.launch +++ b/igvc_navigation/launch/navigation_simulation.launch @@ -23,12 +23,7 @@ - - - - - - + diff --git a/igvc_navigation/src/differential_drive/CMakeLists.txt b/igvc_navigation/src/differential_drive/CMakeLists.txt deleted file mode 100644 index 105517d49..000000000 --- a/igvc_navigation/src/differential_drive/CMakeLists.txt +++ /dev/null @@ -1,10 +0,0 @@ -add_executable(differential_drive differential_drive.cpp) -add_dependencies(differential_drive ${catkin_EXPORTED_TARGETS}) -target_link_libraries(differential_drive ${catkin_LIBRARIES}) - -install( - TARGETS differential_drive - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/igvc_navigation/src/differential_drive/differential_drive.cpp b/igvc_navigation/src/differential_drive/differential_drive.cpp deleted file mode 100644 index 13e98b01c..000000000 --- a/igvc_navigation/src/differential_drive/differential_drive.cpp +++ /dev/null @@ -1,43 +0,0 @@ -#include -#include - -#include "differential_drive.h" - -DifferentialDrive::DifferentialDrive() -{ - ros::NodeHandle nh; - ros::NodeHandle pNh("~"); - - assertions::param(pNh, "axle_length", axle_length_, 0.48); - assertions::param(pNh, "max_vel", max_vel_, 3.0); - mbf_twist_ = nh.subscribe("/cmd_vel", 1, &DifferentialDrive::twistToVelocity, this); - vel_pub_ = nh.advertise("/motors", 1); -} - -void DifferentialDrive::twistToVelocity(geometry_msgs::Twist twist) -{ - double speed = twist.linear.x; - double rotation = twist.angular.z; - double vel_right = speed + (rotation * axle_length_) / 2; - double vel_left = speed - (rotation * axle_length_) / 2; - - double max_calc_vel = fmax(vel_right, vel_left); - if (max_calc_vel > max_vel_) - { - vel_right *= max_vel_ / max_calc_vel; - vel_left *= max_vel_ / max_calc_vel; - } - - igvc_msgs::velocity_pair vel_msg; - vel_msg.right_velocity = vel_right; - vel_msg.left_velocity = vel_left; - vel_msg.duration = 0.02; - vel_msg.header.stamp = ros::Time::now(); - vel_pub_.publish(vel_msg); -} -int main(int argc, char** argv) -{ - ros::init(argc, argv, "differential_drive"); - DifferentialDrive differential_drive; - ros::spin(); -} diff --git a/igvc_navigation/src/differential_drive/differential_drive.h b/igvc_navigation/src/differential_drive/differential_drive.h deleted file mode 100644 index 9541a02dc..000000000 --- a/igvc_navigation/src/differential_drive/differential_drive.h +++ /dev/null @@ -1,27 +0,0 @@ -/** - * Converts Twist messages to velocity pairs for the motors of a differential drive robot - * - * Author: Tan Gemicioglu - */ - -#ifndef DIFFERENTIALDRIVE_H -#define DIFFERENTIALDRIVE_H - -#include -#include - -class DifferentialDrive -{ -public: - DifferentialDrive(); - ros::Subscriber mbf_twist_; - -private: - ros::Publisher vel_pub_; - double axle_length_{}; - double max_vel_{}; - - void twistToVelocity(geometry_msgs::Twist twist); -}; - -#endif // DIFFERENTIALDRIVE_H From 7ccb0820dc26fc196d998e272903d58f90e66569 Mon Sep 17 00:00:00 2001 From: matthewhannay567 Date: Sun, 20 Feb 2022 18:37:57 -0500 Subject: [PATCH 08/27] Fixed issues --- igvc_navigation/launch/localization.launch | 2 +- .../launch/navigation_simulation.launch | 5 +---- .../src/swerve_wheel_odometer/swerve_odometer.cpp | 14 +++++++------- 3 files changed, 9 insertions(+), 12 deletions(-) diff --git a/igvc_navigation/launch/localization.launch b/igvc_navigation/launch/localization.launch index 6ef04afe6..1ed0cbe5a 100644 --- a/igvc_navigation/launch/localization.launch +++ b/igvc_navigation/launch/localization.launch @@ -7,7 +7,7 @@ --> - + diff --git a/igvc_navigation/launch/navigation_simulation.launch b/igvc_navigation/launch/navigation_simulation.launch index fcf9c1efa..3b7d570bc 100644 --- a/igvc_navigation/launch/navigation_simulation.launch +++ b/igvc_navigation/launch/navigation_simulation.launch @@ -1,15 +1,12 @@ - - - - + diff --git a/igvc_navigation/src/swerve_wheel_odometer/swerve_odometer.cpp b/igvc_navigation/src/swerve_wheel_odometer/swerve_odometer.cpp index 500b4f4aa..24c947ba7 100644 --- a/igvc_navigation/src/swerve_wheel_odometer/swerve_odometer.cpp +++ b/igvc_navigation/src/swerve_wheel_odometer/swerve_odometer.cpp @@ -150,7 +150,7 @@ void SwerveOdometer::enc_callback(const igvc_msgs::velocity_quad msg) if (inf_all) { - ROS_INFO_STREAM("inf_all"); + ROS_DEBUG_STREAM("inf_all"); angular = 0; for (int i = 0; i < num_wheels; ++i) { @@ -166,14 +166,14 @@ void SwerveOdometer::enc_callback(const igvc_msgs::velocity_quad msg) if ((fabs(intersections[i][0] - intersections[i - 1][0]) > intersection_tol_ || fabs(intersections[i][1] - intersections[i - 1][1]) > intersection_tol_)) { - ROS_ERROR_STREAM("intersections are not close enough to get an average, dropping!"); + ROS_DEBUG_STREAM("intersections are not close enough to get an average, dropping!"); for (int i = 0; i < num_wheels; ++i) { - ROS_WARN_STREAM("theta, omega: " << wheel_info[i][1] << " " << wheel_info[i][0]); + ROS_DEBUG_STREAM("theta, omega: " << wheel_info[i][1] << " " << wheel_info[i][0]); } for (size_t i = 0; i < intersections.size(); ++i) { - ROS_WARN_STREAM("intersection i:" << i << " , " << intersections[i][0] << " " << intersections[i][1]); + ROS_DEBUG_STREAM("intersection i:" << i << " , " << intersections[i][0] << " " << intersections[i][1]); } return; } @@ -208,9 +208,9 @@ void SwerveOdometer::enc_callback(const igvc_msgs::velocity_quad msg) positions_list[i][1] - average_intersection[1] }; if (isinf(icr_wh[0]) || isinf(icr_wh[1])) - ROS_WARN_STREAM("icr_wh is inf"); + ROS_DEBUG_STREAM("icr_wh is inf"); if (isclose(icr_wh[0], 0) || isclose(icr_wh[1], 0)) - ROS_WARN_STREAM("icr_wh for wheel " << i << " is zero. icr is just over it!"); + ROS_DEBUG_STREAM("icr_wh for wheel " << i << " is zero. icr is just over it!"); double ang_x = (wheel_info[i][0] * wheel_radius * sin(wheel_info[i][1])) / (icr_wh[0]); double ang_y = (wheel_info[i][0] * wheel_radius * cos(wheel_info[i][1])) / (icr_wh[1]); @@ -220,7 +220,7 @@ void SwerveOdometer::enc_callback(const igvc_msgs::velocity_quad msg) if (isinf(angular)) { - ROS_WARN_STREAM("angular is the problem"); + ROS_DEBUG_STREAM("angular is the problem"); return; } } From ea920712b391d70f62a7c026767659caf1c29037 Mon Sep 17 00:00:00 2001 From: matthewhannay567 Date: Wed, 23 Feb 2022 20:48:43 -0500 Subject: [PATCH 09/27] Modified config. need to work on magnetometer --- .../config/ekf_localization_node_params.yaml | 19 +++++++++++++------ 1 file changed, 13 insertions(+), 6 deletions(-) diff --git a/igvc_navigation/config/ekf_localization_node_params.yaml b/igvc_navigation/config/ekf_localization_node_params.yaml index 9396e976d..dd41e3081 100644 --- a/igvc_navigation/config/ekf_localization_node_params.yaml +++ b/igvc_navigation/config/ekf_localization_node_params.yaml @@ -83,7 +83,7 @@ odom0: /wheel_odometry odom0_config: [ false, false, false, false, false, false, true, true, false, - false, false, false, + false, false, true, false, false, false] # If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase @@ -139,7 +139,7 @@ imu0_config: [ false, false, false, false, false, false, false, false, false, false, false, true, - true, false, false] + true, true, false] imu0_nodelay: false imu0_differential: false imu0_relative: false @@ -148,6 +148,13 @@ imu0_queue_size: 5 #imu0_twist_rejection_threshold: 0.8 # #imu0_linear_acceleration_rejection_threshold: 0.8 # +# imu1: /magnetometer +# imu1_config: [ false, false, false, +# false, false, true, +# false, false, false, +# false, false, false, +# false, false, false] + # [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set # this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame. imu0_remove_gravitational_acceleration: true @@ -173,15 +180,15 @@ control_timeout: 0.2 # Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw. control_config: [true, false, false, false, false, true] # Places limits on how large the acceleration term will be. Should match your robot's kinematics. -acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4] +acceleration_limits: [1.3, 1.3, 0.0, 0.0, 0.0, 3.4] # Acceleration and deceleration limits are not always the same for robots. -deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5] +deceleration_limits: [1.3, 1.3, 0.0, 0.0, 0.0, 4.5] # If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these # gains -acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9] +acceleration_gains: [0.8, 0.8, 0.0, 0.0, 0.0, 0.9] # If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these # gains -deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0] +deceleration_gains: [1.0, 1.0, 0.0, 0.0, 0.0, 1.0] # [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is # exposed as a configuration parameter. This matrix represents the noise we add to the total error after each From 32f7bb208a7b621b45dc7c8d2411d18ab91b6673 Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Sun, 27 Feb 2022 03:23:39 -0500 Subject: [PATCH 10/27] Changed IMU yaw offsets to zero I think that it is fine for these values to be zero because on the real robot the yostlab driver can add an offset which allows for the IMU to report zero when facing east. Thus, having the simulated IMUs report zero when facing east should be fine. --- igvc_description/urdf/jessii.urdf.xacro | 4 ++-- igvc_description/urdf/swervi.urdf.xacro | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/igvc_description/urdf/jessii.urdf.xacro b/igvc_description/urdf/jessii.urdf.xacro index 148d5693c..976203af5 100644 --- a/igvc_description/urdf/jessii.urdf.xacro +++ b/igvc_description/urdf/jessii.urdf.xacro @@ -599,7 +599,7 @@ 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 - -1.57079632679 + 0 @@ -619,7 +619,7 @@ 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 - -1.57079632679 + 0 diff --git a/igvc_description/urdf/swervi.urdf.xacro b/igvc_description/urdf/swervi.urdf.xacro index 539efc7f7..b8a7fd2e4 100644 --- a/igvc_description/urdf/swervi.urdf.xacro +++ b/igvc_description/urdf/swervi.urdf.xacro @@ -386,7 +386,7 @@ 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 - -1.57079632679 + 0 @@ -406,7 +406,7 @@ 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 - -1.57079632679 + 0 From 602ec436133dc3eace53f09b96eb2dc2d6d62d31 Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Sun, 27 Feb 2022 03:27:20 -0500 Subject: [PATCH 11/27] updated navsat yaw offset to match updated simulated IMU yaw values --- igvc_navigation/launch/localization.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/igvc_navigation/launch/localization.launch b/igvc_navigation/launch/localization.launch index 1ed0cbe5a..75fdf971c 100644 --- a/igvc_navigation/launch/localization.launch +++ b/igvc_navigation/launch/localization.launch @@ -23,7 +23,7 @@ - + From 5d33856f511dfc7dc2100bd4980bfbd87072cc98 Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Sun, 27 Feb 2022 03:28:19 -0500 Subject: [PATCH 12/27] updated control_config because commands can also specify vy --- .../config/ekf_localization_node_params.yaml | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/igvc_navigation/config/ekf_localization_node_params.yaml b/igvc_navigation/config/ekf_localization_node_params.yaml index dd41e3081..c31e33a21 100644 --- a/igvc_navigation/config/ekf_localization_node_params.yaml +++ b/igvc_navigation/config/ekf_localization_node_params.yaml @@ -148,12 +148,12 @@ imu0_queue_size: 5 #imu0_twist_rejection_threshold: 0.8 # #imu0_linear_acceleration_rejection_threshold: 0.8 # -# imu1: /magnetometer -# imu1_config: [ false, false, false, -# false, false, true, -# false, false, false, -# false, false, false, -# false, false, false] +imu1: /magnetometer +imu1_config: [ false, false, false, + false, false, true, + false, false, false, + false, false, false, + false, false, false] # [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set # this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame. @@ -178,7 +178,7 @@ stamped_control: false # The last issued control command will be used in prediction for this period. Defaults to 0.2. control_timeout: 0.2 # Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw. -control_config: [true, false, false, false, false, true] +control_config: [true, true, false, false, false, true] # Places limits on how large the acceleration term will be. Should match your robot's kinematics. acceleration_limits: [1.3, 1.3, 0.0, 0.0, 0.0, 3.4] # Acceleration and deceleration limits are not always the same for robots. From e0f4452ce7a0eecf2fc9093292f14e383c11ca40 Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Sun, 27 Feb 2022 04:02:51 -0500 Subject: [PATCH 13/27] adding back velocity pair so build passes --- igvc_msgs/msg/velocity_pair.msg | 4 ++++ 1 file changed, 4 insertions(+) create mode 100644 igvc_msgs/msg/velocity_pair.msg diff --git a/igvc_msgs/msg/velocity_pair.msg b/igvc_msgs/msg/velocity_pair.msg new file mode 100644 index 000000000..5f36f7c6b --- /dev/null +++ b/igvc_msgs/msg/velocity_pair.msg @@ -0,0 +1,4 @@ +Header header +float64 left_velocity +float64 right_velocity +float64 duration From 1599622448bb2a6f63f8a69dffa2d2131048f11d Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Sun, 27 Feb 2022 04:06:22 -0500 Subject: [PATCH 14/27] add velocity pair back to cmake --- igvc_msgs/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/igvc_msgs/CMakeLists.txt b/igvc_msgs/CMakeLists.txt index 6677ed573..39b90b662 100644 --- a/igvc_msgs/CMakeLists.txt +++ b/igvc_msgs/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(catkin REQUIRED COMPONENTS add_message_files( DIRECTORY msg FILES + velocity_pair.msg velocity_quad.msg lights.msg system_stats.msg From dcba1bc06fffdd0f6ac6e9c462bcdb1fbd854a93 Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Sun, 27 Feb 2022 16:10:13 -0500 Subject: [PATCH 15/27] minor localization changes --- igvc_navigation/launch/localization.launch | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/igvc_navigation/launch/localization.launch b/igvc_navigation/launch/localization.launch index 75fdf971c..9a52b3b63 100644 --- a/igvc_navigation/launch/localization.launch +++ b/igvc_navigation/launch/localization.launch @@ -29,13 +29,15 @@ - + + + From 669553da8cb4d8544a95c2c17fc1029659914fc2 Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Wed, 9 Mar 2022 20:36:03 -0500 Subject: [PATCH 16/27] Fixed issues with swerve wheel odometery --- .../launch/swerve_wheel_odometry.launch | 1 + .../swerve_wheel_odometer/swerve_odometer.cpp | 23 ++++++++++++++----- .../swerve_wheel_odometer/swerve_odometer.h | 4 ++++ 3 files changed, 22 insertions(+), 6 deletions(-) diff --git a/igvc_navigation/launch/swerve_wheel_odometry.launch b/igvc_navigation/launch/swerve_wheel_odometry.launch index c8d65b080..9ee38a86c 100644 --- a/igvc_navigation/launch/swerve_wheel_odometry.launch +++ b/igvc_navigation/launch/swerve_wheel_odometry.launch @@ -8,6 +8,7 @@ + diff --git a/igvc_navigation/src/swerve_wheel_odometer/swerve_odometer.cpp b/igvc_navigation/src/swerve_wheel_odometer/swerve_odometer.cpp index 24c947ba7..68cffa67a 100644 --- a/igvc_navigation/src/swerve_wheel_odometer/swerve_odometer.cpp +++ b/igvc_navigation/src/swerve_wheel_odometer/swerve_odometer.cpp @@ -16,6 +16,7 @@ SwerveOdometer::SwerveOdometer() : pNh{ "~" } return; } + curr_time = ros::Time::now(); prev_time = ros::Time::now(); sub = nh.subscribe("/encoders", 10, &SwerveOdometer::enc_callback, this); pub = nh.advertise("/wheel_odometry", 10); @@ -55,7 +56,7 @@ void SwerveOdometer::enc_callback(const igvc_msgs::velocity_quad msg) } } - ros::Time curr_time = ros::Time::now(); + curr_time = ros::Time::now(); double deltaT = msg.duration; for (int i = 0; i < num_wheels; ++i) @@ -207,10 +208,14 @@ void SwerveOdometer::enc_callback(const igvc_msgs::velocity_quad msg) auto icr_wh = std::array{ positions_list[i][0] - average_intersection[0], positions_list[i][1] - average_intersection[1] }; - if (isinf(icr_wh[0]) || isinf(icr_wh[1])) - ROS_DEBUG_STREAM("icr_wh is inf"); - if (isclose(icr_wh[0], 0) || isclose(icr_wh[1], 0)) - ROS_DEBUG_STREAM("icr_wh for wheel " << i << " is zero. icr is just over it!"); + if (isinf(icr_wh[0]) || isinf(icr_wh[1])) { + ROS_DEBUG_STREAM("icr_wh is inf! Discarding!"); + continue; + } + if (isclose(icr_wh[0], 0) || isclose(icr_wh[1], 0)) { + ROS_DEBUG_STREAM("icr_wh for wheel " << i << " is zero! Discarding!"); + continue; + } double ang_x = (wheel_info[i][0] * wheel_radius * sin(wheel_info[i][1])) / (icr_wh[0]); double ang_y = (wheel_info[i][0] * wheel_radius * cos(wheel_info[i][1])) / (icr_wh[1]); @@ -218,7 +223,7 @@ void SwerveOdometer::enc_callback(const igvc_msgs::velocity_quad msg) angular += ang_computed; - if (isinf(angular)) + if (isinf(angular) || angular > angular_limit_) { ROS_DEBUG_STREAM("angular is the problem"); return; @@ -243,6 +248,11 @@ void SwerveOdometer::enc_callback(const igvc_msgs::velocity_quad msg) x += linear_x_ * deltaT; y += linear_y_ * deltaT; + // publishes odometry message + SwerveOdometer::pubOdometry(); +} + +void SwerveOdometer::pubOdometry() { geometry_msgs::Vector3 linearVelocities; linearVelocities.z = 0; linearVelocities.x = linear_x_; @@ -295,6 +305,7 @@ bool SwerveOdometer::getParams() assertions::param(pNh, "inf_tol", inf_tol, 5.0); assertions::param(pNh, "intersection_tol_", intersection_tol_, 0.8); assertions::param(pNh, "alpha", alpha, 0.5); + assertions::param(pNh, "angular_limit_", angular_limit_, 20.0); XmlRpc::XmlRpcValue xml_list; diff --git a/igvc_navigation/src/swerve_wheel_odometer/swerve_odometer.h b/igvc_navigation/src/swerve_wheel_odometer/swerve_odometer.h index a7ac6e3fa..301f771e9 100644 --- a/igvc_navigation/src/swerve_wheel_odometer/swerve_odometer.h +++ b/igvc_navigation/src/swerve_wheel_odometer/swerve_odometer.h @@ -40,10 +40,13 @@ class SwerveOdometer double linear_y_; // [m/s] double angular_; // [rad/s] + double angular_limit_; + double inf_tol; double intersection_tol_; double alpha; + ros::Time curr_time; ros::Time prev_time; // callback for encoder subscriber @@ -52,5 +55,6 @@ class SwerveOdometer double theta_map(const double& theta); double isclose(const double& a, const double& b, const double tol = 0.025, const double bias = 0); void ang_callback(const nav_msgs::Odometry msg); + void pubOdometry(); }; #endif From bbb8ddbd37f01baa23c67abe7aa2b069e0c12d2a Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Wed, 9 Mar 2022 20:37:24 -0500 Subject: [PATCH 17/27] elevated swervi and tuned PID variables so swervi is able to go over ramps --- igvc_description/urdf/swervi_prop.urdf.xacro | 6 +++--- igvc_gazebo/config/swervi_joint_control.yaml | 8 ++++---- igvc_gazebo/launch/swervi_control.launch | 10 +++++----- 3 files changed, 12 insertions(+), 12 deletions(-) diff --git a/igvc_description/urdf/swervi_prop.urdf.xacro b/igvc_description/urdf/swervi_prop.urdf.xacro index afc4a5b45..005729c29 100644 --- a/igvc_description/urdf/swervi_prop.urdf.xacro +++ b/igvc_description/urdf/swervi_prop.urdf.xacro @@ -14,7 +14,7 @@ - + @@ -43,12 +43,12 @@ - + - + diff --git a/igvc_gazebo/config/swervi_joint_control.yaml b/igvc_gazebo/config/swervi_joint_control.yaml index aafe93fb0..12bae8e95 100644 --- a/igvc_gazebo/config/swervi_joint_control.yaml +++ b/igvc_gazebo/config/swervi_joint_control.yaml @@ -22,19 +22,19 @@ front_left_swivel_controller: type: effort_controllers/JointPositionController joint: fl_swivel_rev - pid: {p: 20.0, d: 3.0, i: 0.0} + pid: {p: 30.0, d: 4.0, i: 0.0} back_left_swivel_controller: type: effort_controllers/JointPositionController joint: bl_swivel_rev - pid: {p: 20.0, d: 3.0, i: 0.0} + pid: {p: 30.0, d: 4.0, i: 0.0} front_right_swivel_controller: type: effort_controllers/JointPositionController joint: fr_swivel_rev - pid: {p: 20.0, d: 3.0, i: 0.0} + pid: {p: 30.0, d: 4.0, i: 0.0} back_right_swivel_controller: type: effort_controllers/JointPositionController joint: br_swivel_rev - pid: {p: 20.0, d: 3.0, i: 0.0} + pid: {p: 30.0, d: 4.0, i: 0.0} # shock controllers ---------------------------------------- front_left_shock_controller: diff --git a/igvc_gazebo/launch/swervi_control.launch b/igvc_gazebo/launch/swervi_control.launch index ca2e3fcfa..402bbe089 100644 --- a/igvc_gazebo/launch/swervi_control.launch +++ b/igvc_gazebo/launch/swervi_control.launch @@ -13,10 +13,10 @@ - - - - + + + + @@ -26,7 +26,7 @@ - + From 17fab1d51d947869e538dd6ef0680a89dca61076 Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Wed, 9 Mar 2022 20:40:12 -0500 Subject: [PATCH 18/27] increased acceleration in TEB --- igvc_navigation/config/teb_local_planner_params.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/igvc_navigation/config/teb_local_planner_params.yaml b/igvc_navigation/config/teb_local_planner_params.yaml index 8a3319793..81b1c1f57 100644 --- a/igvc_navigation/config/teb_local_planner_params.yaml +++ b/igvc_navigation/config/teb_local_planner_params.yaml @@ -24,8 +24,8 @@ TebLocalPlannerROS: max_vel_x_backwards: 2.0 max_vel_y: 2.0 max_vel_theta: 4.0 - acc_lim_x: 0.4 - acc_lim_y: 0.4 + acc_lim_x: 1.0 + acc_lim_y: 1.0 acc_lim_theta: 2.0 min_turning_radius: 0.0 # swerve-drive robot (can turn on place!) From 1390944d7653b5889cb2ea219191b9f9c5c81129 Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Wed, 9 Mar 2022 20:41:14 -0500 Subject: [PATCH 19/27] Configured robot_localization for 3D localization --- .../config/ekf_localization_node_params.yaml | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/igvc_navigation/config/ekf_localization_node_params.yaml b/igvc_navigation/config/ekf_localization_node_params.yaml index c31e33a21..f6f49176d 100644 --- a/igvc_navigation/config/ekf_localization_node_params.yaml +++ b/igvc_navigation/config/ekf_localization_node_params.yaml @@ -13,7 +13,7 @@ silent_tf_failure: false # set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar # environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected # by, for example, an IMU. Defaults to false if unspecified. -two_d_mode: true +two_d_mode: false # Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for # future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if @@ -122,7 +122,7 @@ odom0_relative: false # Further input parameter examples odom1: /odometry/gps -odom1_config: [ true, true, false, +odom1_config: [ true, true, true, false, false, false, false, false, false, false, false, false, @@ -136,10 +136,10 @@ odom1_nodelay: false imu0: /imu imu0_config: [ false, false, false, - false, false, false, + true, true, false, false, false, false, false, false, true, - true, true, false] + true, true, true] imu0_nodelay: false imu0_differential: false imu0_relative: false @@ -200,15 +200,15 @@ deceleration_gains: [1.0, 1.0, 0.0, 0.0, 0.0, 1.0] # unspecified. process_noise_covariance: [ .6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, .6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, .1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, .1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, .1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, .6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, .3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, .3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, .3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, .1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, .1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, .1, 0, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, .1, 0, 0, 0, 0, 0, - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, .1, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, .5, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, .5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, .5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, .1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, .1, 0, From 94d8173d5855d7c2da872c15f676f28e821f02f2 Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Wed, 9 Mar 2022 20:58:41 -0500 Subject: [PATCH 20/27] removed all jessi stuff --- igvc_description/urdf/jessi.urdf | 411 ----------- igvc_description/urdf/jessii.urdf.xacro | 670 ------------------ igvc_description/urdf/meshes/jessi_simple.dae | 63 -- igvc_gazebo/launch/simulation.launch | 3 - igvc_gazebo/launch/simulation_low.launch | 3 - igvc_navigation/CMakeLists.txt | 1 - .../launch/differential_drive.launch | 11 - igvc_navigation/launch/wheel_odometry.launch | 6 - .../src/wheel_odometer/CMakeLists.txt | 10 - .../src/wheel_odometer/Odometer.cpp | 119 ---- igvc_navigation/src/wheel_odometer/Odometer.h | 30 - igvc_platform/CMakeLists.txt | 1 - .../src/joystick_driver/CMakeLists.txt | 10 - .../src/joystick_driver/joystick_driver.cpp | 77 -- igvc_platform/src/tests/CMakeLists.txt | 8 - .../tests/test/test_differential_drive.test | 7 - .../src/tests/test/test_joystick_driver.test | 4 - .../src/tests/test_differential_drive.cpp | 218 ------ .../src/tests/test_joystick_driver.cpp | 126 ---- igvc_utils/CMakeLists.txt | 1 - igvc_utils/src/speed_compare/CMakeLists.txt | 10 - igvc_utils/src/speed_compare/main.cpp | 74 -- 22 files changed, 1863 deletions(-) delete mode 100644 igvc_description/urdf/jessi.urdf delete mode 100644 igvc_description/urdf/jessii.urdf.xacro delete mode 100644 igvc_description/urdf/meshes/jessi_simple.dae delete mode 100644 igvc_navigation/launch/differential_drive.launch delete mode 100644 igvc_navigation/launch/wheel_odometry.launch delete mode 100644 igvc_navigation/src/wheel_odometer/CMakeLists.txt delete mode 100644 igvc_navigation/src/wheel_odometer/Odometer.cpp delete mode 100644 igvc_navigation/src/wheel_odometer/Odometer.h delete mode 100644 igvc_platform/src/joystick_driver/CMakeLists.txt delete mode 100644 igvc_platform/src/joystick_driver/joystick_driver.cpp delete mode 100644 igvc_platform/src/tests/test/test_differential_drive.test delete mode 100644 igvc_platform/src/tests/test/test_joystick_driver.test delete mode 100644 igvc_platform/src/tests/test_differential_drive.cpp delete mode 100644 igvc_platform/src/tests/test_joystick_driver.cpp delete mode 100644 igvc_utils/src/speed_compare/CMakeLists.txt delete mode 100644 igvc_utils/src/speed_compare/main.cpp diff --git a/igvc_description/urdf/jessi.urdf b/igvc_description/urdf/jessi.urdf deleted file mode 100644 index 610145140..000000000 --- a/igvc_description/urdf/jessi.urdf +++ /dev/null @@ -1,411 +0,0 @@ - - - - - - - - - - - - - - Gazebo/Grey - - - - Gazebo/Grey - 0.2 - 0.2 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Grey - 0.7 - 0.75 - - - - - - - - - - - - - - - - - - - - - - Gazebo/Grey - 0.7 - 0.75 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - false - 10 - - - - - - 100 - 3.6 - -3.14159265359 - 3.14159265359 - - - - 0.05 - 30 - 0.02 - - - - /scan - /lidar - - - - - - - - - - - - - - - - - - - 2.239 - 1.995 - - 640 - 512 - - - 0.1 - 100 - - - - 1 - 30 - - true - 30.0 - usb_cam_center - image_raw - camera_info - optical_cam_center - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - gazebo_ros_control/DefaultRobotHWSim - - - /imu/calibrate - 200.0 - imu - imu - /imu - 0 0 0 - 0 0 0 - 0.00000001 - 0.00000001 0.00000001 0.00000001 - 0.00000001 0.00000001 0.00000001 - 0.00000001 0.00000001 0.00000001 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 1.5707963 - - - - /magnetometer/calibrate - 200.0 - magnetometer - magnetometer - /magnetometer - 0 0 0 - 0 0 0 - 0.00000001 - 0.00000001 0.00000001 0.00000001 - 0.00000001 0.00000001 0.00000001 - 0.00000001 0.00000001 0.00000001 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0 - - - - 20.0 - base_link - base_link - /fix - /fix_velocity - 33.774497 - -84.405001 - 309.0 - 0.001 0.001 0.001 - 0.0001 0.0001 0.0001 - 0 0 0 - 0.005 0.005 0.05 - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - imu - front_ball - 0 0 0 0 0 0 - - - - 0 - 0.2 - - - 0 - 0.2 - - - - - - diff --git a/igvc_description/urdf/jessii.urdf.xacro b/igvc_description/urdf/jessii.urdf.xacro deleted file mode 100644 index 976203af5..000000000 --- a/igvc_description/urdf/jessii.urdf.xacro +++ /dev/null @@ -1,670 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - - hardware_interface/EffortJointInterface - - 1 - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - - hardware_interface/EffortJointInterface - 1 - - - - - - - - - - - - - - 10 - - - - 0.000000001 - 0.2 - 1e+10 - 10 - 0.01 - 0.005 - - - - - - - - - - - - - - - 0.7 - 0.75 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 0.2 - 0.25 - - - - - base_link - caster - 0 0 0 0 0 0 - - - - 0 - 0.2 - - - 0 - 0.2 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1.2290609 - - 640 - 480 - - - 0.1 - 100 - - - - 1 - 30 - - true - 30 - cam/center/raw - image - camera_info - cam/center_optical - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1.2290609 - - 640 - 480 - - - 0.1 - 100 - - - - 1 - 30 - - true - 30 - cam/right/raw - image - camera_info - cam/right_optical - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1.2290609 - - 640 - 480 - - - 0.1 - 100 - - - - 1 - 30 - - true - 30 - cam/left/raw - image - camera_info - cam/left_optical - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - - - - - - - - - - - Gazebo/Grey - - - - - - - Gazebo/Grey - - - - - - - - - - - Gazebo/Grey - 0.2 - 0.2 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - gazebo_ros_control/DefaultRobotHWSim - - - - /imu/calibrate - 200.0 - imu - imu - /imu - 0 0 0 - 0.00000005 - 0.00000001 0.00000001 0.00000001 - 0.00000001 0.00000001 0.00000001 - 0.00000001 0.00000001 0.00000001 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0 - - - - /magnetometer/calibrate - 200.0 - magnetometer - magnetometer - /magnetometer - 0 0 0 - 0.00000005 - 0.00000001 0.00000001 0.00000001 - 0.00000001 0.00000001 0.00000001 - 0.00000001 0.00000001 0.00000001 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0 - - - - 200.0 - .0000489162 - magnetometer - /magnetometer/vector - 0.0 - -5.2290 - 62.2126 - 0 0 0 - 0 0 0 - 0 0 0 - 0.00000005 0.00000005 0.00000005 - - - - 20.0 - base_link - base_link - /fix - /fix_velocity - 33.774497 - -84.405001 - 90 - 309.0 - 0.001 0.001 0.001 - 0.0001 0.0001 0.0001 - 0 0 0 - 0.005 0.005 0.05 - - - - true - 200.0 - base_footprint - ground_truth/state_raw - 0.00 - world - - 0 0 0 - 0 0 0 - - - - - - diff --git a/igvc_description/urdf/meshes/jessi_simple.dae b/igvc_description/urdf/meshes/jessi_simple.dae deleted file mode 100644 index f39244a86..000000000 --- a/igvc_description/urdf/meshes/jessi_simple.dae +++ /dev/null @@ -1,63 +0,0 @@ - - - - - VCGLab - VCGLib | MeshLab - - Y_UP - Sun Jan 20 20:53:41 2019 - Sun Jan 20 20:53:41 2019 - - - - - - - - - -0.0756748 -0.596477 -0.0896 -0.0756748 -0.34843 0.07314 0.0756752 -0.596477 -0.0896 0.0756752 -0.596477 0.07314 -0.0756748 -0.596477 0.07314 -0.283388 0.215131 0.75959 0.283388 -0.017736 1.16359 0.283388 0.012264 0.75959 0.283388 0.215131 0.75959 0.283388 -0.34843 0.355 0.283388 -0.121742 -0.2286 -0.283388 -0.121742 0 -0.283388 -0.34843 0.355 -0.283388 -0.121742 -0.2286 -0.283388 0.215131 0.355 -0.283388 0.012264 0.75959 -0.283388 -0.017736 0.355 -0.283388 -0.017736 1.16359 -0.283388 0.012264 1.16359 -0.283388 0.34843 0 0.283388 0.215131 0.355 0.283388 -0.121742 0 0.283388 -0.017736 0.355 0.283388 0.34843 0 0.283388 0.012264 1.16359 0.283388 -0.34843 -0.2286 -0.283388 -0.34843 -0.2286 -0.0756748 -0.34843 -0.0896 0.0756752 -0.34843 -0.0896 0.0756752 -0.34843 0.07314 -0.283388 0.34843 0.355 0.283388 0.34843 0.355 - - - - - - - - - - 0 1 0 0 1 0 0 0 1 0 0 1 -1 0 0 -1 0 0 0 0 1 0 0 1 1 0 0 1 0 0 0 0 -1 0 0 -1 0 -1 0 0 -1 0 0 1 0 0 1 0 0 -1 0 0 -1 0 0 0 1 0 0 1 0 0 1 0 0 1 0 1 0 0 1 0 0 0 -1 0 0 -1 0 0 -1 0 0 -1 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 0 1 0 0 1 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 0 1 0 0 1 - - - - - - - - - - - - - - -

7 0 15 0 24 0 24 1 15 1 18 1 17 2 6 2 18 2 18 3 6 3 24 3 1 4 27 4 4 4 4 5 27 5 0 5 29 6 1 6 3 6 3 7 1 7 4 7 28 8 29 8 2 8 2 9 29 9 3 9 27 10 28 10 0 10 0 11 28 11 2 11 2 12 3 12 0 12 0 13 3 13 4 13 20 14 14 14 8 14 8 15 14 15 5 15 16 16 22 16 17 16 17 17 22 17 6 17 15 18 7 18 5 18 5 19 7 19 8 19 22 20 16 20 9 20 9 21 16 21 12 21 11 22 21 22 13 22 13 23 21 23 10 23 13 24 10 24 26 24 26 25 10 25 25 25 21 26 11 26 23 26 23 27 11 27 19 27 30 28 19 28 14 28 14 29 19 29 11 29 14 30 11 30 16 30 16 31 11 31 12 31 12 32 11 32 26 32 26 33 11 33 13 33 14 34 16 34 15 34 15 35 16 35 17 35 15 36 17 36 18 36 15 37 5 37 14 37 23 38 19 38 31 38 31 39 19 39 30 39 23 40 20 40 21 40 21 41 20 41 22 41 21 42 22 42 9 42 23 43 31 43 20 43 8 44 7 44 20 44 20 45 7 45 22 45 24 46 6 46 7 46 7 47 6 47 22 47 9 48 25 48 21 48 21 49 25 49 10 49 1 50 26 50 27 50 27 51 26 51 25 51 27 52 25 52 28 52 28 53 25 53 29 53 29 54 25 54 9 54 29 55 9 55 1 55 1 56 9 56 12 56 1 57 12 57 26 57 14 58 20 58 30 58 30 59 20 59 31 59

- - - - - - - - - - - - - - - - - - - diff --git a/igvc_gazebo/launch/simulation.launch b/igvc_gazebo/launch/simulation.launch index ef220086b..4a8fe87e2 100644 --- a/igvc_gazebo/launch/simulation.launch +++ b/igvc_gazebo/launch/simulation.launch @@ -17,9 +17,6 @@ - - - diff --git a/igvc_gazebo/launch/simulation_low.launch b/igvc_gazebo/launch/simulation_low.launch index 297cc8d8e..5b1b68eaa 100644 --- a/igvc_gazebo/launch/simulation_low.launch +++ b/igvc_gazebo/launch/simulation_low.launch @@ -17,9 +17,6 @@ - - - diff --git a/igvc_navigation/CMakeLists.txt b/igvc_navigation/CMakeLists.txt index 19ff8688c..a6b1b43fd 100644 --- a/igvc_navigation/CMakeLists.txt +++ b/igvc_navigation/CMakeLists.txt @@ -214,5 +214,4 @@ add_subdirectory(src/swerve_drive) add_subdirectory(src/mapper) add_subdirectory(src/navigation_client) add_subdirectory(src/navigation_server) -add_subdirectory(src/wheel_odometer) add_subdirectory(src/swerve_wheel_odometer) diff --git a/igvc_navigation/launch/differential_drive.launch b/igvc_navigation/launch/differential_drive.launch deleted file mode 100644 index bad31bbd1..000000000 --- a/igvc_navigation/launch/differential_drive.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - diff --git a/igvc_navigation/launch/wheel_odometry.launch b/igvc_navigation/launch/wheel_odometry.launch deleted file mode 100644 index a3f65c108..000000000 --- a/igvc_navigation/launch/wheel_odometry.launch +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - diff --git a/igvc_navigation/src/wheel_odometer/CMakeLists.txt b/igvc_navigation/src/wheel_odometer/CMakeLists.txt deleted file mode 100644 index 3e5ce2591..000000000 --- a/igvc_navigation/src/wheel_odometer/CMakeLists.txt +++ /dev/null @@ -1,10 +0,0 @@ -add_executable(odometer Odometer.cpp) -add_dependencies(odometer ${catkin_EXPORTED_TARGETS}) -target_link_libraries(odometer ${catkin_LIBRARIES}) - -install( - TARGETS odometer - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/igvc_navigation/src/wheel_odometer/Odometer.cpp b/igvc_navigation/src/wheel_odometer/Odometer.cpp deleted file mode 100644 index 80114e39f..000000000 --- a/igvc_navigation/src/wheel_odometer/Odometer.cpp +++ /dev/null @@ -1,119 +0,0 @@ -#include "Odometer.h" -#include -#include -#include -#include -#include "Odometer.h" - -double wheel_separation; - -/** - * Coneverts wheel velocities to odometry message using trigonometry for calculations - * In the ros coordinate convention x is forward, y is leftward, and z is upward relative to the robot - * The position is published in an absolute reference frame relative to the initial position - * The velocities (twist) is in a reference frame relative to the robot - */ -void Odometer::enc_callback(const igvc_msgs::velocity_pair& msg) -{ - float leftVelocity = msg.left_velocity; - float rightVelocity = msg.right_velocity; - float deltaT = msg.duration; - - float angularVelocity = (rightVelocity - leftVelocity) / wheel_sep; - float deltaTheta = angularVelocity * deltaT; - float velocity = (rightVelocity + leftVelocity) / 2; - - geometry_msgs::Vector3 linearVelocities; - linearVelocities.z = 0; - - if (fabs(rightVelocity - leftVelocity) > 1e-4) - { // 1e-4 is the point where less of a difference is straight - linearVelocities.y = velocity * sin(deltaTheta); - linearVelocities.x = velocity * cos(deltaTheta); - } - else - { - // limit where turn radius is infinite (ie. straight line) - linearVelocities.y = 0; - linearVelocities.x = velocity; - } - - // set angular velocities - assuming 2D operation - geometry_msgs::Vector3 angularVelocities; - angularVelocities.x = 0.0; - angularVelocities.y = 0.0; - angularVelocities.z = angularVelocity; - - nav_msgs::Odometry odom; - odom.twist.twist.linear = linearVelocities; - odom.twist.twist.angular = angularVelocities; - - // update global positions - // note x and y velocities are local reference frame - convert to global then increment poition - float dx = linearVelocities.x * deltaT; - float dy = linearVelocities.y * deltaT; - x += dx * cos(yaw) - dy * sin(yaw); - y += dx * sin(yaw) + dy * cos(yaw); - yaw += deltaTheta; - - // enter message info for global position - odom.pose.pose.position.x = x; - odom.pose.pose.position.y = y; - odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(yaw); - - // Row-major representation of the 6x6 covariance matrix - // The orientation parameters use a fixed-axis representation. - // In order, the parameters are: - // (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis) - odom.twist.covariance = { 0.02, 1e-4, 1e-4, 1e-4, 1e-4, 1e-4, 1e-4, 0.25, 1e-4, 1e-4, 1e-4, 1e-4, - 1e-4, 1e-4, 1e6, 1e-4, 1e-4, 1e-4, 1e-4, 1e-4, 1e-4, 1e6, 1e-4, 1e-4, - 1e-4, 1e-4, 1e-4, 1e-4, 1e6, 1e-4, 1e-4, 1e-4, 1e-4, 1e-4, 1e-4, 0.62 }; - // the position covariance takes same form as twist covariance above - // this grows without bounds as error accumulates - disregard exact reading with high variance - odom.pose.covariance = { 0.01, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 0.01, 1e-6, 1e-6, 1e-6, 1e-6, - 1e-6, 1e-6, 1e6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e6, 1e-6, 1e-6, - 1e-6, 1e-6, 1e-6, 1e-6, 1e6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e6 }; - - // setting sequence of message - odom.header.seq = seq++; - - // setting reference frames - odom.header.frame_id = "odom"; - odom.child_frame_id = "base_footprint"; - - // set time then publish - odom.header.stamp = ros::Time::now(); - pub.publish(odom); -} - -Odometer::Odometer(ros::NodeHandle& nh) -{ - nh.param("wheel_sep", wheel_sep, 0.52); - - sub = nh.subscribe("/encoders", 10, &Odometer::enc_callback, this); - pub = nh.advertise("/wheel_odometry", 10); - - // initializing sequence number for messages - seq = 0; - - // initialize position - map published is relative to position at time t=0 - x = 0; - y = 0; - yaw = 0; -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "wheel_odom"); - ros::NodeHandle nh; - - ros::NodeHandle pNh("~"); - - pNh.param("wheel_separation", wheel_separation, 0.83); - - Odometer odom(nh); - - ROS_INFO_STREAM("wheel odometry node has started"); - - ros::spin(); -} diff --git a/igvc_navigation/src/wheel_odometer/Odometer.h b/igvc_navigation/src/wheel_odometer/Odometer.h deleted file mode 100644 index 6c441ee3b..000000000 --- a/igvc_navigation/src/wheel_odometer/Odometer.h +++ /dev/null @@ -1,30 +0,0 @@ -#ifndef ODOMETER_H -#define ODOMETER_H -#include -#include -#include - -class Odometer -{ -public: - Odometer(ros::NodeHandle&); - -private: - // ros infastructure - ros::Publisher pub; - ros::Subscriber sub; - tf::TransformBroadcaster odom_broadcaster; - int seq; - - // robot constant - double wheel_sep; - - // keeping track of global position - float x; - float y; - float yaw; - - // callback for encoder subscriber - void enc_callback(const igvc_msgs::velocity_pair&); -}; -#endif diff --git a/igvc_platform/CMakeLists.txt b/igvc_platform/CMakeLists.txt index 4def98066..6601fd7e2 100644 --- a/igvc_platform/CMakeLists.txt +++ b/igvc_platform/CMakeLists.txt @@ -169,7 +169,6 @@ if (CATKIN_ENABLE_TESTING) endif () add_subdirectory(src/imu) -add_subdirectory(src/joystick_driver) add_subdirectory(src/swerve_joystick_driver) add_subdirectory(src/motor_controller) #add_subdirectory(src/system_stats) diff --git a/igvc_platform/src/joystick_driver/CMakeLists.txt b/igvc_platform/src/joystick_driver/CMakeLists.txt deleted file mode 100644 index af62a4547..000000000 --- a/igvc_platform/src/joystick_driver/CMakeLists.txt +++ /dev/null @@ -1,10 +0,0 @@ -add_executable(joystick_driver joystick_driver.cpp) -add_dependencies(joystick_driver ${catkin_EXPORTED_TARGETS}) -target_link_libraries(joystick_driver ${catkin_LIBRARIES}) - -install( - TARGETS joystick_driver - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/igvc_platform/src/joystick_driver/joystick_driver.cpp b/igvc_platform/src/joystick_driver/joystick_driver.cpp deleted file mode 100644 index 931716b36..000000000 --- a/igvc_platform/src/joystick_driver/joystick_driver.cpp +++ /dev/null @@ -1,77 +0,0 @@ -#include -#include -#include -#include -#include -#include - -#include -#include - -ros::Publisher cmd_pub; -ros::NodeHandle* nhp; -std::unique_ptr updater_ptr; - -void joystick_diagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat) -{ - stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Joystick Online"); - double absoluteMaxVel, maxVel, maxVelIncr; - nhp->param(std::string("absoluteMaxVel"), absoluteMaxVel, 1.0); - nhp->param(std::string("maxVel"), maxVel, 1.6); - nhp->param(std::string("maxVelIncr"), maxVelIncr, 0.1); - stat.add("absolute_max_velocity", absoluteMaxVel); - stat.add("max_velocity", maxVel); - stat.add("max_velocity_increment", maxVelIncr); -} - -void joyCallback(const sensor_msgs::Joy::ConstPtr& msg) -{ - double absoluteMaxVel, maxVel, maxVelIncr; - nhp->param(std::string("absoluteMaxVel"), absoluteMaxVel, 1.0); - nhp->param(std::string("maxVel"), maxVel, 1.6); - nhp->param(std::string("maxVelIncr"), maxVelIncr, 0.1); - - if (msg->buttons[1]) - maxVel -= maxVelIncr; - else if (msg->buttons[3]) - maxVel += maxVelIncr; - maxVel = std::min(maxVel, absoluteMaxVel); - maxVel = std::max(maxVel, 0.0); - - nhp->setParam("maxVel", maxVel); - - int leftJoyAxis, rightJoyAxis; - bool leftInverted, rightInverted; - nhp->param(std::string("leftAxis"), leftJoyAxis, 1); - nhp->param(std::string("rightAxis"), rightJoyAxis, 4); - nhp->param(std::string("leftInverted"), leftInverted, false); - nhp->param(std::string("rightInverted"), rightInverted, false); - - updater_ptr->update(); - - igvc_msgs::velocity_pair cmd; - cmd.left_velocity = msg->axes[leftJoyAxis] * maxVel * (leftInverted ? -1.0 : 1.0); - cmd.right_velocity = msg->axes[rightJoyAxis] * maxVel * (rightInverted ? -1.0 : 1.0); - cmd.header.stamp = ros::Time::now(); - - cmd_pub.publish(cmd); -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "joystick_driver"); - ros::NodeHandle nh; - nhp = new ros::NodeHandle("~"); - - cmd_pub = nh.advertise("/motors", 1); - - ros::Subscriber joy_sub = nh.subscribe("/joy", 1, joyCallback); - - updater_ptr = std::make_unique(); - updater_ptr->setHardwareID("Joystick"); - updater_ptr->add("Joystick Diagnostic", joystick_diagnostic); - - ros::spin(); - delete nhp; - return 0; -} diff --git a/igvc_platform/src/tests/CMakeLists.txt b/igvc_platform/src/tests/CMakeLists.txt index b08e04750..3b9dd8855 100644 --- a/igvc_platform/src/tests/CMakeLists.txt +++ b/igvc_platform/src/tests/CMakeLists.txt @@ -1,15 +1,7 @@ -add_rostest_gtest(TestJoystickDriver test/test_joystick_driver.test test_joystick_driver.cpp) -add_dependencies(TestJoystickDriver ${catkin_EXPORTED_TARGETS} joystick_driver) -target_link_libraries(TestJoystickDriver ${catkin_LIBRARIES}) - add_rostest_gtest(TestSwerveJoystickDriver test/test_swerve_joystick_driver.test test_swerve_joystick_driver.cpp) add_dependencies(TestSwerveJoystickDriver ${catkin_EXPORTED_TARGETS} joystick_driver) target_link_libraries(TestSwerveJoystickDriver ${catkin_LIBRARIES}) -add_rostest_gtest(TestDifferentialDrive test/test_differential_drive.test test_differential_drive.cpp) -add_dependencies(TestDifferentialDrive ${catkin_EXPORTED_TARGETS} differential_drive) -target_link_libraries(TestDifferentialDrive ${catkin_LIBRARIES}) - add_rostest_gtest(TestScanToPointCloud test/test_scan_to_pointcloud.test test_scan_to_pointcloud.cpp) add_dependencies(TestScanToPointCloud ${catkin_EXPORTED_TARGETS} scan_to_pointcloud) target_link_libraries(TestScanToPointCloud ${catkin_LIBRARIES}) diff --git a/igvc_platform/src/tests/test/test_differential_drive.test b/igvc_platform/src/tests/test/test_differential_drive.test deleted file mode 100644 index 859be84cc..000000000 --- a/igvc_platform/src/tests/test/test_differential_drive.test +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/igvc_platform/src/tests/test/test_joystick_driver.test b/igvc_platform/src/tests/test/test_joystick_driver.test deleted file mode 100644 index 7f6d78594..000000000 --- a/igvc_platform/src/tests/test/test_joystick_driver.test +++ /dev/null @@ -1,4 +0,0 @@ - - - - diff --git a/igvc_platform/src/tests/test_differential_drive.cpp b/igvc_platform/src/tests/test_differential_drive.cpp deleted file mode 100644 index 879b938b6..000000000 --- a/igvc_platform/src/tests/test_differential_drive.cpp +++ /dev/null @@ -1,218 +0,0 @@ -#include -#include -#include -#include -#include - -class TestDifferentialDrive : public testing::Test -{ -public: - TestDifferentialDrive() : mock_pub(handle.advertise("/cmd_vel", 1)) - { - } - -protected: - ros::NodeHandle handle; - ros::Publisher mock_pub; -}; - -geometry_msgs::Twist createTwistMsg(float forward, float spin) -{ - geometry_msgs::Twist twist_msg; - twist_msg.angular.x = 0.0; - twist_msg.angular.y = 0.0; - twist_msg.angular.z = spin; - twist_msg.linear.x = forward; - twist_msg.linear.y = 0.0; - twist_msg.linear.z = 0.0; - - return twist_msg; -} - -igvc_msgs::velocity_pair twistToVelocity(geometry_msgs::Twist twist, double axle_length_, double max_vel_) -{ - double speed = twist.linear.x; - double rotation = twist.angular.z; - double vel_right = speed + (rotation * axle_length_) / 2; - double vel_left = speed - (rotation * axle_length_) / 2; - - double max_calc_vel = fmax(vel_right, vel_left); - if (max_calc_vel > max_vel_) - { - vel_right *= max_vel_ / max_calc_vel; - vel_left *= max_vel_ / max_calc_vel; - } - - igvc_msgs::velocity_pair vel_msg; - vel_msg.right_velocity = vel_right; - vel_msg.left_velocity = vel_left; - return vel_msg; -} - -TEST_F(TestDifferentialDrive, StopTest) -{ - MockSubscriber mock_sub("/motors"); - ASSERT_TRUE(mock_sub.waitForPublisher()); - ASSERT_TRUE(mock_sub.waitForSubscriber(mock_pub)); - - const float stop = 0.0; - mock_pub.publish(createTwistMsg(stop, stop)); - - ASSERT_TRUE(mock_sub.spinUntilMessages()); - - ASSERT_EQ(mock_sub.messages().size(), 1LU); - const igvc_msgs::velocity_pair& response = mock_sub.front(); - - EXPECT_EQ(response.left_velocity, stop); - EXPECT_EQ(response.right_velocity, stop); -} - -TEST_F(TestDifferentialDrive, ForwardTest) -{ - MockSubscriber mock_sub("/motors"); - ASSERT_TRUE(mock_sub.waitForPublisher()); - ASSERT_TRUE(mock_sub.waitForSubscriber(mock_pub)); - - const float forward = 2.0; - mock_pub.publish(createTwistMsg(forward, 0.0)); - - ASSERT_TRUE(mock_sub.spinUntilMessages()); - - ASSERT_EQ(mock_sub.messages().size(), 1LU); - const igvc_msgs::velocity_pair& response = mock_sub.front(); - - EXPECT_EQ(response.left_velocity, forward); - EXPECT_EQ(response.right_velocity, forward); -} - -TEST_F(TestDifferentialDrive, TurnTest) -{ - MockSubscriber mock_sub("/motors"); - ASSERT_TRUE(mock_sub.waitForPublisher()); - ASSERT_TRUE(mock_sub.waitForSubscriber(mock_pub)); - - double axle_length_; - handle.getParam("differential_drive/axle_length", axle_length_); - double max_vel_; - handle.getParam("differential_drive/max_vel", max_vel_); - - const float forward = 1.0; - const float spin = 1.0; - - const geometry_msgs::Twist twist_msg = createTwistMsg(forward, spin); - - mock_pub.publish(twist_msg); - - const igvc_msgs::velocity_pair vel_msg = twistToVelocity(twist_msg, axle_length_, max_vel_); - - ASSERT_TRUE(mock_sub.spinUntilMessages()); - - ASSERT_EQ(mock_sub.messages().size(), 1LU); - const igvc_msgs::velocity_pair& response = mock_sub.front(); - - EXPECT_EQ(response.left_velocity, vel_msg.left_velocity); - EXPECT_EQ(response.right_velocity, vel_msg.right_velocity); -} - -TEST_F(TestDifferentialDrive, SpinTest) -{ - MockSubscriber mock_sub("/motors"); - ASSERT_TRUE(mock_sub.waitForPublisher()); - ASSERT_TRUE(mock_sub.waitForSubscriber(mock_pub)); - - double axle_length_; - handle.getParam("differential_drive/axle_length", axle_length_); - double max_vel_; - handle.getParam("differential_drive/max_vel", max_vel_); - - const float forward = 0.0; - const float spin = 1.0; - - const geometry_msgs::Twist twist_msg = createTwistMsg(forward, spin); - - mock_pub.publish(twist_msg); - - const igvc_msgs::velocity_pair vel_msg = twistToVelocity(twist_msg, axle_length_, max_vel_); - - ASSERT_TRUE(mock_sub.spinUntilMessages()); - - ASSERT_EQ(mock_sub.messages().size(), 1LU); - const igvc_msgs::velocity_pair& response = mock_sub.front(); - - EXPECT_EQ(response.left_velocity, vel_msg.left_velocity); - EXPECT_EQ(response.right_velocity, vel_msg.right_velocity); -} - -TEST_F(TestDifferentialDrive, MaxSpeedTest) -{ - MockSubscriber mock_sub("/motors"); - ASSERT_TRUE(mock_sub.waitForPublisher()); - ASSERT_TRUE(mock_sub.waitForSubscriber(mock_pub)); - - double axle_length_; - handle.getParam("differential_drive/axle_length", axle_length_); - double max_vel_; - handle.getParam("differential_drive/max_vel", max_vel_); - - const float forward = 4.0; - const float spin = 1.0; - - const geometry_msgs::Twist twist_msg = createTwistMsg(forward, spin); - - mock_pub.publish(twist_msg); - - const igvc_msgs::velocity_pair vel_msg = twistToVelocity(twist_msg, axle_length_, max_vel_); - - ASSERT_TRUE(mock_sub.spinUntilMessages()); - - ASSERT_EQ(mock_sub.messages().size(), 1LU); - const igvc_msgs::velocity_pair& response = mock_sub.front(); - - EXPECT_EQ(response.left_velocity, vel_msg.left_velocity); - EXPECT_EQ(response.right_velocity, vel_msg.right_velocity); -} - -TEST_F(TestDifferentialDrive, NoiseIgnoreTest) -{ - MockSubscriber mock_sub("/motors"); - ASSERT_TRUE(mock_sub.waitForPublisher()); - ASSERT_TRUE(mock_sub.waitForSubscriber(mock_pub)); - - double axle_length_; - handle.getParam("differential_drive/axle_length", axle_length_); - double max_vel_; - handle.getParam("differential_drive/max_vel", max_vel_); - - const float forward = 4.0; - const float spin = 1.0; - - geometry_msgs::Twist twist_msg; - twist_msg.angular.x = 2.0; - twist_msg.angular.y = 3.0; - twist_msg.angular.z = spin; - twist_msg.linear.x = forward; - twist_msg.linear.y = 4.0; - twist_msg.linear.z = 2.0; - - mock_pub.publish(twist_msg); - - const igvc_msgs::velocity_pair vel_msg = twistToVelocity(twist_msg, axle_length_, max_vel_); - - ASSERT_TRUE(mock_sub.spinUntilMessages()); - - ASSERT_EQ(mock_sub.messages().size(), 1LU); - const igvc_msgs::velocity_pair& response = mock_sub.front(); - - EXPECT_EQ(response.left_velocity, vel_msg.left_velocity); - EXPECT_EQ(response.right_velocity, vel_msg.right_velocity); -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "test_differential_drive"); - testing::InitGoogleTest(&argc, argv); - - int ret = RUN_ALL_TESTS(); - ros::shutdown(); - return ret; -} diff --git a/igvc_platform/src/tests/test_joystick_driver.cpp b/igvc_platform/src/tests/test_joystick_driver.cpp deleted file mode 100644 index 839754c04..000000000 --- a/igvc_platform/src/tests/test_joystick_driver.cpp +++ /dev/null @@ -1,126 +0,0 @@ -// -// Created by matt on 1/29/16. -// - -#include -#include -#include -#include -#include - -class TestJoystickDriver : public testing::Test -{ -public: - TestJoystickDriver() : mock_joy_pub(handle.advertise("/joy", 1)) - { - } - -protected: - ros::NodeHandle handle; - ros::Publisher mock_joy_pub; -}; - -sensor_msgs::Joy createJoyMsg(float left, float right) -{ - sensor_msgs::Joy joy_msg; - joy_msg.axes = { 0, left, 0, 0, right }; - joy_msg.buttons = { 0, 0, 0, 0 }; - - return joy_msg; -}; - -TEST_F(TestJoystickDriver, FullForward) -{ - MockSubscriber mock_sub("/motors"); - ASSERT_TRUE(mock_sub.waitForPublisher()); - ASSERT_TRUE(mock_sub.waitForSubscriber(mock_joy_pub)); - - const float full_speed = 1.0; - mock_joy_pub.publish(createJoyMsg(full_speed, full_speed)); - - ASSERT_TRUE(mock_sub.spinUntilMessages()); - - ASSERT_EQ(mock_sub.messages().size(), 1LU); - const igvc_msgs::velocity_pair& response = mock_sub.front(); - - EXPECT_EQ(response.left_velocity, full_speed); - EXPECT_EQ(response.right_velocity, full_speed); -} - -TEST_F(TestJoystickDriver, FullReverse) -{ - MockSubscriber mock_sub("/motors"); - ASSERT_TRUE(mock_sub.waitForPublisher()); - ASSERT_TRUE(mock_sub.waitForSubscriber(mock_joy_pub)); - const float full_speed = 1.0; - mock_joy_pub.publish(createJoyMsg(-full_speed, -full_speed)); - - ASSERT_TRUE(mock_sub.spinUntilMessages()); - - ASSERT_EQ(mock_sub.messages().size(), 1LU); - const igvc_msgs::velocity_pair& response = mock_sub.front(); - - EXPECT_EQ(response.left_velocity, -full_speed); - EXPECT_EQ(response.right_velocity, -full_speed); -} - -TEST_F(TestJoystickDriver, SpinRight) -{ - MockSubscriber mock_sub("/motors"); - ASSERT_TRUE(mock_sub.waitForPublisher()); - ASSERT_TRUE(mock_sub.waitForSubscriber(mock_joy_pub)); - const float full_speed = 1.0; - mock_joy_pub.publish(createJoyMsg(full_speed, -full_speed)); - - ASSERT_TRUE(mock_sub.spinUntilMessages()); - - ASSERT_EQ(mock_sub.messages().size(), 1LU); - const igvc_msgs::velocity_pair& response = mock_sub.front(); - - EXPECT_EQ(response.left_velocity, full_speed); - EXPECT_EQ(response.right_velocity, -full_speed); -} - -TEST_F(TestJoystickDriver, SpinLeft) -{ - MockSubscriber mock_sub("/motors"); - ASSERT_TRUE(mock_sub.waitForPublisher()); - ASSERT_TRUE(mock_sub.waitForSubscriber(mock_joy_pub)); - const float full_speed = 1.0; - mock_joy_pub.publish(createJoyMsg(-full_speed, full_speed)); - - ASSERT_TRUE(mock_sub.spinUntilMessages()); - - ASSERT_EQ(mock_sub.messages().size(), 1LU); - const igvc_msgs::velocity_pair& response = mock_sub.front(); - - EXPECT_EQ(response.left_velocity, -full_speed); - EXPECT_EQ(response.right_velocity, full_speed); -} - -TEST_F(TestJoystickDriver, HalfSpeedForward) -{ - MockSubscriber mock_sub("/motors"); - ASSERT_TRUE(mock_sub.waitForPublisher()); - ASSERT_TRUE(mock_sub.waitForSubscriber(mock_joy_pub)); - const float half_speed = 0.5; - mock_joy_pub.publish(createJoyMsg(half_speed, half_speed)); - - ASSERT_TRUE(mock_sub.spinUntilMessages()); - - ASSERT_EQ(mock_sub.messages().size(), 1LU); - const igvc_msgs::velocity_pair& response = mock_sub.front(); - - EXPECT_EQ(response.left_velocity, half_speed); - EXPECT_EQ(response.right_velocity, half_speed); -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "test_joystick_driver"); - testing::InitGoogleTest(&argc, argv); - - int ret = RUN_ALL_TESTS(); - ros::shutdown(); - return ret; -} diff --git a/igvc_utils/CMakeLists.txt b/igvc_utils/CMakeLists.txt index 218cd3367..c9a14f8e8 100644 --- a/igvc_utils/CMakeLists.txt +++ b/igvc_utils/CMakeLists.txt @@ -160,7 +160,6 @@ install(DIRECTORY include add_subdirectory(src/pid_tester) add_subdirectory(src/ethernet) -add_subdirectory(src/speed_compare) add_subdirectory(src/system_stats) add_subdirectory(src/quaternion_to_rpy) add_subdirectory(src/state) \ No newline at end of file diff --git a/igvc_utils/src/speed_compare/CMakeLists.txt b/igvc_utils/src/speed_compare/CMakeLists.txt deleted file mode 100644 index 9e61d7f6d..000000000 --- a/igvc_utils/src/speed_compare/CMakeLists.txt +++ /dev/null @@ -1,10 +0,0 @@ -add_executable(speed_compare main.cpp) -add_dependencies(speed_compare ${catkin_EXPORTED_TARGETS}) -target_link_libraries(speed_compare ${catkin_LIBRARIES}) - -install( - TARGETS speed_compare - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/igvc_utils/src/speed_compare/main.cpp b/igvc_utils/src/speed_compare/main.cpp deleted file mode 100644 index fb1a1b483..000000000 --- a/igvc_utils/src/speed_compare/main.cpp +++ /dev/null @@ -1,74 +0,0 @@ -#include -#include -#include -#include -#include -#include - -bool enabled = false; -bool first = true; -std::ofstream file; -float target_left_vel; -float target_right_vel; -ros::Time start_time; - -void enabled_callback(const std_msgs::BoolConstPtr& msg) -{ - enabled = msg->data; - if (enabled && first) - { - first = false; - ROS_INFO_STREAM("The robot is enabled. Data will be written"); - } - else if (!enabled) - { - first = true; - ROS_INFO_STREAM("The robot is disabled. Data will not be written."); - } -} - -void motors_callback(const igvc_msgs::velocity_pair& msg) -{ - if (enabled) - { - target_left_vel = msg.left_velocity; - target_right_vel = msg.right_velocity; - } -} - -void encoders_callback(const igvc_msgs::velocity_pair& msg) -{ - if (enabled) - { - ROS_INFO_STREAM("Data stored"); - ros::Duration time_diff = msg.header.stamp - start_time; - file << time_diff.sec << "." << time_diff.nsec << ","; - file << msg.left_velocity << "," << msg.right_velocity << ","; - file << target_left_vel << "," << target_right_vel << std::endl; - } -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "pidtester"); - - ros::NodeHandle nh; - ros::NodeHandle pNh("~"); - - start_time = ros::Time::now(); - - ros::Subscriber enabled_sub = nh.subscribe("/robot_enabled", 1, enabled_callback); - ros::Subscriber motors_sub = nh.subscribe("/motors", 1, motors_callback); - ros::Subscriber encoders_sub = nh.subscribe("/encoders", 1, encoders_callback); - - std::string location; - - assertions::getParam(pNh, "file", location); - - file.open(location); - file << "timestamp, left_velocity, right_velocity, target_left_velocity, target_right_velocity" << std::endl; - - ros::spin(); - - return 0; -} From af1cf4f999d8cab87c7bc07b04ac94e06da5cd87 Mon Sep 17 00:00:00 2001 From: VAM7686 Date: Wed, 9 Mar 2022 20:59:18 -0500 Subject: [PATCH 21/27] formatted --- .../src/swerve_wheel_odometer/swerve_odometer.cpp | 9 ++++++--- .../src/swerve_wheel_odometer/swerve_odometer.h | 2 +- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/igvc_navigation/src/swerve_wheel_odometer/swerve_odometer.cpp b/igvc_navigation/src/swerve_wheel_odometer/swerve_odometer.cpp index 68cffa67a..f06c8fa9c 100644 --- a/igvc_navigation/src/swerve_wheel_odometer/swerve_odometer.cpp +++ b/igvc_navigation/src/swerve_wheel_odometer/swerve_odometer.cpp @@ -208,11 +208,13 @@ void SwerveOdometer::enc_callback(const igvc_msgs::velocity_quad msg) auto icr_wh = std::array{ positions_list[i][0] - average_intersection[0], positions_list[i][1] - average_intersection[1] }; - if (isinf(icr_wh[0]) || isinf(icr_wh[1])) { + if (isinf(icr_wh[0]) || isinf(icr_wh[1])) + { ROS_DEBUG_STREAM("icr_wh is inf! Discarding!"); continue; } - if (isclose(icr_wh[0], 0) || isclose(icr_wh[1], 0)) { + if (isclose(icr_wh[0], 0) || isclose(icr_wh[1], 0)) + { ROS_DEBUG_STREAM("icr_wh for wheel " << i << " is zero! Discarding!"); continue; } @@ -252,7 +254,8 @@ void SwerveOdometer::enc_callback(const igvc_msgs::velocity_quad msg) SwerveOdometer::pubOdometry(); } -void SwerveOdometer::pubOdometry() { +void SwerveOdometer::pubOdometry() +{ geometry_msgs::Vector3 linearVelocities; linearVelocities.z = 0; linearVelocities.x = linear_x_; diff --git a/igvc_navigation/src/swerve_wheel_odometer/swerve_odometer.h b/igvc_navigation/src/swerve_wheel_odometer/swerve_odometer.h index 301f771e9..261036b3d 100644 --- a/igvc_navigation/src/swerve_wheel_odometer/swerve_odometer.h +++ b/igvc_navigation/src/swerve_wheel_odometer/swerve_odometer.h @@ -40,7 +40,7 @@ class SwerveOdometer double linear_y_; // [m/s] double angular_; // [rad/s] - double angular_limit_; + double angular_limit_; double inf_tol; double intersection_tol_; From 294ec577e6f285ed779d45e87711a1338dfa561d Mon Sep 17 00:00:00 2001 From: matthewhannay567 Date: Wed, 6 Apr 2022 20:28:59 -0400 Subject: [PATCH 22/27] Added masking for slope/traversability layer --- .../src/mapper/traversability_layer.cpp | 2 +- igvc_perception/CMakeLists.txt | 1 + igvc_perception/config/elevation_mapping.yaml | 2 +- igvc_perception/config/slope_filter.yaml | 13 ++++++---- igvc_perception/package.xml | 1 + .../src/slope_filter/slope_filter.cpp | 24 +++++++++++++++---- .../src/slope_filter/slope_filter.h | 5 +++- 7 files changed, 36 insertions(+), 12 deletions(-) diff --git a/igvc_navigation/src/mapper/traversability_layer.cpp b/igvc_navigation/src/mapper/traversability_layer.cpp index 4959dcce9..00a0744aa 100644 --- a/igvc_navigation/src/mapper/traversability_layer.cpp +++ b/igvc_navigation/src/mapper/traversability_layer.cpp @@ -82,7 +82,7 @@ void TraversabilityLayer::slopeMapCallback(const grid_map_msgs::GridMap &slope_m { grid_map::Position pos; slope_map.getPosition((*it), pos); - if (map_.isInside(pos)) + if (map_.isInside(pos) && slope_map.get("mask_dilated")((*it)[0], (*it)[1]) > 0) { float slope = slope_map.get("slope")((*it)[0], (*it)[1]); grid_map::Index map_index; diff --git a/igvc_perception/CMakeLists.txt b/igvc_perception/CMakeLists.txt index b1a49812a..8ea62b168 100644 --- a/igvc_perception/CMakeLists.txt +++ b/igvc_perception/CMakeLists.txt @@ -25,6 +25,7 @@ find_package(catkin REQUIRED COMPONENTS grid_map_core grid_map_ros grid_map_msgs + grid_map_cv ) set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) diff --git a/igvc_perception/config/elevation_mapping.yaml b/igvc_perception/config/elevation_mapping.yaml index f067c5085..926e91526 100644 --- a/igvc_perception/config/elevation_mapping.yaml +++ b/igvc_perception/config/elevation_mapping.yaml @@ -19,7 +19,7 @@ min_variance: 0.000009 max_variance: 0.01 mahalanobis_distance_threshold: 2.5 multi_height_noise: 0.0000009 -fused_map_publishing_rate: 0 +fused_map_publishing_rate: 1.0 enable_visibility_cleanup: false # SENSOR PARAMETERS diff --git a/igvc_perception/config/slope_filter.yaml b/igvc_perception/config/slope_filter.yaml index f20e777ac..ad96525d3 100644 --- a/igvc_perception/config/slope_filter.yaml +++ b/igvc_perception/config/slope_filter.yaml @@ -4,11 +4,11 @@ slope_map_filters: type: gridMapFilters/BufferNormalizerFilter # Delete unneeded layers. - - name: delete_original_layers - type: gridMapFilters/DeletionFilter - params: - layers: [horizontal_variance_x, horizontal_variance_y, horizontal_variance_xy, - color, time, standard_deviation, horizontal_standard_deviation, two_sigma_bound] # List of layers. + # - name: delete_original_layers + # type: gridMapFilters/DeletionFilter + # params: + # layers: [horizontal_variance_x, horizontal_variance_y, horizontal_variance_xy, + # color, time, standard_deviation, horizontal_standard_deviation, two_sigma_bound] # List of layers. # Boxblur as an alternative to the inpaint and radial blurring filter above. - name: boxblur @@ -36,3 +36,6 @@ slope_map_filters: params: output_layer: slope expression: acos(normal_vectors_z) + +erode_radius: 7 +dilate_radius: 6 \ No newline at end of file diff --git a/igvc_perception/package.xml b/igvc_perception/package.xml index 0d189c4ad..958b84294 100644 --- a/igvc_perception/package.xml +++ b/igvc_perception/package.xml @@ -46,6 +46,7 @@ grid_map_core grid_map_ros grid_map_msgs + grid_map_cv image_transport cv_bridge diff --git a/igvc_perception/src/slope_filter/slope_filter.cpp b/igvc_perception/src/slope_filter/slope_filter.cpp index 065fca83b..b9920e6ea 100644 --- a/igvc_perception/src/slope_filter/slope_filter.cpp +++ b/igvc_perception/src/slope_filter/slope_filter.cpp @@ -1,13 +1,19 @@ #include "slope_filter.h" #include +#include +#include TraversabilityFilter::TraversabilityFilter() : filter_chain_("grid_map::GridMap") { + ROS_INFO_STREAM("RAW INFO RECEIVED"); private_nh_ = ros::NodeHandle("~"); - elevation_map_sub_ = private_nh_.subscribe("/elevation_mapping/elevation_map_raw", 1, + elevation_map_sub_ = private_nh_.subscribe("/elevation_mapping/elevation_map", 1, &TraversabilityFilter::elevationMapCallback, this); traversability_map_pub_ = private_nh_.advertise("/slope/gridmap", 1); + assertions::getParam(private_nh_, "erode_radius", erode_radius); + assertions::getParam(private_nh_, "dilate_radius", dilate_radius); + // setup filter chain if (!filter_chain_.configure("/slope_filter/slope_map_filters", private_nh_)) { @@ -21,15 +27,25 @@ void TraversabilityFilter::elevationMapCallback(const grid_map_msgs::GridMap& me grid_map::GridMap input_map; grid_map::GridMapRosConverter::fromMessage(message, input_map); - grid_map::GridMap output_map; - if (!filter_chain_.update(input_map, output_map)) + grid_map::GridMap slope_map; + if (!filter_chain_.update(input_map, slope_map)) { ROS_ERROR_STREAM("Could not update the grid map filter chain!"); return; } + cv::Mat elevation_data_mask; + grid_map::GridMapCvConverter::toImage(input_map, "elevation", CV_8U, -1.0, 0.0, elevation_data_mask); + grid_map::GridMapCvConverter::addLayerFromImage(elevation_data_mask, "mask_original", slope_map, 0, 255, 0); + + cv::erode(elevation_data_mask, elevation_data_mask, cv::getStructuringElement(cv::MORPH_RECT, cv::Size(2*erode_radius+1, 2*erode_radius+1), cv::Point(erode_radius, erode_radius))); + grid_map::GridMapCvConverter::addLayerFromImage(elevation_data_mask, "mask_eroded", slope_map, 0, 255, 0); + + cv::dilate(elevation_data_mask, elevation_data_mask, cv::getStructuringElement(cv::MORPH_RECT, cv::Size(2*dilate_radius+1, 2*dilate_radius+1), cv::Point(dilate_radius, dilate_radius))); + grid_map::GridMapCvConverter::addLayerFromImage(elevation_data_mask, "mask_dilated", slope_map, 0, 255, 0); + grid_map_msgs::GridMap output_message; - grid_map::GridMapRosConverter::toMessage(output_map, output_message); + grid_map::GridMapRosConverter::toMessage(slope_map, output_message); traversability_map_pub_.publish(output_message); } diff --git a/igvc_perception/src/slope_filter/slope_filter.h b/igvc_perception/src/slope_filter/slope_filter.h index 61a72bfc2..760ed1de2 100644 --- a/igvc_perception/src/slope_filter/slope_filter.h +++ b/igvc_perception/src/slope_filter/slope_filter.h @@ -2,7 +2,7 @@ #define SRC_SLOPE_FILTER_H #include -#include +#include #include #include @@ -17,6 +17,9 @@ class TraversabilityFilter ros::Publisher traversability_map_pub_; filters::FilterChain filter_chain_; + int erode_radius; + int dilate_radius; + void elevationMapCallback(const grid_map_msgs::GridMap& message); }; From 49ee831cd4b36b7f36c44b38b960b00fe3509137 Mon Sep 17 00:00:00 2001 From: matthewhannay567 Date: Wed, 6 Apr 2022 20:51:24 -0400 Subject: [PATCH 23/27] first pass --- igvc_description/launch/spawn_jessii.launch | 34 - igvc_description/urdf/jessi.urdf | 411 ------------ igvc_description/urdf/jessii.urdf.xacro | 670 -------------------- igvc_gazebo/CMakeLists.txt | 1 - igvc_gazebo/config/igvc_joint_control.yaml | 22 - igvc_gazebo/launch/autonav.launch | 1 - igvc_gazebo/launch/autonav_low.launch | 1 - igvc_gazebo/launch/autonav_mesh.launch | 1 - igvc_gazebo/launch/igvc_control.launch | 25 - igvc_gazebo/launch/qual_low.launch | 1 - igvc_gazebo/launch/qualification.launch | 1 - igvc_gazebo/launch/ramp_lane.launch | 1 - igvc_gazebo/launch/simulation.launch | 47 +- igvc_gazebo/launch/simulation_low.launch | 47 +- igvc_gazebo/nodes/control/CMakeLists.txt | 10 - igvc_gazebo/nodes/control/main.cpp | 176 ----- 16 files changed, 22 insertions(+), 1427 deletions(-) delete mode 100644 igvc_description/launch/spawn_jessii.launch delete mode 100644 igvc_description/urdf/jessi.urdf delete mode 100644 igvc_description/urdf/jessii.urdf.xacro delete mode 100644 igvc_gazebo/config/igvc_joint_control.yaml delete mode 100644 igvc_gazebo/launch/igvc_control.launch delete mode 100644 igvc_gazebo/nodes/control/CMakeLists.txt delete mode 100644 igvc_gazebo/nodes/control/main.cpp diff --git a/igvc_description/launch/spawn_jessii.launch b/igvc_description/launch/spawn_jessii.launch deleted file mode 100644 index 34b1b0311..000000000 --- a/igvc_description/launch/spawn_jessii.launch +++ /dev/null @@ -1,34 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/igvc_description/urdf/jessi.urdf b/igvc_description/urdf/jessi.urdf deleted file mode 100644 index 610145140..000000000 --- a/igvc_description/urdf/jessi.urdf +++ /dev/null @@ -1,411 +0,0 @@ - - - - - - - - - - - - - - Gazebo/Grey - - - - Gazebo/Grey - 0.2 - 0.2 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Grey - 0.7 - 0.75 - - - - - - - - - - - - - - - - - - - - - - Gazebo/Grey - 0.7 - 0.75 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - false - 10 - - - - - - 100 - 3.6 - -3.14159265359 - 3.14159265359 - - - - 0.05 - 30 - 0.02 - - - - /scan - /lidar - - - - - - - - - - - - - - - - - - - 2.239 - 1.995 - - 640 - 512 - - - 0.1 - 100 - - - - 1 - 30 - - true - 30.0 - usb_cam_center - image_raw - camera_info - optical_cam_center - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - gazebo_ros_control/DefaultRobotHWSim - - - /imu/calibrate - 200.0 - imu - imu - /imu - 0 0 0 - 0 0 0 - 0.00000001 - 0.00000001 0.00000001 0.00000001 - 0.00000001 0.00000001 0.00000001 - 0.00000001 0.00000001 0.00000001 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 1.5707963 - - - - /magnetometer/calibrate - 200.0 - magnetometer - magnetometer - /magnetometer - 0 0 0 - 0 0 0 - 0.00000001 - 0.00000001 0.00000001 0.00000001 - 0.00000001 0.00000001 0.00000001 - 0.00000001 0.00000001 0.00000001 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0 - - - - 20.0 - base_link - base_link - /fix - /fix_velocity - 33.774497 - -84.405001 - 309.0 - 0.001 0.001 0.001 - 0.0001 0.0001 0.0001 - 0 0 0 - 0.005 0.005 0.05 - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - imu - front_ball - 0 0 0 0 0 0 - - - - 0 - 0.2 - - - 0 - 0.2 - - - - - - diff --git a/igvc_description/urdf/jessii.urdf.xacro b/igvc_description/urdf/jessii.urdf.xacro deleted file mode 100644 index 148d5693c..000000000 --- a/igvc_description/urdf/jessii.urdf.xacro +++ /dev/null @@ -1,670 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - - hardware_interface/EffortJointInterface - - 1 - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - - hardware_interface/EffortJointInterface - 1 - - - - - - - - - - - - - - 10 - - - - 0.000000001 - 0.2 - 1e+10 - 10 - 0.01 - 0.005 - - - - - - - - - - - - - - - 0.7 - 0.75 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 0.2 - 0.25 - - - - - base_link - caster - 0 0 0 0 0 0 - - - - 0 - 0.2 - - - 0 - 0.2 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1.2290609 - - 640 - 480 - - - 0.1 - 100 - - - - 1 - 30 - - true - 30 - cam/center/raw - image - camera_info - cam/center_optical - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1.2290609 - - 640 - 480 - - - 0.1 - 100 - - - - 1 - 30 - - true - 30 - cam/right/raw - image - camera_info - cam/right_optical - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - 1.2290609 - - 640 - 480 - - - 0.1 - 100 - - - - 1 - 30 - - true - 30 - cam/left/raw - image - camera_info - cam/left_optical - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - 0.0 - - - - - - - - - - - Gazebo/Grey - - - - - - - Gazebo/Grey - - - - - - - - - - - Gazebo/Grey - 0.2 - 0.2 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - gazebo_ros_control/DefaultRobotHWSim - - - - /imu/calibrate - 200.0 - imu - imu - /imu - 0 0 0 - 0.00000005 - 0.00000001 0.00000001 0.00000001 - 0.00000001 0.00000001 0.00000001 - 0.00000001 0.00000001 0.00000001 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - -1.57079632679 - - - - /magnetometer/calibrate - 200.0 - magnetometer - magnetometer - /magnetometer - 0 0 0 - 0.00000005 - 0.00000001 0.00000001 0.00000001 - 0.00000001 0.00000001 0.00000001 - 0.00000001 0.00000001 0.00000001 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - -1.57079632679 - - - - 200.0 - .0000489162 - magnetometer - /magnetometer/vector - 0.0 - -5.2290 - 62.2126 - 0 0 0 - 0 0 0 - 0 0 0 - 0.00000005 0.00000005 0.00000005 - - - - 20.0 - base_link - base_link - /fix - /fix_velocity - 33.774497 - -84.405001 - 90 - 309.0 - 0.001 0.001 0.001 - 0.0001 0.0001 0.0001 - 0 0 0 - 0.005 0.005 0.05 - - - - true - 200.0 - base_footprint - ground_truth/state_raw - 0.00 - world - - 0 0 0 - 0 0 0 - - - - - - diff --git a/igvc_gazebo/CMakeLists.txt b/igvc_gazebo/CMakeLists.txt index 6be28971a..8a0b91ee1 100644 --- a/igvc_gazebo/CMakeLists.txt +++ b/igvc_gazebo/CMakeLists.txt @@ -47,7 +47,6 @@ install(DIRECTORY config/ ) add_subdirectory(nodes/magnetometer) -add_subdirectory(nodes/control) add_subdirectory(nodes/swerve_control) add_subdirectory(nodes/scan_to_pointcloud) add_subdirectory(nodes/ground_truth) diff --git a/igvc_gazebo/config/igvc_joint_control.yaml b/igvc_gazebo/config/igvc_joint_control.yaml deleted file mode 100644 index 8be4dcd22..000000000 --- a/igvc_gazebo/config/igvc_joint_control.yaml +++ /dev/null @@ -1,22 +0,0 @@ - # Publish all joint states ----------------------------------- - joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 50 - - # effort Controllers --------------------------------------- - left_wheel_effort_controller: - type: effort_controllers/JointEffortController - joint: left_axle - right_wheel_effort_controller: - type: effort_controllers/JointEffortController - joint: right_axle - - # shock controllers ---------------------------------------- - left_wheel_shock_controller: - joint: left_drive_shock - type: effort_controllers/JointPositionController - pid: {p: 10000, i: 0.0, d: 20} - right_wheel_shock_controller: - joint: right_drive_shock - type: effort_controllers/JointPositionController - pid: {p: 10000, i: 0.0, d: 20} diff --git a/igvc_gazebo/launch/autonav.launch b/igvc_gazebo/launch/autonav.launch index 74dac1750..e001a3512 100644 --- a/igvc_gazebo/launch/autonav.launch +++ b/igvc_gazebo/launch/autonav.launch @@ -22,7 +22,6 @@ - diff --git a/igvc_gazebo/launch/autonav_low.launch b/igvc_gazebo/launch/autonav_low.launch index 016ecb911..6ddf23dfb 100644 --- a/igvc_gazebo/launch/autonav_low.launch +++ b/igvc_gazebo/launch/autonav_low.launch @@ -22,7 +22,6 @@ - diff --git a/igvc_gazebo/launch/autonav_mesh.launch b/igvc_gazebo/launch/autonav_mesh.launch index 9436225f4..2b4ba8fe4 100644 --- a/igvc_gazebo/launch/autonav_mesh.launch +++ b/igvc_gazebo/launch/autonav_mesh.launch @@ -22,7 +22,6 @@ - diff --git a/igvc_gazebo/launch/igvc_control.launch b/igvc_gazebo/launch/igvc_control.launch deleted file mode 100644 index 1f9af7338..000000000 --- a/igvc_gazebo/launch/igvc_control.launch +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/igvc_gazebo/launch/qual_low.launch b/igvc_gazebo/launch/qual_low.launch index 70be87820..ba56a5562 100644 --- a/igvc_gazebo/launch/qual_low.launch +++ b/igvc_gazebo/launch/qual_low.launch @@ -31,7 +31,6 @@ - diff --git a/igvc_gazebo/launch/qualification.launch b/igvc_gazebo/launch/qualification.launch index 8e24acd64..6e63e03d3 100644 --- a/igvc_gazebo/launch/qualification.launch +++ b/igvc_gazebo/launch/qualification.launch @@ -31,7 +31,6 @@ - diff --git a/igvc_gazebo/launch/ramp_lane.launch b/igvc_gazebo/launch/ramp_lane.launch index 1ef27cfbb..80c2f0044 100644 --- a/igvc_gazebo/launch/ramp_lane.launch +++ b/igvc_gazebo/launch/ramp_lane.launch @@ -19,7 +19,6 @@ - diff --git a/igvc_gazebo/launch/simulation.launch b/igvc_gazebo/launch/simulation.launch index eac1002ac..4a8fe87e2 100644 --- a/igvc_gazebo/launch/simulation.launch +++ b/igvc_gazebo/launch/simulation.launch @@ -17,9 +17,6 @@ - - - @@ -37,41 +34,19 @@ - - - - - - - - - - - - - - - - - - - - - - + + + + + + + - - - - - - - - + + + - - - + diff --git a/igvc_gazebo/launch/simulation_low.launch b/igvc_gazebo/launch/simulation_low.launch index 9357a7802..5b1b68eaa 100644 --- a/igvc_gazebo/launch/simulation_low.launch +++ b/igvc_gazebo/launch/simulation_low.launch @@ -17,9 +17,6 @@ - - - @@ -37,41 +34,19 @@ - - - - - - - - - - - - - - - - - - - - - - + + + + + + + - - - - - - - - + + + - - - + diff --git a/igvc_gazebo/nodes/control/CMakeLists.txt b/igvc_gazebo/nodes/control/CMakeLists.txt deleted file mode 100644 index e22cf4a9d..000000000 --- a/igvc_gazebo/nodes/control/CMakeLists.txt +++ /dev/null @@ -1,10 +0,0 @@ -add_executable(control main.cpp) -add_dependencies(control ${catkin_EXPORTED_TARGETS}) -target_link_libraries(control ${catkin_LIBRARIES}) - -install( - TARGETS control - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/igvc_gazebo/nodes/control/main.cpp b/igvc_gazebo/nodes/control/main.cpp deleted file mode 100644 index 0ea0b64ad..000000000 --- a/igvc_gazebo/nodes/control/main.cpp +++ /dev/null @@ -1,176 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include - -double speed_set_point_left = 0.0; -double speed_set_point_right = 0.0; -double speed_measured_left = 0.0; -double speed_measured_right = 0.0; -double wheel_radius; - -void speedCallback(const igvc_msgs::velocity_pair::ConstPtr &msg) -{ - if (msg->left_velocity == msg->left_velocity) - { - speed_set_point_left = msg->left_velocity; - } - if (msg->right_velocity == msg->right_velocity) - { - speed_set_point_right = msg->right_velocity; - } -} - -void jointStateCallback(const sensor_msgs::JointStateConstPtr &msg) -{ - auto iter = std::find(msg->name.begin(), msg->name.end(), std::string{ "left_axle" }); - - if (iter != msg->name.end()) - { - auto index = std::distance(msg->name.begin(), iter); - - speed_measured_left = (msg->velocity[index]) * (wheel_radius); - } - - iter = std::find(msg->name.begin(), msg->name.end(), std::string{ "right_axle" }); - - if (iter != msg->name.end()) - { - auto index = std::distance(msg->name.begin(), iter); - - speed_measured_right = (msg->velocity[index]) * (wheel_radius); - } - - // ROS_INFO_STREAM("right speed: " << speed_measured_right << " left speed: " << speed_measured_left); -} - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "jessii_controller"); - - ros::NodeHandle handle; - ros::NodeHandle pNh("~"); - - ros::Publisher left_wheel_effort_publisher = - handle.advertise("/left_wheel_effort_controller/command", 1); - ros::Publisher right_wheel_effort_publisher = - handle.advertise("/right_wheel_effort_controller/command", 1); - ros::Publisher wheel_speed_publisher = handle.advertise("/encoders", 1); - - ros::Publisher right_wheel_shock_publisher = - handle.advertise("right_wheel_shock_controller/command", 1, true); - ros::Publisher left_wheel_shock_publisher = - handle.advertise("left_wheel_shock_controller/command", 1, true); - - std_msgs::Float64 shock_set_point; - shock_set_point.data = 0.0; - right_wheel_shock_publisher.publish(shock_set_point); - left_wheel_shock_publisher.publish(shock_set_point); - - double speed_P_left, speed_P_right, speed_D_left, speed_D_right, speed_I_left, speed_I_right; - double rate_var; - double max_effort = 0.0; - - assertions::param(pNh, "speed_P_left", speed_P_left, 5.0); - assertions::param(pNh, "speed_P_right", speed_P_right, 5.0); - assertions::param(pNh, "speed_D_left", speed_D_left, 1.0); - assertions::param(pNh, "speed_D_right", speed_D_right, 1.0); - assertions::param(pNh, "speed_I_left", speed_I_left, 0.0); - assertions::param(pNh, "speed_I_right", speed_I_right, 0.0); - assertions::param(pNh, "wheel_radius", wheel_radius, 0.3); - assertions::param(pNh, "max_effort", max_effort, 4.0); - assertions::param(pNh, "rate", rate_var, 60.0); - - // Publish that the robot is enabled - ros::Publisher enabled_pub = handle.advertise("/robot_enabled", 1, /* latch = */ true); - std_msgs::Bool enabled_msg; - enabled_msg.data = true; - enabled_pub.publish(enabled_msg); - - ros::Subscriber speed_sub = handle.subscribe("/motors", 1, speedCallback); - - ros::Subscriber state_sub = handle.subscribe("/joint_states", 1, jointStateCallback); - - ros::Time prev, now; - prev = ros::Time::now(); - - ros::Rate rate{ rate_var }; - - double speed_last_error_left = 0.0; - double speed_last_error_right = 0.0; - double speed_left_error_accum = 0.0; - double speed_right_error_accum = 0.0; - double effort_right = 0.0; - double effort_left = 0.0; - ros::Time prev_time = ros::Time::now(); - while (ros::ok()) - { - ros::spinOnce(); - - ros::Time cur_time = ros::Time::now(); - double dt = cur_time.toSec() - prev_time.toSec(); - prev_time = cur_time; - - double error_left = speed_set_point_left - speed_measured_left; - double dError_left = (error_left - speed_last_error_left) / dt; - speed_left_error_accum += error_left; - speed_last_error_left = error_left; - - // ROS_INFO_STREAM("error left = " << error_left); - // ROS_INFO_STREAM("effort_left = " << effort_left); - - effort_left += speed_P_left * error_left + speed_D_left * dError_left + speed_I_left * speed_left_error_accum; - - double error_right = speed_set_point_right - speed_measured_right; - double dError_right = (error_right - speed_last_error_right) / dt; - speed_right_error_accum += error_right; - speed_last_error_right = error_right; - - effort_right += - speed_P_right * error_right + speed_D_right * dError_right + speed_I_right * speed_right_error_accum; - - effort_right = std::min(max_effort, std::max(-max_effort, effort_right)); - effort_left = std::min(max_effort, std::max(-max_effort, effort_left)); - - // ROS_INFO_STREAM("Publishing effort: " << effort_right); - // ROS_INFO_STREAM("left: " << effort_left << " right: " << effort_right); - std_msgs::Float64 left_wheel_message; - std_msgs::Float64 right_wheel_message; - - left_wheel_message.data = effort_left; - right_wheel_message.data = effort_right; - /* - if(speed_set_point_left == 0) { - effort_left = 0.0; - left_wheel_message.data = 0.0; - } else { - left_wheel_message.data = effort_left; - } - if(speed_set_point_right == 0) { - effort_right = 0; - right_wheel_message.data = 0.0; - } else { - right_wheel_message.data = effort_right; - } - */ - - left_wheel_effort_publisher.publish(left_wheel_message); - right_wheel_effort_publisher.publish(right_wheel_message); - igvc_msgs::velocity_pair speed_measured; - speed_measured.left_velocity = speed_measured_left; - speed_measured.right_velocity = speed_measured_right; - now = ros::Time::now(); - ros::Duration duration = now - prev; - speed_measured.duration = duration.toSec(); - speed_measured.header.stamp = ros::Time::now(); - wheel_speed_publisher.publish(speed_measured); - rate.sleep(); - prev = now; - } - - return 0; -} From 97e31184c2b6f8d1e7b5b53f34eff8c5c8ea0c21 Mon Sep 17 00:00:00 2001 From: matthewhannay567 Date: Sun, 10 Apr 2022 16:26:44 -0400 Subject: [PATCH 24/27] second pass --- igvc_description/launch/robot_state_publisher.launch | 2 +- igvc_description/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/igvc_description/launch/robot_state_publisher.launch b/igvc_description/launch/robot_state_publisher.launch index 75dfb6190..51b7eb7f2 100644 --- a/igvc_description/launch/robot_state_publisher.launch +++ b/igvc_description/launch/robot_state_publisher.launch @@ -3,7 +3,7 @@ This file launches the robot_state_publisher node that broadcasts the tf structure of the robot. --> - + diff --git a/igvc_description/package.xml b/igvc_description/package.xml index 6d60c4e27..a155b2295 100644 --- a/igvc_description/package.xml +++ b/igvc_description/package.xml @@ -3,7 +3,7 @@ igvc_description 1.0.0 - igvc_description contains the description of the IGVC Jessii platform + igvc_description contains the description of the IGVC Swervi platform Jason Gibson From 2979ffc5df97f483f5a9ca90d5b7619273ba8690 Mon Sep 17 00:00:00 2001 From: matthewhannay567 Date: Sun, 10 Apr 2022 17:26:51 -0400 Subject: [PATCH 25/27] third pass --- igvc_msgs/CMakeLists.txt | 1 - igvc_msgs/msg/action_path.msg | 2 +- igvc_msgs/msg/velocity_pair.msg | 4 - igvc_navigation/CMakeLists.txt | 2 - .../launch/differential_drive.launch | 11 - igvc_navigation/launch/localization.launch | 2 +- .../launch/navigation_simulation.launch | 4 +- igvc_navigation/launch/wheel_odometry.launch | 6 - .../src/differential_drive/CMakeLists.txt | 10 - .../differential_drive/differential_drive.cpp | 43 -- .../differential_drive/differential_drive.h | 27 - .../swerve_wheel_odometer/swerve_odometer.cpp | 30 +- .../src/wheel_odometer/CMakeLists.txt | 10 - .../src/wheel_odometer/Odometer.cpp | 119 ---- igvc_navigation/src/wheel_odometer/Odometer.h | 30 - igvc_platform/CMakeLists.txt | 3 +- .../src/joystick_driver/CMakeLists.txt | 10 - .../src/joystick_driver/joystick_driver.cpp | 59 -- .../src/joystick_driver/joystick_driver.h | 40 -- .../src/motor_controller/motor_controller.cpp | 656 +++++++++--------- .../src/motor_controller/motor_controller.h | 194 +++--- igvc_platform/src/tests/CMakeLists.txt | 8 - .../tests/test/test_differential_drive.test | 7 - .../src/tests/test/test_joystick_driver.test | 12 - .../src/tests/test_differential_drive.cpp | 218 ------ .../src/tests/test_joystick_driver.cpp | 177 ----- .../igvc_rviz_plugins_old/example_panel.h | 52 -- .../igvc_rviz_plugins_old/speed_panel.h | 40 -- .../igvc_rviz_plugins_old/example_panel.cpp | 49 -- .../src/igvc_rviz_plugins_old/speed_panel.cpp | 60 -- igvc_utils/CMakeLists.txt | 4 +- 31 files changed, 447 insertions(+), 1443 deletions(-) delete mode 100644 igvc_msgs/msg/velocity_pair.msg delete mode 100644 igvc_navigation/launch/differential_drive.launch delete mode 100644 igvc_navigation/launch/wheel_odometry.launch delete mode 100644 igvc_navigation/src/differential_drive/CMakeLists.txt delete mode 100644 igvc_navigation/src/differential_drive/differential_drive.cpp delete mode 100644 igvc_navigation/src/differential_drive/differential_drive.h delete mode 100644 igvc_navigation/src/wheel_odometer/CMakeLists.txt delete mode 100644 igvc_navigation/src/wheel_odometer/Odometer.cpp delete mode 100644 igvc_navigation/src/wheel_odometer/Odometer.h delete mode 100644 igvc_platform/src/joystick_driver/CMakeLists.txt delete mode 100644 igvc_platform/src/joystick_driver/joystick_driver.cpp delete mode 100644 igvc_platform/src/joystick_driver/joystick_driver.h delete mode 100644 igvc_platform/src/tests/test/test_differential_drive.test delete mode 100644 igvc_platform/src/tests/test/test_joystick_driver.test delete mode 100644 igvc_platform/src/tests/test_differential_drive.cpp delete mode 100644 igvc_platform/src/tests/test_joystick_driver.cpp delete mode 100644 igvc_rviz_plugins/include/igvc_rviz_plugins_old/example_panel.h delete mode 100644 igvc_rviz_plugins/include/igvc_rviz_plugins_old/speed_panel.h delete mode 100644 igvc_rviz_plugins/src/igvc_rviz_plugins_old/example_panel.cpp delete mode 100644 igvc_rviz_plugins/src/igvc_rviz_plugins_old/speed_panel.cpp diff --git a/igvc_msgs/CMakeLists.txt b/igvc_msgs/CMakeLists.txt index a59f1c354..3d2d984e7 100644 --- a/igvc_msgs/CMakeLists.txt +++ b/igvc_msgs/CMakeLists.txt @@ -14,7 +14,6 @@ find_package(catkin REQUIRED COMPONENTS add_message_files( DIRECTORY msg FILES - velocity_pair.msg velocity_quad.msg lights.msg action_path.msg diff --git a/igvc_msgs/msg/action_path.msg b/igvc_msgs/msg/action_path.msg index bd98ea5c1..74917e995 100644 --- a/igvc_msgs/msg/action_path.msg +++ b/igvc_msgs/msg/action_path.msg @@ -1,2 +1,2 @@ Header header -velocity_pair[] actions +velocity_quad[] actions diff --git a/igvc_msgs/msg/velocity_pair.msg b/igvc_msgs/msg/velocity_pair.msg deleted file mode 100644 index 5f36f7c6b..000000000 --- a/igvc_msgs/msg/velocity_pair.msg +++ /dev/null @@ -1,4 +0,0 @@ -Header header -float64 left_velocity -float64 right_velocity -float64 duration diff --git a/igvc_navigation/CMakeLists.txt b/igvc_navigation/CMakeLists.txt index db265d779..a6b1b43fd 100644 --- a/igvc_navigation/CMakeLists.txt +++ b/igvc_navigation/CMakeLists.txt @@ -210,10 +210,8 @@ target_link_libraries(GraphSearch ${OpenCV_LIBRARIES} ${catkin_LIBRARIES}) add_subdirectory(src/back_circle) add_subdirectory(src/back_up_recovery) -add_subdirectory(src/differential_drive) add_subdirectory(src/swerve_drive) add_subdirectory(src/mapper) add_subdirectory(src/navigation_client) add_subdirectory(src/navigation_server) -add_subdirectory(src/wheel_odometer) add_subdirectory(src/swerve_wheel_odometer) diff --git a/igvc_navigation/launch/differential_drive.launch b/igvc_navigation/launch/differential_drive.launch deleted file mode 100644 index bad31bbd1..000000000 --- a/igvc_navigation/launch/differential_drive.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - diff --git a/igvc_navigation/launch/localization.launch b/igvc_navigation/launch/localization.launch index 6ef04afe6..1ed0cbe5a 100644 --- a/igvc_navigation/launch/localization.launch +++ b/igvc_navigation/launch/localization.launch @@ -7,7 +7,7 @@ --> - + diff --git a/igvc_navigation/launch/navigation_simulation.launch b/igvc_navigation/launch/navigation_simulation.launch index 9283ef056..d6419989e 100644 --- a/igvc_navigation/launch/navigation_simulation.launch +++ b/igvc_navigation/launch/navigation_simulation.launch @@ -18,8 +18,8 @@ - - + + diff --git a/igvc_navigation/launch/wheel_odometry.launch b/igvc_navigation/launch/wheel_odometry.launch deleted file mode 100644 index a3f65c108..000000000 --- a/igvc_navigation/launch/wheel_odometry.launch +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - diff --git a/igvc_navigation/src/differential_drive/CMakeLists.txt b/igvc_navigation/src/differential_drive/CMakeLists.txt deleted file mode 100644 index 105517d49..000000000 --- a/igvc_navigation/src/differential_drive/CMakeLists.txt +++ /dev/null @@ -1,10 +0,0 @@ -add_executable(differential_drive differential_drive.cpp) -add_dependencies(differential_drive ${catkin_EXPORTED_TARGETS}) -target_link_libraries(differential_drive ${catkin_LIBRARIES}) - -install( - TARGETS differential_drive - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/igvc_navigation/src/differential_drive/differential_drive.cpp b/igvc_navigation/src/differential_drive/differential_drive.cpp deleted file mode 100644 index 13e98b01c..000000000 --- a/igvc_navigation/src/differential_drive/differential_drive.cpp +++ /dev/null @@ -1,43 +0,0 @@ -#include -#include - -#include "differential_drive.h" - -DifferentialDrive::DifferentialDrive() -{ - ros::NodeHandle nh; - ros::NodeHandle pNh("~"); - - assertions::param(pNh, "axle_length", axle_length_, 0.48); - assertions::param(pNh, "max_vel", max_vel_, 3.0); - mbf_twist_ = nh.subscribe("/cmd_vel", 1, &DifferentialDrive::twistToVelocity, this); - vel_pub_ = nh.advertise("/motors", 1); -} - -void DifferentialDrive::twistToVelocity(geometry_msgs::Twist twist) -{ - double speed = twist.linear.x; - double rotation = twist.angular.z; - double vel_right = speed + (rotation * axle_length_) / 2; - double vel_left = speed - (rotation * axle_length_) / 2; - - double max_calc_vel = fmax(vel_right, vel_left); - if (max_calc_vel > max_vel_) - { - vel_right *= max_vel_ / max_calc_vel; - vel_left *= max_vel_ / max_calc_vel; - } - - igvc_msgs::velocity_pair vel_msg; - vel_msg.right_velocity = vel_right; - vel_msg.left_velocity = vel_left; - vel_msg.duration = 0.02; - vel_msg.header.stamp = ros::Time::now(); - vel_pub_.publish(vel_msg); -} -int main(int argc, char** argv) -{ - ros::init(argc, argv, "differential_drive"); - DifferentialDrive differential_drive; - ros::spin(); -} diff --git a/igvc_navigation/src/differential_drive/differential_drive.h b/igvc_navigation/src/differential_drive/differential_drive.h deleted file mode 100644 index 9541a02dc..000000000 --- a/igvc_navigation/src/differential_drive/differential_drive.h +++ /dev/null @@ -1,27 +0,0 @@ -/** - * Converts Twist messages to velocity pairs for the motors of a differential drive robot - * - * Author: Tan Gemicioglu - */ - -#ifndef DIFFERENTIALDRIVE_H -#define DIFFERENTIALDRIVE_H - -#include -#include - -class DifferentialDrive -{ -public: - DifferentialDrive(); - ros::Subscriber mbf_twist_; - -private: - ros::Publisher vel_pub_; - double axle_length_{}; - double max_vel_{}; - - void twistToVelocity(geometry_msgs::Twist twist); -}; - -#endif // DIFFERENTIALDRIVE_H diff --git a/igvc_navigation/src/swerve_wheel_odometer/swerve_odometer.cpp b/igvc_navigation/src/swerve_wheel_odometer/swerve_odometer.cpp index 500b4f4aa..5813796ea 100644 --- a/igvc_navigation/src/swerve_wheel_odometer/swerve_odometer.cpp +++ b/igvc_navigation/src/swerve_wheel_odometer/swerve_odometer.cpp @@ -150,7 +150,7 @@ void SwerveOdometer::enc_callback(const igvc_msgs::velocity_quad msg) if (inf_all) { - ROS_INFO_STREAM("inf_all"); + //ROS_INFO_STREAM("inf_all"); angular = 0; for (int i = 0; i < num_wheels; ++i) { @@ -166,15 +166,15 @@ void SwerveOdometer::enc_callback(const igvc_msgs::velocity_quad msg) if ((fabs(intersections[i][0] - intersections[i - 1][0]) > intersection_tol_ || fabs(intersections[i][1] - intersections[i - 1][1]) > intersection_tol_)) { - ROS_ERROR_STREAM("intersections are not close enough to get an average, dropping!"); - for (int i = 0; i < num_wheels; ++i) - { - ROS_WARN_STREAM("theta, omega: " << wheel_info[i][1] << " " << wheel_info[i][0]); - } - for (size_t i = 0; i < intersections.size(); ++i) - { - ROS_WARN_STREAM("intersection i:" << i << " , " << intersections[i][0] << " " << intersections[i][1]); - } + // ROS_ERROR_STREAM("intersections are not close enough to get an average, dropping!"); + // for (int i = 0; i < num_wheels; ++i) + // { + // ROS_WARN_STREAM("theta, omega: " << wheel_info[i][1] << " " << wheel_info[i][0]); + // } + // for (size_t i = 0; i < intersections.size(); ++i) + // { + // ROS_WARN_STREAM("intersection i:" << i << " , " << intersections[i][0] << " " << intersections[i][1]); + // } return; } else @@ -207,10 +207,10 @@ void SwerveOdometer::enc_callback(const igvc_msgs::velocity_quad msg) auto icr_wh = std::array{ positions_list[i][0] - average_intersection[0], positions_list[i][1] - average_intersection[1] }; - if (isinf(icr_wh[0]) || isinf(icr_wh[1])) - ROS_WARN_STREAM("icr_wh is inf"); - if (isclose(icr_wh[0], 0) || isclose(icr_wh[1], 0)) - ROS_WARN_STREAM("icr_wh for wheel " << i << " is zero. icr is just over it!"); + // if (isinf(icr_wh[0]) || isinf(icr_wh[1])) + // ROS_WARN_STREAM("icr_wh is inf"); + // if (isclose(icr_wh[0], 0) || isclose(icr_wh[1], 0)) + // ROS_WARN_STREAM("icr_wh for wheel " << i << " is zero. icr is just over it!"); double ang_x = (wheel_info[i][0] * wheel_radius * sin(wheel_info[i][1])) / (icr_wh[0]); double ang_y = (wheel_info[i][0] * wheel_radius * cos(wheel_info[i][1])) / (icr_wh[1]); @@ -220,7 +220,7 @@ void SwerveOdometer::enc_callback(const igvc_msgs::velocity_quad msg) if (isinf(angular)) { - ROS_WARN_STREAM("angular is the problem"); + //ROS_WARN_STREAM("angular is the problem"); return; } } diff --git a/igvc_navigation/src/wheel_odometer/CMakeLists.txt b/igvc_navigation/src/wheel_odometer/CMakeLists.txt deleted file mode 100644 index 3e5ce2591..000000000 --- a/igvc_navigation/src/wheel_odometer/CMakeLists.txt +++ /dev/null @@ -1,10 +0,0 @@ -add_executable(odometer Odometer.cpp) -add_dependencies(odometer ${catkin_EXPORTED_TARGETS}) -target_link_libraries(odometer ${catkin_LIBRARIES}) - -install( - TARGETS odometer - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/igvc_navigation/src/wheel_odometer/Odometer.cpp b/igvc_navigation/src/wheel_odometer/Odometer.cpp deleted file mode 100644 index 80114e39f..000000000 --- a/igvc_navigation/src/wheel_odometer/Odometer.cpp +++ /dev/null @@ -1,119 +0,0 @@ -#include "Odometer.h" -#include -#include -#include -#include -#include "Odometer.h" - -double wheel_separation; - -/** - * Coneverts wheel velocities to odometry message using trigonometry for calculations - * In the ros coordinate convention x is forward, y is leftward, and z is upward relative to the robot - * The position is published in an absolute reference frame relative to the initial position - * The velocities (twist) is in a reference frame relative to the robot - */ -void Odometer::enc_callback(const igvc_msgs::velocity_pair& msg) -{ - float leftVelocity = msg.left_velocity; - float rightVelocity = msg.right_velocity; - float deltaT = msg.duration; - - float angularVelocity = (rightVelocity - leftVelocity) / wheel_sep; - float deltaTheta = angularVelocity * deltaT; - float velocity = (rightVelocity + leftVelocity) / 2; - - geometry_msgs::Vector3 linearVelocities; - linearVelocities.z = 0; - - if (fabs(rightVelocity - leftVelocity) > 1e-4) - { // 1e-4 is the point where less of a difference is straight - linearVelocities.y = velocity * sin(deltaTheta); - linearVelocities.x = velocity * cos(deltaTheta); - } - else - { - // limit where turn radius is infinite (ie. straight line) - linearVelocities.y = 0; - linearVelocities.x = velocity; - } - - // set angular velocities - assuming 2D operation - geometry_msgs::Vector3 angularVelocities; - angularVelocities.x = 0.0; - angularVelocities.y = 0.0; - angularVelocities.z = angularVelocity; - - nav_msgs::Odometry odom; - odom.twist.twist.linear = linearVelocities; - odom.twist.twist.angular = angularVelocities; - - // update global positions - // note x and y velocities are local reference frame - convert to global then increment poition - float dx = linearVelocities.x * deltaT; - float dy = linearVelocities.y * deltaT; - x += dx * cos(yaw) - dy * sin(yaw); - y += dx * sin(yaw) + dy * cos(yaw); - yaw += deltaTheta; - - // enter message info for global position - odom.pose.pose.position.x = x; - odom.pose.pose.position.y = y; - odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(yaw); - - // Row-major representation of the 6x6 covariance matrix - // The orientation parameters use a fixed-axis representation. - // In order, the parameters are: - // (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis) - odom.twist.covariance = { 0.02, 1e-4, 1e-4, 1e-4, 1e-4, 1e-4, 1e-4, 0.25, 1e-4, 1e-4, 1e-4, 1e-4, - 1e-4, 1e-4, 1e6, 1e-4, 1e-4, 1e-4, 1e-4, 1e-4, 1e-4, 1e6, 1e-4, 1e-4, - 1e-4, 1e-4, 1e-4, 1e-4, 1e6, 1e-4, 1e-4, 1e-4, 1e-4, 1e-4, 1e-4, 0.62 }; - // the position covariance takes same form as twist covariance above - // this grows without bounds as error accumulates - disregard exact reading with high variance - odom.pose.covariance = { 0.01, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 0.01, 1e-6, 1e-6, 1e-6, 1e-6, - 1e-6, 1e-6, 1e6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e6, 1e-6, 1e-6, - 1e-6, 1e-6, 1e-6, 1e-6, 1e6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e-6, 1e6 }; - - // setting sequence of message - odom.header.seq = seq++; - - // setting reference frames - odom.header.frame_id = "odom"; - odom.child_frame_id = "base_footprint"; - - // set time then publish - odom.header.stamp = ros::Time::now(); - pub.publish(odom); -} - -Odometer::Odometer(ros::NodeHandle& nh) -{ - nh.param("wheel_sep", wheel_sep, 0.52); - - sub = nh.subscribe("/encoders", 10, &Odometer::enc_callback, this); - pub = nh.advertise("/wheel_odometry", 10); - - // initializing sequence number for messages - seq = 0; - - // initialize position - map published is relative to position at time t=0 - x = 0; - y = 0; - yaw = 0; -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "wheel_odom"); - ros::NodeHandle nh; - - ros::NodeHandle pNh("~"); - - pNh.param("wheel_separation", wheel_separation, 0.83); - - Odometer odom(nh); - - ROS_INFO_STREAM("wheel odometry node has started"); - - ros::spin(); -} diff --git a/igvc_navigation/src/wheel_odometer/Odometer.h b/igvc_navigation/src/wheel_odometer/Odometer.h deleted file mode 100644 index 6c441ee3b..000000000 --- a/igvc_navigation/src/wheel_odometer/Odometer.h +++ /dev/null @@ -1,30 +0,0 @@ -#ifndef ODOMETER_H -#define ODOMETER_H -#include -#include -#include - -class Odometer -{ -public: - Odometer(ros::NodeHandle&); - -private: - // ros infastructure - ros::Publisher pub; - ros::Subscriber sub; - tf::TransformBroadcaster odom_broadcaster; - int seq; - - // robot constant - double wheel_sep; - - // keeping track of global position - float x; - float y; - float yaw; - - // callback for encoder subscriber - void enc_callback(const igvc_msgs::velocity_pair&); -}; -#endif diff --git a/igvc_platform/CMakeLists.txt b/igvc_platform/CMakeLists.txt index 4def98066..d53344d1e 100644 --- a/igvc_platform/CMakeLists.txt +++ b/igvc_platform/CMakeLists.txt @@ -169,7 +169,6 @@ if (CATKIN_ENABLE_TESTING) endif () add_subdirectory(src/imu) -add_subdirectory(src/joystick_driver) add_subdirectory(src/swerve_joystick_driver) -add_subdirectory(src/motor_controller) +#add_subdirectory(src/motor_controller) #add_subdirectory(src/system_stats) diff --git a/igvc_platform/src/joystick_driver/CMakeLists.txt b/igvc_platform/src/joystick_driver/CMakeLists.txt deleted file mode 100644 index af62a4547..000000000 --- a/igvc_platform/src/joystick_driver/CMakeLists.txt +++ /dev/null @@ -1,10 +0,0 @@ -add_executable(joystick_driver joystick_driver.cpp) -add_dependencies(joystick_driver ${catkin_EXPORTED_TARGETS}) -target_link_libraries(joystick_driver ${catkin_LIBRARIES}) - -install( - TARGETS joystick_driver - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/igvc_platform/src/joystick_driver/joystick_driver.cpp b/igvc_platform/src/joystick_driver/joystick_driver.cpp deleted file mode 100644 index e133964b9..000000000 --- a/igvc_platform/src/joystick_driver/joystick_driver.cpp +++ /dev/null @@ -1,59 +0,0 @@ -#include "joystick_driver.h" - -JoystickDriver::JoystickDriver() : pNh{ "~" } -{ - cmd_pub = nh.advertise("/motors", 1); - joy_sub = nh.subscribe("/joy", 1, &JoystickDriver::joyCallback, this); - - updater_ptr = std::make_unique(); - updater_ptr->setHardwareID("Joystick"); - updater_ptr->add("Joystick Diagnostic", this, &JoystickDriver::joystick_diagnostic); - - assertions::param(pNh, "absoluteMaxVel", absoluteMaxVel, 1.0); - assertions::param(pNh, "maxVel", maxVel, 1.6); - assertions::param(pNh, "maxVelIncr", maxVelIncr, 0.1); - assertions::param(pNh, "leftJoyAxis", leftJoyAxis, 1); - assertions::param(pNh, "rightJoyAxis", rightJoyAxis, 4); - assertions::param(pNh, "leftInverted", leftInverted, false); - assertions::param(pNh, "rightInverted", rightInverted, false); -} - -void JoystickDriver::joystick_diagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat) -{ - stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Joystick Online"); - stat.add("absolute_max_velocity", absoluteMaxVel); - stat.add("max_velocity", maxVel); - stat.add("max_velocity_increment", maxVelIncr); -} - -void JoystickDriver::joyCallback(const sensor_msgs::Joy::ConstPtr& msg) -{ - if (msg->buttons[1]) - { - maxVel -= maxVelIncr; - } - else if (msg->buttons[3]) - { - maxVel += maxVelIncr; - } - maxVel = std::min(maxVel, absoluteMaxVel); - maxVel = std::max(maxVel, 0.0); - - pNh.setParam("maxVel", maxVel); - - updater_ptr->update(); - - igvc_msgs::velocity_pair cmd; - cmd.left_velocity = msg->axes[leftJoyAxis] * maxVel * (leftInverted ? -1.0 : 1.0); - cmd.right_velocity = msg->axes[rightJoyAxis] * maxVel * (rightInverted ? -1.0 : 1.0); - cmd.header.stamp = ros::Time::now(); - - cmd_pub.publish(cmd); -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "joystick_driver"); - JoystickDriver joystick_driver; - ros::spin(); -} diff --git a/igvc_platform/src/joystick_driver/joystick_driver.h b/igvc_platform/src/joystick_driver/joystick_driver.h deleted file mode 100644 index a6c7c23dc..000000000 --- a/igvc_platform/src/joystick_driver/joystick_driver.h +++ /dev/null @@ -1,40 +0,0 @@ -#ifndef JOYSTICKDRIVER_H -#define JOYSTICKDRIVER_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -class JoystickDriver -{ -public: - JoystickDriver(); - -private: - // node handles - ros::NodeHandle pNh; - ros::NodeHandle nh; - - // publisher - ros::Publisher cmd_pub; - - // subscriber - ros::Subscriber joy_sub; - - std::unique_ptr updater_ptr; - - double absoluteMaxVel, maxVel, maxVelIncr; - int leftJoyAxis, rightJoyAxis; - bool leftInverted, rightInverted; - - void joystick_diagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat); - void joyCallback(const sensor_msgs::Joy::ConstPtr& msg); -}; - -#endif // JOYSTICKDRIVER_H \ No newline at end of file diff --git a/igvc_platform/src/motor_controller/motor_controller.cpp b/igvc_platform/src/motor_controller/motor_controller.cpp index 4fe9511b5..8bc3654cd 100644 --- a/igvc_platform/src/motor_controller/motor_controller.cpp +++ b/igvc_platform/src/motor_controller/motor_controller.cpp @@ -1,328 +1,328 @@ -#include -#include - -#include - -// nanopb header files for protobuffer encoding/decoding -#include -#include -#include -#include "igvc_platform/nanopb/protos/igvc.pb.h" // compiled protobuf definition - -#include "motor_controller.h" - -MotorController::MotorController(ros::NodeHandle* nodehandle) : nh_(*nodehandle) -{ - // initialize private node handle - ros::NodeHandle pNh("~"); - - // initialize subscriber to /motors topic - cmd_sub_ = nh_.subscribe("/motors", 1, &MotorController::cmdCallback, this); - - // initialize publishers to publish mbed stats - enc_pub_ = nh_.advertise("/encoders", 1000); - enabled_pub_ = nh_.advertise("/robot_enabled", 1); - battery_pub_ = nh_.advertise("/battery", 1); - - // get server ip address and port number from the launch file - assertions::getParam(pNh, std::string("ip_addr"), ip_addr_); - assertions::getParam(pNh, std::string("port"), tcpport_); - - ROS_INFO_STREAM("Connecting to server:" - << "\n\tIP: " << ip_addr_ << "\n\tPort: " << std::to_string(tcpport_)); - sock_ = std::make_unique(ip_addr_, tcpport_); - ROS_INFO_STREAM("Using Boost " << (*sock_).getBoostVersion()); - ROS_INFO_STREAM("Successfully Connected to TCP Host:" - << "\n\tIP: " << (*sock_).getIP() << "\n\tPort: " << (*sock_).getPort()); - - assertions::getParam(pNh, std::string("battery_alpha"), battery_alpha_); - assertions::getParam(pNh, std::string("min_battery_voltage"), min_battery_voltage_); - - battery_avg_ = min_battery_voltage_; - - // PID variables - assertions::getParam(pNh, std::string("p_l"), p_l_); - assertions::getParam(pNh, std::string("p_r"), p_r_); - assertions::getParam(pNh, std::string("d_l"), d_l_); - assertions::getParam(pNh, std::string("d_r"), d_r_); - assertions::getParam(pNh, std::string("i_r"), i_r_); - assertions::getParam(pNh, std::string("i_l"), i_l_); - assertions::getParam(pNh, "kv_l", kv_l_); - assertions::getParam(pNh, "kv_r", kv_r_); - - assertions::getParam(pNh, "watchdog_delay", watchdog_delay_); - - assertions::param(pNh, "log_period", log_period_, 5.0); - - // Diagnostic_updator - - mc_updater_.setHardwareID("Motor Controller"); - mc_updater_.add("MC Diagnostic", this, &MotorController::mc_diagnostic); - battery_updater_.setHardwareID("Battery Controller"); - battery_updater_.add("Battery Diagnostic", this, &MotorController::battery_diagnostic); - - // communication frequency - assertions::getParam(pNh, std::string("frequency"), frequency_); - ros::Rate rate(frequency_); - - setPID(); // Set PID Values on mbed - - // send motor commands - while (ros::ok()) - { - sendRequest(); - recieveResponse(); - ros::spinOnce(); - mc_updater_.update(); - battery_updater_.update(); - rate.sleep(); - } -} - -void MotorController::cmdCallback(const igvc_msgs::velocity_pair::ConstPtr& msg) -{ - current_motor_command_ = *msg; - last_motors_message_ = ros::Time::now(); -} - -void MotorController::mc_diagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat) -{ - stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Motor Controller Online"); - stat.add("mc publishing freq", std::to_string(mc_hertz_) + " Hz"); -} - -void MotorController::battery_diagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat) -{ - if (battery_avg_ < min_battery_voltage_) - { - stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Battery voltage dangerously low"); - } - else if (battery_avg_ < (min_battery_voltage_ + 0.25)) - { - stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Battery voltage low"); - } - else - { - stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Battery voltage okay"); - } - stat.add("battery voltage", battery_avg_); -} - -void MotorController::setPID() -{ - ros::Rate rate(frequency_); - ROS_INFO_STREAM("Setting PID Values:" - << "\n\t P => L: " << p_l_ << " R: " << p_r_ << "\n\t D => L: " << d_l_ << " R: " << d_r_ - << "\n\t I => L: " << i_l_ << " R: " << i_r_ << "\n\t Kv => L: " << kv_l_ << " R: " << kv_r_); - - bool valid_values = false; // pid values have been set correctly - - /* This is the buffer where we will store the request message. */ - uint8_t requestbuffer[256]; - size_t message_length; - bool status; - - /* allocate space for the request message to the server */ - RequestMessage request = RequestMessage_init_zero; - - /* Create a stream that will write to our buffer. */ - pb_ostream_t ostream = pb_ostream_from_buffer(requestbuffer, sizeof(requestbuffer)); - - /* indicate that pid fields will contain values */ - request.has_p_l = true; - request.has_p_r = true; - request.has_i_l = true; - request.has_i_r = true; - request.has_d_l = true; - request.has_d_r = true; - request.has_kv_l = true; - request.has_kv_r = true; - - /* fill in the message fields */ - request.p_l = static_cast(p_l_); - request.p_r = static_cast(p_r_); - request.i_l = static_cast(i_l_); - request.i_r = static_cast(i_r_); - request.d_l = static_cast(d_l_); - request.d_r = static_cast(d_r_); - request.kv_l = static_cast(kv_l_); - request.kv_r = static_cast(kv_r_); - - /* encode the protobuffer */ - status = pb_encode(&ostream, RequestMessage_fields, &request); - message_length = ostream.bytes_written; - - /* check for any errors.. */ - if (!status) - { - ROS_ERROR_STREAM("Encoding failed: " << PB_GET_ERROR(&ostream)); - mc_updater_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "PID Encoding Failed"); - battery_updater_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "PID Encoding Failed. Lost battery tracking."); - ros::shutdown(); - } - - size_t n; // n is the response from socket: 0 means connection closed, otherwise n = num bytes read - uint8_t buffer[256]; // buffer to read response into - // unsigned char responsebuffer[256]; - - /* Send PID values via ethernet and recieve response to ensure proper setting */ - while (ros::ok() && !valid_values) - { - (*sock_).sendMessage(reinterpret_cast(requestbuffer), message_length); - - memset(buffer, 0, sizeof(buffer)); - n = (*sock_).readMessage(buffer); // blocks until data is read - // memcpy(responsebuffer, buffer, sizeof(responsebuffer)); - - if (n == 0) - { - ROS_ERROR_STREAM("Connection closed by server"); - mc_updater_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "Failed to send PID. Connection Closed by " - "server."); - battery_updater_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "Failed to send PID. Lost battery " - "tracking."); - ros::shutdown(); - } - - /* Allocate space for the decoded message. */ - ResponseMessage response = ResponseMessage_init_zero; - - /* Create a stream that reads from the buffer. */ - pb_istream_t istream = pb_istream_from_buffer(buffer, n); - - /* decode the message. */ - status = pb_decode(&istream, ResponseMessage_fields, &response); - - /* check for any errors.. */ - if (!status) - { - ROS_ERROR_STREAM("Decoding Failed: " << PB_GET_ERROR(&istream)); - mc_updater_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "PID decoding failed. Shutting Down."); - battery_updater_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "PID decoding failed. Lost battery " - "tracking."); - ros::shutdown(); - } - - valid_values = (response.p_l == static_cast(p_l_)) && (response.p_r == static_cast(p_r_)) && - (response.i_l == static_cast(i_l_)) && (response.i_r == static_cast(i_r_)) && - (response.d_l == static_cast(d_l_)) && (response.d_r == static_cast(d_r_)) && - (response.kv_l == static_cast(kv_l_)) && (response.kv_r == static_cast(kv_r_)); - - rate.sleep(); - } - ROS_INFO_ONCE("Sucessfully set all PID values"); -} - -void MotorController::sendRequest() -{ - /* This is the buffer where we will store the request message. */ - uint8_t requestbuffer[256]; - - /* allocate space for the request message to the server */ - RequestMessage request = RequestMessage_init_zero; - - /* Create a stream that will write to our buffer. */ - pb_ostream_t ostream = pb_ostream_from_buffer(requestbuffer, sizeof(requestbuffer)); - - /* indicate that speed fields will contain values */ - request.has_speed_l = true; - request.has_speed_r = true; - - /*double dt = (ros::Time::now() - last_motors_message_).toSec(); - if(dt > watchdog_delay_) { - current_motor_command_.left_velocity = 0.0; - current_motor_command_.right_velocity = 0.0; - ROS_ERROR_STREAM_THROTTLE(1, "TIMEOUT on motor controller, too large a difference between current time and last - motor: " << dt); - }*/ - - /* fill in the message fields */ - request.speed_l = static_cast(current_motor_command_.left_velocity); - request.speed_r = static_cast(current_motor_command_.right_velocity); - - /* encode the protobuffer */ - bool status = pb_encode(&ostream, RequestMessage_fields, &request); - size_t message_length = ostream.bytes_written; - - /* check for any errors.. */ - if (!status) - { - ROS_ERROR_STREAM("Encoding failed: " << PB_GET_ERROR(&ostream)); - ros::shutdown(); - } - - /* Send the message strapped to a pigeon's leg! */ - (*sock_).sendMessage(reinterpret_cast(requestbuffer), message_length); -} - -void MotorController::recieveResponse() -{ - /* Read response from the server */ - size_t n; // n is the response from socket: 0 means connection closed, otherwise n = num bytes read - uint8_t buffer[256]; // buffer to read response into - - memset(buffer, 0, sizeof(buffer)); - /* read from the buffer */ - n = (*sock_).readMessage(buffer); // blocks until data is read - - if (n == 0) - { - ROS_ERROR_STREAM("Connection closed by server"); - ros::shutdown(); - } - - /* Allocate space for the decoded message. */ - ResponseMessage response = ResponseMessage_init_zero; - - /* Create a stream that reads from the buffer. */ - pb_istream_t istream = pb_istream_from_buffer(buffer, n); - - /* decode the message. */ - bool status = pb_decode(&istream, ResponseMessage_fields, &response); - - /* check for any errors.. */ - if (!status) - { - ROS_ERROR_STREAM("Decoding Failed: " << PB_GET_ERROR(&istream)); - ros::shutdown(); - } - - publishResponse(response); - mc_hertz_ = 1 / response.dt_sec; -} - -void MotorController::publishResponse(const ResponseMessage& response) -{ - /* update the exponentially weighted moving voltage average and publish */ - std_msgs::Float64 battery_msg; - battery_avg_ = (battery_alpha_ * battery_avg_) + ((1 - battery_alpha_) * response.voltage); - battery_msg.data = battery_avg_; - battery_pub_.publish(battery_msg); - - if (battery_avg_ < min_battery_voltage_) - { - ROS_ERROR_STREAM_THROTTLE(log_period_, "Battery voltage dangerously low:" - << "\n\tCurr. Voltage: " << battery_avg_ - << "\n\tMin. Voltage: " << min_battery_voltage_); - } - - std_msgs::Bool enabled_msg; - enabled_msg.data = response.estop; - enabled_pub_.publish(enabled_msg); - - /* publish encoder feedback */ - igvc_msgs::velocity_pair enc_msg; - enc_msg.left_velocity = response.speed_l; - enc_msg.right_velocity = response.speed_r; - enc_msg.duration = response.dt_sec; - enc_msg.header.stamp = ros::Time::now() - ros::Duration(response.dt_sec); - enc_pub_.publish(enc_msg); -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "motor_controller"); - ros::NodeHandle nh; - MotorController motor_controller(&nh); - return 0; -} +// #include +// #include + +// #include + +// // nanopb header files for protobuffer encoding/decoding +// #include +// #include +// #include +// #include "igvc_platform/nanopb/protos/igvc.pb.h" // compiled protobuf definition + +// #include "motor_controller.h" + +// MotorController::MotorController(ros::NodeHandle* nodehandle) : nh_(*nodehandle) +// { +// // initialize private node handle +// ros::NodeHandle pNh("~"); + +// // initialize subscriber to /motors topic +// cmd_sub_ = nh_.subscribe("/motors", 1, &MotorController::cmdCallback, this); + +// // initialize publishers to publish mbed stats +// enc_pub_ = nh_.advertise("/encoders", 1000); +// enabled_pub_ = nh_.advertise("/robot_enabled", 1); +// battery_pub_ = nh_.advertise("/battery", 1); + +// // get server ip address and port number from the launch file +// assertions::getParam(pNh, std::string("ip_addr"), ip_addr_); +// assertions::getParam(pNh, std::string("port"), tcpport_); + +// ROS_INFO_STREAM("Connecting to server:" +// << "\n\tIP: " << ip_addr_ << "\n\tPort: " << std::to_string(tcpport_)); +// sock_ = std::make_unique(ip_addr_, tcpport_); +// ROS_INFO_STREAM("Using Boost " << (*sock_).getBoostVersion()); +// ROS_INFO_STREAM("Successfully Connected to TCP Host:" +// << "\n\tIP: " << (*sock_).getIP() << "\n\tPort: " << (*sock_).getPort()); + +// assertions::getParam(pNh, std::string("battery_alpha"), battery_alpha_); +// assertions::getParam(pNh, std::string("min_battery_voltage"), min_battery_voltage_); + +// battery_avg_ = min_battery_voltage_; + +// // PID variables +// assertions::getParam(pNh, std::string("p_l"), p_l_); +// assertions::getParam(pNh, std::string("p_r"), p_r_); +// assertions::getParam(pNh, std::string("d_l"), d_l_); +// assertions::getParam(pNh, std::string("d_r"), d_r_); +// assertions::getParam(pNh, std::string("i_r"), i_r_); +// assertions::getParam(pNh, std::string("i_l"), i_l_); +// assertions::getParam(pNh, "kv_l", kv_l_); +// assertions::getParam(pNh, "kv_r", kv_r_); + +// assertions::getParam(pNh, "watchdog_delay", watchdog_delay_); + +// assertions::param(pNh, "log_period", log_period_, 5.0); + +// // Diagnostic_updator + +// mc_updater_.setHardwareID("Motor Controller"); +// mc_updater_.add("MC Diagnostic", this, &MotorController::mc_diagnostic); +// battery_updater_.setHardwareID("Battery Controller"); +// battery_updater_.add("Battery Diagnostic", this, &MotorController::battery_diagnostic); + +// // communication frequency +// assertions::getParam(pNh, std::string("frequency"), frequency_); +// ros::Rate rate(frequency_); + +// setPID(); // Set PID Values on mbed + +// // send motor commands +// while (ros::ok()) +// { +// sendRequest(); +// recieveResponse(); +// ros::spinOnce(); +// mc_updater_.update(); +// battery_updater_.update(); +// rate.sleep(); +// } +// } + +// void MotorController::cmdCallback(const igvc_msgs::velocity_pair::ConstPtr& msg) +// { +// current_motor_command_ = *msg; +// last_motors_message_ = ros::Time::now(); +// } + +// void MotorController::mc_diagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat) +// { +// stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Motor Controller Online"); +// stat.add("mc publishing freq", std::to_string(mc_hertz_) + " Hz"); +// } + +// void MotorController::battery_diagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat) +// { +// if (battery_avg_ < min_battery_voltage_) +// { +// stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Battery voltage dangerously low"); +// } +// else if (battery_avg_ < (min_battery_voltage_ + 0.25)) +// { +// stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Battery voltage low"); +// } +// else +// { +// stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Battery voltage okay"); +// } +// stat.add("battery voltage", battery_avg_); +// } + +// void MotorController::setPID() +// { +// ros::Rate rate(frequency_); +// ROS_INFO_STREAM("Setting PID Values:" +// << "\n\t P => L: " << p_l_ << " R: " << p_r_ << "\n\t D => L: " << d_l_ << " R: " << d_r_ +// << "\n\t I => L: " << i_l_ << " R: " << i_r_ << "\n\t Kv => L: " << kv_l_ << " R: " << kv_r_); + +// bool valid_values = false; // pid values have been set correctly + +// /* This is the buffer where we will store the request message. */ +// uint8_t requestbuffer[256]; +// size_t message_length; +// bool status; + +// /* allocate space for the request message to the server */ +// RequestMessage request = RequestMessage_init_zero; + +// /* Create a stream that will write to our buffer. */ +// pb_ostream_t ostream = pb_ostream_from_buffer(requestbuffer, sizeof(requestbuffer)); + +// /* indicate that pid fields will contain values */ +// request.has_p_l = true; +// request.has_p_r = true; +// request.has_i_l = true; +// request.has_i_r = true; +// request.has_d_l = true; +// request.has_d_r = true; +// request.has_kv_l = true; +// request.has_kv_r = true; + +// /* fill in the message fields */ +// request.p_l = static_cast(p_l_); +// request.p_r = static_cast(p_r_); +// request.i_l = static_cast(i_l_); +// request.i_r = static_cast(i_r_); +// request.d_l = static_cast(d_l_); +// request.d_r = static_cast(d_r_); +// request.kv_l = static_cast(kv_l_); +// request.kv_r = static_cast(kv_r_); + +// /* encode the protobuffer */ +// status = pb_encode(&ostream, RequestMessage_fields, &request); +// message_length = ostream.bytes_written; + +// /* check for any errors.. */ +// if (!status) +// { +// ROS_ERROR_STREAM("Encoding failed: " << PB_GET_ERROR(&ostream)); +// mc_updater_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "PID Encoding Failed"); +// battery_updater_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "PID Encoding Failed. Lost battery tracking."); +// ros::shutdown(); +// } + +// size_t n; // n is the response from socket: 0 means connection closed, otherwise n = num bytes read +// uint8_t buffer[256]; // buffer to read response into +// // unsigned char responsebuffer[256]; + +// /* Send PID values via ethernet and recieve response to ensure proper setting */ +// while (ros::ok() && !valid_values) +// { +// (*sock_).sendMessage(reinterpret_cast(requestbuffer), message_length); + +// memset(buffer, 0, sizeof(buffer)); +// n = (*sock_).readMessage(buffer); // blocks until data is read +// // memcpy(responsebuffer, buffer, sizeof(responsebuffer)); + +// if (n == 0) +// { +// ROS_ERROR_STREAM("Connection closed by server"); +// mc_updater_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "Failed to send PID. Connection Closed by " +// "server."); +// battery_updater_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "Failed to send PID. Lost battery " +// "tracking."); +// ros::shutdown(); +// } + +// /* Allocate space for the decoded message. */ +// ResponseMessage response = ResponseMessage_init_zero; + +// /* Create a stream that reads from the buffer. */ +// pb_istream_t istream = pb_istream_from_buffer(buffer, n); + +// /* decode the message. */ +// status = pb_decode(&istream, ResponseMessage_fields, &response); + +// /* check for any errors.. */ +// if (!status) +// { +// ROS_ERROR_STREAM("Decoding Failed: " << PB_GET_ERROR(&istream)); +// mc_updater_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "PID decoding failed. Shutting Down."); +// battery_updater_.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, "PID decoding failed. Lost battery " +// "tracking."); +// ros::shutdown(); +// } + +// valid_values = (response.p_l == static_cast(p_l_)) && (response.p_r == static_cast(p_r_)) && +// (response.i_l == static_cast(i_l_)) && (response.i_r == static_cast(i_r_)) && +// (response.d_l == static_cast(d_l_)) && (response.d_r == static_cast(d_r_)) && +// (response.kv_l == static_cast(kv_l_)) && (response.kv_r == static_cast(kv_r_)); + +// rate.sleep(); +// } +// ROS_INFO_ONCE("Sucessfully set all PID values"); +// } + +// void MotorController::sendRequest() +// { +// /* This is the buffer where we will store the request message. */ +// uint8_t requestbuffer[256]; + +// /* allocate space for the request message to the server */ +// RequestMessage request = RequestMessage_init_zero; + +// /* Create a stream that will write to our buffer. */ +// pb_ostream_t ostream = pb_ostream_from_buffer(requestbuffer, sizeof(requestbuffer)); + +// /* indicate that speed fields will contain values */ +// request.has_speed_l = true; +// request.has_speed_r = true; + +// /*double dt = (ros::Time::now() - last_motors_message_).toSec(); +// if(dt > watchdog_delay_) { +// current_motor_command_.left_velocity = 0.0; +// current_motor_command_.right_velocity = 0.0; +// ROS_ERROR_STREAM_THROTTLE(1, "TIMEOUT on motor controller, too large a difference between current time and last +// motor: " << dt); +// }*/ + +// /* fill in the message fields */ +// request.speed_l = static_cast(current_motor_command_.left_velocity); +// request.speed_r = static_cast(current_motor_command_.right_velocity); + +// /* encode the protobuffer */ +// bool status = pb_encode(&ostream, RequestMessage_fields, &request); +// size_t message_length = ostream.bytes_written; + +// /* check for any errors.. */ +// if (!status) +// { +// ROS_ERROR_STREAM("Encoding failed: " << PB_GET_ERROR(&ostream)); +// ros::shutdown(); +// } + +// /* Send the message strapped to a pigeon's leg! */ +// (*sock_).sendMessage(reinterpret_cast(requestbuffer), message_length); +// } + +// void MotorController::recieveResponse() +// { +// /* Read response from the server */ +// size_t n; // n is the response from socket: 0 means connection closed, otherwise n = num bytes read +// uint8_t buffer[256]; // buffer to read response into + +// memset(buffer, 0, sizeof(buffer)); +// /* read from the buffer */ +// n = (*sock_).readMessage(buffer); // blocks until data is read + +// if (n == 0) +// { +// ROS_ERROR_STREAM("Connection closed by server"); +// ros::shutdown(); +// } + +// /* Allocate space for the decoded message. */ +// ResponseMessage response = ResponseMessage_init_zero; + +// /* Create a stream that reads from the buffer. */ +// pb_istream_t istream = pb_istream_from_buffer(buffer, n); + +// /* decode the message. */ +// bool status = pb_decode(&istream, ResponseMessage_fields, &response); + +// /* check for any errors.. */ +// if (!status) +// { +// ROS_ERROR_STREAM("Decoding Failed: " << PB_GET_ERROR(&istream)); +// ros::shutdown(); +// } + +// publishResponse(response); +// mc_hertz_ = 1 / response.dt_sec; +// } + +// void MotorController::publishResponse(const ResponseMessage& response) +// { +// /* update the exponentially weighted moving voltage average and publish */ +// std_msgs::Float64 battery_msg; +// battery_avg_ = (battery_alpha_ * battery_avg_) + ((1 - battery_alpha_) * response.voltage); +// battery_msg.data = battery_avg_; +// battery_pub_.publish(battery_msg); + +// if (battery_avg_ < min_battery_voltage_) +// { +// ROS_ERROR_STREAM_THROTTLE(log_period_, "Battery voltage dangerously low:" +// << "\n\tCurr. Voltage: " << battery_avg_ +// << "\n\tMin. Voltage: " << min_battery_voltage_); +// } + +// std_msgs::Bool enabled_msg; +// enabled_msg.data = response.estop; +// enabled_pub_.publish(enabled_msg); + +// /* publish encoder feedback */ +// igvc_msgs::velocity_pair enc_msg; +// enc_msg.left_velocity = response.speed_l; +// enc_msg.right_velocity = response.speed_r; +// enc_msg.duration = response.dt_sec; +// enc_msg.header.stamp = ros::Time::now() - ros::Duration(response.dt_sec); +// enc_pub_.publish(enc_msg); +// } + +// int main(int argc, char** argv) +// { +// ros::init(argc, argv, "motor_controller"); +// ros::NodeHandle nh; +// MotorController motor_controller(&nh); +// return 0; +// } diff --git a/igvc_platform/src/motor_controller/motor_controller.h b/igvc_platform/src/motor_controller/motor_controller.h index 0e364683f..afb8fad84 100644 --- a/igvc_platform/src/motor_controller/motor_controller.h +++ b/igvc_platform/src/motor_controller/motor_controller.h @@ -1,97 +1,97 @@ -#include -#include -#include - -#include -#include - -#include -#include -#include -#include - -class MotorController -{ -public: - MotorController(ros::NodeHandle* nodehandle); - -private: - ros::NodeHandle nh_; - ros::Time last_motors_message_; - - // seconds to wait before stopping if no new motors command comes in - double watchdog_delay_; - - igvc_msgs::velocity_pair current_motor_command_; // desired motor velocities - double p_l_, p_r_, d_l_, d_r_, i_l_, i_r_; // PID Values - double kv_l_, kv_r_; - - // launch parameters - std::string ip_addr_; // server ip address - int tcpport_; // server tcp tcp port - double min_battery_voltage_; // min battery voltage before warnings - double log_period_; // Period for logging messages - double frequency_; // communicate frequency_ with the mbed - double battery_alpha_; // alpha value for voltage exponentially weighted moving average - // approximate # of timesteps average taken over = 1 / (1-alpha) - - // current battery voltage - double battery_avg_; - - std::unique_ptr sock_; - - // subscribers - ros::Subscriber cmd_sub_; - - // publishers - ros::Publisher enc_pub_; - ros::Publisher enabled_pub_; - ros::Publisher battery_pub_; - - // diagnostics - diagnostic_updater::Updater mc_updater_; - diagnostic_updater::Updater battery_updater_; - double mc_hertz_ = 0; - - /** - updates the motor controller diagnostics - - @param[in] Diagnostics Status Wrapper - */ - void mc_diagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat); - /** - updates the battery diagnostics - - @param[in] Diagnostics Status Wrapper - */ - void battery_diagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat); - /** - get current motor command from the /motors topic - - @param[in] msg current message on the /motors topic - */ - void cmdCallback(const igvc_msgs::velocity_pair::ConstPtr& msg); - /** - Sets PID values on the mbed - */ - void setPID(); - /** - Sends a request to the mbed using the current motor velocities, as read from - the /motors topic - */ - void sendRequest(); - /** - Recieves a message from the mbed containing battery voltage, enabled status, and - encoder information - */ - void recieveResponse(); - /** - Publishes robot status recieved from the mbed to the following topics: - - /encoders - - /robot_enabled - - /battery - - @param[in] response the response message recieved from the mbed - */ - void publishResponse(const ResponseMessage& response); -}; +// #include +// #include +// #include + +// #include +// #include + +// #include +// #include +// #include +// #include + +// class MotorController +// { +// public: +// MotorController(ros::NodeHandle* nodehandle); + +// private: +// ros::NodeHandle nh_; +// ros::Time last_motors_message_; + +// // seconds to wait before stopping if no new motors command comes in +// double watchdog_delay_; + +// igvc_msgs::velocity_pair current_motor_command_; // desired motor velocities +// double p_l_, p_r_, d_l_, d_r_, i_l_, i_r_; // PID Values +// double kv_l_, kv_r_; + +// // launch parameters +// std::string ip_addr_; // server ip address +// int tcpport_; // server tcp tcp port +// double min_battery_voltage_; // min battery voltage before warnings +// double log_period_; // Period for logging messages +// double frequency_; // communicate frequency_ with the mbed +// double battery_alpha_; // alpha value for voltage exponentially weighted moving average +// // approximate # of timesteps average taken over = 1 / (1-alpha) + +// // current battery voltage +// double battery_avg_; + +// std::unique_ptr sock_; + +// // subscribers +// ros::Subscriber cmd_sub_; + +// // publishers +// ros::Publisher enc_pub_; +// ros::Publisher enabled_pub_; +// ros::Publisher battery_pub_; + +// // diagnostics +// diagnostic_updater::Updater mc_updater_; +// diagnostic_updater::Updater battery_updater_; +// double mc_hertz_ = 0; + +// /** +// updates the motor controller diagnostics + +// @param[in] Diagnostics Status Wrapper +// */ +// void mc_diagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat); +// /** +// updates the battery diagnostics + +// @param[in] Diagnostics Status Wrapper +// */ +// void battery_diagnostic(diagnostic_updater::DiagnosticStatusWrapper& stat); +// /** +// get current motor command from the /motors topic + +// @param[in] msg current message on the /motors topic +// */ +// void cmdCallback(const igvc_msgs::velocity_pair::ConstPtr& msg); +// /** +// Sets PID values on the mbed +// */ +// void setPID(); +// /** +// Sends a request to the mbed using the current motor velocities, as read from +// the /motors topic +// */ +// void sendRequest(); +// /** +// Recieves a message from the mbed containing battery voltage, enabled status, and +// encoder information +// */ +// void recieveResponse(); +// /** +// Publishes robot status recieved from the mbed to the following topics: +// - /encoders +// - /robot_enabled +// - /battery + +// @param[in] response the response message recieved from the mbed +// */ +// void publishResponse(const ResponseMessage& response); +// }; diff --git a/igvc_platform/src/tests/CMakeLists.txt b/igvc_platform/src/tests/CMakeLists.txt index b08e04750..3b9dd8855 100644 --- a/igvc_platform/src/tests/CMakeLists.txt +++ b/igvc_platform/src/tests/CMakeLists.txt @@ -1,15 +1,7 @@ -add_rostest_gtest(TestJoystickDriver test/test_joystick_driver.test test_joystick_driver.cpp) -add_dependencies(TestJoystickDriver ${catkin_EXPORTED_TARGETS} joystick_driver) -target_link_libraries(TestJoystickDriver ${catkin_LIBRARIES}) - add_rostest_gtest(TestSwerveJoystickDriver test/test_swerve_joystick_driver.test test_swerve_joystick_driver.cpp) add_dependencies(TestSwerveJoystickDriver ${catkin_EXPORTED_TARGETS} joystick_driver) target_link_libraries(TestSwerveJoystickDriver ${catkin_LIBRARIES}) -add_rostest_gtest(TestDifferentialDrive test/test_differential_drive.test test_differential_drive.cpp) -add_dependencies(TestDifferentialDrive ${catkin_EXPORTED_TARGETS} differential_drive) -target_link_libraries(TestDifferentialDrive ${catkin_LIBRARIES}) - add_rostest_gtest(TestScanToPointCloud test/test_scan_to_pointcloud.test test_scan_to_pointcloud.cpp) add_dependencies(TestScanToPointCloud ${catkin_EXPORTED_TARGETS} scan_to_pointcloud) target_link_libraries(TestScanToPointCloud ${catkin_LIBRARIES}) diff --git a/igvc_platform/src/tests/test/test_differential_drive.test b/igvc_platform/src/tests/test/test_differential_drive.test deleted file mode 100644 index 859be84cc..000000000 --- a/igvc_platform/src/tests/test/test_differential_drive.test +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/igvc_platform/src/tests/test/test_joystick_driver.test b/igvc_platform/src/tests/test/test_joystick_driver.test deleted file mode 100644 index beae18432..000000000 --- a/igvc_platform/src/tests/test/test_joystick_driver.test +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/igvc_platform/src/tests/test_differential_drive.cpp b/igvc_platform/src/tests/test_differential_drive.cpp deleted file mode 100644 index 879b938b6..000000000 --- a/igvc_platform/src/tests/test_differential_drive.cpp +++ /dev/null @@ -1,218 +0,0 @@ -#include -#include -#include -#include -#include - -class TestDifferentialDrive : public testing::Test -{ -public: - TestDifferentialDrive() : mock_pub(handle.advertise("/cmd_vel", 1)) - { - } - -protected: - ros::NodeHandle handle; - ros::Publisher mock_pub; -}; - -geometry_msgs::Twist createTwistMsg(float forward, float spin) -{ - geometry_msgs::Twist twist_msg; - twist_msg.angular.x = 0.0; - twist_msg.angular.y = 0.0; - twist_msg.angular.z = spin; - twist_msg.linear.x = forward; - twist_msg.linear.y = 0.0; - twist_msg.linear.z = 0.0; - - return twist_msg; -} - -igvc_msgs::velocity_pair twistToVelocity(geometry_msgs::Twist twist, double axle_length_, double max_vel_) -{ - double speed = twist.linear.x; - double rotation = twist.angular.z; - double vel_right = speed + (rotation * axle_length_) / 2; - double vel_left = speed - (rotation * axle_length_) / 2; - - double max_calc_vel = fmax(vel_right, vel_left); - if (max_calc_vel > max_vel_) - { - vel_right *= max_vel_ / max_calc_vel; - vel_left *= max_vel_ / max_calc_vel; - } - - igvc_msgs::velocity_pair vel_msg; - vel_msg.right_velocity = vel_right; - vel_msg.left_velocity = vel_left; - return vel_msg; -} - -TEST_F(TestDifferentialDrive, StopTest) -{ - MockSubscriber mock_sub("/motors"); - ASSERT_TRUE(mock_sub.waitForPublisher()); - ASSERT_TRUE(mock_sub.waitForSubscriber(mock_pub)); - - const float stop = 0.0; - mock_pub.publish(createTwistMsg(stop, stop)); - - ASSERT_TRUE(mock_sub.spinUntilMessages()); - - ASSERT_EQ(mock_sub.messages().size(), 1LU); - const igvc_msgs::velocity_pair& response = mock_sub.front(); - - EXPECT_EQ(response.left_velocity, stop); - EXPECT_EQ(response.right_velocity, stop); -} - -TEST_F(TestDifferentialDrive, ForwardTest) -{ - MockSubscriber mock_sub("/motors"); - ASSERT_TRUE(mock_sub.waitForPublisher()); - ASSERT_TRUE(mock_sub.waitForSubscriber(mock_pub)); - - const float forward = 2.0; - mock_pub.publish(createTwistMsg(forward, 0.0)); - - ASSERT_TRUE(mock_sub.spinUntilMessages()); - - ASSERT_EQ(mock_sub.messages().size(), 1LU); - const igvc_msgs::velocity_pair& response = mock_sub.front(); - - EXPECT_EQ(response.left_velocity, forward); - EXPECT_EQ(response.right_velocity, forward); -} - -TEST_F(TestDifferentialDrive, TurnTest) -{ - MockSubscriber mock_sub("/motors"); - ASSERT_TRUE(mock_sub.waitForPublisher()); - ASSERT_TRUE(mock_sub.waitForSubscriber(mock_pub)); - - double axle_length_; - handle.getParam("differential_drive/axle_length", axle_length_); - double max_vel_; - handle.getParam("differential_drive/max_vel", max_vel_); - - const float forward = 1.0; - const float spin = 1.0; - - const geometry_msgs::Twist twist_msg = createTwistMsg(forward, spin); - - mock_pub.publish(twist_msg); - - const igvc_msgs::velocity_pair vel_msg = twistToVelocity(twist_msg, axle_length_, max_vel_); - - ASSERT_TRUE(mock_sub.spinUntilMessages()); - - ASSERT_EQ(mock_sub.messages().size(), 1LU); - const igvc_msgs::velocity_pair& response = mock_sub.front(); - - EXPECT_EQ(response.left_velocity, vel_msg.left_velocity); - EXPECT_EQ(response.right_velocity, vel_msg.right_velocity); -} - -TEST_F(TestDifferentialDrive, SpinTest) -{ - MockSubscriber mock_sub("/motors"); - ASSERT_TRUE(mock_sub.waitForPublisher()); - ASSERT_TRUE(mock_sub.waitForSubscriber(mock_pub)); - - double axle_length_; - handle.getParam("differential_drive/axle_length", axle_length_); - double max_vel_; - handle.getParam("differential_drive/max_vel", max_vel_); - - const float forward = 0.0; - const float spin = 1.0; - - const geometry_msgs::Twist twist_msg = createTwistMsg(forward, spin); - - mock_pub.publish(twist_msg); - - const igvc_msgs::velocity_pair vel_msg = twistToVelocity(twist_msg, axle_length_, max_vel_); - - ASSERT_TRUE(mock_sub.spinUntilMessages()); - - ASSERT_EQ(mock_sub.messages().size(), 1LU); - const igvc_msgs::velocity_pair& response = mock_sub.front(); - - EXPECT_EQ(response.left_velocity, vel_msg.left_velocity); - EXPECT_EQ(response.right_velocity, vel_msg.right_velocity); -} - -TEST_F(TestDifferentialDrive, MaxSpeedTest) -{ - MockSubscriber mock_sub("/motors"); - ASSERT_TRUE(mock_sub.waitForPublisher()); - ASSERT_TRUE(mock_sub.waitForSubscriber(mock_pub)); - - double axle_length_; - handle.getParam("differential_drive/axle_length", axle_length_); - double max_vel_; - handle.getParam("differential_drive/max_vel", max_vel_); - - const float forward = 4.0; - const float spin = 1.0; - - const geometry_msgs::Twist twist_msg = createTwistMsg(forward, spin); - - mock_pub.publish(twist_msg); - - const igvc_msgs::velocity_pair vel_msg = twistToVelocity(twist_msg, axle_length_, max_vel_); - - ASSERT_TRUE(mock_sub.spinUntilMessages()); - - ASSERT_EQ(mock_sub.messages().size(), 1LU); - const igvc_msgs::velocity_pair& response = mock_sub.front(); - - EXPECT_EQ(response.left_velocity, vel_msg.left_velocity); - EXPECT_EQ(response.right_velocity, vel_msg.right_velocity); -} - -TEST_F(TestDifferentialDrive, NoiseIgnoreTest) -{ - MockSubscriber mock_sub("/motors"); - ASSERT_TRUE(mock_sub.waitForPublisher()); - ASSERT_TRUE(mock_sub.waitForSubscriber(mock_pub)); - - double axle_length_; - handle.getParam("differential_drive/axle_length", axle_length_); - double max_vel_; - handle.getParam("differential_drive/max_vel", max_vel_); - - const float forward = 4.0; - const float spin = 1.0; - - geometry_msgs::Twist twist_msg; - twist_msg.angular.x = 2.0; - twist_msg.angular.y = 3.0; - twist_msg.angular.z = spin; - twist_msg.linear.x = forward; - twist_msg.linear.y = 4.0; - twist_msg.linear.z = 2.0; - - mock_pub.publish(twist_msg); - - const igvc_msgs::velocity_pair vel_msg = twistToVelocity(twist_msg, axle_length_, max_vel_); - - ASSERT_TRUE(mock_sub.spinUntilMessages()); - - ASSERT_EQ(mock_sub.messages().size(), 1LU); - const igvc_msgs::velocity_pair& response = mock_sub.front(); - - EXPECT_EQ(response.left_velocity, vel_msg.left_velocity); - EXPECT_EQ(response.right_velocity, vel_msg.right_velocity); -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "test_differential_drive"); - testing::InitGoogleTest(&argc, argv); - - int ret = RUN_ALL_TESTS(); - ros::shutdown(); - return ret; -} diff --git a/igvc_platform/src/tests/test_joystick_driver.cpp b/igvc_platform/src/tests/test_joystick_driver.cpp deleted file mode 100644 index 404d581c3..000000000 --- a/igvc_platform/src/tests/test_joystick_driver.cpp +++ /dev/null @@ -1,177 +0,0 @@ -// -// Created by matt on 1/29/16. -// - -#include -#include -#include -#include -#include - -class TestJoystickDriver : public testing::Test -{ -public: - TestJoystickDriver() : mock_joy_pub(handle.advertise("/joy", 1)) - { - } - -protected: - ros::NodeHandle handle; - ros::Publisher mock_joy_pub; -}; - -sensor_msgs::Joy createJoyMsg(float left, float right) -{ - sensor_msgs::Joy joy_msg; - joy_msg.axes = { 0, left, 0, 0, right }; - joy_msg.buttons = { 0, 0, 0, 0 }; - - return joy_msg; -}; - -TEST_F(TestJoystickDriver, FullForward) -{ - MockSubscriber mock_sub("/motors"); - ASSERT_TRUE(mock_sub.waitForPublisher()); - ASSERT_TRUE(mock_sub.waitForSubscriber(mock_joy_pub)); - - const float full_speed = 1.0; - mock_joy_pub.publish(createJoyMsg(full_speed, full_speed)); - - ASSERT_TRUE(mock_sub.spinUntilMessages()); - - ASSERT_EQ(mock_sub.messages().size(), 1LU); - const igvc_msgs::velocity_pair& response = mock_sub.front(); - - EXPECT_EQ(response.left_velocity, full_speed); - EXPECT_EQ(response.right_velocity, full_speed); -} - -TEST_F(TestJoystickDriver, FullReverse) -{ - MockSubscriber mock_sub("/motors"); - ASSERT_TRUE(mock_sub.waitForPublisher()); - ASSERT_TRUE(mock_sub.waitForSubscriber(mock_joy_pub)); - const float full_speed = 1.0; - mock_joy_pub.publish(createJoyMsg(-full_speed, -full_speed)); - - ASSERT_TRUE(mock_sub.spinUntilMessages()); - - ASSERT_EQ(mock_sub.messages().size(), 1LU); - const igvc_msgs::velocity_pair& response = mock_sub.front(); - - EXPECT_EQ(response.left_velocity, -full_speed); - EXPECT_EQ(response.right_velocity, -full_speed); -} - -TEST_F(TestJoystickDriver, SpinRight) -{ - MockSubscriber mock_sub("/motors"); - ASSERT_TRUE(mock_sub.waitForPublisher()); - ASSERT_TRUE(mock_sub.waitForSubscriber(mock_joy_pub)); - const float full_speed = 1.0; - mock_joy_pub.publish(createJoyMsg(full_speed, -full_speed)); - - ASSERT_TRUE(mock_sub.spinUntilMessages()); - - ASSERT_EQ(mock_sub.messages().size(), 1LU); - const igvc_msgs::velocity_pair& response = mock_sub.front(); - - EXPECT_EQ(response.left_velocity, full_speed); - EXPECT_EQ(response.right_velocity, -full_speed); -} - -TEST_F(TestJoystickDriver, SpinLeft) -{ - MockSubscriber mock_sub("/motors"); - ASSERT_TRUE(mock_sub.waitForPublisher()); - ASSERT_TRUE(mock_sub.waitForSubscriber(mock_joy_pub)); - const float full_speed = 1.0; - mock_joy_pub.publish(createJoyMsg(-full_speed, full_speed)); - - ASSERT_TRUE(mock_sub.spinUntilMessages()); - - ASSERT_EQ(mock_sub.messages().size(), 1LU); - const igvc_msgs::velocity_pair& response = mock_sub.front(); - - EXPECT_EQ(response.left_velocity, -full_speed); - EXPECT_EQ(response.right_velocity, full_speed); -} - -TEST_F(TestJoystickDriver, HalfSpeedForward) -{ - MockSubscriber mock_sub("/motors"); - ASSERT_TRUE(mock_sub.waitForPublisher()); - ASSERT_TRUE(mock_sub.waitForSubscriber(mock_joy_pub)); - const float half_speed = 0.5; - mock_joy_pub.publish(createJoyMsg(half_speed, half_speed)); - - ASSERT_TRUE(mock_sub.spinUntilMessages()); - - ASSERT_EQ(mock_sub.messages().size(), 1LU); - const igvc_msgs::velocity_pair& response = mock_sub.front(); - - EXPECT_EQ(response.left_velocity, half_speed); - EXPECT_EQ(response.right_velocity, half_speed); -} - -TEST_F(TestJoystickDriver, HalfSpeedReverse) -{ - MockSubscriber mock_sub("/motors"); - ASSERT_TRUE(mock_sub.waitForPublisher()); - ASSERT_TRUE(mock_sub.waitForSubscriber(mock_joy_pub)); - const float half_speed = 0.5; - mock_joy_pub.publish(createJoyMsg(-half_speed, -half_speed)); - - ASSERT_TRUE(mock_sub.spinUntilMessages()); - - ASSERT_EQ(mock_sub.messages().size(), 1LU); - const igvc_msgs::velocity_pair& response = mock_sub.front(); - - EXPECT_EQ(response.left_velocity, -half_speed); - EXPECT_EQ(response.right_velocity, -half_speed); -} - -TEST_F(TestJoystickDriver, HalfSpinRight) -{ - MockSubscriber mock_sub("/motors"); - ASSERT_TRUE(mock_sub.waitForPublisher()); - ASSERT_TRUE(mock_sub.waitForSubscriber(mock_joy_pub)); - const float half_speed = 0.5; - mock_joy_pub.publish(createJoyMsg(half_speed, -half_speed)); - - ASSERT_TRUE(mock_sub.spinUntilMessages()); - - ASSERT_EQ(mock_sub.messages().size(), 1LU); - const igvc_msgs::velocity_pair& response = mock_sub.front(); - - EXPECT_EQ(response.left_velocity, half_speed); - EXPECT_EQ(response.right_velocity, -half_speed); -} - -TEST_F(TestJoystickDriver, HalfSpinLeft) -{ - MockSubscriber mock_sub("/motors"); - ASSERT_TRUE(mock_sub.waitForPublisher()); - ASSERT_TRUE(mock_sub.waitForSubscriber(mock_joy_pub)); - const float half_speed = 0.5; - mock_joy_pub.publish(createJoyMsg(-half_speed, half_speed)); - - ASSERT_TRUE(mock_sub.spinUntilMessages()); - - ASSERT_EQ(mock_sub.messages().size(), 1LU); - const igvc_msgs::velocity_pair& response = mock_sub.front(); - - EXPECT_EQ(response.left_velocity, -half_speed); - EXPECT_EQ(response.right_velocity, half_speed); -} - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "test_joystick_driver"); - testing::InitGoogleTest(&argc, argv); - - int ret = RUN_ALL_TESTS(); - ros::shutdown(); - return ret; -} diff --git a/igvc_rviz_plugins/include/igvc_rviz_plugins_old/example_panel.h b/igvc_rviz_plugins/include/igvc_rviz_plugins_old/example_panel.h deleted file mode 100644 index 9da8c001b..000000000 --- a/igvc_rviz_plugins/include/igvc_rviz_plugins_old/example_panel.h +++ /dev/null @@ -1,52 +0,0 @@ -#ifndef PROJECT_EXAMPLEPANEL_H -#define PROJECT_EXAMPLEPANEL_H - -#include -#include -#include -#include - -/* - * All of our panels need to be under the igvc_rviz_plugins_old namespace. - */ -namespace igvc_rviz_plugins -{ -/* - * Each panel is a subclass of rviz::Panel - */ -class ExamplePanel : public rviz::Panel -{ - /* - * rviz is based on QT, so our panels need to be QObjects, which requires this macro keyword. - */ - Q_OBJECT -public: - /** - * This is a standard QWidget constructor. - * @param parent The parent widget, which will be responsible for the lifetime of this widget. - */ - ExamplePanel(QWidget *parent = 0); - -protected: - /* - * Be sure to make any publishers / subscribers members of the class. This will keep them alive throughout - * the lifetime of the widget. - */ - ros::Subscriber speed_subscriber; - - /** - * Declare any ROS callbacks you need here. Be sure to create a parameter for any UI elements you need to update. - * @param msg The ROS message that triggers this callback. - * @param label A QT label whose text we will update based on the message contents. - */ - void speed_callback(const igvc_msgs::velocity_pairConstPtr &msg, QLabel *label); - - /** - * If you need to paint custom graphics on your panel, uncomment and implement the paintEvent method. - * You can find out more about this method here: http://doc.qt.io/qt-5/qwidget.html#paintEvent - */ - // void paintEvent(QPaintEvent *event) override; -}; -} // namespace igvc_rviz_plugins - -#endif // PROJECT_EXAMPLEPANEL_H diff --git a/igvc_rviz_plugins/include/igvc_rviz_plugins_old/speed_panel.h b/igvc_rviz_plugins/include/igvc_rviz_plugins_old/speed_panel.h deleted file mode 100644 index b069ed7f3..000000000 --- a/igvc_rviz_plugins/include/igvc_rviz_plugins_old/speed_panel.h +++ /dev/null @@ -1,40 +0,0 @@ -#ifndef SPEED_PANEL_H -#define SPEED_PANEL_H - -#include -#include -#include - -#include - -#include -#include "speedometer.h" -class QLineEdit; - -namespace rviz_plugins -{ -class SpeedPanel : public rviz::Panel -{ - Q_OBJECT -public: - SpeedPanel(QWidget* parent = 0); - -private: - void subCallback(const igvc_msgs::velocity_pair& msg); - double speed; - Speedometer* speedometer; - -public Q_SLOTS: - -protected Q_SLOTS: - -protected: - ros::Subscriber sub; - ros::NodeHandle nh_; - QLabel* velocity_label; - QLabel* velocity_right_label; - QLabel* velocity_left_label; -}; -} // namespace rviz_plugins - -#endif // SPEED_PANEL_H \ No newline at end of file diff --git a/igvc_rviz_plugins/src/igvc_rviz_plugins_old/example_panel.cpp b/igvc_rviz_plugins/src/igvc_rviz_plugins_old/example_panel.cpp deleted file mode 100644 index c67f12832..000000000 --- a/igvc_rviz_plugins/src/igvc_rviz_plugins_old/example_panel.cpp +++ /dev/null @@ -1,49 +0,0 @@ -#include -#include -#include - -/* - * Just as in the header, everything needs to happen in the igvc_rviz_plugins_old namespace. - */ -namespace igvc_rviz_plugins -{ -ExamplePanel::ExamplePanel(QWidget *parent) : rviz::Panel(parent) // Base class constructor -{ - // Panels are allowed to interact with NodeHandles directly just like ROS nodes. - ros::NodeHandle handle; - - // Initialize a label for displaying some data - QLabel *label = new QLabel("0 m/s"); - - /* Initialize our subscriber to listen to the /speed topic. - * Note the use of boost::bind, which allows us to give the callback a pointer to our UI label. - */ - speed_subscriber = handle.subscribe( - "/encoders", 1, boost::bind(&ExamplePanel::speed_callback, this, _1, label)); - - /* Use QT layouts to add widgets to the panel. - * Here, we're using a VBox layout, which allows us to stack all of our widgets vertically. - */ - QVBoxLayout *layout = new QVBoxLayout; - layout->addWidget(label); - setLayout(layout); -} - -void ExamplePanel::speed_callback(const igvc_msgs::velocity_pairConstPtr &msg, QLabel *label) -{ - // Create the new contents of the label based on the speed message. - auto text = "Left: " + std::to_string(msg->left_velocity) + " m/s\n" + - "Right: " + std::to_string(msg->right_velocity) + " m/s"; - // Set the contents of the label. - label->setText(text.c_str()); -} - -// void ExamplePanel::paintEvent(QPaintEvent *event) { -// -//} -} // namespace igvc_rviz_plugins - -/* - * IMPORTANT! This macro must be filled out correctly for your panel class. - */ -PLUGINLIB_EXPORT_CLASS(igvc_rviz_plugins::ExamplePanel, rviz::Panel) diff --git a/igvc_rviz_plugins/src/igvc_rviz_plugins_old/speed_panel.cpp b/igvc_rviz_plugins/src/igvc_rviz_plugins_old/speed_panel.cpp deleted file mode 100644 index 26623a0ff..000000000 --- a/igvc_rviz_plugins/src/igvc_rviz_plugins_old/speed_panel.cpp +++ /dev/null @@ -1,60 +0,0 @@ -#include -#include -#include -#include -#include - -namespace rviz_plugins -{ -void SpeedPanel::subCallback(const igvc_msgs::velocity_pair& msg) -{ - speed = (msg.left_velocity + msg.right_velocity) / 2; - std::string str = std::to_string(speed); - speedometer->setValue(speed); - // QString qstr = QString::fromStdString(str); - // velocity_label->setText(qstr); - // std::string str2=std::to_string(msg.left_velocity); - // QString qstr2 = QString::fromStdString(str2); - // velocity_left_label->setText(qstr2); - // std::string str3=std::to_string(msg.right_velocity); - // QString qstr3 = QString::fromStdString(str3); - // velocity_right_label->setText(qstr3); -} - -SpeedPanel::SpeedPanel(QWidget* parent) : rviz::Panel(parent) -{ - speedometer = new Speedometer(parent); - QHBoxLayout* speedDisp = new QHBoxLayout; - speedDisp->addWidget(new QLabel("SPEEDOMETER")); - speedDisp->addWidget(speedometer); - - /*QHBoxLayout* velocity_layout = new QHBoxLayout; - velocity_layout->addWidget( new QLabel( "VELOCITY:" ) ); - velocity_label = new QLabel("COOLBEFORE"); - velocity_layout->addWidget( velocity_label ); - - QHBoxLayout* velocity_left_layout = new QHBoxLayout; - velocity_left_layout->addWidget( new QLabel( "VELOCITY_LEFT:" ) ); - velocity_left_label = new QLabel("COOLBEFORE"); - velocity_left_layout->addWidget( velocity_left_label ); - - QHBoxLayout* velocity_right_layout = new QHBoxLayout; - velocity_right_layout->addWidget( new QLabel( "VELOCITY_RIGHT:" ) ); - velocity_right_label = new QLabel("COOLBEFORE"); - velocity_right_layout->addWidget( velocity_right_label ); -*/ - QVBoxLayout* layout = new QVBoxLayout; - /*layout->addLayout( velocity_layout ); - layout->addLayout( velocity_left_layout ); - layout->addLayout( velocity_right_layout);*/ - layout->addLayout(speedDisp); - setLayout(layout); - - sub = nh_.subscribe("/encoders", 1, &SpeedPanel::subCallback, this); - - // connect( this, SIGNAL( changeText() ), output_topic_editor_, SLOT( setTextLabel() )); -} -} // namespace rviz_plugins - -#include -PLUGINLIB_EXPORT_CLASS(rviz_plugins::SpeedPanel, rviz::Panel) diff --git a/igvc_utils/CMakeLists.txt b/igvc_utils/CMakeLists.txt index 218cd3367..d91233b5d 100644 --- a/igvc_utils/CMakeLists.txt +++ b/igvc_utils/CMakeLists.txt @@ -158,9 +158,9 @@ install(DIRECTORY include ## Testing ## ############# -add_subdirectory(src/pid_tester) +#add_subdirectory(src/pid_tester) add_subdirectory(src/ethernet) -add_subdirectory(src/speed_compare) +#add_subdirectory(src/speed_compare) add_subdirectory(src/system_stats) add_subdirectory(src/quaternion_to_rpy) add_subdirectory(src/state) \ No newline at end of file From d4628c5c6d797f6b084b7af654196844e3328b6b Mon Sep 17 00:00:00 2001 From: Vivek Mhatre Date: Tue, 31 May 2022 01:53:48 -0400 Subject: [PATCH 26/27] fixed build issues --- igvc_msgs/msg/velocity_pair.msg | 4 ++++ igvc_navigation/config/teb_local_planner_params.yaml | 7 ------- 2 files changed, 4 insertions(+), 7 deletions(-) create mode 100644 igvc_msgs/msg/velocity_pair.msg diff --git a/igvc_msgs/msg/velocity_pair.msg b/igvc_msgs/msg/velocity_pair.msg new file mode 100644 index 000000000..5f36f7c6b --- /dev/null +++ b/igvc_msgs/msg/velocity_pair.msg @@ -0,0 +1,4 @@ +Header header +float64 left_velocity +float64 right_velocity +float64 duration diff --git a/igvc_navigation/config/teb_local_planner_params.yaml b/igvc_navigation/config/teb_local_planner_params.yaml index dbfc15e0c..cbcbec7e9 100644 --- a/igvc_navigation/config/teb_local_planner_params.yaml +++ b/igvc_navigation/config/teb_local_planner_params.yaml @@ -71,19 +71,12 @@ TebLocalPlannerROS: penalty_epsilon: 0.1 obstacle_cost_exponent: 2 weight_max_vel_x: 1 -<<<<<<< HEAD - weight_max_vel_theta: 1 - weight_acc_lim_x: 1 - weight_acc_lim_theta: 1 - weight_kinematics_nh: 0.01 -======= weight_max_vel_y: 1 weight_max_vel_theta: 1 weight_acc_lim_x: 1 weight_acc_lim_y: 1 weight_acc_lim_theta: 1 weight_kinematics_nh: 1 ->>>>>>> master weight_kinematics_forward_drive: 1 weight_kinematics_turning_radius: 1 weight_optimaltime: 1 # must be > 0 From 6812fbd4b3f001650a24de0e5d8cb9d4d2639271 Mon Sep 17 00:00:00 2001 From: Vivek Mhatre Date: Tue, 31 May 2022 02:01:16 -0400 Subject: [PATCH 27/27] formatting --- .../src/slope_filter/slope_filter.cpp | 21 ++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/igvc_perception/src/slope_filter/slope_filter.cpp b/igvc_perception/src/slope_filter/slope_filter.cpp index b9920e6ea..54f5baf73 100644 --- a/igvc_perception/src/slope_filter/slope_filter.cpp +++ b/igvc_perception/src/slope_filter/slope_filter.cpp @@ -7,8 +7,8 @@ TraversabilityFilter::TraversabilityFilter() : filter_chain_("grid_map::GridMap" { ROS_INFO_STREAM("RAW INFO RECEIVED"); private_nh_ = ros::NodeHandle("~"); - elevation_map_sub_ = private_nh_.subscribe("/elevation_mapping/elevation_map", 1, - &TraversabilityFilter::elevationMapCallback, this); + elevation_map_sub_ = + private_nh_.subscribe("/elevation_mapping/elevation_map", 1, &TraversabilityFilter::elevationMapCallback, this); traversability_map_pub_ = private_nh_.advertise("/slope/gridmap", 1); assertions::getParam(private_nh_, "erode_radius", erode_radius); @@ -36,13 +36,20 @@ void TraversabilityFilter::elevationMapCallback(const grid_map_msgs::GridMap& me cv::Mat elevation_data_mask; grid_map::GridMapCvConverter::toImage(input_map, "elevation", CV_8U, -1.0, 0.0, elevation_data_mask); - grid_map::GridMapCvConverter::addLayerFromImage(elevation_data_mask, "mask_original", slope_map, 0, 255, 0); + grid_map::GridMapCvConverter::addLayerFromImage(elevation_data_mask, "mask_original", slope_map, 0, + 255, 0); - cv::erode(elevation_data_mask, elevation_data_mask, cv::getStructuringElement(cv::MORPH_RECT, cv::Size(2*erode_radius+1, 2*erode_radius+1), cv::Point(erode_radius, erode_radius))); - grid_map::GridMapCvConverter::addLayerFromImage(elevation_data_mask, "mask_eroded", slope_map, 0, 255, 0); + cv::erode(elevation_data_mask, elevation_data_mask, + cv::getStructuringElement(cv::MORPH_RECT, cv::Size(2 * erode_radius + 1, 2 * erode_radius + 1), + cv::Point(erode_radius, erode_radius))); + grid_map::GridMapCvConverter::addLayerFromImage(elevation_data_mask, "mask_eroded", slope_map, 0, 255, + 0); - cv::dilate(elevation_data_mask, elevation_data_mask, cv::getStructuringElement(cv::MORPH_RECT, cv::Size(2*dilate_radius+1, 2*dilate_radius+1), cv::Point(dilate_radius, dilate_radius))); - grid_map::GridMapCvConverter::addLayerFromImage(elevation_data_mask, "mask_dilated", slope_map, 0, 255, 0); + cv::dilate(elevation_data_mask, elevation_data_mask, + cv::getStructuringElement(cv::MORPH_RECT, cv::Size(2 * dilate_radius + 1, 2 * dilate_radius + 1), + cv::Point(dilate_radius, dilate_radius))); + grid_map::GridMapCvConverter::addLayerFromImage(elevation_data_mask, "mask_dilated", slope_map, 0, + 255, 0); grid_map_msgs::GridMap output_message; grid_map::GridMapRosConverter::toMessage(slope_map, output_message);