From 5e526e5ec31a46853889c6acaf3f96d4582999b3 Mon Sep 17 00:00:00 2001 From: jieshoudaxue <1641395022@qq.com> Date: Thu, 16 Nov 2023 10:07:56 +0800 Subject: [PATCH] [fix] up navigation --- mbot_navigation/CMakeLists.txt | 6 +-- .../move_base/dwa_local_planner_params.yaml | 4 +- mbot_navigation/scripts/exploring_house.py | 39 ++++++++++++++ mbot_navigation/scripts/move_test.py | 42 --------------- mbot_navigation/src/exploring_house.cpp | 52 +++++++++++++++++++ mbot_navigation/src/move_test.cpp | 36 ------------- 6 files changed, 96 insertions(+), 83 deletions(-) delete mode 100755 mbot_navigation/scripts/move_test.py delete mode 100644 mbot_navigation/src/move_test.cpp diff --git a/mbot_navigation/CMakeLists.txt b/mbot_navigation/CMakeLists.txt index 88c1d3b..161e50b 100644 --- a/mbot_navigation/CMakeLists.txt +++ b/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} ) diff --git a/mbot_navigation/config/move_base/dwa_local_planner_params.yaml b/mbot_navigation/config/move_base/dwa_local_planner_params.yaml index fb143c8..8855ba5 100644 --- a/mbot_navigation/config/move_base/dwa_local_planner_params.yaml +++ b/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 diff --git a/mbot_navigation/scripts/exploring_house.py b/mbot_navigation/scripts/exploring_house.py index c542202..ac59507 100755 --- a/mbot_navigation/scripts/exploring_house.py +++ b/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() \ No newline at end of file diff --git a/mbot_navigation/scripts/move_test.py b/mbot_navigation/scripts/move_test.py deleted file mode 100755 index 9a4561b..0000000 --- a/mbot_navigation/scripts/move_test.py +++ /dev/null @@ -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() \ No newline at end of file diff --git a/mbot_navigation/src/exploring_house.cpp b/mbot_navigation/src/exploring_house.cpp index 30b75db..b86e2d9 100644 --- a/mbot_navigation/src/exploring_house.cpp +++ b/mbot_navigation/src/exploring_house.cpp @@ -1,8 +1,60 @@ #include +#include +#include #include #include +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_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 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; diff --git a/mbot_navigation/src/move_test.cpp b/mbot_navigation/src/move_test.cpp deleted file mode 100644 index df301bb..0000000 --- a/mbot_navigation/src/move_test.cpp +++ /dev/null @@ -1,36 +0,0 @@ -#include -#include -#include - -int main(int argc, char** argv) { - ros::init(argc, argv, "move_test"); - - actionlib::SimpleActionClient 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; -} \ No newline at end of file