BlockManipulationAction() :
block_detection_action_("block_detection", true),
interactive_manipulation_action_("interactive_manipulation", true),
pick_and_place_action_("pick_and_place", true),
reset_arm_action_("reset_arm", true)
{
// Load parameters
nh_.param<std::string>("/block_manipulation_action_demo/arm_link", arm_link, "/base_link");
nh_.param<double>("/block_manipulation_action_demo/gripper_open", gripper_open, 0.042);
nh_.param<double>("/block_manipulation_action_demo/gripper_closed", gripper_closed, 0.024);
nh_.param<double>("/block_manipulation_action_demo/z_up", z_up, 0.12);
nh_.param<double>("/block_manipulation_action_demo/table_height", z_down, 0.01);
nh_.param<double>("/block_manipulation_action_demo/block_size", block_size, 0.03);
nh_.param<bool>("once", once, false);
ROS_INFO("Block size %f", block_size);
ROS_INFO("Table height %f", z_down);
// Initialize goals
block_detection_goal_.frame = arm_link;
block_detection_goal_.table_height = z_down;
block_detection_goal_.block_size = block_size;
pick_and_place_goal_.frame = arm_link;
pick_and_place_goal_.z_up = z_up;
pick_and_place_goal_.gripper_open = gripper_open;
pick_and_place_goal_.gripper_closed = gripper_closed;
pick_and_place_goal_.topic = pick_and_place_topic;
interactive_manipulation_goal_.block_size = block_size;
interactive_manipulation_goal_.frame = arm_link;
ROS_INFO("Finished initializing, waiting for servers...");
block_detection_action_.waitForServer();
ROS_INFO("Found block detection server.");
interactive_manipulation_action_.waitForServer();
ROS_INFO("Found interactive manipulation.");
pick_and_place_action_.waitForServer();
ROS_INFO("Found pick and place server.");
ROS_INFO("Found servers.");
reset_arm_action_.sendGoal(simple_arm_actions::ResetArmGoal());
ROS_INFO("Reseted arm action.");
detectBlocks();
}
// ****** MAIN FUNCTION IN LOOP ******* //
void ImageProcessor::doStuff() {
//std::cout<<"doing stuff"<<std::endl;
clearCameraCache();
clock_t start = clock(); // for debug
vid_cap.retrieve(frame); // get a new frame from camera
cv::resize(frame,frame,cv::Size(0,0), 1, 1, cv::INTER_LINEAR);
detectWall(frame);
detectBlocks(frame);
if(getFoundCube()){
//std::cout << "found cube!" << std::endl;
std::cout << "dist: " <<getNearestCubeDist()<< " angle: "<<getNearestCubeAngle()<<std::endl;
std::string s1 = std::to_string(getNearestCubeDist());
std::string s2 = std::to_string(getNearestCubeAngle());
std::string send = s1+","+s2;
const char *out = send.c_str();
std::cout<<s1+","+s2<<std::endl;
write(fd, out, 20);
}
else{
std::cout << "no cube :(" << std::endl;
std::string send = "no";
const char *out = send.c_str();
write(fd, out, 2);
}
// cv::imshow("frame", frame);
/*
if(detectingPurpleLine == 1) {
detectPurpleLine(frame);
}
*/
//cv::namedWindow("frame", 1);
//cv::imshow("frame", frame);
//cv::namedWindow("local_map", 2);
//cv::imshow("local_map", local_map.cvtImage());
// for debug
clock_t end = clock();
cpu_time = ((double) (end - start)) / CLOCKS_PER_SEC;
}
void finish(const actionlib::SimpleClientGoalState& state, const PickAndPlaceResultConstPtr& result)
{
ROS_INFO("Got pick and place callback. Finished!");
if (state == actionlib::SimpleClientGoalState::SUCCEEDED)
ROS_INFO("Succeeded!");
else
ROS_INFO("Did not succeed! %s", state.toString().c_str());
reset_arm_action_.sendGoal(simple_arm_actions::ResetArmGoal());
if (once)
ros::shutdown();
else
detectBlocks();
}
int main()
{
detectBlocks();
grabBlocks();
}