• ROS1云课→31欢乐卷假期

    ROS1云课→30导航仿真演示

     

    新增加一个主题地图：

    sudo thunar

    使用超级权限打开：

    复制节日专属地图：

     

    然后修改使用如下配置：

    1. export TURTLEBOT_STDR_MAP_FILE=/opt/ros/kinetic/share/turtlebot_stdr/maps/wish.yaml
    2.  
    3. roslaunch turtlebot_stdr turtlebot_in_stdr.launch

     

    启动后，效果如下： 

    如何在其中引入导航和区域覆盖？

    区域覆盖

    多点巡逻如何实现？

    如何使用新功能如smach

    何时使用SMACH？
    当希望机器人执行一些复杂的计划时，SMACH非常有用，其中可以明确描述所有可能的状态和状态转换。这基本上消除了将不同模块集成在一起的黑客行为，使移动机器人操作器等系统做一些有趣的事情。
    快速原型：基于Python的简单SMACH语法使快速原型化状态机和开始运行状态机变得容易。
    复杂状态机：SMACH允许设计、维护和调试大型、复杂的分层状态机。可以在这里找到一个复杂的分层状态机示例。
    内省：SMACH在状态机、状态转换、数据流等方面为您提供了全面的内省。有关更多详细信息，请参阅SMACH_viewer。


    何时不应使用SMACH？
    非结构化任务：SMACH将无法满足任务日程安排的要求。
    低级系统：SMACH不是用来作为需要高效率的低级系统的状态机，SMACH是一种任务级架构。
    粉碎：当想粉碎某物时，不要使用SMACH，因为那样会用到粉碎smash。

    SMACH只是一个有限状态机库吗？
    可以使用SMACH构建有限状态机，但SMACH可以做得更多。SMACH是一个用于任务级执行和协调的库，并提供了几种类型的“状态容器”。一个这样的容器是有限状态机，但这个容器也可以是另一个容器中的状态。有关SMACH中内置的容器和状态的列表，请参阅教程页面。 

    如果通过一个cpp案例实现？参考如下：

    1. #include 
    2.  
    3. #include 
    4. #include 
    5. #include 
    6. #include 
    7. #include 
    8. #include 
    9. #include 
    10. #include 
    11. #include 
    12. #include 
    13.  
    14. struct Pose
    15. {
    16.   double x, y, theta;
    17.   std::string frame;
    18.   
    19. };
    20.  
    21. class Multigoal
    22. {
    23. private:
    24.   void callActionServer(move_base_msgs::MoveBaseGoal goal);
    25.   void getGoals();
    26.   void setGoals(Pose final_pose,double goal_num);
    27.   void run(int status);
    28.   
    29.   int goal_count;
    30.   bool goal_reached ,goal_sended, operation_started;
    31.  
    32.   
    33.   double real_start_time, real_end_time;
    34.   double new_pose_x , new_pose_y , old_pose_x , old_pose_y, dist;
    35.  
    36.   int i , j;
    37.  
    38.   ros::NodeHandle nh_;
    39.   ros::Subscriber sub;
    40.   ros::Subscriber sub2;
    41.   ros::Subscriber odom_sub;
    42.   move_base_msgs::MoveBaseGoal goal;
    43.   move_base_msgs::MoveBaseGoal goal2;
    44.   
    45.   int check_status(int status);
    46.   double get_start_time(double start_time);
    47.   double get_end_time(double end_time);
    48.   double calculate_distance(double curr_pos, double last_pos);
    49.   int goal_status;
    50.   // std::map goal_map_;
    51.   
    52.  
    53. public:
    54.   void resultCallback(const actionlib_msgs::GoalStatusArray::ConstPtr &msg);
    55.   void odomCallback(const nav_msgs::Odometry::Ptr &msg);
    56.   Multigoal(ros::NodeHandle nh);
    57.   ~Multigoal();
    58. };
    59.  
    60. Multigoal::Multigoal(ros::NodeHandle nh)
    61. {
    62.   i = 0;
    63.   j = 0;
    64.   getGoals();
    65.   odom_sub = nh.subscribe("/husky_velocity_controller/odom",1,&Multigoal::odomCallback,this);
    66.   sub = nh.subscribe("/move_base/status",1,&Multigoal::resultCallback,this);
    67.   goal_count = 0;
    68.   goal_status = 0;
    69.   operation_started = false;
    70.   old_pose_x = 0; old_pose_y = 0;
    71.   dist = 0;
    72. }
    73.  
    74. Multigoal::~Multigoal()
    75. {
    76. }
    77.  
    78. void Multigoal::run(int status)
    79. {
    80.   
    81.   if (status == 3 && goal_count == 0) { // this is the first stage 
    82.     
    83.     // still in the first stage but already in some position from previous action
    84.     goal_reached = false;
    85.     callActionServer(goal);
    86.     goal_count = goal_count + 1;
    87.     
    88.   }
    89.   if(goal_count == 1 && status == 1)
    90.   {
    91.     // already succesfully sending the goal.
    92.     goal_count = goal_count + 1;
    93.     
    94.   }
    95.   if (goal_count == 2 && status == 3) {
    96.     // already send the goal and the first goal is reached
    97.     callActionServer(goal2);
    98.     goal_count = goal_count + 1;
    99.     
    100.   }
    101.   if (goal_count == 3 && status == 1) {
    102.     // already successfully sending the second goal.
    103.     goal_count = goal_count + 1;
    104.     
    105.   }
    106.   if (goal_count == 4 && status == 3) {
    107.     ROS_INFO("all goal has reaced succesfully!");
    108.     goal_count = goal_count + 1;
    109.     operation_started = false;
    110.     ROS_INFO("total distance traveled : %f",dist);
    111.   }
    112.   else
    113.   {
    114.     //do nothing
    115.   }
    116.   
    117.   
    118. }
    119.  
    120. int Multigoal::check_status(int status)
    121. {
    122.   goal_status = status;
    123.   // ROS_INFO("goal_status %i",goal_status);
    124.   if (goal_status == 1) {
    125.     // ROS_INFO("goal_sended ");
    126.     goal_sended = true;
    127.     goal_reached = false;
    128.   }
    129.   if (goal_status == 3)
    130.   {
    131.     // ROS_INFO("goal reached");
    132.     goal_reached = true;
    133.     goal_sended = false;
    134.   }
    135.   run(goal_status);
    136.   return goal_status;
    137.   // check the current status
    138.  
    139. }
    140. double Multigoal::get_start_time(double start_time)
    141. {
    142.   
    143.   if (start_time > 0 && i < 1 ) {
    144.     real_start_time = start_time;
    145.     operation_started = true;
    146.     std::cout << "real start time is :" << real_start_time << std::endl;
    147.     return real_start_time;
    148.     
    149.   }
    150.   
    151. }
    152. double Multigoal::get_end_time(double end_time){
    153.  
    154.   if (end_time > 0 && j < 1 ) {
    155.     real_end_time = end_time;
    156.     std::cout << "real end time is :" << real_end_time << std::endl;
    157.     return real_end_time;
    158.     
    159.   }
    160.  
    161. }
    162.  
    163. void Multigoal::odomCallback(const nav_msgs::Odometry::Ptr & msg)
    164. {
    165.  
    166.   if (operation_started) {
    167.   new_pose_x = msg->pose.pose.position.x;
    168.   new_pose_y = msg->pose.pose.position.y;
    169.   double diff_x , diff_y;
    170.  
    171.   diff_x = new_pose_x - old_pose_x;
    172.   diff_y = new_pose_y - old_pose_y;
    173.   dist = calculate_distance(diff_x,diff_y) + dist;
    174.   old_pose_x = new_pose_x; old_pose_y = new_pose_y;
    175.   
    176.   }
    177.   else
    178.   {
    179.     //
    180.   }
    181.   
    182. }
    183.  
    184. double Multigoal::calculate_distance(double diff_x, double diff_y)
    185. {
    186.   // calculate the distance 
    187.   double result = hypot (diff_x, diff_y);
    188.   return result;
    189. }
    190.  
    191. void Multigoal::resultCallback(const actionlib_msgs::GoalStatusArray::ConstPtr &msg){
    192. // check if goal is reached 
    193.  
    194. int goal_stat;
    195. if (msg->status_list.empty()) {
    196.   goal_stat = 3;
    197. }
    198. else
    199. {
    200.   goal_stat = msg->status_list[0].status;
    201. }
    202.  
    203. check_status(goal_stat);
    204. double start_time , finish_time;
    205.  
    206. if (goal_count == 1)
    207. 		{
    208. 			// status is not clear, no goal is sended yet!
    209. 			start_time = msg->header.stamp.toSec(); // get the time from message
    210.       get_start_time(start_time);
    211.       i = i + 1;
    212.       
    213.       // getGoals();
    214.      
    215. 		}
    216. if ( goal_count == 5)
    217.     {
    218.       finish_time = msg->header.stamp.toSec(); // get the time from message
    219.       get_end_time(finish_time);
    220.       j = j + 1;
    221.       double total_time = abs(real_start_time - real_end_time);
    222.       ROS_INFO("total time = %f",total_time);
    223.       goal_count = goal_count + 1;
    224.     }
    225.     
    226. }
    227.  
    228.  
    229. void Multigoal::getGoals()
    230. {
    231.  
    232. std::string param_name;
    233.   
    234.   if (nh_.searchParam("/multi_goal/multi_goal_driver/goals", param_name))
    235.   {
    236.    XmlRpc::XmlRpcValue goals;
    237.   if (!nh_.hasParam("/multi_goal/multi_goal_driver/goals"))
    238.   {
    239.     ROS_ERROR("No stations on parameterserver");
    240.   }
    241.   
    242.   nh_.getParam("/multi_goal/multi_goal_driver/goals", goals);
    243.   for (size_t i = 0 ; i < goals.size(); i++)
    244.   {
    245.     XmlRpc::XmlRpcValue goal = goals[i];
    246.     Pose final_pose;
    247.     XmlRpc::XmlRpcValue poses = goal["poses"];
    248.     std::string frame = goal["frame_id"];
    249.     XmlRpc::XmlRpcValue pose_back = poses[poses.size()-1];
    250.     final_pose.x = pose_back[0];
    251.     final_pose.y = pose_back[1];
    252.     final_pose.theta = pose_back[2];
    253.     final_pose.frame = frame;
    254.     setGoals(final_pose,i);
    255.   }
    256.   // after the goal has been obtained, now run the code to go to the destination
    257.   
    258.   }
    259.   else
    260.   {
    261.     ROS_INFO("No param 'goals' found in an upward search");
    262.   }
    263.  
    264. }
    265.  
    266. void Multigoal::setGoals(Pose final_pose,double goal_num)
    267. {
    268.   
    269.   if (goal_num == 0) {
    270.   geometry_msgs::PoseStamped target_pose;
    271.  
    272.   //we'll send a goal to the robot to move 1 meter forward
    273.   goal.target_pose.header.frame_id = final_pose.frame;
    274.   goal.target_pose.header.stamp = ros::Time::now();
    275.  
    276.   goal.target_pose.pose.position.x = final_pose.x;
    277.   goal.target_pose.pose.orientation.w = final_pose.theta;
    278.   std::cout << "first goal is :  " << final_pose.x << ", " << final_pose.y << ", " << final_pose.theta << ", " << final_pose.frame << std::endl;
    279.   
    280.   
    281.   }
    282.   if (goal_num == 1)
    283.   {
    284.   geometry_msgs::PoseStamped target_pose;
    285.  
    286.   //we'll send a goal to the robot to move 1 meter forward
    287.   goal2.target_pose.header.frame_id = final_pose.frame;
    288.   goal2.target_pose.header.stamp = ros::Time::now();
    289.   
    290.   goal2.target_pose.pose.position.x = final_pose.x;
    291.   goal2.target_pose.pose.orientation.w = final_pose.theta;
    292.   
    293.   std::cout << " second goal is: " << final_pose.x << ", " << final_pose.y << ", " << final_pose.theta << ", " << final_pose.frame << std::endl;
    294.     
    295.     
    296.   }
    297.   
    298. }
    299.  
    300. void Multigoal::callActionServer(move_base_msgs::MoveBaseGoal goal)
    301. {
    302.   typedef actionlib::SimpleActionClient MoveBaseClient;
    303.    MoveBaseClient ac("/move_base", true);
    304.  
    305.   while(!ac.waitForServer(ros::Duration(5.0))){
    306.   ROS_INFO("Waiting for the move_base action server to come up");
    307.     
    308.   }
    309.   //we'll send a goal to the robot the goal we get from previous function
    310.   ac.sendGoal(goal);
    311.   
    312.   ROS_INFO("Sending goal");
    313.   
    314.   
    315. }
    316.  
    317. int main(int argc, char** argv){
    318.     ros::init(argc,argv,"multi_goal_driver");
    319.     ros::NodeHandle nh;
    320.     Multigoal Multigoal(nh);  
    321.     ros::Rate rate(5);
    322.     ros::spin();
    323.  
    324.     return 0;
    325.  
    326.   //wait for the action server to come up
    327.  
    328. }
    329.  
    330.

    python案例参考：

    1. #!/usr/bin/env python
    2.  
    3. from random import sample
    4. from math import pow, sqrt
    5. from actionlib_msgs.msg import *
    6. from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist
    7. from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
    8. import rospy
    9. import actionlib
    10.  
    11.  
    12.  
    13. class MultiPointNav():
    14.     def __init__(self):
    15.         rospy.init_node('multi_point_nav', anonymous=True)
    16.                 
    17.         rospy.on_shutdown(self.shutdown)
    18.         
    19.         # How long in seconds should the robot pause at each location?
    20.         self.rest_time = rospy.get_param("~rest_time", 10)
    21.         
    22.         # Are we running in the fake simulator?
    23.         self.fake_test = rospy.get_param("~fake_test", False)
    24.         
    25.         # Goal state return values
    26.         goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED', 
    27.                        'SUCCEEDED', 'ABORTED', 'REJECTED',
    28.                        'PREEMPTING', 'RECALLING', 'RECALLED',
    29.                        'LOST']
    30.         
    31.   
    32.         locations = dict()
    33.         
    34.         locations['Point1'] = Pose(Point(-15.2, -21.2, 0.000), Quaternion(0.000, 0.000, -0.60710, 0.70710))
    35.         locations['Point2'] = Pose(Point(-14.1, -17.1, 0.000), Quaternion(0.000, 0.000, -0.670, 0.743))
    36.         locations['Point3'] = Pose(Point(-3.719, 4.401, 0.000), Quaternion(0.000, 0.000, 0.733, 0.680))
    37.         locations['Point4'] = Pose(Point(0.720, 2.229, 0.000), Quaternion(0.000, 0.000, 0.786, 0.618))
    38.         #locations['Point5'] = Pose(Point(1.471, 1.007, 0.000), Quaternion(0.000, 0.000, 0.480, 0.877))
    39.         #locations['Point6'] = Pose(Point(-0.861, -0.019, 0.000), Quaternion(0.000, 0.000, 0.892, -0.451))
    40.         
    41.         # Publisher to manually control the robot (e.g. to stop it, queue_size=5)
    42.         self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
    43.         
    44.         # Subscribe to the move_base action server
    45.         self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
    46.         
    47.         rospy.loginfo("Waiting for move_base action server...")
    48.         
    49.         # Wait 60 seconds for the action server to become available
    50.         self.move_base.wait_for_server(rospy.Duration(60))
    51.         
    52.         rospy.loginfo("Connected to move base server")
    53.         
    54.         # A variable to hold the initial pose of the robot to be set by 
    55.         # the user in RViz
    56.         initial_pose = PoseWithCovarianceStamped()
    57.         
    58.         # Variables to keep track of success rate, running time,
    59.         # and distance traveled
    60.         n_locations = len(locations)
    61.         n_goals = 0
    62.         n_successes = 0
    63.         i = n_locations
    64.         distance_traveled = 0
    65.         start_time = rospy.Time.now()
    66.         running_time = 0
    67.         location = ""
    68.         last_location = ""
    69.         
    70.         # Get the initial pose from the user
    71.         rospy.loginfo("*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose...")
    72.         rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)
    73.         self.last_location = Pose()
    74.         rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose)
    75.         
    76.         # Make sure we have the initial pose
    77.         while initial_pose.header.stamp == "":
    78.             rospy.sleep(1)
    79.             
    80.         rospy.loginfo("Starting navigation test")
    81.         
    82.         # Begin the main loop and run through a sequence of locations
    83.         while not rospy.is_shutdown():
    84.             # If we've gone through the current sequence,
    85.             # start with a new random sequence
    86.             if i == n_locations:
    87.                 i = 0
    88.                 sequence = ['Point1','Point2','Point3','Point4']
    89.                 # Skip over first location if it is the same as
    90.                 # the last location
    91.                 if sequence[0] == last_location:
    92.                     i = 1
    93.             
    94.             # Get the next location in the current sequence
    95.             location = sequence[i]
    96.                         
    97.             # Keep track of the distance traveled.
    98.             # Use updated initial pose if available.
    99.             if initial_pose.header.stamp == "":
    100.                 distance = sqrt(pow(locations[location].position.x - 
    101.                                     locations[last_location].position.x, 2) +
    102.                                 pow(locations[location].position.y - 
    103.                                     locations[last_location].position.y, 2))
    104.             else:
    105.                 rospy.loginfo("Updating current pose.")
    106.                 distance = sqrt(pow(locations[location].position.x - 
    107.                                     initial_pose.pose.pose.position.x, 2) +
    108.                                 pow(locations[location].position.y - 
    109.                                     initial_pose.pose.pose.position.y, 2))
    110.                 initial_pose.header.stamp = ""
    111.             
    112.             # Store the last location for distance calculations
    113.             last_location = location
    114.             
    115.             # Increment the counters
    116.             i += 1
    117.             n_goals += 1
    118.         
    119.             # Set up the next goal location
    120.             self.goal = MoveBaseGoal()
    121.             self.goal.target_pose.pose = locations[location]
    122.             self.goal.target_pose.header.frame_id = 'map'
    123.             self.goal.target_pose.header.stamp = rospy.Time.now()
    124.             
    125.             # Let the user know where the robot is going next
    126.             rospy.loginfo("Going to: " + str(location))
    127.             
    128.             # Start the robot toward the next location
    129.             self.move_base.send_goal(self.goal)
    130.             
    131.             # Allow 5 minutes to get there
    132.             finished_within_time = self.move_base.wait_for_result(rospy.Duration(300)) 
    133.             
    134.             # Check for success or failure
    135.             if not finished_within_time:
    136.                 self.move_base.cancel_goal()
    137.                 rospy.loginfo("Timed out achieving goal")
    138.             else:
    139.                 state = self.move_base.get_state()
    140.                 if state == GoalStatus.SUCCEEDED:
    141.                     rospy.loginfo("Goal succeeded!")
    142.                     n_successes += 1
    143.                     distance_traveled += distance
    144.                     rospy.loginfo("State:" + str(state))
    145.                 else:
    146.                   rospy.loginfo("Goal failed with error code: " + str(goal_states[state]))
    147.             
    148.             # How long have we been running?
    149.             running_time = rospy.Time.now() - start_time
    150.             running_time = running_time.secs / 60.0
    151.             
    152.             # Print a summary success/failure, distance traveled and time elapsed
    153.             rospy.loginfo("Success so far: " + str(n_successes) + "/" + 
    154.                           str(n_goals) + " = " + 
    155.                           str(100 * n_successes/n_goals) + "%")
    156.             rospy.loginfo("Running time: " + str(trunc(running_time, 1)) + 
    157.                           " min Distance: " + str(trunc(distance_traveled, 1)) + " m")
    158.             rospy.sleep(self.rest_time)
    159.             
    160.     def update_initial_pose(self, initial_pose):
    161.         self.initial_pose = initial_pose
    162.  
    163.     def shutdown(self):
    164.         rospy.loginfo("Stopping the robot...")
    165.         self.move_base.cancel_goal()
    166.         rospy.sleep(2)
    167.         self.cmd_vel_pub.publish(Twist())
    168.         rospy.sleep(1)
    169.       
    170. def trunc(f, n):
    171.     # Truncates/pads a float f to n decimal places without rounding
    172.     slen = len('%.*f' % (n, f))
    173.     return float(str(f)[:slen])
    174.  
    175. if __name__ == '__main__':
    176.     try:
    177.         MultiPointNav()
    178.         rospy.spin()
    179.     except rospy.ROSInterruptException:
    180.         rospy.loginfo("AMCL navigation test finished.")

     

     

    [stdr_gui_node_63383890b90245a083c4839a_6417_7727749313635804950-5] process has died [pid 6463, exit code -11, cmd /opt/ros/kinetic/lib/stdr_gui/stdr_gui_node __name:=stdr_gui_node_63383890b90245a083c4839a_6417_7727749313635804950 __log:=/home/shiyanlou/.ros/log/f43b8780-4188-11ed-9830-0242c0a82a04/stdr_gui_node_63383890b90245a083c4839a_6417_7727749313635804950-5.log].
    log file: /home/shiyanlou/.ros/log/f43b8780-4188-11ed-9830-0242c0a82a04/stdr_gui_node_63383890b90245a083c4839a_6417_7727749313635804950-5*.log
    需要更新对应软件包，稳定性会好一些，仅此而已。 

     

  • 相关阅读:
    C++ vector模拟实现
    摘要与关键词 写作
    一文详解手眼标定公式推导
    【JVM技术专题】 深入学习JIT编译器实现机制「 原理篇」
    Vue 3 中的 setup 函数是如何工作的？
    安卓常见设计模式3.2------工厂模式，工厂方法模式，抽象工厂模式对比（Kotlin版）
    springBoot集成swagger3.0.0
    dotNet8 全局异常处理
    chrome插件开发入门-保姆级攻略
    Java计算机毕业设计德云社剧场网上售票系统源码+系统+数据库+lw文档
  • 原文地址：https://blog.csdn.net/ZhangRelay/article/details/127137394