fix and close issue ai-winter#46

phatcvo · Jan 27, 2024 · bd6be12 · bd6be12
1 parent eb641eb
commit bd6be12
Show file tree

Hide file tree

Showing 6 changed files with 9 additions and 7 deletions.
diff --git a/src/core/local_planner/local_planner/src/local_planner.cpp b/src/core/local_planner/local_planner/src/local_planner.cpp
@@ -337,8 +337,6 @@ void LocalPlanner::getLookAheadPoint(double lookahead_dist, geometry_msgs::PoseS
     double py = prev_pose_it->pose.position.y;
     double gx = goal_pose_it->pose.position.x;
     double gy = goal_pose_it->pose.position.y;
-    double ppx = pprev_pose_it->pose.position.x;
-    double ppy = pprev_pose_it->pose.position.y;
 
     // transform to the robot frame so that the circle centers at (0,0)
     std::pair<double, double> prev_p(px - rx, py - ry);

diff --git a/src/core/utils/include/math_helper.h b/src/core/utils/include/math_helper.h
@@ -44,6 +44,7 @@ double dist(const geometry_msgs::PoseStamped& node1, const geometry_msgs::PoseSt
  */
 double angle(const Node& node1, const Node& node2);
 double angle(const std::pair<double, double>& node1, const std::pair<double, double>& node2);
+double angle(const geometry_msgs::PoseStamped& node1, const geometry_msgs::PoseStamped& node2);
 
 /**
  * @brief Perform modulus operation on 2π.

diff --git a/src/core/utils/src/math_helper.cpp b/src/core/utils/src/math_helper.cpp
@@ -7,9 +7,9 @@
  * @date: 2024-01-03
  * @version: 1.2
  *
- * Copyright (c) 2024, Yang Haodong. 
+ * Copyright (c) 2024, Yang Haodong.
  * All rights reserved.
- * 
+ *
  * --------------------------------------------------------
  *
  * ********************************************************
@@ -60,6 +60,10 @@ double angle(const std::pair<double, double>& node1, const std::pair<double, dou
   return atan2(node2.second - node1.second, node2.first - node1.first);
 }
 
+double angle(const geometry_msgs::PoseStamped& node1, const geometry_msgs::PoseStamped& node2)
+{
+  return atan2(node2.pose.position.y - node1.pose.position.y, node2.pose.position.x - node1.pose.position.x);
+}
 /**
  * @brief Perform modulus operation on 2π.
  * @param theta    the angle to modulu

diff --git a/src/sim_env/config/costmap/global_costmap_params.yaml b/src/sim_env/config/costmap/global_costmap_params.yaml
@@ -6,8 +6,6 @@ global_costmap:
   publish_frequency: 1.0
   transform_tolerance: 0.5
 
-  static_map: true
-
   inflation_radius: 1.0
 
   ### Sensor management parameters ###

diff --git a/src/sim_env/launch/include/robots/start_robots.launch.xml b/src/sim_env/launch/include/robots/start_robots.launch.xml
@@ -2,7 +2,7 @@
 <launch>
 	<arg name="robot_number" default="1" />
 	<arg name="robot1_type" value="turtlebot3_waffle" />
-	<arg name="robot1_global_planner" value="a_star" />
+	<arg name="robot1_global_planner" value="lpa_star" />
 	<arg name="robot1_local_planner" value="pid" />
 	<arg name="robot1_x_pos" value="0.0" />
 	<arg name="robot1_y_pos" value="0.0" />

diff --git a/src/sim_env/launch/main.launch b/src/sim_env/launch/main.launch
@@ -1,6 +1,7 @@
 <?xml version='1.0' encoding='utf-8'?>
 <launch>
 	<arg name="world_parameter" value="warehouse" />
+	<node pkg="dynamic_xml_config" type="obstacles_generate_ros.py" name="obstacles_generate" args="user_config.yaml" output="screen" />
 	<include file="$(find sim_env)/launch/config.launch">
 		<arg name="world" value="$(arg world_parameter)" />
 		<arg name="map" value="warehouse" />