PX4与仿真入门教程-dronedoc-使用 MoveIt! 移动 Gazebo 模型

PX4与仿真入门教程-dronedoc-使用 MoveIt! 移动 Gazebo 模型

说明：

介绍如何利用MoveIt! 移动 Gazebo 模型
我们将使用目前创建的 iris_moveit_config 包和 MoveIt! RViz 插件来规划操作和控制无人机。
为了能够移动无人机，需要实现插件，动作服务器等
所以首先我将解释如何使用MoveIt移动机器人！以及这次使用的应用程序的概述。

控制方法：

使用 MoveIt 移动机器人，主要是

使用 MoveIt 提供的控制器管理器！
使用 ros_control

有两种方法。下面分别介绍这两种方法

使用控制器管理器：

在 MoveIt! Rviz 演示中，我们使用 RViz 来规划无人机的操作。当运动计划完成时，生成的无人机的运动显示在 RViz 中，但由于只有计划显示在 RViz 中，因此需要向机器人发送控制输入等的控制器来实际移动机器人。
控制机器人所需的控制规则和输入因机器人类型而异，因此不同的机器人必须使用不同的控制器。然而，MoveIt! 开发人员每次都为不同的机器人实现所有控制器，或者将某人实现的控制器合并到MoveIt! 中是不现实的。因此，MoveIt! 使用了ROS 动作机制，因此任何支持 MoveIt! 提供的动作的控制器都可以使用。
ROS action 是一种节点之间进行一对一通信的机制，类似于 ROS 服务。当客户端向服务器发送请求时，服务在返回结果之前不会收到服务器的任何反馈，但动作会根据执行状态返回反馈。另外，动作可以在中间中断，适用于机器人从开始到目标移动等耗时处理。
在MoveIt!中，如下图，控制器接口（以下简称接口）中定义的动作客户端向控制器中定义的动作服务器发送动作目标，即动作的目的，以及接收它的动作。服务器采取向机器人输入控制命令的形式。所使用的接口取决于所使用的操作类型。
下图中的控制器管理器（以下简称管理器）具有向 MoveIt 注册接口的作用！通过向 MoveIt 注册接口，当接收到执行生成操作的命令时，将操作信息发送到接口，并从那里发送到控制器。管理器有多种类型，每种类型都有不同的可用接口。

请输入图片描述

执行生成操作的流程如下所示。

1.有关要执行的操作的信息被发送到接口
2.接口向动作服务器发送动作目标
3.接收动作目标的控制器向机器人发送控制输入

请输入图片描述

综上所述，在 MoveIt! 中，接口执行的操作信息被发送到控制器，接收到它的控制器将操作机器人的控制输入发送给机器人。管理器必须向 MoveIt! 注册接口，以便接口能够向控制器发送有关要采取的操作的信息。
在下文中，我们将仔细研究管理器、界面和控制器的每个元素。

管理器:

MoveIt 提供了两个管理器！默认情况下：

MoveItFakeControllerManager
MoveItSimpleControllerManager
MoveItFakeControllerManager 是模拟中使用的管理器，在使用 RViz 可视化操作时使用。
MoveItSimpleControllerManager支持两个接口，FollowJointTrajectory和GripperCommand，可用于实际控制机器人。
管理器controller_list使用 MoveIt 注册参数中指定的接口！
同时，还给出了要控制的关节等信息，根据这些信息控制机器人。
有关参数，请参阅低级控制器。
此外，moveit_controller_manager可以通过更改值来指定要使用的管理器。

接口:

MoveIt! 默认提供以下两个接口。
.

FollowJointTrajectory 接口
控制手臂或机器人本身

GripperCommand 接口
夹爪控制
分别使用FollowJointTrajectory操作和GripperCommand操作

控制器:

如果要使用 MoveItSimpleControllerManager，则必须自己实现控制器。
如上所述，FollowJointTrajectory 接口使用FollowJointTrajectory 动作，GripperCommand 使用GripperCommand 动作，因此如果要使用这些接口，则需要实现合适的动作服务器。
有关动作服务器的实现，请参阅actionlib 教程

ros_control:

ros_control有多个插件用于关节速度控制、力控制、位置控制等。
对于 ros_control，以下链接会有所帮助。
教程：ROS 控制
Controller和Hardware Interface之间的处理机制（一、机器人模型的定义和注册）
Controller与硬件接口的处理机制（二、关于RobotHWSim插件）
Controller与硬件接口的处理机制（三、关于Controller）
操作实际机器时的规格 · 第 46 期 · Nishida-Lab / motoman_project

应用配置:

本次使用的整个应用的配置如下

请输入图片描述

设置运动计划的 RViz 和模拟无人机的 PX4 SITL 模拟器被添加到上述基本配置中。
在此配置中，控制器接收有关通过 ROS 操作生成的行为的信息，/mavros/setpoint_position/local并通过将生成的航点发布到主题来控制无人机。
正如我们在尝试 MoveIt! 的 Rviz 演示中看到的那样，RViz将路线设置发送到 MoveIt! 端，并根据 MoveIt! 生成的路线信息对其进行可视化。

管理器:

MoveItSimpleControllerManager 只支持FollowJointTrajectory 接口和GripperCommand 接口，FollowJointTrajectory 接口不能用于控制多自由度关节，需要新建一个管理器。
这次我们将创建一个名为 MoveItMultiDOFControllerManager 的新管理器。
有关代码的更多信息，请参阅代码说明（管理器）。
moveit_multi_dof_controller_manager.cpp代码如下：

/**
 * @file moveit_multi_dof_controller_manager.cpp
 * @brief Controller manager for multi DOF joint
 */

#include <ros/ros.h>
#include <moveit/controller_manager/controller_manager.h>
#include <dronedoc/follow_multi_dof_joint_trajectory_controller_handle.hpp>
#include <sensor_msgs/JointState.h>
#include <pluginlib/class_list_macros.hpp>

namespace dronedoc
{

class MoveItMultiDOFControllerManager : public moveit_controller_manager::MoveItControllerManager
{
public:

  /**
   * @brief Default constructor
   */
  MoveItMultiDOFControllerManager() : node_handle_("~")
  {
    // Error if controller_list param is not set
    if (!node_handle_.hasParam("controller_list"))
    {
      ROS_ERROR_STREAM_NAMED("manager", "No controller_list specified.");
      return;
    }

    XmlRpc::XmlRpcValue controller_list;
    node_handle_.getParam("controller_list", controller_list);

    // Error if controller_list is not an array
    if (controller_list.getType() != XmlRpc::XmlRpcValue::TypeArray)
    {
      ROS_ERROR("Parameter controller_list should be specified as an array");
      return;
    }

    // Error if multiple controller is defined
    if(controller_list.size() > 1)
    {
      ROS_ERROR("This controller manager expects only one controller.");
      return;
    }

    // Error if controller not have required fields
    if (!controller_list[0].hasMember("name") || !controller_list[0].hasMember("joints"))
    {
      ROS_ERROR_STREAM_NAMED("manager", "Name and joints must be specifed for each controller");
      return;
    }

    try
    {
      std::string name = std::string(controller_list[0]["name"]);

      std::string action_ns;

      // Warn if controller has "ns" field
      if (controller_list[0].hasMember("ns"))
      {
        action_ns = std::string(controller_list[0]["ns"]);
        ROS_WARN_NAMED("manager", "Use of 'ns' is deprecated, use 'action_ns' instead.");
      } // Set action namespace
      else if (controller_list[0].hasMember("action_ns"))
        action_ns = std::string(controller_list[0]["action_ns"]);
      else // Warn if "action_ns" not specified
        ROS_WARN_NAMED("manager", "Please note that 'action_ns' no longer has a default value.");

      // Error if "joints" field is not array
      if (controller_list[0]["joints"].getType() != XmlRpc::XmlRpcValue::TypeArray)
      {
        ROS_ERROR_STREAM_NAMED("manager", "The list of joints for controller " << name
                                                                               << " is not specified as an array");
        return;
      }

      // Error if controller not have "type" field
      if (!controller_list[0].hasMember("type"))
      {
        ROS_ERROR_STREAM_NAMED("manager", "No type specified for controller " << name);
        return;
      }

      std::string type = std::string(controller_list[0]["type"]);

      // Set controller handle if "type" is FollowMultiDOFJointTrajectory
      moveit_simple_controller_manager::ActionBasedControllerHandleBasePtr new_handle;
      if (type == "FollowMultiDOFJointTrajectory")
      {
        new_handle.reset(new FollowMultiDOFJointTrajectoryControllerHandle(name, action_ns));
        if (static_cast<FollowMultiDOFJointTrajectoryControllerHandle*>(new_handle.get())->isConnected())
        {
          ROS_INFO_STREAM_NAMED("manager", "Added FollowJointTrajectory controller for " << name);
          controller_ = new_handle;
        }
      }
      else
      {
        ROS_ERROR_STREAM_NAMED("manager", "Unknown controller type: " << type.c_str());
        return;
      }

      /* add list of joints, used by controller manager and MoveIt! */
      for (int i = 0; i < controller_list[0]["joints"].size(); ++i)
        controller_->addJoint(std::string(controller_list[0]["joints"][i]));
    }
    catch (...)
    {
      ROS_ERROR_STREAM_NAMED("manager", "Caught unknown exception while parsing controller information");
    }
  }

  /**
   * @brief Destructor
   */
  ~MoveItMultiDOFControllerManager() override
  {
  }

  /**
   * @brief Returns pointer to controller handle
   * @param name
   */
  moveit_controller_manager::MoveItControllerHandlePtr getControllerHandle(const std::string& name) override
  {
    return static_cast<moveit_controller_manager::MoveItControllerHandlePtr>(controller_);
  }

  /**
   * @brief Add FollowMultiDOFJointTrajectory to controller list
   * @param names
   *
   * This manager only deals FollowMultiDOFJointTrajectory controller
   */
  void getControllersList(std::vector<std::string>& names) override
  {
    names.push_back("FollowMultiDOFJointTrajectory");
  }

  /**
   * @brief Get all controllers
   * @param names
   *
   * This plugin assumes that all controllers are already active -- and if they are not, well, it has no way to deal
   * with it anyways!
   */
  void getActiveControllers(std::vector<std::string>& names) override
  {
    getControllersList(names);
  }

  /**
   * @brief Get all controllers
   * @param names
   *
   * Controller must be loaded to be active, see comment above about active controllers...
   */
  virtual void getLoadedControllers(std::vector<std::string>& names)
  {
    getControllersList(names);
  }

  /**
   * @brief Get the list of joints that a controller can control.
   * @param name Controller name
   * @param joints List of joints
   */
  void getControllerJoints(const std::string& name, std::vector<std::string>& joints) override
  {
    if (name == "FollowMultiDOFJointTrajectory")
    {
      controller_->getJoints(joints);
    }
    else
    {
      ROS_WARN_NAMED("manager", "The joints for controller '%s' are not known. Perhaps the controller configuration is "
                                "not loaded on the param server?",
                     name.c_str());
      joints.clear();
    }
  }

  /**
   * @brief Get state of controller specified by name
   * @param name
   *
   * Controllers are all active and default.
   */
  moveit_controller_manager::MoveItControllerManager::ControllerState
  getControllerState(const std::string& name) override
  {
    moveit_controller_manager::MoveItControllerManager::ControllerState state;
    state.active_ = true;
    state.default_ = true;
    return state;
  }

  /**
   * @brief Switch controllers
   * @param activate
   * @param deactivate
   *
   * Cannot switch our controllers
   */
  bool switchControllers(const std::vector<std::string>& activate, const std::vector<std::string>& deactivate) override
  {
    return false;
  }

protected:
  ros::NodeHandle node_handle_;
  moveit_simple_controller_manager::ActionBasedControllerHandleBasePtr controller_;
};

}  // end namespace dronedoc

PLUGINLIB_EXPORT_CLASS(dronedoc::MoveItMultiDOFControllerManager,
                       moveit_controller_manager::MoveItControllerManager);

接口：

如上所述，FollowJointTrajectory 不能用于控制其他自由度的关节，因此我们将为其他自由度的关节创建一个新的 FollowMultiDOFJointTrajectory 接口。
将接口保存在 include / <package_name> 目录中，而不是 src 目录中。
请根据您的环境更改 <package_name> 部分。
有关代码的更多信息，请参阅代码说明（接口）。
follow_multi_dof_joint_trajectory_controller_handle.hpp代码如下：

/**
 * @file follow_multi_dof_joint_trajectory_controller_handle.cpp
 * @brief Action client for ExecuteTrajectory action
 */

#ifndef MOVEIT_PLUGINS_FOLLOW_MULTI_DOF_TRAJECTORY_CONTROLLER_HANDLE
#define MOVEIT_PLUGINS_FOLLOW_MULTI_DOF_TRAJECTORY_CONTROLLER_HANDLE

#include <moveit_simple_controller_manager/action_based_controller_handle.h>
#include <moveit_msgs/ExecuteTrajectoryAction.h>

namespace dronedoc
{
/**
 * @brief Controller for multi DOF joint trajectory
 *
 * This is generally used for arms, but could also be used for multi-dof hands,
 * or anything using a control_mgs/FollowJointTrajectoryAction.
 */
class FollowMultiDOFJointTrajectoryControllerHandle
    : public moveit_simple_controller_manager::ActionBasedControllerHandle<moveit_msgs::ExecuteTrajectoryAction>
{
public:

  /**
   * @brief Constructor
   * @param name
   * @param action_ns
   */
  FollowMultiDOFJointTrajectoryControllerHandle(const std::string& name, const std::string& action_ns)
    : ActionBasedControllerHandle<moveit_msgs::ExecuteTrajectoryAction>(name, action_ns)
  {
  }

  /**
   * @brief Send ExecuteTrajectoryGoal message to action server
   * @param trajectory Trajectory to follow
   */
  bool sendTrajectory(const moveit_msgs::RobotTrajectory& trajectory) override
  {
    ROS_DEBUG_STREAM_NAMED("FollowMultiDOFJointTrajectoryController", "new trajectory to " << name_);

    if (!controller_action_client_)
      return false;

    if (!trajectory.joint_trajectory.points.empty())
    {
      ROS_WARN_NAMED("FollowMultiDOFJointTrajectoryController", "%s cannot execute trajectories(trajectory_msgs/JointTrajectory).", name_.c_str());
    }

    if (done_)
      ROS_DEBUG_STREAM_NAMED("FollowMultiDOFJointTrajectoryController", "sending trajectory to " << name_);
    else
      ROS_DEBUG_STREAM_NAMED("FollowMultiDOFJointTrajectoryController",
                             "sending continuation for the currently executed trajectory to " << name_);

    // Send ExecuteTrajectoryGoal message
    moveit_msgs::ExecuteTrajectoryGoal goal;
    goal.trajectory = trajectory;
    controller_action_client_->sendGoal(
        goal, boost::bind(&FollowMultiDOFJointTrajectoryControllerHandle::controllerDoneCallback, this, _1, _2),
        boost::bind(&FollowMultiDOFJointTrajectoryControllerHandle::controllerActiveCallback, this),
        boost::bind(&FollowMultiDOFJointTrajectoryControllerHandle::controllerFeedbackCallback, this, _1));
    done_ = false;
    last_exec_ = moveit_controller_manager::ExecutionStatus::RUNNING;
    return true;
  }

  /**
   * @brief Cancel trajecotry execution
   */
  bool cancelExecution() override
  {
    // do whatever is needed to cancel execution
    return true;
  }

  /**
   * @brief Wait trajectory execution
   * @param duration
   */
  bool waitForExecution(const ros::Duration& duration) override
  {
    // wait for the current execution to finish
    return true;
  }

protected:

  /**
   * @brief Called when server complete action
   * @param state
   * @param result
   */
  void controllerDoneCallback(const actionlib::SimpleClientGoalState& state,
                              const moveit_msgs::ExecuteTrajectoryResultConstPtr& result)
  {
    // Output custom error message for FollowJointTrajectoryResult if necessary
    if (result)
    {
      if(result->error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
      {
        ROS_INFO_NAMED("FollowMultiDOFTrajectoryContoller", "Execution Succeeded.");
      }
      else
      {
        ROS_ERROR("Returned Error Code %d", result->error_code.val);
        ROS_ERROR("For Detailse of Error Code, see moveit_msgs/MoveItErrorCodes.msg");
      }
    }
    else
    {
      ROS_WARN_STREAM("Controller " << name_ << ": no result returned");
    }

    finishControllerExecution(state);
  }

  /**
   * @brief Called when goal become active
   */
  void controllerActiveCallback()
  {
    ROS_DEBUG_STREAM_NAMED("FollowMultiDOFJointTrajectoryController", name_ << " started execution");
  }

  /**
   * @brief Called when feedback arrived from server
   * @param feedback
   */
  void controllerFeedbackCallback(const moveit_msgs::ExecuteTrajectoryFeedbackConstPtr& feedback)
  {
  }
};

}  // end namespace dronedoc

#endif // MOVEIT_PLUGINS_FOLLOW_MULTI_DOF_TRAJECTORY_CONTROLLER_HANDLE

控制器:

这一次，FollowMultiDOFJointTrajectory我们将使用一个节点作为控制器，接收动作的目标并将其发送给无人机作为目标位置。
有关代码的更多信息，请参阅代码说明（控制器）
drone_controller.cpp代码如下：

/**
 * @file drone_controller.cpp
 * @brief PX4 based UAV controller for moveit
 */

#include <vector>
#include <array>
#include <string>
#include <cmath>

#include <ros/ros.h>

#include <geometry_msgs/Transform.h>
#include <geometry_msgs/PoseStamped.h>

#include <moveit_msgs/ExecuteTrajectoryAction.h>
#include <moveit_msgs/RobotTrajectory.h>

#include <mavros_msgs/State.h>

#include <trajectory_msgs/MultiDOFJointTrajectory.h>
#include <trajectory_msgs/MultiDOFJointTrajectoryPoint.h>

#include <actionlib/server/simple_action_server.h>

class DroneController{

private:
    ros::NodeHandle nh_;
    //! Action name of ExecuteTrajectory
    std::string action_name_;
    //! Server of ExecuteTrajectoryAction
    actionlib::SimpleActionServer<moveit_msgs::ExecuteTrajectoryAction> as_;
    //! Feedback message of ExecuteTrajectoryAction
    moveit_msgs::ExecuteTrajectoryFeedback feedback_;
    //! Result message of ExecuteTrajectoryAction
    moveit_msgs::ExecuteTrajectoryResult result_;
    //! Position command publisher
    ros::Publisher cmd_pos_pub_;
    //! Subscriber of local position
    ros::Subscriber local_pos_sub_;
    //! Subscriber of UAV state
    ros::Subscriber state_sub_;
    //! Current pose of UAV
    geometry_msgs::PoseStamped current_pose_;
    //! Current state of UAV
    mavros_msgs::State current_state_;

public:

    /**
     * @brief Constructor
     * @param action_name Action name
     */
    DroneController(std::string action_name) :
        action_name_(action_name),
        as_(nh_, action_name, boost::bind(&DroneController::executeCb, this, _1), false)
    {
        as_.start();

        // Position command publisher setup
        std::string cmd_pos_topic;
        nh_.param<std::string>("mavros_setpoint_topic", cmd_pos_topic, "/mavros/setpoint_position/local");
        cmd_pos_pub_ = nh_.advertise<geometry_msgs::PoseStamped>(cmd_pos_topic, 10);

        // Local position subscriber setup
        std::string local_pos_topic;
        nh_.param<std::string>("mavros_localpos_topic", local_pos_topic, "/mavros/local_position/pose");
        auto local_pos_cb = boost::bind(&DroneController::localPosCb, this, _1);
        local_pos_sub_ = nh_.subscribe<geometry_msgs::PoseStamped>(local_pos_topic, 10, local_pos_cb);

        // UAV state subscriber setup
        std::string state_topic;
        nh_.param<std::string>("mavros_state_topic", state_topic, "/mavros/state");
        auto state_cb = boost::bind(&DroneController::stateCb, this, _1);
        state_sub_ = nh_.subscribe<mavros_msgs::State>(state_topic, 10, state_cb);
        ROS_INFO("Action server initialized.");
    };

    /**
     * @brief Destructor
     */
    ~DroneController()
    {
    };

private:

    /**
     * @brief Callback of ExecuteTrajectory action
     * @param goal Goal of ExecuteTrajectory action
     */
    void executeCb(const moveit_msgs::ExecuteTrajectoryGoalConstPtr &goal)
    {
        ROS_INFO("Action received.");
        ros::Rate rate(20);

        std::vector<trajectory_msgs::MultiDOFJointTrajectoryPoint> trajectory;
        trajectory = goal->trajectory.multi_dof_joint_trajectory.points;

        // Wait for connection
        ROS_INFO("Waiting for FCU connection...");
        while (ros::ok() and not current_state_.connected)
        {
            ros::spinOnce();
            rate.sleep();
        }
        ROS_INFO("FCU connection established.");

        // Position command need to be published to switch mode to OFFBOARD
        for(int i=0; i<10; ++i)
        {
            cmd_pos_pub_.publish(getPoseFromTrajectory(trajectory.front()));
        }


        for(int i=0; i < trajectory.size()-1; ++i)
        {
            ROS_INFO("Moving to waypoint No. %d", i);

            // Send feedback (progress of path)
            feedback_.state = std::to_string(i);
            as_.publishFeedback(feedback_);

            // Get PoseStamped message from MultiDOFJointTrajectoryPoint
            geometry_msgs::PoseStamped start = getPoseFromTrajectory(trajectory.at(i));
            geometry_msgs::PoseStamped goal = getPoseFromTrajectory(trajectory.at(i+1));
            // Get interpolated path from two waypoints
            std::vector<geometry_msgs::PoseStamped> path = getBilinearPath(start, goal);

            // Publish all interpolated points
            for(auto pose: path)
            {
                // Publish same message till drone arrives to local goal
                while(not isGoalReached(pose) and not as_.isPreemptRequested())
                {
                    cmd_pos_pub_.publish(pose);
                    ros::spinOnce();
                    rate.sleep();
                }

                // Exit loop if new action arrived
                if(as_.isPreemptRequested() or not ros::ok())
                {
                    result_.error_code.val = moveit_msgs::MoveItErrorCodes::PREEMPTED;
                    as_.setPreempted();
                    ROS_INFO("Action preempted.");
                    break;
                }
            }

            // Exit loop if new action arrived
            if(as_.isPreemptRequested() or not ros::ok())
            {
                result_.error_code.val = moveit_msgs::MoveItErrorCodes::PREEMPTED;
                as_.setPreempted();
                ROS_INFO("Action preempted.");
                break;
            }
            rate.sleep();
        }

        // Success
        result_.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
        as_.setSucceeded(result_);
        ROS_INFO("Action completed.");
    }

    /**
     * @brief Return interpolated path using bilinear interpolation from two waypoints
     * @param start Start waypoint
     * @param goal Goal waypoint
     * @param step Step size of interpolation
     * @return Vector of interpolated path
     */
    std::vector<geometry_msgs::PoseStamped> getBilinearPath(const geometry_msgs::PoseStamped &start,
                                                            const geometry_msgs::PoseStamped &goal,
                                                            const double step=0.05)
    {
        std::vector<geometry_msgs::PoseStamped> bilinear_path;

        // Store x-y and x-z coordinates of start point
        std::array<double, 2> start_xy = {start.pose.position.x, start.pose.position.y};
        std::array<double, 2> start_xz = {start.pose.position.x, start.pose.position.z};
        // Store x-y and x-z coordinates of goal point
        std::array<double, 2> goal_xy = {goal.pose.position.x, goal.pose.position.y};
        std::array<double, 2> goal_xz = {goal.pose.position.x, goal.pose.position.z};

        // x-y and x-z coordinates of interpolated points
        std::array<std::vector<double>, 2> points_xy = linearInterp(start_xy, goal_xy, step);
        std::array<std::vector<double>, 2> points_xz = linearInterp(start_xz, goal_xz, step);

        // Number of generated points by interpolation
        int num_points = points_xy.at(0).size();

        try
        {
            // Generate PoseStamped message from std::array
            for(int i=0; i<num_points; ++i)
            {
                geometry_msgs::PoseStamped pose;
                pose.pose.orientation = start.pose.orientation;
                pose.pose.position.x = points_xy.front().at(i);
                pose.pose.position.y = points_xy.back().at(i);
                pose.pose.position.z = points_xz.back().at(i);

                bilinear_path.push_back(pose);
            }
        }
        catch (std::out_of_range &ex)
        {
            ROS_ERROR("%s", ex.what());
        }

        return bilinear_path;
    }

    /**
     * @brief Perform linear interpolation
     * @param p1 First point
     * @param p2 Second point
     * @param step Step size of interpolation
     * @return Array of interpolated points in the shape of [x-points, y-points]
     */
    std::array<std::vector<double>, 2> linearInterp(const std::array<double, 2> &p1,
                                                    const std::array<double, 2> &p2, const double step)
    {
        // Gradient
        double a = (p1.at(1) - p2.at(1)) / (p1.at(0) - p2.at(0));
        // Intercept
        double b = p1.at(1) - a*p1.at(0);

        // Number of steps
        int num_steps = std::floor((p2.at(0) - p1.at(0))/step);

        // Initialize container for interpolated points
        std::vector<double> points_x(num_steps+1);

        // Set interpolated points
        points_x.front() = p1.at(0);
        for(int i=1; i<num_steps; ++i)
        {
            points_x.at(i) = step * i + p1.at(0);
        }
        points_x.back() = p2.at(0);

        // Initialize container for interpolated points
        std::vector<double> points_y(num_steps+1);

        // Set interpolated points
        points_y.front() = p1.at(1);
        for(int i=1; i<num_steps; ++i)
        {
            points_y.at(i) = a*(p1.at(0) + i*step) + b;
        }
        points_y.back() = p2.at(1);

        // Initialize container for vector of points
        std::array<std::vector<double>, 2> points;
        points.front() = points_x;
        points.back() = points_y;

        return points;
    }

    /**
     * @brief Convert MultiDOFJointTrajectoryPoint message to PoseStamped message
     * @param trajectory_pt MultiDOFJointTrajectoryPoint message
     * @return Pose converted from Trajectory msg
     */
    geometry_msgs::PoseStamped getPoseFromTrajectory(const trajectory_msgs::MultiDOFJointTrajectoryPoint &trajectory_pt)
    {
        geometry_msgs::PoseStamped pose;

        pose.header.stamp = ros::Time::now();
        pose.pose.position.x = trajectory_pt.transforms.front().translation.x;
        pose.pose.position.y = trajectory_pt.transforms.front().translation.y;
        pose.pose.position.z = trajectory_pt.transforms.front().translation.z;
        pose.pose.orientation = trajectory_pt.transforms.front().rotation;

        return pose;
    }

    /**
     * @brief Returns true if drone is reached goal
     * @param goal
     * @param tolerance Consider drone reached goal if drone is within the circle with diameter of this value
     * @return True if drone is reched goal, false if else
     */
    inline bool isGoalReached(const geometry_msgs::PoseStamped &goal, const double tolerance=0.1)
    {
        double error = std::sqrt(std::pow(goal.pose.position.x - current_pose_.pose.position.x, 2)
                                 + std::pow(goal.pose.position.y - current_pose_.pose.position.y, 2)
                                 + std::pow(goal.pose.position.z - current_pose_.pose.position.z, 2));
        return error < tolerance ? true : false;
    }

    /**
     * @brief Callback for local position subscriber
     * @msg Incoming message
     */
    void localPosCb(const geometry_msgs::PoseStamped::ConstPtr &msg)
    {
        current_pose_ = *msg;
    }

    /**
     * @brief Callback for state subscriber
     * @msg Incoming message
     */
    void stateCb(const mavros_msgs::State::ConstPtr &msg)
    {
        current_state_ = *msg;
    }

};

int main(int argv, char **argc)
{
    ros::init(argv, argc, "drone_controller");

    DroneController controller("iris_group_controller/follow_multi_dof_joint_trajectory");
    ros::spin();

    return 0;
}

CMakeLists.txt：

find_package 添加要使用的包

find_package(catkin REQUIRED COMPONENTS
    ...
    actionlib
    trajectory_msgs
    pluginlib
    moveit_msgs
    moveit_core
)

catkin_package(
    INCLUDE_DIRS include
    ...
)

添加以下内容以编译控制器管理器并链接库。控制器管理器是一个库，而不是一个节点，所以add_library使用
为库名添加前缀，使其不会与其他包中的库名重复${PROJECT_NAME}_。
由于这次的项目名称是这样，您可以dronedoc通过这样做libdronedoc_moveit_multi_dof_controller_manager来创建一个名为的库

add_library(${PROJECT_NAME}_moveit_multi_dof_controller_manager src/moveit_multi_dof_controller_manager.cpp)
target_link_libraries(${PROJECT_NAME}_moveit_multi_dof_controller_manager ${catkin_LIBRARIES} ${Boost_LIBRARIES})

添加以下内容以编译控制器并链接库

add_executable(drone_controller src/drone_controller.cpp)
target_link_libraries(drone_controller ${catkin_LIBRARIES})

插件信息文件：

创建一个描述插件信息的文件，以便插件加载器可以找到插件
moveit_multi_dof_controller_manager_plugin_description.xml

<library path="libdronedoc_moveit_multi_dof_controller_manager">
  <class name="dronedoc/MoveItMultiDOFControllerManager"
         type="dronedoc::MoveItMultiDOFControllerManager"
         base_class_type="moveit_controller_manager::MoveItControllerManager">
    <description>
    </description>
  </class>

</library>

每个标签和字段的含义如下。

path
库路径。库名称是将名称添加到CMakeLists.txt 中add_library指定名称开头的名称lib（在本例中为libdronedoc_moveit_multi_dof_controller_manager）。

type
插件的型号名称。必须从命名空间中指定。

base_class
插件基类的模型名称。必须从命名空间中指定。

name
插件名称

description
插件说明

package.xml：

由于我们使用的是 pluginlib，因此将 pluginlib 添加到 package.xml 中的依赖包中

<depend>pluginlib</depend>

它还导出标签内的插件。 moveit_core该部分指定了插件基类所在的包名。
这次moveit_core,MoveItSimpleControllerManager是在包中定义的，所以指定这个

<export>
  <moveit_core plugin="${prefix}/moveit_multi_dof_controller_manager_plugin_description.xml"/>
</export>

要检查插件是否已注册rospack，请在构建包后使用以下命令

rospack plugins --attrib=plugin moveit_core

启动文件：

创建并编辑一些 Launch 文件以运行您目前构建的应用程序
iris_moveit.launch 启动模拟环境（Gazebo + PX4 SITL + MAVROS）、MoveIt! 节点和控制器
iris_moveit.launch内容如下：

<launch>

    <!-- robot description for px4 sitl -->
    <arg name="model_sitl" default="$(find px4_sim_pkg)/models/iris_depth_camera/xacro_sitl/urdf/iris_base.xacro" />
    <arg name="mavlink_udp_port" default="14560"/>
    <arg name="rotors_description_dir_sitl" default="$(find px4_sim_pkg)/models/iris_depth_camera/xacro_sitl" />
    <arg name="cmd_sitl" default="$(find xacro)/xacro $(arg model_sitl) rotors_description_dir:=$(arg rotors_description_dir_sitl) mavlink_udp_port:=$(arg mavlink_udp_port) --inorder"/>
    <param command="$(arg cmd_sitl)" name="robot_description_sitl" />

    <!-- robot description for moveit -->
    <arg name="model" default="$(find px4_sim_pkg)/models/iris_depth_camera/xacro/urdf/iris_base.xacro" />
    <arg name="rotors_description_dir" default="$(find px4_sim_pkg)/models/iris_depth_camera/xacro" />
    <arg name="cmd" default="$(find xacro)/xacro $(arg model) rotors_description_dir:=$(arg rotors_description_dir) mavlink_udp_port:=$(arg mavlink_udp_port) --inorder"/>
    <param command="$(arg cmd)" name="robot_description" />

    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />

    <!-- PX4 configs -->
    <arg name="est" default="ekf2"/>
    <arg name="vehicle" default="iris_2d_lidar"/>
    <arg name="interactive" default="true"/>
    <env name="PX4_SIM_MODEL" value="$(arg vehicle)" />
    <env name="PX4_ESTIMATOR" value="$(arg est)" />
    <!-- PX4 SITL and Gazebo -->
    <arg unless="$(arg interactive)" name="px4_command_arg1" value="-d"/>
    <arg     if="$(arg interactive)" name="px4_command_arg1" value=""/>
    <node name="sitl" pkg="px4" type="px4" output="screen"
        args="$(find px4)/ROMFS/px4fmu_common -s etc/init.d-posix/rcS $(arg px4_command_arg1)" required="true"/>

    <!-- Launching gazebo -->
    <arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/empty.world"/>
    <arg name="debug" default="false"/>
    <arg name="verbose" default="false"/>
    <arg name="gui" default="true"/>
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="world_name" value="$(arg world)"/>
        <arg name="verbose" value="$(arg verbose)"/>
        <arg name="gui" value="$(arg gui)"/>
        <arg name="debug" value="$(arg debug)"/>
    </include>

    <!-- MAVROS -->
    <arg name="fcu_url" default="udp://:14540@localhost:14557"/>
    <arg name="gcs_url" default="" />
    <arg name="respawn_mavros" default="false" />
    <include file="$(find mavros)/launch/px4.launch">
        <!-- GCS link is provided by SITL -->
        <arg name="gcs_url" value=""/>
        <arg name="fcu_url" value="$(arg fcu_url)"/>
        <arg name="respawn_mavros" value="$(arg respawn_mavros)"/>
    </include>
    <!-- mavros -->
    <param name="/mavros/local_position/tf/send" type="bool" value="true" />
    <param name="/mavros/local_position/frame_id" type="str" value="base_link" />
    <param name="/mavros/local_position/tf/frame_id" type="str" value="map" />

    <!-- spawn robot -->
    <arg name="x" default="0"/>
    <arg name="y" default="0"/>
    <arg name="z" default="0"/>
    <arg name="R" default="0"/>
    <arg name="P" default="0"/>
    <arg name="Y" default="0"/>
    <node name="$(anon vehicle_spawn)" pkg="gazebo_ros" type="spawn_model" output="screen" args="-param robot_description_sitl -urdf -model $(arg vehicle) -package_to_model -x $(arg x) -y $(arg y) -z $(arg z) -R $(arg R) -P $(arg P) -Y $(arg Y)"/>

    <!-- static tf from map to world which is base frame for planning -->
    <node pkg="tf" name="map2world" type="static_transform_publisher" args="0 0 0 0 0 0 map world 100"/>
    <!-- publish tf to supress warnings -->
    <node pkg="tf" name="map2origin" type="static_transform_publisher" args="0 0 0 0 0 0 map local_origin 100"/>
    <node pkg="tf" name="base2fcu" type="static_transform_publisher" args="0 0 0 0 0 0 base_link fcu 100"/>
    <!-- Moveit -->
    <include file="$(find iris_moveit_config)/launch/move_group.launch" >
        <arg name="fake_execution" value="false" />
    </include>

    <node pkg="px4_sim_pkg" name="drone_controller" type="drone_controller" output="screen"/>

</launch>

评论：

从 xacro 文件生成用于 PX4 SITL 模拟的 URDF。
如创建 iris_moveit_config 包中所述，PX4 SITL 模拟中使用的 xacro 文件和 MoveIt! 中使用的 xacro 文件不能相同，因此单独加载它们。
在创建 iris_moveit_config 包时，用package://rotors_description所有内容package://mavlink_sitl_gazebo/models/rotors_description/替换multirotor_base.xacro文件中的所有内容，但准备一个您没有在 xacro_sitl 目录中替换的模式并加载它

<arg name="model_sitl" default="$(find px4_sim_pkg)/models/iris_depth_camera/xacro_sitl/urdf/iris_base.xacro" />
<arg name="mavlink_udp_port" default="14560"/>
<arg name="rotors_description_dir_sitl" default="$(find px4_sim_pkg)/models/iris_depth_camera/xacro_sitl" />
<arg name="cmd_sitl" default="$(find xacro)/xacro $(arg model_sitl) rotors_description_dir:=$(arg rotors_description_dir_sitl) mavlink_udp_port:=$(arg mavlink_udp_port) --inorder"/>
<param command="$(arg cmd_sitl)" name="robot_description_sitl" />

为 MoveIt 加载 URDF! 并使用joint_state_publisher 和robot_state_publisher 节点将机器人关节和框架的状态发布为TF。

<arg name="model" default="$(find px4_sim_pkg)/models/iris_depth_camera/xacro/urdf/iris_base.xacro" />
<arg name="rotors_description_dir" default="$(find px4_sim_pkg)/models/iris_depth_camera/xacro" />
<arg name="cmd" default="$(find xacro)/xacro $(arg model) rotors_description_dir:=$(arg rotors_description_dir) mavlink_udp_port:=$(arg mavlink_udp_port) --inorder"/>
<param command="$(arg cmd)" name="robot_description" />

<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />

设置为将 TF 发送到基本链接

<param name="/mavros/local_position/tf/send" type="bool" value="true" />
<param name="/mavros/local_position/frame_id" type="str" value="base_link" />
<param name="/mavros/local_position/tf/frame_id" type="str" value="map" />

iris_group 的 parent frame 是world因为我不得不定义了 TF map to world，以便您可以使用 TF world to base_link

<node pkg="tf" name="map2world" type="static_transform_publisher" args="0 0 0 0 0 0 map world 100"/>

由于您将收到 TF 不可用的警告，请定义以下 TF 以防止出现警告。它不一定是。

<node pkg="tf" name="map2origin" type="static_transform_publisher" args="0 0 0 0 0 0 map local_origin 100"/>
<node pkg="tf" name="base2fcu" type="static_transform_publisher" args="0 0 0 0 0 0 base_link fcu 100"/>

启动 MoveIt! 节点。由于命令发送到实际机器人，因此fake_execution设置为false

<include file="$(find iris_moveit_config)/launch/move_group.launch" >
    <arg name="fake_execution" value="false" />
</include>

管理器参数文件：

iris_moveit_controller_manager.launch.xml 指定控制器管理器并加载参数。
moveit_controller_manager更改参数以使用您创建的控制器管理器。
它还加载了下面描述的controllers.yaml 文件来加载控制器管理器的参数
iris_moveit_controller_manager.launch.xml内容如下：

<launch>
 <!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
 <arg name="moveit_controller_manager" default="dronedoc/MoveItMultiDOFControllerManager" />
 <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
 <!-- load controller_list -->
 <rosparam file="$(find iris_moveit_config)/config/controllers.yaml"/>
</launch>

控制器参数文件：

该文件描述了控制器管理器使用的参数。有关参数，请参阅低级控制器
controllers.yaml内容：

controller_list:
 - name: iris_group_controller
   action_ns: follow_multi_dof_joint_trajectory
   type: FollowMultiDOFJointTrajectory
   default: true
   joints:
    - virtual_joint

trajectory_execution.launch.xml：

trajectory_execution.launch.xml设置与动作执行相关的参数
如果当前位置与起始位置相距较远，则会发生错误且无法执行操作。
默认情况下，距离1cm就不能执行，所以trajectory_execution/allowed_start_tolerance改变参数的值，即使距离很远也能执行操作。

<param name="trajectory_execution/allowed_start_tolerance" value="0.1"/>

此外，默认情况下，自操作开始执行后经过一定时间后，操作的执行会中断，因此请设置trajectory_execution/execution_duration_monitoring参数false以防止超时

<param name="trajectory_execution/execution_duration_monitoring" value="false" />

测试：

启动模拟环境和 MoveIt! 节点

cd ~/tools/dronedoc/
source load_environment.sh
roslaunch px4_sim_pkg iris_moveit.launch

将无人机起飞，输入 commander takeoff
启动 RViz，其中包含用于路线规划的 MoveIt! 插件

cd ~/tools/dronedoc/
source load_environment.sh
roslaunch iris_moveit_config moveit_rviz.launch config:=true

将无人机起飞，配置好设置，点击“规划”选项卡中的“规划”，在RViz中显示生成的路径。
如果开始在起飞前位置，则将“计划”选项卡“查询”中的“选择开始状态”下拉菜单设置为“当前”，然后按“更新”按钮开始当前位置
按下“执行”按钮follow_multi_dof_joint_trajectory会将目标发送到动作服务器，动作服务器将开始通过 mavros 主题发送目标位置。
/mavros/setpoint_position/local将模式更改为 OFFBOARD 以使用主题消息控制无人机

rosrun mavros mavsys mode -c OFFBOARD

如果无人机移动到如下图所示的目标位置，则成功。

请输入图片描述

如果有障碍物，它会生成一条路线来避开它

请输入图片描述

目前在规划路线时没有对滚转角或俯仰角的限制，因此可能会输出无人机无法采取的姿态。 setpoint_position/local在使用topic的控制中，只反映了偏航角，所以无人机的姿态不会变得异常，但是由于MoveIt产生的运动！不能忠实地执行，可以通过显示RViz来避免。你可能会撞到存在的障碍。您可能想要使用移动组界面，它允许您限制路线

概括:

在本文中，我们通过将使用 MoveIt! 规划的路线的航点通过 mavros 发送到无人机来移动无人机。
这次我使用了线性插值来对生成的路径进行插值，并将其交给setpoint_position主题来控制无人机，但您可能想尝试使用waypoints来控制无人机的位置

参考:

##插件库相关
pluginlib / 故障排除
如何使用从导航堆栈中学到的pluginlib
##移动组界面
这一次，我从 RViz 设置目标和计划器，但是有一个接口可以从 C++ 或 Python 代码设置这些。
移动组 C++ 接口
移动组 Python 接口
##机械手控制
ROS控制机械手
使用 Gazebo 模拟机械手

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号