Dev strategy kosta simple (#222)

* ForwardStuck working, finetune strategy for 6 on gallery * finetuned stuck and PID, motion is great now * Max speed, strategy works * in progress, yellow strategy for big-kosta-robot * Add kosta_yellow strategy, replace robot_left_connector by arm_connector, set start position for small yellow side * fix server name prefix * update blacklist * fix lidar constrain * fix inflator * update nav2 params * remove servos, need new Arm angles * update goal checker Co-authored-by: Filip Parag <filip@parag.rs>
memristor · May 25, 2022 · 882732b · 882732b
1 parent 0371896
commit 882732b
Show file tree

Hide file tree

Showing 13 changed files with 683 additions and 280 deletions.
diff --git a/mep3_behavior_tree/assets/strategies/small/kosta.xml b/mep3_behavior_tree/assets/strategies/small/kosta.xml
diff --git a/mep3_behavior_tree/assets/strategies/small/kosta_homologation.xml b/mep3_behavior_tree/assets/strategies/small/kosta_homologation.xml
@@ -0,0 +1,53 @@
+
+<root main_tree_to_execute="BehaviorTree">
+    <!-- ////////// -->
+    <BehaviorTree ID="BehaviorTree">
+        <SequenceStar>
+            <ForceSuccess>
+                <Timeout msec="100000">
+
+                    <Sequence>
+
+                        <WaitMatchStart state="1"/>
+
+                        <Motion command="forward" velocity_linear="0.5" acceleration_linear="0.5" velocity_angular="6.0" acceleration_angular="8.0" value="0" />
+
+                        <WaitMatchStart state="2"/>
+
+                        <SubTreePlus ID="SafePreciseNavigate" goal="0.915;0.45;180" type="safe" node="{node}" bt_loop_duration="{bt_loop_duration}" server_timeout="{server_timeout}" />
+
+                        <SubTreePlus ID="SafePreciseNavigate" goal="0.915;0.7;180" type="safe" node="{node}" bt_loop_duration="{bt_loop_duration}" server_timeout="{server_timeout}" />
+
+
+                        <SubTreePlus ID="SafePreciseNavigate" goal="0.456;0.7;180" type="safe" node="{node}" bt_loop_duration="{bt_loop_duration}" server_timeout="{server_timeout}" />
+
+                        <SubTreePlus ID="SafePreciseNavigate" goal="1.26;0.7;180" type="backward_safe" node="{node}" bt_loop_duration="{bt_loop_duration}" server_timeout="{server_timeout}" />
+
+                        <Wait duration="10000.0" />
+
+                    </Sequence>
+                </Timeout>
+            </ForceSuccess>
+
+            <CanbusSend can_id="8192" message="15" />
+            <CanbusSend can_id="8192" message="15" />
+            <CanbusSend can_id="8192" message="15" />
+        </SequenceStar>
+    </BehaviorTree>
+    <!-- ////////// -->
+    <BehaviorTree ID="SafePreciseNavigate">
+        <SequenceStar>
+            <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap" />
+            <RecoveryNode number_of_retries="10" name="FollowPath">
+                <PreciseNavigate goal="{goal}" behavior_tree="{type}" />
+                <Sequence>
+                    <Wait duration="2.5" />
+                    <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap" />
+                    <Wait duration="0.5" />
+                </Sequence>
+            </RecoveryNode>
+        </SequenceStar>
+    </BehaviorTree>
+    <!-- ////////// -->
+</root>
+
diff --git a/...behavior_tree/assets/strategies/kosta.xml → .../assets/strategies/small/kosta_yellow.xml b/...behavior_tree/assets/strategies/kosta.xml → .../assets/strategies/small/kosta_yellow.xml
diff --git a/mep3_behavior_tree/include/mep3_behavior_tree/team_color_strategy_mirror.hpp b/mep3_behavior_tree/include/mep3_behavior_tree/team_color_strategy_mirror.hpp
@@ -204,6 +204,15 @@ class StrategyMirror {
     return stripped_name;
   }
 
+  static std::string strip_server_name(const std::string& full_name) {
+    std::string stripped_name = full_name;
+    auto separator = full_name.find_last_of("/");
+    if (separator != std::string::npos && separator < stripped_name.length() - 1) {
+      stripped_name = stripped_name.substr(separator + 1, stripped_name.length() - (separator + 1));
+    }
+    return stripped_name;
+  }
+
   TeamColor color, default_color;
   std::vector<std::string> mirror_angle_blacklist, mirror_name_blacklist;
 

diff --git a/mep3_bringup/launch/robot_launch.py b/mep3_bringup/launch/robot_launch.py
@@ -16,10 +16,10 @@
 from launch.substitutions import PathJoinSubstitution
 
 INITIAL_POSE_MATRIX = [
+    ('big', 'purple', [1.249, 0.102, pi/2]),
     ('small', 'purple', [1.2755, 0.443, pi]),
     ('big', 'yellow', [-1.249, 0.102, pi/2]),
-    ('big', 'purple', [1.249, 0.102, pi/2]),
-    ('small', 'yellow', [-1.242, 0.452, pi/2]),
+    ('small', 'yellow', [-1.2755, 0.443, 0]),
 ]
 
 PREDEFINED_TABLE_NAMES = [

diff --git a/mep3_driver/mep3_driver/vacuum_pump_driver.py b/mep3_driver/mep3_driver/vacuum_pump_driver.py
@@ -17,11 +17,7 @@
 
 VACUUM_PUMP_CAN_ID = 0x00006C10
 VACUUMPUMPS = [
-    {'id': 1, 'name': 'arm_right_connector'},
-    {'id': 2, 'name': 'arm_left_connector'},
-    {'id': 3, 'name': 'arm_mid_connector'},
-    # {'id': 0, 'name': 'robot_right_connector'},
-    {'id': 5, 'name': 'robot_left_connector'}
+    {'id': 5, 'name': 'arm_connector'}
 ]
 
 

diff --git a/mep3_driver/resource/config_small.urdf b/mep3_driver/resource/config_small.urdf
@@ -3,13 +3,13 @@
     <ros2_control name="HardwareControl" type="system">
         <hardware>
             <plugin>mep3_driver::RobotHardwareInterface</plugin>
-            <param name="kp_linear">150.0</param>
-            <param name="ki_linear">7.0</param>
-            <param name="kd_linear">2.0</param>
+            <param name="kp_linear">170.0</param>
+            <param name="ki_linear">2.0</param>
+            <param name="kd_linear">80.0</param>
 
-            <param name="kp_angular">40.0</param>
-            <param name="ki_angular">1.1</param>
-            <param name="kd_angular">20.0</param>
+            <param name="kp_angular">60.0</param>
+            <param name="ki_angular">1.0</param>
+            <param name="kd_angular">30.0</param>
 
             <param name="update_rate">100.0</param>
         </hardware>

diff --git a/mep3_driver/resource/dynamixel_config_small.yaml b/mep3_driver/resource/dynamixel_config_small.yaml
@@ -18,12 +18,9 @@ small:
       # flipper.id: 69
       # gripper.id: 41
       #
-      joint_names: ["fork_right", "fork_left", "rail", "base", "mid", "gripper","flipper_right","flipper_left"]
+      joint_names: ["fork_right", "fork_left", "rail", "base", "mid", "gripper"]
       fork_right.id: 16
       fork_left.id: 15
-      flipper_right: 55
-      flipper_left: 66
-
       rail.id: 14
       base.id: 12
       mid.id: 13

diff --git a/mep3_navigation/params/config_regulator_small.yaml b/mep3_navigation/params/config_regulator_small.yaml
@@ -1,17 +1,17 @@
 small:
   distance_angle_regulator:
     ros__parameters:
-      kp_distance: 5.0
+      kp_distance: 11.0
       ki_distance: 0.0
-      kd_distance: 2.0
+      kd_distance: 3.0
       d_term_filter_distance: 0.1
 
-      kp_angle: 6.0
+      kp_angle: 9.0
       ki_angle: 0.0
-      kd_angle: 4.0
+      kd_angle: 2.0
       d_term_filter_angle: 0.1
 
       distance_goal_tolerance: 0.002
       angle_goal_tolerance: 0.018
 
-      control_frequency: 100.0
+      control_frequency: 100.0
diff --git a/mep3_navigation/params/nav2_params_big.yaml b/mep3_navigation/params/nav2_params_big.yaml
@@ -69,23 +69,23 @@ controller_server:
     # Goal checker parameters
     goal_checker:
       plugin: "nav2_controller::SimpleGoalChecker"
-      xy_goal_tolerance: 0.1
-      yaw_goal_tolerance: 0.15
+      xy_goal_tolerance: 0.08
+      yaw_goal_tolerance: 0.05
       stateful: true
     # Controller parameters
     # RPP Custom controller for Memristor
     FollowPath:
       plugin: "mep3_navigation::RegulatedPurePursuitController"
-      desired_linear_vel: 0.5
+      desired_linear_vel: 0.8
       max_linear_accel: 0.8
       max_linear_decel: 0.8
       max_linear_jerk: 99999.0
       max_angular_jerk: 99999.0
       lookahead_dist: 0.2
       min_lookahead_dist: 0.15
       max_lookahead_dist: 0.5
-      lookahead_time: 1.0
-      rotate_to_heading_angular_vel: 14.0
+      lookahead_time: 0.625
+      rotate_to_heading_angular_vel: 15.0
       transform_tolerance: 0.1
       use_velocity_scaled_lookahead_dist: true
       min_approach_linear_velocity: 0.05

diff --git a/mep3_navigation/params/nav2_params_small.yaml b/mep3_navigation/params/nav2_params_small.yaml
@@ -69,8 +69,8 @@ controller_server:
     # Goal checker parameters
     goal_checker:
       plugin: "nav2_controller::SimpleGoalChecker"
-      xy_goal_tolerance: 0.04
-      yaw_goal_tolerance: 0.045
+      xy_goal_tolerance: 0.08
+      yaw_goal_tolerance: 0.05
       stateful: true
     # Controller parameters
     # RPP Custom controller for Memristor
@@ -84,8 +84,8 @@ controller_server:
       lookahead_dist: 0.2
       min_lookahead_dist: 0.15
       max_lookahead_dist: 0.6
-      lookahead_time: 1.5
-      rotate_to_heading_angular_vel: 5.0
+      lookahead_time: 0.75
+      rotate_to_heading_angular_vel: 15.0
       transform_tolerance: 0.1
       use_velocity_scaled_lookahead_dist: true
       min_approach_linear_velocity: 0.05
@@ -99,7 +99,7 @@ controller_server:
       use_rotate_to_heading: false
       use_rotate_to_goal: true
       rotate_to_heading_min_angle: 0.785
-      max_angular_accel: 6.0
+      max_angular_accel: 15.0
       cost_scaling_dist: 0.7
       cost_scaling_gain: 1.0
       inflation_cost_scaling_factor: 5.0

diff --git a/mep3_navigation/src/distance_angle/distance_angle_regulator.cpp b/mep3_navigation/src/distance_angle/distance_angle_regulator.cpp
@@ -60,8 +60,8 @@ DistanceAngleRegulator::DistanceAngleRegulator(const rclcpp::NodeOptions & optio
   this->get_parameter("kp_distance", regulator_distance_.kp);
   this->get_parameter("ki_distance", regulator_distance_.ki);
   this->get_parameter("kd_distance", regulator_distance_.kd);
-  regulator_distance_.clamp_min = -1.4;
-  regulator_distance_.clamp_max = 1.4;
+  regulator_distance_.clamp_min = -2.5;
+  regulator_distance_.clamp_max = 2.5;
   regulator_distance_.integrator_min = -0.08;
   regulator_distance_.integrator_max = 0.08;
   this->get_parameter("d_term_filter_distance", regulator_distance_.d_term_filter_coefficient);
@@ -74,8 +74,8 @@ DistanceAngleRegulator::DistanceAngleRegulator(const rclcpp::NodeOptions & optio
   this->get_parameter("kp_angle", regulator_angle_.kp);
   this->get_parameter("ki_angle", regulator_angle_.ki);
   this->get_parameter("kd_angle", regulator_angle_.kd);
-  regulator_angle_.clamp_min = -10.0;
-  regulator_angle_.clamp_max = 10.0;
+  regulator_angle_.clamp_min = -20.0;
+  regulator_angle_.clamp_max = 20.0;
   regulator_angle_.integrator_min = -1.0;
   regulator_angle_.integrator_max = 1.0;
   this->get_parameter("d_term_filter_angle", regulator_angle_.d_term_filter_coefficient);
@@ -393,10 +393,29 @@ void DistanceAngleRegulator::control_loop()
       if (project_command.linear.x < 0.05) {
         project_command.linear.x = 0.2;
       }
-      geometry_msgs::msg::Pose2D projected_pose =
-        projectPose(current_pose_2d, project_command, 0.6);
 
-      bool is_collision_ahead = !collision_checker_->isCollisionFree(projected_pose);
+      bool is_collision_ahead = false;
+
+      const double projection_time = 0.02 / project_command.linear.x;
+
+      int project_cnt = 1;
+      while (true) {
+        if (project_cnt * projection_time >= 0.5) {
+          break;
+        }
+
+        project_cnt++;
+
+        geometry_msgs::msg::Pose2D projected_pose =
+          projectPose(current_pose_2d, project_command, project_cnt * projection_time);
+
+        is_collision_ahead = !collision_checker_->isCollisionFree(projected_pose);
+
+        if (is_collision_ahead) {
+          break;
+        }
+
+      }
 
       if (is_collision_ahead && check_collision_) {
         RCLCPP_INFO(this->get_logger(), "COLLISION AHEAD!");

diff --git a/mep3_navigation/src/laser_inflator/laser_inflator.cpp b/mep3_navigation/src/laser_inflator/laser_inflator.cpp
@@ -141,7 +141,7 @@ class LaserInflator : public rclcpp::Node
       const double y = point_range * sinf(point_angle) + y_offset;
 
       // Remove total stop button that out lidar can see
-      if (point_range < 0.18) {
+      if (point_range < 0.12) {
         *it = std::numeric_limits<float>::infinity();
         continue;
       }
@@ -169,7 +169,9 @@ class LaserInflator : public rclcpp::Node
     sensor_msgs::msg::LaserScan scan = *msg;
     if (constrain_scan(scan)) {
       inflate_scan(scan);
-      publisher_->publish(scan);
+      if (constrain_scan(scan)) {
+        publisher_->publish(scan);
+      }
     }
   }
 };