#include <rei_robot_cruise/cruise.h>
#include "oryxbot_cruise_ex/oryxbot_task.h"
#include <geometry_msgs/Twist.h>
// 实例化对象
ORYXTask oryxTask;
geometry_msgs::Twist workStationAbovePose_;
geometry_msgs::Twist boxAbovePose_;
std::vector<geometry_msgs::Twist> boxPose_;
std::vector<geometry_msgs::Twist> workStationPoses_;
std::vector<int> boxState_(2,0);
bool StartCallback0(){
return oryxTask.ResetArm(boxAbovePose_.linear.x, boxAbovePose_.linear.y,
boxAbovePose_.linear.z);
}
bool chargeCallback(int value){
int taskStep = value;
int taskState;
std::vector<int> arIds;
int placeTime = 0;
ros::Rate loop(1);
while(ros::ok()){
if(rei_cruise::RobotCruise::getInstance().GetTaskState() == rei_cruise::PAUSE){
rei_cruise::RobotCruise::getInstance().SetCallBackFuncStep(taskStep);
break;
}else if(rei_cruise::RobotCruise::getInstance().GetTaskState() == rei_cruise::STOP){
break;
}
taskState = rei_cruise::RobotCruise::getInstance().GetTaskState();
if(taskState == rei_cruise::TaskState::ACTIVE){
switch (taskStep)
{
case 0:
if(oryxTask.ArAdjust(0))
taskStep = 1;
else
rei_cruise::RobotCruise::getInstance().Pause();
break;
case 1:
if(oryxTask.RelativeMove(0.12,0,0,"odom")){
ros::Time start = ros::Time::now();
while (ros::Time::now() - start < ros::Duration(2.0) && ros::ok()) {
if(rei_cruise::RobotCruise::getInstance().GetTaskState() == rei_cruise::PAUSE) {
rei_cruise::RobotCruise::getInstance().SetCallBackFuncStep(taskStep);
return true;
}
else if(rei_cruise::RobotCruise::getInstance().GetTaskState() == rei_cruise::STOP) {
return true;
}
ros::Duration(0.1).sleep();
}
taskStep = 2;
}
else
rei_cruise::RobotCruise::getInstance().Pause();
break;
case 2:
if(oryxTask.RelativeMove(-0.12,0,0,"odom")){
return true;
}else return false;
break;
}
}else if(taskState == rei_cruise::TaskState::STOP){
break;
}
loop.sleep();
}
return true;
}
bool StartCallback1(){
return oryxTask.ResetArm(boxAbovePose_.linear.x, boxAbovePose_.linear.y,
boxAbovePose_.linear.z);
}
bool renwua_aplaceCallback(int value){
int taskStep = value;
int taskState;
std::vector<int> arIds;
int placeTime = 0;
ros::Rate loop(1);
while(ros::ok()){
if(rei_cruise::RobotCruise::getInstance().GetTaskState() == rei_cruise::PAUSE){
rei_cruise::RobotCruise::getInstance().SetCallBackFuncStep(taskStep);
break;
}else if(rei_cruise::RobotCruise::getInstance().GetTaskState() == rei_cruise::STOP){
break;
}
taskState = rei_cruise::RobotCruise::getInstance().GetTaskState();
if(taskState == rei_cruise::TaskState::ACTIVE){
switch (taskStep)
{
case 0:
if(oryxTask.ArAdjust(1))
taskStep = 1;
else
rei_cruise::RobotCruise::getInstance().Pause();
break;
case 1:
if(oryxTask.RelativeMove(0.12,0,0,"odom")){
taskStep = 2;
placeTime = 0;
}
else
rei_cruise::RobotCruise::getInstance().Pause();
break;
case 2:
if(oryxTask.GotoPosition(boxAbovePose_.linear.x,
boxAbovePose_.linear.y, boxAbovePose_.linear.z)){
taskStep=3;
}
else
rei_cruise::RobotCruise::getInstance().Pause();
break;
case 3:
if(oryxTask.GetArIDs(arIds))
taskStep=4;
else
if(placeTime>0)
taskStep=5;
else
rei_cruise::RobotCruise::getInstance().Pause();
break;
case 4:
if(oryxTask.PickPlace(4,70, -160, 10)){
taskStep=5;
}else
rei_cruise::RobotCruise::getInstance().Pause();
break;
case 5:
if(oryxTask.ResetArm(boxAbovePose_.linear.x,
boxAbovePose_.linear.y, boxAbovePose_.linear.z)){
taskStep=6;
}else
rei_cruise::RobotCruise::getInstance().Pause();
break;
case 6:
if(oryxTask.RelativeMove(-0.12,0,0,"odom")){
return true;
}else return false;
break;
}
}else if(taskState == rei_cruise::TaskState::STOP){
break;
}
loop.sleep();
}
return true;
}
bool StartCallback2(){
return oryxTask.ResetArm(boxAbovePose_.linear.x, boxAbovePose_.linear.y,
boxAbovePose_.linear.z);
}
bool renwua_bplaceCallback(int value){
int taskStep = value;
int taskState;
std::vector<int> arIds;
double p=3.141592653589793238462643383279502884197169399375105820974944923078164;
int placeTime = 0;
ros::Rate loop(1);
while(ros::ok()){
if(rei_cruise::RobotCruise::getInstance().GetTaskState() == rei_cruise::PAUSE){
rei_cruise::RobotCruise::getInstance().SetCallBackFuncStep(taskStep);
break;
}else if(rei_cruise::RobotCruise::getInstance().GetTaskState() == rei_cruise::STOP){
break;
}
taskState = rei_cruise::RobotCruise::getInstance().GetTaskState();
if(taskState == rei_cruise::TaskState::ACTIVE){
switch (taskStep)
{
case 0:
if(oryxTask.ArAdjust(1))
taskStep = 1;
else
rei_cruise::RobotCruise::getInstance().Pause();
break;
case 1:
if(oryxTask.RelativeMove(0.12,0,0,"odom")){
taskStep = 2;
placeTime = 0;
}
else
rei_cruise::RobotCruise::getInstance().Pause();
break;
case 2:
if(oryxTask.GotoPosition(boxAbovePose_.linear.x,
boxAbovePose_.linear.y, boxAbovePose_.linear.z)){
taskStep=3;
}
else
rei_cruise::RobotCruise::getInstance().Pause();
break;
case 3:
if(oryxTask.GetArIDs(arIds))
taskStep=4;
else
if(placeTime>0)
taskStep=5;
else
rei_cruise::RobotCruise::getInstance().Pause();
break;
case 4:
if(oryxTask.PickPlace(6,130, -163, 10)){
taskStep=5;
}else
rei_cruise::RobotCruise::getInstance().Pause();
break;
case 5:
if(oryxTask.ResetArm(boxAbovePose_.linear.x,
boxAbovePose_.linear.y, boxAbovePose_.linear.z)){
taskStep=6;
}else
rei_cruise::RobotCruise::getInstance().Pause();
break;
case 6:
if(oryxTask.RelativeMove(-0.12,0,0,"odom")){
return true;
}else return false;
break;
}
}else if(taskState == rei_cruise::TaskState::STOP){
break;
}
loop.sleep();
}
return true;
}
bool StartCallback3(){
return oryxTask.ResetArm(boxAbovePose_.linear.x, boxAbovePose_.linear.y,
boxAbovePose_.linear.z);
}
bool renwub_aplaceCallback(int value){
int taskStep = value;
int taskState;
std::vector<int> arIds;
int placeTime = 0;
ros::Rate loop(1);
while(ros::ok()){
if(rei_cruise::RobotCruise::getInstance().GetTaskState() == rei_cruise::PAUSE){
rei_cruise::RobotCruise::getInstance().SetCallBackFuncStep(taskStep);
break;
}else if(rei_cruise::RobotCruise::getInstance().GetTaskState() == rei_cruise::STOP){
break;
}
taskState = rei_cruise::RobotCruise::getInstance().GetTaskState();
if(taskState == rei_cruise::TaskState::ACTIVE){
switch (taskStep)
{
case 0:
if(oryxTask.ArAdjust(1))
taskStep = 1;
else
rei_cruise::RobotCruise::getInstance().Pause();
break;
case 1:
if(oryxTask.RelativeMove(0.12,0,0,"odom")){
taskStep = 2;
placeTime = 0;
}
else
rei_cruise::RobotCruise::getInstance().Pause();
break;
case 2:
if(oryxTask.GotoPosition(workStationAbovePose_.linear.x,
workStationAbovePose_.linear.y, workStationAbovePose_.linear.z)){
taskStep=3;
}
else
rei_cruise::RobotCruise::getInstance().Pause();
break;
case 3:
if(oryxTask.GetArIDs(arIds))
taskStep=4;
else
if(placeTime>0)
taskStep=5;
else
rei_cruise::RobotCruise::getInstance().Pause();
break;
case 4:
if(oryxTask.PickPlace(arIds[0], 105, 123,20))
{
taskStep=5;
}else
rei_cruise::RobotCruise::getInstance().Pause();
break;
case 5:
if(oryxTask.ResetArm(boxAbovePose_.linear.x,
boxAbovePose_.linear.y, boxAbovePose_.linear.z)){
taskStep=6;
}else
rei_cruise::RobotCruise::getInstance().Pause();
break;
case 6:
if(oryxTask.RelativeMove(-0.12,0,0,"odom")){
return true;
}else return false;
break;
}
}else if(taskState == rei_cruise::TaskState::STOP){
break;
}
loop.sleep();
}
return true;
}
bool StartCallback4(){
return oryxTask.ResetArm(boxAbovePose_.linear.x, boxAbovePose_.linear.y,
boxAbovePose_.linear.z);
}
bool renwub_bplaceCallback(int value){
int taskStep = value;
int taskState;
std::vector<int> arIds;
int placeTime = 1;
double p=3.141592653589793238462643383279502884197169399375105820974944;
ros::Rate loop(1);
while(ros::ok()){
if(rei_cruise::RobotCruise::getInstance().GetTaskState() == rei_cruise::PAUSE){
rei_cruise::RobotCruise::getInstance().SetCallBackFuncStep(taskStep);
break;
}else if(rei_cruise::RobotCruise::getInstance().GetTaskState() == rei_cruise::STOP){
break;
}
taskState = rei_cruise::RobotCruise::getInstance().GetTaskState();
if(taskState == rei_cruise::TaskState::ACTIVE){
switch (taskStep)
{
case 0:
if(oryxTask.ArAdjust(1))
taskStep = 1;
else
rei_cruise::RobotCruise::getInstance().Pause();
break;
case 1:
if(oryxTask.RelativeMove(0.12,0,0,"odom")){
taskStep = 2;
placeTime = 1;
}
else
rei_cruise::RobotCruise::getInstance().Pause();
break;
case 2:
if(oryxTask.GotoPosition(workStationAbovePose_.linear.x+20,
workStationAbovePose_.linear.y, workStationAbovePose_.linear.z)){
taskStep=3;
}
else
rei_cruise::RobotCruise::getInstance().Pause();
break;
case 3:
if(oryxTask.GetArIDs(arIds))
taskStep=4;
else
if(placeTime>0)
taskStep=5;
else
rei_cruise::RobotCruise::getInstance().Pause();
break;
case 4:
if(oryxTask.PickPlace(arIds[0], 105, 183,20))
{
taskStep=5;
}else
rei_cruise::RobotCruise::getInstance().Pause();
break;
case 5:
if(oryxTask.ResetArm(boxAbovePose_.linear.x,
boxAbovePose_.linear.y, boxAbovePose_.linear.z)){
taskStep=6;
}else
rei_cruise::RobotCruise::getInstance().Pause();
break;
case 6:
if(oryxTask.RelativeMove(-0.12,0,0,"odom")){
return true;
}else return false;
break;
}
}else if(taskState == rei_cruise::TaskState::STOP){
break;
}
loop.sleep();
}
return true;
}
int main(int argc, char** argv){
ros::init(argc, argv, "cruise_test_node");
ros::NodeHandle nh;
//ros::Publisher Vel_c;
// Vel_c = nh.advertise<geometry_msgs::Twist>("cmd_vel",10);
// geometry_msgs::Twist vel_msg;
// vel_msg.linear.x=0.4;
// vel_msg.angular.z=1.0;
if (!oryxTask.Init(nh)){
return 0;
}
// 仿真小车白色盒子观测位置
boxAbovePose_.linear.x = 105.0;
boxAbovePose_.linear.y = 120.0;
boxAbovePose_.linear.z = 85.0;
boxAbovePose_.angular.z = 0.0;
if(!oryxTask.ResetArm(boxAbovePose_.linear.x,
boxAbovePose_.linear.y, 60, 0)){
return 0;
}
// 模拟加工台观测位置
workStationAbovePose_.linear.x = 70.0;
workStationAbovePose_.linear.y = -160.0;
workStationAbovePose_.linear.z = 80.0;
workStationAbovePose_.angular.z = 0.0;
//仿真小车两个盒子位置
geometry_msgs::Twist tmpPose;
tmpPose.linear.x = 105;
tmpPose.linear.y = 123;
tmpPose.linear.z = 30;
tmpPose.angular.z = 0.0;
boxPose_.push_back(tmpPose);
tmpPose.linear.x = 105;
tmpPose.linear.y = 183;
tmpPose.linear.z = 30;
tmpPose.angular.z = 0.0;
boxPose_.push_back(tmpPose);
//模拟加工台的放置位置
tmpPose.linear.x = 65;
tmpPose.linear.y = -160;
tmpPose.linear.z = 30.0;
tmpPose.angular.z = 0.0;
workStationPoses_.push_back(tmpPose);
tmpPose.linear.x = 125;
tmpPose.linear.y = -160;
tmpPose.linear.z = 30.0;
tmpPose.angular.z = 0.0;
workStationPoses_.push_back(tmpPose);
rei_cruise::RobotCruise& cruise = rei_cruise::RobotCruise::getInstance();
if(!cruise.Init(nh)) return 0;
cruise.AddStartCallBack(StartCallback1);
if(cruise.AddCallBack("renwua_a_place", renwua_aplaceCallback))
{
ROS_INFO("set callback renwua_aplaceCallback success");
}
cruise.AddStartCallBack(StartCallback2);
if(cruise.AddCallBack("renwua_b_place", renwua_bplaceCallback))
{
ROS_INFO("set callback renwua_bplaceCallback success");
}
cruise.AddStartCallBack(StartCallback3);
if(cruise.AddCallBack("renwub_a_place", renwub_aplaceCallback))
{
ROS_INFO("set callback WorkStationPickCallback success");
}
cruise.AddStartCallBack(StartCallback4);
if(cruise.AddCallBack("renwub_b_place", renwub_bplaceCallback))
{
ROS_INFO("set callback WorkStationPickCallback success");
}
if(cruise.AddCallBack("charge", chargeCallback))
{
ROS_INFO("set callback WorkStationPickCallback success");
}
cruise.AddStartCallBack(StartCallback0);
cruise.RunNavTask();
return 0;
}clear_old_goal: True #bool型,是否清除之前存储的目标点
goals_name: ["home","work5", "work2","work3","work1","work4","charge"] #string型,目标点列表,以","分隔
home:
frame_id: map #string型,目标点基坐标
x: 0.00
y: 0.00
theta: 0.0
work5:
frame_id: map #string型,目标点基坐标
x: 0.64
y: 1.15
theta: 0
work2:
frame_id: map #string型,目标点基坐标
x: 2.16
y: 1.11
theta: 0
work3:
frame_id: map #string型,目标点基坐标
x: 2.16
y: 0.12
theta: 0
work1:
frame_id: map #string型,目标点基坐标
x: 2.16
y: 2.11
theta: -0.09
work4:
frame_id: map #string型,目标点基坐标
x: 0.64
y: 2.15
theta: 0
charge:
frame_id: map #string型,目标点基坐标
x: 0.35
y: 2.10
theta: 1.57
nav_order: ["work3","work2","home","work1","work5","charge","home"] #string型,任务顺序列表,以空格分隔
callback:
["renwua_a_place","renwua_b_place","delauft","renwub_b_place","renwub_a_place","charge","delauft"] #string型,对应任务顺序列表中每个任务点的回调任务,以空格分隔
loop_time: 1 #string型,任务循环次数。怎么修改代码可以利用pid二次定位在home点可以精准定位