Add full support for gripper

RosAria.cpp

@@ -83,6 +83,17 @@ class RosAriaNode
     bool stop_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
     bool start_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
 
+    //Gripper
+    ros::ServiceServer gripper_open_srv;
+    ros::ServiceServer gripper_close_srv;
+    ros::ServiceServer gripper_up_srv;
+    ros::ServiceServer gripper_down_srv;
+
+    bool gripper_open_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
+    bool gripper_close_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
+    bool gripper_up_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
+    bool gripper_down_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
+
     ros::Time veltime;
 
     std::string serial_port;
@@ -348,6 +359,12 @@ RosAriaNode::RosAriaNode(ros::NodeHandle nh) :
     enable_srv = n.advertiseService("enable_motors", &RosAriaNode::enable_motors_cb, this);
     disable_srv = n.advertiseService("disable_motors", &RosAriaNode::disable_motors_cb, this);
 
+    //Damian Gripper
+    gripper_open_srv = n.advertiseService("gripper_open", &RosAriaNode::gripper_open_cb, this);
+    gripper_close_srv = n.advertiseService("gripper_close", &RosAriaNode::gripper_close_cb, this);
+    gripper_up_srv = n.advertiseService("gripper_up", &RosAriaNode::gripper_up_cb, this);
+    gripper_down_srv = n.advertiseService("gripper_down", &RosAriaNode::gripper_down_cb, this);
+
     //Damian usługi stopu awaryjnego
     stop_motors_srv = n.advertiseService("stop_motors", &RosAriaNode::stop_motors_cb, this);
     start_motors_srv = n.advertiseService("start_motors", &RosAriaNode::start_motors_cb, this);
@@ -355,7 +372,7 @@ RosAriaNode::RosAriaNode(ros::NodeHandle nh) :
     stop_motors_msg.data=false;
     veltime = ros::Time::now();
 
-  gripper->gripOpen();
+
 }
 
 RosAriaNode::~RosAriaNode()
@@ -376,6 +393,7 @@ int RosAriaNode::Setup()
     // called once per instance, and these objects need to persist until the process terminates.
 
     robot = new ArRobot();
+    gripper = new ArGripper(robot);
     ArArgumentBuilder *args = new ArArgumentBuilder(); //  never freed
     ArArgumentParser *argparser = new ArArgumentParser(args); // Warning never freed
     argparser->loadDefaultArguments(); // adds any arguments given in /etc/Aria.args.  Useful on robots with unusual serial port or baud rate (e.g. pioneer lx)
@@ -506,7 +524,6 @@ int RosAriaNode::Setup()
 
     // Run ArRobot background processing thread
     robot->runAsync(true);
-
     return 0;
 }
 
@@ -749,8 +766,44 @@ bool RosAriaNode::stop_motors_cb(std_srvs::Empty::Request& request, std_srvs::Em
     robot->unlock();
     return true;
 }
-void
+bool  RosAriaNode::gripper_open_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
-RosAriaNode::cmdvel_cb( const geometry_msgs::TwistConstPtr &msg)
+{
+    robot->lock();
+    gripper->gripOpen();
+    ROS_INFO("RosAria: Gripper opening.");
+    robot->unlock();
+    return true;
+}
+
+bool  RosAriaNode::gripper_close_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
+{
+    robot->lock();
+    gripper->gripClose();
+    ROS_INFO("RosAria: Gripper closing.");
+    robot->unlock();
+    return true;
+}
+
+bool  RosAriaNode::gripper_up_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
+{
+    robot->lock();
+    gripper->liftUp();
+    ROS_INFO("RosAria: Gripper moving up");
+    robot->unlock();
+    return true;
+}
+
+bool  RosAriaNode::gripper_down_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
+{
+    robot->lock();
+    gripper->liftDown();
+    ROS_INFO("RosAria: Gripper moving down.");
+    robot->unlock();
+    return true;
+}
+
+
+void RosAriaNode::cmdvel_cb( const geometry_msgs::TwistConstPtr &msg)
 {
     veltime = ros::Time::now();
     watchdog= ros::Time::now();