Feat/add clear costmap (#3)

* comment out ROS_INFO * Fix spell miss * Add service of clear costmaps * Add param of clear costmap
masakifujiwara1 · Aug 7, 2024 · e9bef80 · e9bef80
1 parent 743fe9a
commit e9bef80
Show file tree

Hide file tree

Showing 4 changed files with 23 additions and 3 deletions.
diff --git a/check_robot_moving/src/check_robot_moving_node.cpp b/check_robot_moving/src/check_robot_moving_node.cpp
@@ -181,7 +181,7 @@ auto main(int argc, char **argv) -> int {
     private_nh.param(
         "clear_costmap_srv",
         clear_costmap_srv,
-        std::string("move_base/clear_costmap")
+        std::string("move_base/clear_costmaps")
     );
 
     auto loop_rate = ros::Rate(5);
@@ -242,7 +242,7 @@ auto main(int argc, char **argv) -> int {
             last_moving_time = time(NULL);
         }
 
-        ROS_INFO("delta_pose_dist:%lf\n", delta_pose_dist);
+        // ROS_INFO("delta_pose_dist:%lf\n", delta_pose_dist);
         // ROS_INFO("cmd_vel:%lf\n", vel_x);
         // ROS_INFO("is_reached_goal:%d\n", is_reached_goal.load());
         ROS_INFO("time:%ld, stopped time:%ld\n", time(NULL) - start_time, time(NULL) - last_moving_time);

diff --git a/waypoint_server/launch/waypoint_server.launch b/waypoint_server/launch/waypoint_server.launch
@@ -8,6 +8,7 @@
     <node pkg="waypoint_server" type="waypoint_server_node" name="waypoint_server" output="$(arg output)">
       <rosparam command="load" file="$(arg config_file)" subst_value="true"/>
       <remap from="waypoint/regist_point" to="$(arg regist_goal_topic)"/>
+      <param name="clear_costmap_srv" value="/move_base/clear_costmaps" />
     </node>
     <node pkg="waypoint_server" type="waypoint_to_posestamped_node" name="waypoint_to_posestamped" output="$(arg output)">
       <rosparam command="load" file="$(arg config_file)" subst_value="true"/>

diff --git a/waypoint_server/launch/waypoint_server_with_check.launch b/waypoint_server/launch/waypoint_server_with_check.launch
@@ -8,6 +8,7 @@
     <node pkg="waypoint_server" type="waypoint_server_node" name="waypoint_server" output="$(arg output)">
       <rosparam command="load" file="$(arg config_file)" subst_value="true"/>
       <remap from="waypoint/regist_point" to="$(arg regist_goal_topic)"/>
+      <param name="clear_costmap_srv" value="/move_base/clear_costmaps" />
     </node>
     <node pkg="waypoint_server" type="waypoint_to_posestamped_node" name="waypoint_to_posestamped" output="$(arg output)">
       <rosparam command="load" file="$(arg config_file)" subst_value="true"/>

diff --git a/waypoint_server/src/waypoint_server_node.cpp b/waypoint_server/src/waypoint_server_node.cpp
@@ -15,6 +15,7 @@
 #include <geometry_msgs/PointStamped.h>
 
 #include <std_srvs/Trigger.h>
+#include <std_srvs/Empty.h>
 
 #include <waypoint_manager_msgs/Waypoint.h>
 #include <waypoint_manager_msgs/WaypointStamped.h>
@@ -36,7 +37,8 @@ namespace waypoint_server {
                     append_route_topic,
                     erase_route_topic,
                     insert_route_topic,
-                    waypoints_topic;
+                    waypoints_topic,
+                    clear_costmap_srv;
 
         std::string regist_waypoint_prefix;
 
@@ -91,6 +93,8 @@ namespace waypoint_server {
                                next_waypoint_service,
                                prev_waypoint_service;
 
+            ros::ServiceClient clear_costmap_service;
+
             NodeParameters param;
 
             Map waypoint_map;
@@ -271,6 +275,12 @@ namespace waypoint_server {
             param.goal_publish_frequency,
             0.5
         );
+        private_nh.param(
+            "clear_costmap_srv",
+            param.clear_costmap_srv,
+            std::string("move_base/clear_costmaps")
+        );  
+
 
         waypoint_publisher
             = nh.advertise<waypoint_manager_msgs::Waypoint>(
@@ -391,6 +401,8 @@ namespace waypoint_server {
                 this
             );
 
+        clear_costmap_service = private_nh.serviceClient<std_srvs::Empty>(param.clear_costmap_srv);
+
         is_cancel.store(true);
         regist_goal_id.store(0);
 
@@ -631,6 +643,12 @@ namespace waypoint_server {
         }
         publishGoal();
 
+        ROS_WARN("Call ClearCostmaps");
+        ROS_WARN("%s", param.clear_costmap_srv.c_str());
+
+        std_srvs::Empty data;
+        clear_costmap_service.call(data);
+
         return true;
     }