#include <ros/ros.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 "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"

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

/dobot_M1
    ros::ServiceClient client;

    // 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 end effector parameters
    client = n.serviceClient<dobot::SetEndEffectorParams>("/DobotServer/SetEndEffectorParams");
    dobot::SetEndEffectorParams srv5;
    srv5.request.xBias = 70;
    srv5.request.yBias = 0;
    srv5.request.zBias = 0;
    client.call(srv5);

    // 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::SetPTPCmd>("/DobotServer/SetPTPCmd");
    dobot::SetPTPCmd srv;
/dobot_M1

  //订阅move_base操作服务器
  actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> ac("move_base",true);

  //设置我们要机器人走的几个点。
    geometry_msgs::Point point;
    geometry_msgs::Quaternion quaternion;
    geometry_msgs::Pose pose_list1;
    geometry_msgs::Pose pose_list2;
    
    point.x = 2.829;
    point.y = -3.799;
    point.z = 0.000;
    quaternion.x = 0.000;
    quaternion.y = 0.000;
    quaternion.z = 0.010;
    quaternion.w = 0.999;
    pose_list1.position = point;
    pose_list1.orientation = quaternion;
    
    point.x = 6.866;
    point.y = -3.650;
    point.z = 0.000;
    quaternion.x = 0.000;
    quaternion.y = 0.000;
    quaternion.z = -0.044;
    quaternion.w = 0.999;
    pose_list2.position = point;
    pose_list2.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");
    return 1;
  }
  ROS_INFO("Connected to move base server");
  ROS_INFO("Starting navigation test");
1111111111

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

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

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

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

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

    //等1分钟到达那里
    bool finished_within_time = ac.waitForResult(ros::Duration(60));

    //如果我们没有及时赶到那里，就会中止目标
    if(!finished_within_time)
    {
        ac.cancelGoal();
        ROS_INFO("Timed out achieving goal");
    }
    else
    {
        //We made it!
        if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
        {
            ROS_INFO("Goal succeeded!");
            srv.request.ptpMode = 1;
            srv.request.x = 200;
            srv.request.y = 0;
            srv.request.z = 100;
            srv.request.r = 0;
            client.call(srv);
            if (srv.response.result == 0)
        {
                //break;
            }
        sleep(5);
            srv.request.ptpMode = 1;
            srv.request.x = 400;
            srv.request.y = 0;
            srv.request.z = 80;
            srv.request.r = 0;
            client.call(srv);
            if (srv.response.result == 0)
        {
                //break;
            }
        sleep(5);
            srv.request.ptpMode = 1;
            srv.request.x = 200;
            srv.request.y = 0;
            srv.request.z = 100;
            srv.request.r = 0;
            client.call(srv);
            if (srv.response.result == 0)
        {
                //break;
            }
        sleep(5);
            
        }
        else
        {
            ROS_INFO("The base failed for some reason");
        }
    }

22222222222

     //初始化航点目标
     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_list2;

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

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

    //如果我们没有及时赶到那里，就会中止目标
    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!");
            srv.request.ptpMode = 1;
            srv.request.x = 200;
            srv.request.y = 0;
            srv.request.z = 100;
            srv.request.r = 0;
            client.call(srv);
            if (srv.response.result == 0)
        {
                //break;
            }
        sleep(5);
            srv.request.ptpMode = 1;
            srv.request.x = 400;
            srv.request.y = 0;
            srv.request.z = 80;
            srv.request.r = 0;
            client.call(srv);
            if (srv.response.result == 0)
        {
                //break;
            }
        sleep(5);
            srv.request.ptpMode = 1;
            srv.request.x = 200;
            srv.request.y = 0;
            srv.request.z = 100;
            srv.request.r = 0;
            client.call(srv);
            if (srv.response.result == 0)
        {
                //break;
            }
        sleep(5);

            
        }
        else
        {
            ROS_INFO("The base failed for some reason");
        }
    }
 
  ros::spin();
  ROS_INFO("move_base_square.cpp end...");
  return 0;
}