new spoofer

config/navigation_jackal_icl.lua

@@ -0,0 +1,74 @@
+function deg2rad(deg)
+  return deg * (math.pi / 180)
+end
+
+NavigationParameters = {
+  laser_topic = "velodyne_2dscan_highbeams";
+  odom_topic = "/odom";
+  -- odom_topic = "/odometry/filtered";
+  localization_topic = "localization";
+  image_topic = "/bev/single/compressed";
+  init_topic = "initialpose";
+  enable_topic = "autonomy_arbiter/enabled";
+  laser_loc = {
+    x = 0.065;
+    y = 0;
+  };
+  dt = 0.025;
+  max_linear_accel = 0.5;
+  max_linear_decel = 0.5;
+  -- max_linear_speed = 1.4;
+  max_linear_speed = 1.5;
+  max_angular_accel = 0.5;
+  max_angular_decel = 0.5;
+  max_angular_speed = 1.0;
+  carrot_dist = 250;  -- large carrot distance so the goal does not latch onto the map
+  system_latency = 0.24;
+  obstacle_margin = 0.15;
+  num_options = 11;
+  -- num_options = 15;
+  robot_width = 0.44;
+  robot_length = 0.5;
+  base_link_offset = 0;
+  max_free_path_length = 4.0;
+  -- max_free_path_length = 3.5;  -- ahg demo
+  -- max_free_path_length = 1;  -- ahg demo
+  max_clearance = 1.0;
+  can_traverse_stairs = false;
+  evaluator_type = "terrain2";
+  camera_calibration_path = "config/camera_calibration_kinect.yaml";
+  model_path = "";
+  local_fov = deg2rad(180);  -- do not turn in place when the goal is behind the robot
+  target_dist_tolerance = 0.5;
+  target_vel_tolerance = 0.2;
+  target_angle_tolerance = deg2rad(20);
+  use_map_speed = true;
+  use_kinect = false;
+};
+
+AckermannSampler = {
+  max_curvature = 2.5;
+  -- max_curvature = 5;
+  clearance_path_clip_fraction = 0.8;
+};
+
+TerrainEvaluator = {
+  patch_size_pixels = 1;
+  bev_pixels_per_meter = 150;
+  min_cost = 0.0;
+  max_cost = 1;
+  discount_factor = 0.9;
+  -- discount_factor = 0.8; -- ahg demo
+  rollout_density = 10;
+
+  model_path = "/home/dev/graph_navigation/terrain_models/model.pt";
+
+  -- dist_to_goal_weight = -0.2;
+  -- dist_to_goal_weight = -0.7;
+  -- dist_to_goal_weight = -2.0;
+  dist_to_goal_weight = 0;
+
+  clearance_weight = 0; -- -0.25;
+  fpl_weight = 0; -- -0.75;
+  terrain_weight = 4.0;
+}

config/navigation_spot_icl.lua

@@ -4,8 +4,7 @@ end
 
 NavigationParameters = {
   laser_topic = "velodyne_2dscan_highbeams";
-  odom_topic = "/odom";
-  -- odom_topic = "/odometry/filtered";
+  odom_topic = "/odometry/filtered";
   localization_topic = "localization";
   image_topic = "/bev/single/compressed";
   init_topic = "initialpose";

spoof_goal2.py

@@ -0,0 +1,37 @@
+import roslib
+roslib.load_manifest('graph_navigation')
+
+import rospy
+import numpy as np
+from amrl_msgs.msg import Localization2DMsg
+
+import argparse
+
+
+
+
+
+class Namespace:
+    pub = None
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser()
+    parser.add_argument("-delta", action="store_true")
+    parser.add_argument("-x", type=float, default=5.0)
+    parser.add_argument("-y", type=float, default=0)
+
+    config = parser.parse_args()
+
+    rospy.init_node("spoof_goal")
+    Namespace.pub = rospy.Publisher("/move_base_simple/goal_amrl", Localization2DMsg, queue_size=1)
+
+    while not rospy.is_shutdown():
+        goal = Localization2DMsg()
+        goal.pose.x = 0.0
+        goal.pose.y = 0.0
+        goal.pose.theta = 0.0
+        Namespace.pub.publish(goal)
+        # print("published")
+
+        rospy.sleep(1.0)  # Adjust the sleep duration as needed