ROS1云课→31欢乐卷假期

ROS1云课→30导航仿真演示

新增加一个主题地图：

sudo thunar

使用超级权限打开：

复制节日专属地图：

然后修改使用如下配置：

export TURTLEBOT_STDR_MAP_FILE=/opt/ros/kinetic/share/turtlebot_stdr/maps/wish.yamlroslaunch 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案例实现？参考如下：

#include <ros/ros.h>#include <actionlib/client/simple_action_client.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <move_base_msgs/MoveBaseActionFeedback.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseArray.h>
#include <string.h>
#include <map>
#include <nav_msgs/Odometry.h>
#include <math.h>struct Pose
{double x, y, theta;std::string frame;};class Multigoal
{
private:void callActionServer(move_base_msgs::MoveBaseGoal goal);void getGoals();void setGoals(Pose final_pose,double goal_num);void run(int status);int goal_count;bool goal_reached ,goal_sended, operation_started;double real_start_time, real_end_time;double new_pose_x , new_pose_y , old_pose_x , old_pose_y, dist;int i , j;ros::NodeHandle nh_;ros::Subscriber sub;ros::Subscriber sub2;ros::Subscriber odom_sub;move_base_msgs::MoveBaseGoal goal;move_base_msgs::MoveBaseGoal goal2;int check_status(int status);double get_start_time(double start_time);double get_end_time(double end_time);double calculate_distance(double curr_pos, double last_pos);int goal_status;// std::map<std::string, Pose> goal_map_;public:void resultCallback(const actionlib_msgs::GoalStatusArray::ConstPtr &msg);void odomCallback(const nav_msgs::Odometry::Ptr &msg);Multigoal(ros::NodeHandle nh);~Multigoal();
};Multigoal::Multigoal(ros::NodeHandle nh)
{i = 0;j = 0;getGoals();odom_sub = nh.subscribe("/husky_velocity_controller/odom",1,&Multigoal::odomCallback,this);sub = nh.subscribe("/move_base/status",1,&Multigoal::resultCallback,this);goal_count = 0;goal_status = 0;operation_started = false;old_pose_x = 0; old_pose_y = 0;dist = 0;
}Multigoal::~Multigoal()
{
}void Multigoal::run(int status)
{if (status == 3 && goal_count == 0) { // this is the first stage // still in the first stage but already in some position from previous actiongoal_reached = false;callActionServer(goal);goal_count = goal_count + 1;}if(goal_count == 1 && status == 1){// already succesfully sending the goal.goal_count = goal_count + 1;}if (goal_count == 2 && status == 3) {// already send the goal and the first goal is reachedcallActionServer(goal2);goal_count = goal_count + 1;}if (goal_count == 3 && status == 1) {// already successfully sending the second goal.goal_count = goal_count + 1;}if (goal_count == 4 && status == 3) {ROS_INFO("all goal has reaced succesfully!");goal_count = goal_count + 1;operation_started = false;ROS_INFO("total distance traveled : %f",dist);}else{//do nothing}}int Multigoal::check_status(int status)
{goal_status = status;// ROS_INFO("goal_status %i",goal_status);if (goal_status == 1) {// ROS_INFO("goal_sended ");goal_sended = true;goal_reached = false;}if (goal_status == 3){// ROS_INFO("goal reached");goal_reached = true;goal_sended = false;}run(goal_status);return goal_status;// check the current status}
double Multigoal::get_start_time(double start_time)
{if (start_time > 0 && i < 1 ) {real_start_time = start_time;operation_started = true;std::cout << "real start time is :" << real_start_time << std::endl;return real_start_time;}}
double Multigoal::get_end_time(double end_time){if (end_time > 0 && j < 1 ) {real_end_time = end_time;std::cout << "real end time is :" << real_end_time << std::endl;return real_end_time;}}void Multigoal::odomCallback(const nav_msgs::Odometry::Ptr & msg)
{if (operation_started) {new_pose_x = msg->pose.pose.position.x;new_pose_y = msg->pose.pose.position.y;double diff_x , diff_y;diff_x = new_pose_x - old_pose_x;diff_y = new_pose_y - old_pose_y;dist = calculate_distance(diff_x,diff_y) + dist;old_pose_x = new_pose_x; old_pose_y = new_pose_y;}else{//}}double Multigoal::calculate_distance(double diff_x, double diff_y)
{// calculate the distance double result = hypot (diff_x, diff_y);return result;
}void Multigoal::resultCallback(const actionlib_msgs::GoalStatusArray::ConstPtr &msg){
// check if goal is reached int goal_stat;
if (msg->status_list.empty()) {goal_stat = 3;
}
else
{goal_stat = msg->status_list[0].status;
}check_status(goal_stat);
double start_time , finish_time;if (goal_count == 1){// status is not clear, no goal is sended yet!start_time = msg->header.stamp.toSec(); // get the time from messageget_start_time(start_time);i = i + 1;// getGoals();}
if ( goal_count == 5){finish_time = msg->header.stamp.toSec(); // get the time from messageget_end_time(finish_time);j = j + 1;double total_time = abs(real_start_time - real_end_time);ROS_INFO("total time = %f",total_time);goal_count = goal_count + 1;}}void Multigoal::getGoals()
{std::string param_name;if (nh_.searchParam("/multi_goal/multi_goal_driver/goals", param_name)){XmlRpc::XmlRpcValue goals;if (!nh_.hasParam("/multi_goal/multi_goal_driver/goals")){ROS_ERROR("No stations on parameterserver");}nh_.getParam("/multi_goal/multi_goal_driver/goals", goals);for (size_t i = 0 ; i < goals.size(); i++){XmlRpc::XmlRpcValue goal = goals[i];Pose final_pose;XmlRpc::XmlRpcValue poses = goal["poses"];std::string frame = goal["frame_id"];XmlRpc::XmlRpcValue pose_back = poses[poses.size()-1];final_pose.x = pose_back[0];final_pose.y = pose_back[1];final_pose.theta = pose_back[2];final_pose.frame = frame;setGoals(final_pose,i);}// after the goal has been obtained, now run the code to go to the destination}else{ROS_INFO("No param 'goals' found in an upward search");}}void Multigoal::setGoals(Pose final_pose,double goal_num)
{if (goal_num == 0) {geometry_msgs::PoseStamped target_pose;//we'll send a goal to the robot to move 1 meter forwardgoal.target_pose.header.frame_id = final_pose.frame;goal.target_pose.header.stamp = ros::Time::now();goal.target_pose.pose.position.x = final_pose.x;goal.target_pose.pose.orientation.w = final_pose.theta;std::cout << "first goal is :  " << final_pose.x << ", " << final_pose.y << ", " << final_pose.theta << ", " << final_pose.frame << std::endl;}if (goal_num == 1){geometry_msgs::PoseStamped target_pose;//we'll send a goal to the robot to move 1 meter forwardgoal2.target_pose.header.frame_id = final_pose.frame;goal2.target_pose.header.stamp = ros::Time::now();goal2.target_pose.pose.position.x = final_pose.x;goal2.target_pose.pose.orientation.w = final_pose.theta;std::cout << " second goal is: " << final_pose.x << ", " << final_pose.y << ", " << final_pose.theta << ", " << final_pose.frame << std::endl;}}void Multigoal::callActionServer(move_base_msgs::MoveBaseGoal goal)
{typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;MoveBaseClient ac("/move_base", true);while(!ac.waitForServer(ros::Duration(5.0))){ROS_INFO("Waiting for the move_base action server to come up");}//we'll send a goal to the robot the goal we get from previous functionac.sendGoal(goal);ROS_INFO("Sending goal");}int main(int argc, char** argv){ros::init(argc,argv,"multi_goal_driver");ros::NodeHandle nh;Multigoal Multigoal(nh);  ros::Rate rate(5);ros::spin();return 0;//wait for the action server to come up}

python案例参考：

#!/usr/bin/env pythonfrom random import sample
from math import pow, sqrt
from actionlib_msgs.msg import *
from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
import rospy
import actionlibclass MultiPointNav():def __init__(self):rospy.init_node('multi_point_nav', anonymous=True)rospy.on_shutdown(self.shutdown)# How long in seconds should the robot pause at each location?self.rest_time = rospy.get_param("~rest_time", 10)# Are we running in the fake simulator?self.fake_test = rospy.get_param("~fake_test", False)# Goal state return valuesgoal_states = ['PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED', 'REJECTED','PREEMPTING', 'RECALLING', 'RECALLED','LOST']locations = dict()locations['Point1'] = Pose(Point(-15.2, -21.2, 0.000), Quaternion(0.000, 0.000, -0.60710, 0.70710))locations['Point2'] = Pose(Point(-14.1, -17.1, 0.000), Quaternion(0.000, 0.000, -0.670, 0.743))locations['Point3'] = Pose(Point(-3.719, 4.401, 0.000), Quaternion(0.000, 0.000, 0.733, 0.680))locations['Point4'] = Pose(Point(0.720, 2.229, 0.000), Quaternion(0.000, 0.000, 0.786, 0.618))#locations['Point5'] = Pose(Point(1.471, 1.007, 0.000), Quaternion(0.000, 0.000, 0.480, 0.877))#locations['Point6'] = Pose(Point(-0.861, -0.019, 0.000), Quaternion(0.000, 0.000, 0.892, -0.451))# Publisher to manually control the robot (e.g. to stop it, queue_size=5)self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)# Subscribe to the move_base action serverself.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)rospy.loginfo("Waiting for move_base action server...")# Wait 60 seconds for the action server to become availableself.move_base.wait_for_server(rospy.Duration(60))rospy.loginfo("Connected to move base server")# A variable to hold the initial pose of the robot to be set by # the user in RVizinitial_pose = PoseWithCovarianceStamped()# Variables to keep track of success rate, running time,# and distance traveledn_locations = len(locations)n_goals = 0n_successes = 0i = n_locationsdistance_traveled = 0start_time = rospy.Time.now()running_time = 0location = ""last_location = ""# Get the initial pose from the userrospy.loginfo("*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose...")rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)self.last_location = Pose()rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose)# Make sure we have the initial posewhile initial_pose.header.stamp == "":rospy.sleep(1)rospy.loginfo("Starting navigation test")# Begin the main loop and run through a sequence of locationswhile not rospy.is_shutdown():# If we've gone through the current sequence,# start with a new random sequenceif i == n_locations:i = 0sequence = ['Point1','Point2','Point3','Point4']# Skip over first location if it is the same as# the last locationif sequence[0] == last_location:i = 1# Get the next location in the current sequencelocation = sequence[i]# Keep track of the distance traveled.# Use updated initial pose if available.if initial_pose.header.stamp == "":distance = sqrt(pow(locations[location].position.x - locations[last_location].position.x, 2) +pow(locations[location].position.y - locations[last_location].position.y, 2))else:rospy.loginfo("Updating current pose.")distance = sqrt(pow(locations[location].position.x - initial_pose.pose.pose.position.x, 2) +pow(locations[location].position.y - initial_pose.pose.pose.position.y, 2))initial_pose.header.stamp = ""# Store the last location for distance calculationslast_location = location# Increment the countersi += 1n_goals += 1# Set up the next goal locationself.goal = MoveBaseGoal()self.goal.target_pose.pose = locations[location]self.goal.target_pose.header.frame_id = 'map'self.goal.target_pose.header.stamp = rospy.Time.now()# Let the user know where the robot is going nextrospy.loginfo("Going to: " + str(location))# Start the robot toward the next locationself.move_base.send_goal(self.goal)# Allow 5 minutes to get therefinished_within_time = self.move_base.wait_for_result(rospy.Duration(300)) # Check for success or failureif not finished_within_time:self.move_base.cancel_goal()rospy.loginfo("Timed out achieving goal")else:state = self.move_base.get_state()if state == GoalStatus.SUCCEEDED:rospy.loginfo("Goal succeeded!")n_successes += 1distance_traveled += distancerospy.loginfo("State:" + str(state))else:rospy.loginfo("Goal failed with error code: " + str(goal_states[state]))# How long have we been running?running_time = rospy.Time.now() - start_timerunning_time = running_time.secs / 60.0# Print a summary success/failure, distance traveled and time elapsedrospy.loginfo("Success so far: " + str(n_successes) + "/" + str(n_goals) + " = " + str(100 * n_successes/n_goals) + "%")rospy.loginfo("Running time: " + str(trunc(running_time, 1)) + " min Distance: " + str(trunc(distance_traveled, 1)) + " m")rospy.sleep(self.rest_time)def update_initial_pose(self, initial_pose):self.initial_pose = initial_posedef shutdown(self):rospy.loginfo("Stopping the robot...")self.move_base.cancel_goal()rospy.sleep(2)self.cmd_vel_pub.publish(Twist())rospy.sleep(1)def trunc(f, n):# Truncates/pads a float f to n decimal places without roundingslen = len('%.*f' % (n, f))return float(str(f)[:slen])if __name__ == '__main__':try:MultiPointNav()rospy.spin()except rospy.ROSInterruptException: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
需要更新对应软件包，稳定性会好一些，仅此而已。

ROS1云课→31欢乐卷假期相关推荐

ROS1云课- 1 0 2 4
用如下地图复习一下,ROS1主要内容. 分为如下: 基础部分命令部分图形化工具部分功能包使用部分功能包编译部分导航部分区域覆盖部分 1. 基础部分参考一键配置: ROS1云课→30导航仿 ...
ROS1云课→32愉快大扫除
ROS1云课→31欢乐卷假期案例简述:就是扫地机器人的路径规划复现一下,没了-- 能不能复现别人的区域覆盖算法呢??? ROS1云课→30导航仿真演示导航机器人摇身一变扫地机器人(只有路径规划演示 ...
ROS1云课→18一键配置
ROS1云课→17化繁为简stdr和f1tenth 之前的教程,打开那么多终端,每次都敲那么多指令,为啥? 熟能生巧而已,有些过程不能省略,但是,如果已经熟悉了这些过程,还每次都这么搞. 难道是有点情 ...
ROS1云课→19仿真turtlebot（stage）
ROS1云课→18一键配置 ROS1云课→17化繁为简stdr和f1tenth 依据一键配置将turtlebot仿真案例全部配置好. TurtleBot 是带有开源软件的低成本个人机器人套件. Tur ...
ROS1云课→16机器人模型从urdf到xacro
ROS1云课→15主题与坐标系补充: 2020:ROS机器人URDF建模_zhangrelay的博客-CSDN博客 2022:URDF机器人模型ROS1&2案例(noetic+galacti ...
ROS1云课→29如何借助导航实现走迷宫机器人
ROS1云课→28机器人代价地图配置简述: 在这个项目中,将创建一个机器人,它将进入一个迷宫形式的房间,然后从另一个点离开房间. 详细: 在行业中,有些地方机器人可以收集加工过的物体并将这些物体放入 ...
ROS1云课→17化繁为简stdr和f1tenth
ROS1云课→16机器人模型从urdf到xacro 蓝桥ROS云课可以使用Gazebo/V-Rep/Webots等三维仿真软件,这些都在之前博客中提及,或者有学生撰写对应博客进行分享. 二维的环境主要 ...
ROS1云课→28机器人代价地图配置
ROS1云课→27机器人SLAM小结在前面做的所有工作都成了现在项目的铺垫,而最大的乐趣也即将开始,这是赋予机器人生命的时刻. 后续学习以下内容: 应用程序包开发. 理解导航功能包集及其工作方式. ...
ROS1云课-导航实践测评
资料传递机器人实践教室机器人送资料导航示例-CSDN直播任务简介: 六艺楼S3某教室pgm图如下所示机器人从学生座位区初始位置将资料运送到讲台区目标位置,示意图如下: 结合ROS1云课-01-3 ...

ROS1云课→31欢乐卷假期

ROS1云课→31欢乐卷假期相关推荐

最新文章

热门文章