ROS1云课→31欢乐卷假期
Posted zhangrelay
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了ROS1云课→31欢乐卷假期相关的知识，希望对你有一定的参考价值。
新增加一个主题地图：
sudo thunar
使用超级权限打开：
复制节日专属地图：
然后修改使用如下配置：
export TURTLEBOT_STDR_MAP_FILE=/opt/ros/kinetic/share/turtlebot_stdr/maps/wish.yaml
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案例实现？参考如下：
#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 action
goal_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 reached
callActionServer(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 message
get_start_time(start_time);
i = i + 1;
// getGoals();
if ( goal_count == 5)
finish_time = msg->header.stamp.toSec(); // get the time from message
get_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 forward
goal.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 forward
goal2.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 function
ac.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 python
from 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 actionlib
class 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 values
goal_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 server
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.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 RViz
initial_pose = PoseWithCovarianceStamped()
# Variables to keep track of success rate, running time,
# and distance traveled
n_locations = len(locations)
n_goals = 0
n_successes = 0
i = n_locations
distance_traveled = 0
start_time = rospy.Time.now()
running_time = 0
location = ""
last_location = ""
# Get the initial pose from the user
rospy.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 pose
while initial_pose.header.stamp == "":
rospy.sleep(1)
rospy.loginfo("Starting navigation test")
# Begin the main loop and run through a sequence of locations
while not rospy.is_shutdown():
# If we've gone through the current sequence,
# start with a new random sequence
if i == n_locations:
i = 0
sequence = ['Point1','Point2','Point3','Point4']
# Skip over first location if it is the same as
# the last location
if sequence[0] == last_location:
i = 1
# Get the next location in the current sequence
location = 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 calculations
last_location = location
# Increment the counters
i += 1
n_goals += 1
# Set up the next goal location
self.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 next
rospy.loginfo("Going to: " + str(location))
# Start the robot toward the next location
self.move_base.send_goal(self.goal)
# Allow 5 minutes to get there
finished_within_time = self.move_base.wait_for_result(rospy.Duration(300))
# Check for success or failure
if 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 += 1
distance_traveled += distance
rospy.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_time
running_time = running_time.secs / 60.0
# Print a summary success/failure, distance traveled and time elapsed
rospy.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_pose
def 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 rounding
slen = 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欢乐卷假期的主要内容，如果未能解决你的问题，请参考以下文章