[fix] up navigation

mbot_navigation/CMakeLists.txt

@@ -135,7 +135,7 @@ include_directories(
 ## Declare a C++ executable
 ## With catkin_make all packages are built within a single CMake context
 ## The recommended prefix ensures that target names across packages don't collide
-add_executable(move_test src/move_test.cpp)
+add_executable(exploring_house src/exploring_house.cpp)
 
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
@@ -148,7 +148,7 @@ add_executable(move_test src/move_test.cpp)
 # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Specify libraries to link a library or executable target against
-target_link_libraries(move_test
+target_link_libraries(exploring_house
   ${catkin_LIBRARIES}
 )
 
@@ -162,7 +162,7 @@ target_link_libraries(move_test
 ## Mark executable scripts (Python etc.) for installation
 ## in contrast to setup.py, you can choose the destination
 catkin_install_python(PROGRAMS
-  scripts/move_test.py
+  scripts/exploring_house.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 

mbot_navigation/config/move_base/dwa_local_planner_params.yaml

@@ -8,8 +8,8 @@ DWAPlannerROS:
   min_vel_y: 0.0
 
 # The velocity when robot is moving in a straight line
-  max_trans_vel:  0.22
-  min_trans_vel:  0.11
+  max_trans_vel:  0.44
+  min_trans_vel:  0.22
 
   max_rot_vel: 2.75
   min_rot_vel: 1.37

mbot_navigation/scripts/exploring_house.py

@@ -8,7 +8,46 @@ from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
 
 
 def main():
+  rospy.init_node("move_test", anonymous=True)
+  
+  move_base_client = actionlib.SimpleActionClient("move_base", MoveBaseAction)
+  
+  rospy.loginfo("Waiting for move_base action server...")  
+  
+  while move_base_client.wait_for_server(rospy.Duration(5.0)) == 0:
+    rospy.loginfo("connected to move base server")
+  
+  target_list = []
+  target_list.append(Pose(Point(6.543, 4.779, 0.000), Quaternion(0.000, 0.000, 0.645, 0.764)))
+  target_list.append(Pose(Point(5.543, -4.779, 0.000), Quaternion(0.000, 0.000, 0.645, 0.764)))
+  target_list.append(Pose(Point(-5.543, 4.779, 0.000), Quaternion(0.000, 0.000, 0.645, 0.764)))
+  target_list.append(Pose(Point(-5.543, -4.779, 0.000), Quaternion(0.000, 0.000, 0.645, 0.764)))
+  
 
+  for i, target in enumerate(target_list):
+    start_time = rospy.Time.now()  
+    
+    goal = MoveBaseGoal()
+    goal.target_pose.pose = target
+    goal.target_pose.header.frame_id = 'map'
+    goal.target_pose.header.stamp = rospy.Time.now()
+    
+    rospy.loginfo("going to {0} goal, {1}".format(i, str(target)))
+    
+    move_base_client.send_goal(goal)
+    
+    finished_within_time = move_base_client.wait_for_result(rospy.Duration(300))
+    
+    if not finished_within_time:
+      move_base_client.cancel_goal()
+      rospy.loginfo("time out, failed to goal")
+    else:
+      running_time = (rospy.Time.now() - start_time).to_sec()
+      if move_base_client.get_state() == GoalStatus.SUCCEEDED:
+        rospy.loginfo("go to {0} goal succeeded, run time: {1} sec".format(i, running_time))
+      else:
+        rospy.loginfo("goal failed")
+    
 
 if __name__ == "__main__":
   main()

mbot_navigation/scripts/move_test.py

@@ -1,42 +0,0 @@
-#!/usr/bin/env python3
-
-import rospy
-import actionlib
-from actionlib_msgs.msg import *  
-from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist  
-from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal  
-
-
-def main():
-  rospy.init_node("move_test", anonymous=True)
-  
-  move_base_client = actionlib.SimpleActionClient("move_base", MoveBaseAction)
-  
-  rospy.loginfo("Waiting for move_base action server...")  
-  
-  while move_base_client.wait_for_server(rospy.Duration(5.0)) == 0:
-    rospy.loginfo("connected to move base server")
-  
-  target = Pose(Point(-5.543, 4.779, 0.000), Quaternion(0.000, 0.000, 0.645, 0.764))
-  goal = MoveBaseGoal()
-  goal.target_pose.pose = target
-  goal.target_pose.header.frame_id = 'map'
-  goal.target_pose.header.stamp = rospy.Time.now()
-  
-  rospy.loginfo("going to " + str(target))
-  
-  move_base_client.send_goal(goal)
-  
-  finished_within_time = move_base_client.wait_for_result(rospy.Duration(300))
-  
-  if not finished_within_time:
-    move_base_client.cancel_goal()
-    rospy.loginfo("time out, failed to goal")
-  else:
-    if move_base_client.get_state() == GoalStatus.SUCCEEDED:
-      rospy.loginfo("goal succeeded")
-    else:
-      rospy.loginfo("goal failed")
-
-if __name__ == "__main__":
-  main()

mbot_navigation/src/exploring_house.cpp

@@ -1,8 +1,60 @@
 #include <ros/ros.h>
+#include <list>
+#include <geometry_msgs/Pose.h>
 #include <move_base_msgs/MoveBaseAction.h>
 #include <actionlib/client/simple_action_client.h>
 
+geometry_msgs::Pose createPose(double px, double py, double pz, double ox, double oy, double oz, double ow) {
+    geometry_msgs::Pose pose;
+    pose.position.x = px;
+    pose.position.y = py;
+    pose.position.z = pz;
+    pose.orientation.x = ox;
+    pose.orientation.y = oy;
+    pose.orientation.z = oz;
+    pose.orientation.w = ow;
+    return pose;  
+}
+
 int main(int argc, char** argv) {
+  ros::init(argc, argv, "move_test");
+
+  actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> move_base_client("move_base", true);
+
+  ROS_INFO("Waiting for move_base action server...");  
+  move_base_client.waitForServer();
+  ROS_INFO("connected to move base server");
+
+  std::vector<geometry_msgs::Pose> target_list;
+  target_list.push_back(createPose(6.543, 4.779, 0.000, 0.000, 0.000, 0.645, 0.764));
+  target_list.push_back(createPose(5.543, -4.779, 0.000, 0.000, 0.000, 0.645, 0.764));
+  target_list.push_back(createPose(-5.543, 4.779, 0.000, 0.000, 0.000, 0.645, 0.764));
+  target_list.push_back(createPose(-5.543, -4.779, 0.000, 0.000, 0.000, 0.645, 0.764));
+
+  for (uint8_t i = 0; i < target_list.size(); i ++) {
+    ros::Time start_time = ros::Time::now();
+
+    ROS_INFO("going to %u goal, position: (%f, %f)", i, target_list[i].position.x, target_list[i].position.y);
+
+    move_base_msgs::MoveBaseGoal goal;
+    goal.target_pose.header.frame_id = "map";
+    goal.target_pose.header.stamp = ros::Time::now();
+
+    goal.target_pose.pose = target_list[i];
+
+    move_base_client.sendGoal(goal);
+
+    move_base_client.waitForResult();
+
+    if (move_base_client.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) {
+      ros::Duration running_time =  ros::Time::now() - start_time;
+      ROS_INFO("go to %u goal succeeded, running time %f sec", i, running_time.toSec());
+    } else {
+      ROS_INFO("goal failed");
+    }
+
+  }
+
 
 
   return 0;

mbot_navigation/src/move_test.cpp

@@ -1,36 +0,0 @@
-#include <ros/ros.h>
-#include <move_base_msgs/MoveBaseAction.h>
-#include <actionlib/client/simple_action_client.h>
-
-int main(int argc, char** argv) {
-  ros::init(argc, argv, "move_test");
-
-  actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> move_base_client("move_base", true);
-
-  ROS_INFO("Waiting for move_base action server...");  
-  move_base_client.waitForServer();
-  ROS_INFO("connected to move base server");
-
-  move_base_msgs::MoveBaseGoal goal;
-  goal.target_pose.header.frame_id = "map";
-  goal.target_pose.header.stamp = ros::Time::now();
-
-  goal.target_pose.pose.position.x = -10.543;
-  goal.target_pose.pose.position.y = -4.779;
-
-  goal.target_pose.pose.orientation.z = 0.645;
-  goal.target_pose.pose.orientation.w = 0.764;
-
-  move_base_client.sendGoal(goal);
-
-  move_base_client.waitForResult();
-
-  if (move_base_client.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) {
-    ROS_INFO("goal succeeded");
-  } else {
-    ROS_INFO("goal failed");
-  }
-
-
-  return 0;
-}