From c1d8c7486c5d46d82b33d8b5f81488faeff5a11b Mon Sep 17 00:00:00 2001
From: Truong Giang Vo <truong.giang.vo@ipa.fraunhofer.de>
Date: Fri, 13 Oct 2017 09:17:43 +0200
Subject: [PATCH] Replace ttt_executor_node.cpp
---
src/ttt_executor_node.cpp | 27 +++++++++++++++------------
1 file changed, 15 insertions(+), 12 deletions(-)
diff --git a/src/ttt_executor_node.cpp b/src/ttt_executor_node.cpp
index c8635e4..1211255 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)){
--
GitLab