使用越疆科技的M1-B1机器人进行ROS下导航消毒功能

#include <ros/ros.h>
#include "ros/console.h"
#include <tf/transform_listener.h>
#include <nav_msgs/Odometry.h>
#include <string.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <cmath>
#include <unistd.h>
#include <iostream>
#include <sys/types.h>
#include <stdlib.h>
#include <std_msgs/Bool.h>
#include "std_msgs/Int16.h"

#include "std_msgs/String.h"
#include "dobot/SetCmdTimeout.h"
#include "dobot/SetQueuedCmdClear.h"
#include "dobot/SetQueuedCmdStartExec.h"
#include "dobot/SetQueuedCmdForceStopExec.h"
#include "dobot/GetDeviceVersion.h"

#include "dobot/SetEndEffectorParams.h"
#include "dobot/SetPTPJointParams.h"
#include "dobot/SetPTPCoordinateParams.h"
#include "dobot/SetPTPJumpParams.h"
#include "dobot/SetPTPCommonParams.h"
#include "dobot/SetPTPCmd.h"
#include "dobot/SetEndEffectorGripper.h"
#include "dobot/SetHOMECmd.h"
#include "dobot/SetHOMEParams.h"
#include "dobot/SetIODO.h"
#include "dobot/SetArmOrientation.h"
#include "dobot/SetWAITCmd.h"

using namespace std;

//服务注册
ros::ServiceClient client;
ros::ServiceClient clientPTP;
ros::ServiceClient clientIO;
ros::ServiceClient clientWait;

//函数声明
void setIO(uint8_t address, uint8_t level, bool isQueued, ros::ServiceClient client);
void setPTP(uint8_t ptpMode ,float x, float y, float z, float r, ros::ServiceClient client);
void navigation(actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> &ac, geometry_msgs::Point &point, geometry_msgs::Quaternion &quaternion);
void setWAIT( uint32_t timeout, ros::ServiceClient client);

//执行时间等待指令
void setWAIT( uint32_t timeout, ros::ServiceClient client)
{
    dobot::SetWAITCmd srv;
    srv.request.timeout = timeout;
        client.call(srv); 
}

//执行 PTP 指令
void setPTP(uint8_t ptpMode ,float x, float y, float z, float r, ros::ServiceClient client)
{
    dobot::SetPTPCmd srv;
    srv.request.ptpMode = ptpMode;
        srv.request.x = x;
        srv.request.y = y;
        srv.request.z = z;
        srv.request.r = r;
        client.call(srv);   
}

//设置 I/O 输出电平
void setIO(uint8_t address, uint8_t level, bool isQueued, ros::ServiceClient client)
{
    dobot::SetIODO IO;
    IO.request.address = address;
    IO.request.level = level;
    IO.request.isQueued = isQueued;
    client.call(IO);
    ROS_INFO("SetIODO%d ok, %d", IO.request.address , IO.response.queuedCmdIndex);
}

//导航部分
void navigation(actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> &ac, geometry_msgs::Point &point, geometry_msgs::Quaternion &quaternion)
{
    //设置我们要机器人走的几个点。
    geometry_msgs::Pose pose_list;

    pose_list.position = point;
    pose_list.orientation = quaternion;
    

      ROS_INFO("Waiting for move_base action server...");
      //等待60秒以使操作服务器可用
      if(!ac.waitForServer(ros::Duration(60)))
      {
            ROS_INFO("Can't connected to move base server");
        exit(-1);
      }
      ROS_INFO("Connected to move base server");
      ROS_INFO("Starting navigation test");

         //初始化航点目标
         move_base_msgs::MoveBaseGoal goal1;

         //使用地图框定义目标姿势
         goal1.target_pose.header.frame_id = "map";

         //将时间戳设置为“now”
         goal1.target_pose.header.stamp = ros::Time::now();

        //将目标姿势设置为第i个航点
        goal1.target_pose.pose = pose_list;

        //让机器人向目标移动
        //将目标姿势发送到MoveBaseAction服务器
        ac.sendGoal(goal1);

        //等3分钟到达那里
        bool finished_within_time1 = ac.waitForResult(ros::Duration(180));

        //如果我们没有及时赶到那里，就会中止目标
        if(!finished_within_time1)
        {
            ac.cancelGoal();
            ROS_INFO("Timed out achieving goal");
        }
    else
        {
            //We made it!
            if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
            {
                ROS_INFO("Goal succeeded!");
            }
            else
            {
                ROS_INFO("The base failed for some reason");
            }
        }
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "disinfect_project");
    ros::NodeHandle n;

    //机械臂初始化
    // SetCmdTimeout
    client = n.serviceClient<dobot::SetCmdTimeout>("/DobotServer/SetCmdTimeout");
    dobot::SetCmdTimeout srv1;
    srv1.request.timeout = 3000;
    if (client.call(srv1) == false) {
        ROS_ERROR("Failed to call SetCmdTimeout. Maybe DobotServer isn't started yet!");
        return -1;
    }

    // Clear the command queue
    client = n.serviceClient<dobot::SetQueuedCmdClear>("/DobotServer/SetQueuedCmdClear");
    dobot::SetQueuedCmdClear srv2;
    client.call(srv2);

    // Start running the command queue
    client = n.serviceClient<dobot::SetQueuedCmdStartExec>("/DobotServer/SetQueuedCmdStartExec");
    dobot::SetQueuedCmdStartExec srv3;
    client.call(srv3);

    // Get device version information
    client = n.serviceClient<dobot::GetDeviceVersion>("/DobotServer/GetDeviceVersion");
    dobot::GetDeviceVersion srv4;
    client.call(srv4);
    if (srv4.response.result == 0) {
        ROS_INFO("Device version:%d.%d.%d", srv4.response.majorVersion, srv4.response.minorVersion, srv4.response.revision);
    } else {
        ROS_ERROR("Failed to get device version information!");
    }

    // Set PTP joint parameters
    do {
        client = n.serviceClient<dobot::SetPTPJointParams>("/DobotServer/SetPTPJointParams");
        dobot::SetPTPJointParams srv;

        for (int i = 0; i < 4; i++) {
            srv.request.velocity.push_back(100);
        }
        for (int i = 0; i < 4; i++) {
            srv.request.acceleration.push_back(100);
        }
        client.call(srv);
    } while (0);

    // Set PTP coordinate parameters
    do {
        client = n.serviceClient<dobot::SetPTPCoordinateParams>("/DobotServer/SetPTPCoordinateParams");
        dobot::SetPTPCoordinateParams srv;

        srv.request.xyzVelocity = 100;
        srv.request.xyzAcceleration = 100;
        srv.request.rVelocity = 100;
        srv.request.rAcceleration = 100;
        client.call(srv);
    } while (0);

    // Set PTP jump parameters
    do {
        client = n.serviceClient<dobot::SetPTPJumpParams>("/DobotServer/SetPTPJumpParams");
        dobot::SetPTPJumpParams srv;

        srv.request.jumpHeight = 20;
        srv.request.zLimit = 200;
        client.call(srv);
    } while (0);

    // Set PTP common parameters
    do {
        client = n.serviceClient<dobot::SetPTPCommonParams>("/DobotServer/SetPTPCommonParams");
        dobot::SetPTPCommonParams srv;

        srv.request.velocityRatio = 50;
        srv.request.accelerationRatio = 50;
        client.call(srv);
    } while (0);

    //设置机械臂方向
    client = n.serviceClient<dobot::SetArmOrientation>("/DobotServer/SetArmOrientation");
    dobot::SetArmOrientation srv_arm;
    srv_arm.request.tagArmOrientation = 0;
    srv_arm.request.isQueued =1;
    client.call(srv_arm);

    //运行到抓取点位PTP的Client设置
    clientPTP = n.serviceClient<dobot::SetPTPCmd>("/DobotServer/SetPTPCmd");
    //开启setIO的Client设置
    clientIO = n.serviceClient<dobot::SetIODO>("/DobotServer/SetIODO");
    //开启setIO的Client设置
    clientWait = n.serviceClient<dobot::SetWAITCmd>("/DobotServer/SetWAITCmd");
    //订阅move_base操作服务器
    actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> ac("move_base",true);

    //设置我们要机器人走的点。
    geometry_msgs::Point point;
    geometry_msgs::Quaternion quaternion;
    
    //导航部分
    point.x = 1.193;
    point.y = 1.0313;
    point.z = 0;
    quaternion.x = 0.000;
    quaternion.y = 0.000;
    quaternion.z = 0.6601;
    quaternion.w = 0.75115;

    
    navigation(ac, point, quaternion);
    cout<<"--------------"<<endl;

    //运行点位PTP
    setPTP(1 ,162.12 ,37.83 ,164.7754 ,-88.9264 , clientPTP);

    //运行点位PTP
    setPTP(1 ,268.4478 ,155.9709 ,213.0469 ,28.1389 , clientPTP);
    sleep(2);
    //打开IO
    setIO(19, 0, 1, clientIO);
    setWAIT(1000,clientWait);
    setPTP(1 ,268.4478 ,155.9709 ,124.8461 ,28.1389 , clientPTP);
    setPTP(1 ,268.4478 ,-143.1889 ,124.8461 ,28.1389 , clientPTP);
    setPTP(1 ,268.4478 ,-143.1889 ,213.0469 ,28.1389 , clientPTP);
    setPTP(1 ,268.4478 ,155.9709 ,213.0469 ,28.1389 , clientPTP);
    //关闭IO
    setIO(19, 1, 1, clientIO);
    setWAIT(1000,clientWait);
    setIO(19, 0, 1, clientIO);
    setWAIT(1000,clientWait);
    setIO(19, 1, 1, clientIO);
    setWAIT(1000,clientWait);
    //运行点位PTP
    setPTP(1 ,162.12 ,37.83 ,164.7754 ,-88.9264 , clientPTP);
    sleep(20);

    //导航部分
    point.x = 0.2566;
    point.y = 0.4891;
    point.z = 0;
    quaternion.x = 0.000;
    quaternion.y = 0.000;
    quaternion.z = -0.7284;
    quaternion.w = 0.68507;

    navigation(ac, point, quaternion);    
    cout<<"--------------"<<endl;

    return 0;
    
            
}