Giang

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)){