diff --git a/src/ttt_executor_node.cpp b/src/ttt_executor_node.cpp index c8635e4d8d6dab37988f9afa6d7be8401e376b9e..1211255efa4a132cfd5a14adb3d150ebcfb6d26d 100644 --- a/src/ttt_executor_node.cpp +++ b/src/ttt_executor_node.cpp @@ -22,6 +22,7 @@ int cellSize; bool isObjectPicked = false; bool isGoalReceived = false; int objType, goalCell; +int moveCount = 0; geometry_msgs::Pose stackPose; //Position and orientation of the place where objects are stacked geometry_msgs::Pose c1Pose; //Position and orientation of the 1st cell of 3x3 grid for tictactoe geometry_msgs::Pose goalPose; @@ -54,6 +55,11 @@ geometry_msgs::Pose vector2Pose(std::vector<double> _vtr){ //Location of Cell 1 is known //Going from cell 1 to cell 2 will be 10cm along the x-axis //Going from cell 1 to cell 4 will be 10cm along the y-axis +void changeStackPose(){ + stackPose.position.x += 4*cellSize; + stackPose.position.y += moveCount*cellSize; +} + void goalCell_CB(const std_msgs::Int16::ConstPtr& msg){ goalCell = msg->data; @@ -64,7 +70,9 @@ void goalCell_CB(const std_msgs::Int16::ConstPtr& msg){ c1Pose.position.x += x_trans*cellSize; c1Pose.position.y += y_trans*cellSize; + changeStackPose(); isGoalReceived = true; + } @@ -73,27 +81,22 @@ int main(int argc, char** argv){ ros::NodeHandle nh; - - if(!nh.getParam("stack_pose", stackPose_Vtr)){ - ROS_ERROR("[%s]: %s", ros::this_node::getName().c_str(), "Placed where objects are stacked is not specified"); - return -1; + if(!nh.getParam("cell_size", cellSize)){ + cellSize = 0.1; //default value for cell size is 10cm } - else - stackPose = vector2Pose(stackPose_Vtr); - if(!nh.getParam("c1_pose", c1Pose_Vtr)){ ROS_ERROR("[%s]: %s", ros::this_node::getName().c_str(), "Location of 1st cell is not specified"); return -1; } - else + else{ c1Pose = vector2Pose(c1Pose_Vtr); - - if(!nh.getParam("cell_size", cellSize)){ - cellSize = 0.1; //default value for cell size is 10cm + stackPose = c1Pose; } + + ros::Subscriber cell_sub = nh.subscribe("target_cell", 10, goalCell_CB); //WAM SERVICES @@ -119,7 +122,7 @@ int main(int argc, char** argv){ //pickup stage - go to stack place if(!isObjectPicked){ - srv.request.pose = goalPose; + srv.request.pose = stackPose; srv.request.vel = 0.5; srv.request.acc = 0.5; if(moveArm_client.call(srv)){