kinematics/src/ros.cpp at dev · Rho-Bot/kinematics · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
#include "ros.hpp"
#include "config.hpp"


void normalize(VEC6 &q){
    for (int i = 0; i < q.size(); i++)
    {
        double value = q(i);
        if(value > 2*M_PI) value -=2*M_PI;
        if(value < -2*M_PI) value +=2*M_PI;
        q(i) = value;
    }
}

/**
 * @brief Different implementations of the publisher depending on the gripper type
*/
#if (!USE_GRIPPER || !SIMULATION)
void publishJoints(ros::Publisher &pub, VEC6 &qJ, VEC3 &qG) {
    std_msgs::Float64MultiArray msg;
    // 6 joints only
    msg.data.resize(qJ.size());
    for (int i = 0; i < qJ.size(); i++)
    {
        msg.data[i] = qJ(i,0);
    }
    pub.publish(msg);
}
#elif SOFT_GRIPPER
void publishJoints(ros::Publisher &pub, VEC6 &qJ, VEC3 &qG) {
    std_msgs::Float64MultiArray msg;
    // 6 joints + 2 gripper
    msg.data.resize(qJ.size() + qG.size() - 1);
    for (int i = 0; i < qJ.size(); i++)
    {
        msg.data[i] = qJ(i);
    }

    msg.data[6] = qG(0,0);
    msg.data[7] = qG(1,0);

    pub.publish(msg);
}
#else
void publishJoints(ros::Publisher &pub, VEC6 &qJ, VEC3 &qG) {
    std_msgs::Float64MultiArray msg;
    // 6 joints + 3 gripper
    msg.data.resize(qJ.size() + qG.size());
    for (int i = 0; i < qJ.size(); i++)
    {
        msg.data[i] = qJ(i,0);
    }
    msg.data[6] = qG(0,0);
    msg.data[7] = qG(1,0);
    msg.data[8] = qG(2,0);
    pub.publish(msg);
}
#endif

/*
CONFIGURATION WHEN READING FROM JOINTS STATE TOPIC
    - elbow_joint
    - hand_1_joint
    - hand_2_joint
    - hand_3_joint
    - shoulder_lift_joint
    - shoulder_pan_joint
    - wrist_1_joint
    - wrist_2_joint
    - wrist_3_joint
*/

/**
 * @brief Different implementations of the publisher depending on the gripper type
*/
#if (!USE_GRIPPER || !SIMULATION)
VEC9 readJoints() {
    sensor_msgs::JointState::ConstPtr msg = ros::topic::waitForMessage<sensor_msgs::JointState>(joint_state_subscriber_topic);
    VEC9 data_read;
    data_read.resize(9);

    data_read(2,0) = msg->position[0];
    data_read(1,0) = msg->position[1];
    data_read(0,0) = msg->position[2];
    for (int i = 3; i < data_read.size(); i++)
    {
        data_read(i,0) = msg->position[i];
    }
    return data_read;
}
#elif SOFT_GRIPPER
VEC9 readJoints() {
    sensor_msgs::JointState::ConstPtr msg = ros::topic::waitForMessage<sensor_msgs::JointState>(joint_state_subscriber_topic);
    VEC9 data_read;
    data_read.resize(9);

    data_read(2) = msg->position[0];
    data_read(6) = msg->position[1];
    data_read(7) = msg->position[2];
    data_read(1) = msg->position[3];
    data_read(0) = msg->position[4];
    data_read(3) = msg->position[5];
    data_read(4) = msg->position[6];
    data_read(5) = msg->position[7];
    return data_read;
}
#else
VEC9 readJoints() {
    sensor_msgs::JointState::ConstPtr msg = ros::topic::waitForMessage<sensor_msgs::JointState>(joint_state_subscriber_topic);
    VEC9 data_read;
    data_read.resize(9);

    data_read(2) = msg->position[0];
    data_read(6) = msg->position[1];
    data_read(7) = msg->position[2];
    data_read(8) = msg->position[3];
    data_read(1) = msg->position[4];
    data_read(0) = msg->position[5];
    data_read(3) = msg->position[6];
    data_read(4) = msg->position[7];
    data_read(5) = msg->position[8];
    return data_read;
}
#endif