main.cpp

Go to the documentation of this file.

#include <pen_gripper/pen_gripper.h>
//#include "pen_gripper.cpp"

int main(int argc, char** argv) {

        ros::init(argc, argv, "pen_gripper");

        // node handle
        ros::NodeHandle n;

        // pen gripper object
        PenGripper p(n);

        // to avoid tf errors
        ros::spinOnce();
        ros::Duration wait(3, 0);
        wait.sleep();

        // communication server with the world for the pen gripper
        ros::ServiceServer instr_srv = n.advertiseService(
                        "/alubsc/node_instr/pen_gripper", &PenGripper::handle_req, &p);

        ROS_INFO("The pen gripper is ready to serve...");
        p.status.request.message = "The pen gripper is ready to serve...";
        p.status_clt.call(p.status);

        while (ros::ok()) {
                // active ?
                if (!p.abort) {
                        p.take_start_position();
                        ros::spinOnce();
//                      p.grip_pen();

                        p.success=true;
//                      if (p.success) {
                                p.status.request.message = "pen gripping has finished";
                                p.status_clt.call(p.status);
                                ROS_INFO("pen gripping has finished");
//                      }
//                      if (!p.success) {
//                              ROS_INFO("there was an error during the process");
//                              p.status.request.message =
//                                              "there was an error during the process";
//                              p.status_clt.call(p.status);
//                      }
                        p.reset();

                }
                ros::spinOnce();
        }
        return 0;

}