gripper_controller.cpp

Go to the documentation of this file.

#include <ros/ros.h>
#include <cmath>
#include <string>
#include <std_msgs/Float32.h>
#include <std_msgs/Bool.h>
#include <std_msgs/String.h>
#include <Grijper/command.h>

using namespace std;

void setForce(const std_msgs::Float32::ConstPtr&);
void gripperPhidget(const std_msgs::Float32::ConstPtr&);
void gripperCommand(const std_msgs::String::ConstPtr&);
void shutdown(const std_msgs::Bool::ConstPtr&);
ros::Publisher control;  
bool close_gripper(float);
bool open_gripper(float);
bool gripper_open = true;  
bool force_open = false;  
float last_distance = 0;  
float force = 0.35;  
int main(int argc, char **argv){
        
        ros::init(argc, argv, "gripper_controller");
        ros::NodeHandle n;
        ros::Subscriber force_sub = n.subscribe("gripper_force", 1, setForce);
        ros::Subscriber command_sub = n.subscribe("command", 1000, gripperCommand);
        ros::Subscriber phidget_sub = n.subscribe("phidget_value", 1, gripperPhidget);
        ros::Subscriber sd = n.subscribe("shutdown", 1, shutdown);
        control = n.advertise<Grijper::command>("gripper_control", 1);
        ros::spin();

        return 0;
}

void gripperPhidget(const std_msgs::Float32::ConstPtr& msg){

        float distance = msg->data;
        
        if(abs(distance - last_distance) > 0.5){

                last_distance = distance;

                ROS_INFO("received distance: %f", distance);
                if(close_gripper(distance) && gripper_open && !force_open){
                        ROS_INFO("Closing gripper");
                        Grijper::command msg;
                        msg.cmd = "close";
                        msg.force = force;
                        gripper_open = false;
                        control.publish(msg);
                }
                if(open_gripper(distance) && !gripper_open){
                        ROS_INFO("Opening gripper");
                        Grijper::command msg;
                        msg.cmd = "open";
                        msg.force = force;
                        gripper_open = true;
                        control.publish(msg);
                }

                if(open_gripper(distance) && force_open){
                        ROS_INFO("Resetting force_open");
                        force_open = false;
                }
        }
}

void setForce(const std_msgs::Float32::ConstPtr& msg){
        force = msg->data;      
        ROS_INFO("Setting controller force to: %f\n", force);
}

void gripperCommand(const std_msgs::String::ConstPtr& msg){
        string cmd = msg->data;
        Grijper::command ctl;
        if(cmd.compare("open") == 0){
                ROS_INFO("Opening gripper");
                force_open = true;
                ctl.cmd = "open";
                ctl.force = force;
                gripper_open = true;
                control.publish(ctl);
        }else if(cmd.compare("relax") == 0){
                ROS_INFO("Relaxing gripper");
                force_open = false;
                ctl.cmd = "relax";
                ctl.force = 0;
                control.publish(ctl);
        }else if(cmd.compare("close") == 0){
                ROS_INFO("Closing gripper");
                force_open = false;
                ctl.cmd = "close";
                ctl.force = force;
                gripper_open = false;
                control.publish(ctl);
        }else{
                ROS_INFO("Received invalid command: %s\n", cmd.c_str());
        }
}

bool close_gripper(float distance){
        return distance < 7 && distance != 0; //TODO dit nog bewerken afhankelijk van de sensor.
}

bool open_gripper(float distance){
        return distance > 10;
}

void shutdown(const std_msgs::Bool::ConstPtr& b){
        ros::shutdown();
}