rviz_gripper.cpp

Go to the documentation of this file.

#include <ros/ros.h>
#include <string>
#include <std_msgs/Float32.h>
#include <std_msgs/Bool.h>
#include <sensor_msgs/JointState.h>
//#include <rvix> en dan nog wat dingen

using namespace std;

void rvizUpdater(const std_msgs::Float32::ConstPtr&);
void shutdown(const std_msgs::Bool::ConstPtr&);
float opens; 
const int freq = 250; 
int main(int argc, char **argv){
        
        ros::init(argc, argv, "rviz_gripper");
        ros::NodeHandle n;
        ros::Subscriber subscriber = n.subscribe("gripper_state", 1000, rvizUpdater);
        ros::Subscriber sd = n.subscribe("shutdown", 1, shutdown);
        ros::spinOnce();
        ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("jstates", 1);
        ros::Rate loop_rate(freq);
        ros::spinOnce();

        const double bmdll_max = 0.85;
        const double mdlltp_max = 0.80;
        const double bmdll_start = -0.96;
        const double mdlltp_start = -0.3;
        double bm_angle, mt_angle, step_size = 0.004, cf = 1.5, motor_state = 0;
        double angle_step = 1/(bmdll_max + mdlltp_max*cf);

        sensor_msgs::JointState joint_state;

        while (ros::ok())
        {       
                if((opens == 1 && motor_state < (1-step_size)) || (opens == 0 && motor_state > step_size)){
                        if(opens == 0){
                                        motor_state = motor_state - step_size;
                                
                        }else{
                                        motor_state = motor_state + step_size;
                                
                        }
                        if((motor_state/angle_step) < bmdll_max){
                                mt_angle = mdlltp_start;
                                bm_angle = motor_state/angle_step+bmdll_start;
                        }else{
                                bm_angle = bmdll_start+bmdll_max;
                                mt_angle = (motor_state/angle_step)-bmdll_max+mdlltp_start;
                        }
                        joint_state.header.stamp = ros::Time::now();
                        joint_state.name.resize(2);
                        joint_state.position.resize(2);

                        joint_state.name[0]="rght_base2mdll";
                        joint_state.position[0] = bm_angle;

                        joint_state.name[1]="rght_mdll2tip";
                        joint_state.position[1] = mt_angle;
                        joint_pub.publish(joint_state);
                }
                ros::spinOnce();
        loop_rate.sleep();
                
        }

        return 0;
}

void rvizUpdater(const std_msgs::Float32::ConstPtr& msg){
        opens = msg->data;
        ROS_INFO("open or close: %f", msg->data);
}

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