ROS教程——1.8 ROS action与导航规划

友情提示：380元/半年，儿童学编程，就上码丁实验室。

1.8.1 ROS actions

尽管服务（service）对于简单的获取/设置交互（例如查询状态和管理配置）非常方便，但它们不适用于长时间运行的任务。例如，将机器人移动到较远的位置。ROS actions是实现长时间间隔、目标导向行为接口的最佳方法。类似于服务的请求和响应模式，actions使用目标来发起行为，并在行为完成后发送结果。该操作还可以使用反馈来提供有关行为向目标进展的最新信息，还可以取消目标。此外，actions是异步的（与服务相反）。

官方链接：http://wiki.ros.org/actionlib

（1）actions客户端-服务器交互

ROS教程——1.8 ROS action与导航规划

图1 Action Client-Server Interaction

（2）actions服务器状态转换

ROS教程——1.8 ROS action与导航规划

图2 Action Server State Transitions(1)

ROS教程——1.8 ROS action与导航规划

图2 Action Server State Transitions(2)

（3）.action文件

action规范在.action文件中定义，这些文件位于软件包的”./action目录中，例如：

ROS教程——1.8 ROS action与导航规划

action文件中生成以下消息类型：

DoDishesAction.msg

DoDishesActiongoal.msg

DoDishesActionResult.msg

DoDishesActionFeedback.msg

DoDishesgoal.msg

DoDishesResult.msg

DoDishesFeedback.msg

然后，这些消息将由actionlib在内部使用，用于ActionClient和ActionServer之间的通信。

（4）简单的actions客户端

实现一个简单的客户端，仅支持一个目标。action客户端以action定义为模板，指定要与action服务器通信的消息类型。action客户端接受两个参数：要连接的服务器名称、自动旋转线程的布尔选项（如果您不想使用线程（并且希望actionlib在幕后进行”线程魔术”），那么这对您来说是一个不错的选择。代码如下所示：

ROS教程——1.8 ROS action与导航规划

1.8.2 由代码发送目标命令

（1）send_goals

接下来的C++代码是一个简单的示例，发送目标给移动机器人。此时，目标将是”PoseStamped”类型的消息，包含有关机器人应移动到世界的位置信息。

参考链接：http://wiki.ros.org/navigation/Tutorials/SendingSimpleGoals

首先创建一个名为send_goals的新包，此程序包依赖于于以下程序包：actionlibgeometry_msgsmove_base_msgs

命令如下：

$ cd ~/catkin_ws/src
$ catkin_create_pkg send_goals std_msgs rospy roscpp actionlib tf geometry_msgs move_base_msgs

在/send_goals/src子目录下，创建一个名为send_goals.cpp的新文件。

#include <ros/ros.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <tf/transform_datatypes.h>
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
int main(int argc, char** argv) {
   ros::init(argc, argv, "send_goals_node");
  // Get the goal's x, y and angle from the launch file
   double x, y, theta;
   ros::NodeHandle nh;
   nh.getParam("goal_x", x);
   nh.getParam("goal_y", y);
   nh.getParam("goal_theta", theta);
  // create the action client
   MoveBaseClient ac("move_base", true);
   // Wait 60 seconds for the action server to become available
   ROS_INFO("Waiting for the move_base action server");
   ac.waitForServer(ros::Duration(60));
   ROS_INFO("Connected to move base server");
  // Send a goal to move_base
   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 = x;
   goal.target_pose.pose.position.y = y;
  // Convert the Euler angle to quaternion
   double radians = theta * (M_PI/180);
   tf::Quaternion quaternion;
   quaternion = tf::createQuaternionFromYaw(radians);
   geometry_msgs::Quaternion qMsg;
   tf::quaternionTFToMsg(quaternion, qMsg);
   goal.target_pose.pose.orientation = qMsg;
   // Send the goal command
   ROS_INFO("Sending robot to: x = %f, y = %f, theta = %f", x, y, theta);
   ac.sendGoal(goal); 
   // Wait for the action to return
   ac.waitForResult();
   if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
       ROS_INFO("You have reached the goal!");
   else
       ROS_INFO("The base failed for some reason");
   return 0;
}

（2）将欧拉角转换为四元数

使用欧拉角而不是使用四元数来定义目标姿势更容易，以下代码将角度从弧度转换为四元数表示：

double theta = 90.0;
double radians = theta * (M_PI/180);
tf::Quaternion quaternion;
quaternion = tf::createQuaternionFromYaw(radians);
geometry_msgs::Quaternion qMsg;
tf::quaternionTFToMsg(quaternion, qMsg);
goal.target_pose.pose.orientation = qMsg;

（3）send_goals.launch文件

<launch>  
 <param name="goal_x" value="2" />
 <param name="goal_y" value="-2" />
 <param name="goal_theta" value="180" />
 <param name="/use_sim_time" value="true"/>
 <!-- Launch turtle bot world -->
 <include file="$(find turtlebot_gazebo)/launch/turtlebot_world.launch"/>
 <!-- Launch navigation stack with amcl -->
 <include file="$(find turtlebot_gazebo)/launch/amcl_demo.launch"/>
 <!-- Launch rviz -->
 <include file="$(find turtlebot_rviz_launchers)/launch/view_navigation.launch"/>
 <!-- Launch send goals node -->
 <node name="send_goals_node" pkg="send_goals" type="send_goals_node" output="screen"/>
</launch>

（4）send_goals Demo

启动send_goals.launch:

$ roslaunch send_goals send_goals.launch

现在，您应该看到rviz打开，并且机器人从其初始位姿，移动到启动文件中定义的目标位姿。

ROS教程——1.8 ROS action与导航规划

图3 send_goals Demo(1)

ROS教程——1.8 ROS action与导航规划

图3 send_goals Demo(2)

（5）Nodes Graph

ROS教程——1.8 ROS action与导航规划

图4 Nodes Graph

1.8.3 导航规划

（1）make_plan service

允许外部用户从move_base询问到给定目标位姿的路线，无需实际移动机器人。参数：

起始位姿；

目标位姿；

目标容忍度；

返回类型为”nav_msgs / GetPlan”的消息。

定位是估计机器人相对于地图的位姿的问题。定位问题对物体的确切位置不是非常敏感，因此它可以处理物体位置的小变化。ROS使用”AMCL包”实现定位。

（2）make_plan节点

现在，在我们的send_goal包中，我们将创建一个make_plan，它将调用make_plan服务并打印计划的输出路径。添加一个名为make_plan.cpp的新C ++文件：

#include <ros/ros.h>
#include <nav_msgs/GetPlan.h>
#include <geometry_msgs/PoseStamped.h>
#include <string>
using std::string;
#include <boost/foreach.hpp>
#define forEach BOOST_FOREACH
double goalTolerance = 0.5;
void fillPathRequest(nav_msgs::GetPlan::Request &req);
void callPlanningService(ros::ServiceClient &serviceClient, nav_msgs::GetPlan &srv);
int main(int argc, char** argv)
{
   ros::init(argc, argv, "make_plan_node");
   ros::NodeHandle nh;
   // Init service query for make plan
   string service_name = "move_base/make_plan";
   while (!ros::service::waitForService(service_name, ros::Duration(3.0))) {
       ROS_INFO("Waiting for service move_base/make_plan to become available");
  }
   ros::ServiceClient serviceClient = nh.serviceClient<nav_msgs::GetPlan>(service_name, true);
   if (!serviceClient) {
       ROS_FATAL("Could not initialize get plan service from %s", serviceClient.getService().c_str());
       return -1;
  }
   nav_msgs::GetPlan srv;
   fillPathRequest(srv.request); 
   if (!serviceClient) {
       ROS_FATAL("Persistent service connection to %s failed", serviceClient.getService().c_str());
       return -1;
  } 
   callPlanningService(serviceClient, srv);
}
void fillPathRequest(nav_msgs::GetPlan::Request &request)
{
   request.start.header.frame_id = "map";
   request.start.pose.position.x = 0;
   request.start.pose.position.y = 0;
   request.start.pose.orientation.w = 1.0;
   request.goal.header.frame_id = "map";
   request.goal.pose.position.x = 2.0;
   request.goal.pose.position.y = -2.0;
   request.goal.pose.orientation.w = 1.0;
   request.tolerance = goalTolerance;
}
void callPlanningService(ros::ServiceClient &serviceClient, nav_msgs::GetPlan &srv)
{
   // Perform the actual path planner call
   if (serviceClient.call(srv)) { 
       if (!srv.response.plan.poses.empty()) {
           forEach(const geometry_msgs::PoseStamped &p, srv.response.plan.poses) {
               ROS_INFO("x = %f, y = %f", p.pose.position.x, p.pose.position.y);
          }
      }
       else {
           ROS_WARN("Got empty plan");
      }
  }
   else {
       ROS_ERROR("Failed to call service %s - is the robot moving?", serviceClient.getService().c_str());
  }
}

（3）make_plan.launch

<launch>  
 <param name="goal_x" value="2" />
 <param name="goal_y" value="-2" />
 <param name="goal_theta" value="180" />
 <param name="/use_sim_time" value="true"/>
 <!-- Launch turtle bot world -->
 <include file="$(find turtlebot_gazebo)/launch/turtlebot_world.launch"/>
 <!-- Launch navigation stack with amcl -->
 <include file="$(find turtlebot_gazebo)/launch/amcl_demo.launch"/>
 <!-- Launch send goals node -->
 <node name="make_plan" pkg="send_goals" type="make_plan" output="screen"/>
</launch>

（4）运行make_plan节点

命令如下：

$ roslaunch send_goals make_plan.launch

ROS教程——1.8 ROS action与导航规划

图5 运行make_plan节点

转自公众号：
Robot404

ROS教程——1.8 ROS action与导航规划

1.8.1 ROS actions

1.8.2 由代码发送目标命令

1.8.3 导航规划

与本文相关的文章

您必须登录才能发表评论！

1.8.1 ROS actions

1.8.2 由代码发送目标命令

1.8.3 导航规划

与本文相关的文章

您必须 登录 才能发表评论！

您必须登录才能发表评论！