移动底盘加dobot机械臂抓取代码
#include
#include
#include
#include
#include
#include
#include
#include
#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 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 srv2;
client.call(srv2);
// Start running the command queue
client = n.serviceClient
dobot::SetQueuedCmdStartExec srv3;
client.call(srv3);
// Get device version information
client = n.serviceClient
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 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 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 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 srv;
srv.request.jumpHeight = 20;
srv.request.zLimit = 200;
client.call(srv);
} while (0);
// Set PTP common parameters
do {
client = n.serviceClient
dobot::SetPTPCommonParams srv;
srv.request.velocityRatio = 50;
srv.request.accelerationRatio = 50;
client.call(srv);
} while (0);
client = n.serviceClient
dobot::SetPTPCmd srv;
/////////////////////////////////////////////dobot_M1
//订阅move_base操作服务器
actionlib::SimpleActionClient
//设置我们要机器人走的几个点。
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;
}