Merge pull request #4 from Jacenty00/fixing_tf

[Frame_fixes | Cohan planner | Launch files fixes]

robot/agvota/agvota.model.yaml

@@ -1,5 +1,5 @@
 bodies: # List of named bodies
-  - name: base_footprint
+  - name: base_link
     pose: [0, 0, 0]
     type: dynamic
     color: [1.0, 0.64, 0.0, 1.0] # Orange hopefully
@@ -18,17 +18,17 @@ bodies: # List of named bodies
 plugins:
   - type: DiffDrive
     name: diff_drive
-    body: base_footprint
+    body: base_link
     odom_frame_id: odom
     odom_pub: odom # topic odom is published on
     twist_sub: cmd_vel
     pub_rate: 10
 
   - type: Laser
     name: static_laser
-    frame: laser_link
+    frame: scan_link
     topic: scan
-    body: base_footprint
+    body: base_link
     broadcast_tf: true
     origin: [0.0, 0.0, 0.0]
 

robot/agvota/costmaps/global_costmap_params.yaml

@@ -14,9 +14,9 @@ global_costmap:
 
   inflation_radius: 0.2
 
-  observation_sources: laser
+  observation_sources: scan
 
-  laser:
+  scan:
     {
       topic: scan,
       sensor_frame: scan_link,

robot/agvota/costmaps/hateb_local_planner_params.yaml

@@ -0,0 +1,178 @@
+# Please note that this is a basic parameter file and this config has not yet been configured with this specific robot in mind.
+odom_topic: odom
+map_frame: map
+
+planning_mode: 1
+
+# Trajectory
+teb_autosize: True
+dt_ref: 0.3
+dt_hysteresis: 0.1
+global_plan_overwrite_orientation: True
+allow_init_with_backwards_motion: False
+max_global_plan_lookahead_dist: 0.0
+feasibility_check_no_poses: 2
+global_plan_viapoint_sep: 0.2
+disable_warm_start: True
+shrink_horizon_backup: True
+
+# Robot
+holonomic_robot: true
+type: 0 #(0: Robot, 1: Human)
+max_vel_y: 0.7
+acc_lim_y: 0.3
+max_vel_x: 0.7
+min_vel_x: 0.02
+max_vel_x_backwards: 0.4
+max_vel_theta: 1.2
+min_vel_theta: 0.1
+acc_lim_x: 0.3
+acc_lim_theta: 0.4
+min_turning_radius: 0.0
+footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
+  type: "circular"
+  radius: 0.34 # for type "circular"
+  line_start: [-0.3, 0.0] # for type "line"
+  line_end: [0.3, 0.0] # for type "line"
+  front_offset: 0.2 # for type "two_circles"
+  front_radius: 0.2 # for type "two_circles"
+  rear_offset: 0.2 # for type "two_circles"
+  rear_radius: 0.2 # for type "two_circles"
+  vertices: [
+      [-0.325, -0.325],
+      [-0.325, 0.325],
+      [0.325, 0.325],
+      [0.46, 0.0],
+      [0.325, -0.325],
+    ] # for type "polygon"
+
+# Agents
+agent_radius: 0.31
+robot_radius: 0.47
+min_agent_dist: 0.4
+max_agent_vel_x: 1.3
+max_agent_vel_y: 0.4
+nominal_agent_vel_x: 1.1
+max_agent_vel_x_backwards: 0.01
+max_agent_vel_theta: 1.1
+agent_acc_lim_x: 0.6
+agent_acc_lim_y: 0.3
+agent_acc_lim_theta: 0.8
+use_external_prediction: True
+predict_agent_behind_robot: True
+predict_agent_goal: False
+agent_robot_ttc_scale_alpha: 1.0
+agent_robot_ttcplus_scale_alpha: 1.0
+agent_min_samples: 3
+use_agent_robot_safety_c: True
+use_agent_robot_rel_vel_c: True
+use_agent_robot_visi_c: True
+use_agent_agent_safety_c: True
+use_agent_robot_ttc_c: False
+use_agent_robot_ttclosest_c: False
+use_agent_robot_ttcplus_c: False
+scale_agent_robot_ttc_c: False
+scale_agent_robot_ttcplus_c: False
+add_invisible_humans: True
+ttc_threshold: 5.0
+ttclosest_threshold: 1.0
+ttcplus_threshold: 5.0
+ttcplus_timer: 1.0
+rel_vel_cost_threshold: 1.5
+visibility_cost_threshold: 2.5
+invisible_human_threshold: 0.8
+min_agent_robot_dist: 0.6
+
+# GoalTolerance
+xy_goal_tolerance: 0.2
+yaw_goal_tolerance: 0.1
+free_goal_vel: True
+
+# Obstacles
+min_obstacle_dist: 0.2
+include_costmap_obstacles: True
+costmap_obstacles_behind_robot_dist: 0.5
+obstacle_poses_affected: 1
+costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSRANSAC"
+costmap_converter_spin_thread: True
+costmap_converter_rate: 10
+obstacle_cost_mult: 1.0
+use_nonlinear_obstacle_penalty: True
+costmap_converter/CostmapToLinesDBSRANSAC:
+  cluster_max_distance: 0.8
+  cluster_min_pts: 2
+  ransac_inlier_distance: 0.15
+  ransac_min_inliers: 10
+  ransac_no_iterations: 1500
+  ransac_remainig_outliers: 3
+  ransac_convert_outlier_pts: True
+  ransac_filter_remaining_outlier_pts: False
+  convex_hull_min_pt_separation: 0.1
+
+# Optimization
+no_inner_iterations: 8
+no_outer_iterations: 4
+optimization_activate: True
+optimization_verbose: False
+penalty_epsilon: 0.01
+weight_max_vel_x: 2
+weight_max_vel_y: 2
+weight_max_agent_vel_x: 4
+weight_max_agent_vel_y: 4
+weight_nominal_agent_vel_x: 2
+weight_max_vel_theta: 1
+weight_max_agent_vel_theta: 2
+weight_acc_lim_x: 1
+weight_acc_lim_y: 1
+weight_agent_acc_lim_x: 2
+weight_agent_acc_lim_y: 2
+weight_acc_lim_theta: 1
+weight_agent_acc_lim_theta: 2
+weight_kinematics_nh: 1.0
+weight_kinematics_forward_drive: 1.0
+weight_kinematics_turning_radius: 0
+weight_optimaltime: 1.0
+weight_agent_optimaltime: 3.0
+weight_obstacle: 50.0
+weight_dynamic_obstacle: 50
+weight_viapoint: 0.05
+weight_agent_viapoint: 0.5
+weight_agent_robot_safety: 2.0
+weight_agent_agent_safety: 2.0
+weight_agent_robot_ttc: 1.0
+weight_agent_robot_rel_vel: 5.0
+weight_agent_robot_ttcplus: 1.0
+weight_agent_robot_visibility: 5.0
+weight_invisible_human: 2.0
+weight_shortest_path: 0
+selection_alternative_time_cost: False
+cap_optimaltime_penalty: True
+
+# Homotopy Class Planner
+enable_homotopy_class_planning: False
+enable_multithreading: True
+simple_exploration: True
+max_number_classes: 4
+roadmap_graph_no_samples: 15
+roadmap_graph_area_width: 5
+h_signature_prescaler: 0.5
+h_signature_threshold: 0.1
+obstacle_keypoint_offset: 0.1
+obstacle_heading_threshold: 0.45
+visualize_hc_graph: True
+
+# Visualization
+pose_array_z_scale: 0.5
+publish_agents_global_plans: true
+publish_agents_local_plan_fp_poses: true
+publish_agents_local_plan_poses: true
+publish_agents_local_plans: true
+publish_robot_global_plan: true
+publish_robot_local_plan: true
+publish_robot_local_plan_fp_poses: true
+publish_robot_local_plan_poses: true
+
+# approach
+approach_id: 1
+approach_dist: 1.0
+approach_angle: 3.14

robot/agvota/costmaps/local_costmap_params.yaml

@@ -21,9 +21,9 @@ local_costmap:
 
   inflation_radius: 0.2
 
-  observation_sources: laser
+  observation_sources: scan
 
-  laser:
+  scan:
     {
       topic: scan,
       sensor_frame: scan_link,

robot/agvota/model_params.yaml

@@ -1,5 +1,7 @@
 robot_model: agvota
 robot_radius: 0.629
+robot_base_frame: base_link
+robot_sensor_frame: scan_link
 is_holonomic: False
 actions:
   discrete:

robot/agvota/urdf/agv_base.urdf.xacro

@@ -85,8 +85,8 @@
 
         <plugin name="laser_scan_plugin" filename="libgazebo_ros_laser.so">
             <topicName>scan</topicName>
-            <frameName>${robot_namespace}_base_footprint</frameName>
-            <!-- <tf_prefix>${robot_namespace}</tf_prefix> -->
+            <frameName>scan_link</frameName>
+            <tf_prefix>/</tf_prefix>
         </plugin>
       </sensor>
     </gazebo>  

robot/agvota/urdf/agv_plugin.urdf.xacro

@@ -27,11 +27,11 @@
         <wheelDiameter>0.20</wheelDiameter>
         <wheelTorque>20</wheelTorque>
         <wheelAcceleration>1.8</wheelAcceleration>
-        <commandTopic>${robot_namespace}/cmd_vel</commandTopic>
-        <odometryTopic>${robot_namespace}/odom</odometryTopic>
-        <odometryFrame>${robot_namespace}_odom</odometryFrame>
+        <commandTopic>cmd_vel</commandTopic>
+        <odometryTopic>odom</odometryTopic>
+        <odometryFrame>odom</odometryFrame>
         <odometrySource>world</odometrySource>
-        <robotBaseFrame>${robot_namespace}_base_footprint</robotBaseFrame>
+        <robotBaseFrame>base_link</robotBaseFrame>
 	      <legacyMode>true</legacyMode>
 	      <publishTf>true</publishTf>
         <updateRate>50.0</updateRate>

robot/agvota/urdf/agvota.urdf.xacro

@@ -1,10 +1,6 @@
 <?xml version="1.0"?>
 <robot name="agvota" xmlns:xacro="http://ros.org/wiki/xacro">
 
-	<xacro:arg name="robot_namespace" default="$(optenv ROBOT_NAMESPACE /)" />
-	<xacro:property name="robot_namespace" value="$(arg robot_namespace)" />
-
-
 	<xacro:include filename="$(find arena-simulation-setup)/robot/agvota/urdf/agv_base.urdf.xacro"/>
 	<xacro:include filename="$(find arena-simulation-setup)/robot/agvota/urdf/inertia_tensors.urdf.xacro"/>	
 	<xacro:include filename="$(find arena-simulation-setup)/robot/agvota/urdf/agv_wheel.urdf.xacro"/>

robot/burger/burger.model.yaml

@@ -26,7 +26,7 @@ plugins:
 
   - type: Laser
     name: static_laser
-    frame: laser_link
+    frame: base_scan
     topic: scan
     body: base_footprint
     broadcast_tf: true