Skip to content

Commit efe5055

Browse files
joseballesterjoseballester
committed
Updated version with moved services
1 parent a7362b7 commit efe5055

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

43 files changed

+243
-199
lines changed

‎CMakeLists.txt

+15-15
Original file line numberDiff line numberDiff line change
@@ -33,13 +33,6 @@ add_service_files(DIRECTORY srv FILES
3333
Info/Service_GetJoints.srv
3434
Info/Service_GetRobotAngle.srv
3535
Info/Service_Ping.srv
36-
Info/Service_SetDefaults.srv
37-
Info/Service_SetInertia.srv
38-
Info/Service_SetIOSignal.srv
39-
Info/Service_SetMaxSpeed.srv
40-
Info/Service_SetTool.srv
41-
Info/Service_SetWorkObject.srv
42-
Info/Service_SetZone.srv
4336

4437
# Motion services
4538
Motion/Service_AddToCartesianBuffer.srv
@@ -58,6 +51,13 @@ add_service_files(DIRECTORY srv FILES
5851
Motion/Service_SetMaxAcceleration.srv
5952
Motion/Service_SetMotionSupervision.srv
6053
Motion/Service_Stop.srv
54+
Motion/Service_SetDefaults.srv
55+
Motion/Service_SetInertia.srv
56+
Motion/Service_SetIOSignal.srv
57+
Motion/Service_SetMaxSpeed.srv
58+
Motion/Service_SetTool.srv
59+
Motion/Service_SetWorkObject.srv
60+
Motion/Service_SetZone.srv
6161
)
6262

6363
generate_messages(
@@ -100,14 +100,6 @@ add_executable(abb_robotnode
100100
src/services/Info/GetIK.cpp
101101
src/services/Info/GetJoints.cpp
102102
src/services/Info/GetRobotAngle.cpp
103-
src/services/Info/SetDefaults.cpp
104-
src/services/Info/SetInertia.cpp
105-
src/services/Info/SetIOSignal.cpp
106-
src/services/Info/SetMaxSpeed.cpp
107-
src/services/Info/SetTool.cpp
108-
src/services/Info/SetWorkObject.cpp
109-
src/services/Info/SetZone.cpp
110-
111103
# Motion services
112104
src/services/Motion/AddToCartesianBuffer.cpp
113105
src/services/Motion/ClearCartesianBuffer.cpp
@@ -125,6 +117,14 @@ add_executable(abb_robotnode
125117
src/services/Motion/SetMaxAcceleration.cpp
126118
src/services/Motion/SetMotionSupervision.cpp
127119
src/services/Motion/Stop.cpp
120+
src/services/Motion/SetDefaults.cpp
121+
src/services/Motion/SetInertia.cpp
122+
src/services/Motion/SetIOSignal.cpp
123+
src/services/Motion/SetMaxSpeed.cpp
124+
src/services/Motion/SetTool.cpp
125+
src/services/Motion/SetWorkObject.cpp
126+
src/services/Motion/SetZone.cpp
127+
128128
${PROTO_SRCS}
129129
${PROTO_HDRS}
130130
)

‎examples/launch/irb120.launch

+1-1
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
11
<launch>
2-
<rosparam command="load" file="$(find abb_robotnode)/param/example.yaml"/>
2+
<rosparam command="load" file="$(find abb_robotnode)/examples/param/irb120.yaml"/>
33
<node pkg="abb_robotnode" type="abb_robotnode" name="abb_robotnode1" output="screen" args="1"/>
44
</launch>

‎examples/launch/irb14000.launch

+1-1
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
<launch>
2-
<rosparam command="load" file="$(find abb_robotnode)/param/yumi.yaml"/>
2+
<rosparam command="load" file="$(find abb_robotnode)/examples/param/irb14000.yaml"/>
33
<node pkg="abb_robotnode" type="abb_robotnode" name="abb_robotnode1" output="screen" args="1"/>
44
<node pkg="abb_robotnode" type="abb_robotnode" name="abb_robotnode2" output="screen" args="2"/>
55
</launch>

‎include/abb_robotnode/Services.hpp

+20
Original file line numberDiff line numberDiff line change
@@ -59,6 +59,26 @@ int setTool(double x, double y, double z, double q0, double qx, double qy, doubl
5959
int setWorkObject(double x, double y, double z, double q0, double qx, double qy, double qz); \
6060
int setZone(int z);
6161

62+
#define SERVICE_RESPONSE_FROM_RESULT() \
63+
if(result == 1) { \
64+
res.success = true; \
65+
res.msg = "Ok."; \
66+
} else if(result == -1) { \
67+
res.success = false; \
68+
res.msg = "Wrong answer from robot."; \
69+
} else { \
70+
res.success = false; \
71+
res.msg = "No answer received."; \
72+
} \
73+
return true;
74+
75+
#define SERVICE_CHECK_EGM_OFF() \
76+
if(egmMode != EGM_OFF) { \
77+
res.success = false; \
78+
res.msg = "EGM needs to be stopped."; \
79+
return true; \
80+
}
81+
6282
// All services should be added:
6383
// - below, in the list of service callbacks and advertised services,
6484
// - .srv file in srv/ folder,

‎rapid/irb120/Motion.mod

+1-1
Original file line numberDiff line numberDiff line change
@@ -736,7 +736,7 @@ MODULE Motion
736736
\x:=egm_minmax_lin1 \y:=egm_minmax_lin1 \z:=egm_minmax_lin1
737737
\rx:=egm_minmax_rot1 \ry:=egm_minmax_rot1 \rz:=egm_minmax_rot1\LpFilter:=100\Samplerate:=4\MaxSpeedDeviation:= 1000;
738738
739-
EGMRunPose egmID1, EGM_STOP_HOLD \x \y \z\CondTime:=86400 \RampInTime:=0.05 \RampOutTime:=0.5 \PosCorrGain:=positiongain;
739+
EGMRunPose egmID1, EGM_STOP_HOLD \x \y \z \Rx \Ry \Rz \CondTime:=86400 \RampInTime:=0.05 \RampOutTime:=0.5 \PosCorrGain:=positiongain;
740740
egmSt1:=EGMGetState(egmID1);
741741
ENDIF
742742

‎rapid/irb14000/MTN_L.mod

+1-1
Original file line numberDiff line numberDiff line change
@@ -734,7 +734,7 @@ MODULE MTN_L
734734
IF nParams=2 AND params{1}=1 THEN
735735
SyncMoveOn sync1, all_tasks;
736736
ENDIF
737-
FOR i FROM 1 TO (BUFFER_POS) DO
737+
FOR i FROM 1 TO (BUFFER_POS_C) DO
738738
IF collision=0 THEN
739739
moveComplete:=FALSE;
740740
MoveL bufferTargets{i}, bufferSpeeds{i},currentZoneL,currentToolL\WObj:=currentWobjL;

‎src/RobotController.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -105,7 +105,7 @@ void RobotController::advertiseService(std::string name, auto func, int compatib
105105
if((model & compatibleModels) == model) {
106106
serviceHandlers.push_back(node->advertiseService(name, func, this));
107107
} else {
108-
ROS_INFO("%s service not initialized. Model not compatible. %d notin %d, result is %d", name.c_str(), model, compatibleModels, model & compatibleModels);
108+
// ROS_INFO("%s service not initialized. Model not compatible. %d notin %d, result is %d", name.c_str(), model, compatibleModels, model & compatibleModels);
109109
}
110110
}
111111

‎src/services/Info/SetMaxSpeed.cpp

-38
This file was deleted.

‎src/services/Info/SetZone.cpp

-38
This file was deleted.

‎src/services/Motion/AddToCartesianBuffer.cpp

+2-7
Original file line numberDiff line numberDiff line change
@@ -2,14 +2,9 @@
22

33
SERVICE_CALLBACK_DEF(AddToCartesianBuffer)
44
{
5-
int randNumber = generateRandNumber();
5+
SERVICE_CHECK_EGM_OFF()
66

7-
// Pre-checks
8-
if(egmMode != EGM_OFF) {
9-
res.success = false;
10-
res.msg = "Current EGM mode is incompatible with this task.";
11-
return true;
12-
}
7+
int randNumber = generateRandNumber();
138
if(!prepareCartesian(motionMsg, 30, randNumber, req.x, req.y, req.z, req.q0, req.qx, req.qy, req.qz)) {
149
res.success = false;
1510
res.msg = "Sent pose is not valid.";

‎src/services/Motion/AddToCartesianGripperBuffer.cpp

+24
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,24 @@
1+
#include "abb_robotnode/RobotController.hpp"
2+
3+
SERVICE_CALLBACK_DEF(AddToCartesianGripperBuffer)
4+
{
5+
SERVICE_CHECK_EGM_OFF()
6+
7+
int randNumber = generateRandNumber();
8+
if(!prepareCartesian(motionMsg, 30, randNumber, req.x, req.y, req.z, req.q0, req.qx, req.qy, req.qz, req.gripperPos)) {
9+
res.success = false;
10+
res.msg = "Sent pose is not valid.";
11+
return true;
12+
}
13+
14+
if(sendMotion(randNumber)) {
15+
int ok, idCode;
16+
sscanf(motionReply, "%*d %d %d", &idCode, &ok);
17+
res.success = (bool) ok;
18+
res.msg = (res.success ? "Ok." : "Wrong answer from robot.");
19+
} else {
20+
res.success = false;
21+
res.msg = "No answer received.";
22+
}
23+
return true;
24+
}

‎src/services/Motion/AddToJointBuffer.cpp

+2-7
Original file line numberDiff line numberDiff line change
@@ -2,14 +2,9 @@
22

33
SERVICE_CALLBACK_DEF(AddToJointBuffer)
44
{
5-
int randNumber = generateRandNumber();
5+
SERVICE_CHECK_EGM_OFF()
66

7-
// Pre-checks
8-
if(egmMode != EGM_OFF) {
9-
res.success = false;
10-
res.msg = "Current EGM mode is incompatible with this task.";
11-
return true;
12-
}
7+
int randNumber = generateRandNumber();
138
if(!prepareJoints(motionMsg, 37, randNumber, req.joints)) {
149
res.success = false;
1510
res.msg = "Sent joints are not valid.";

‎src/services/Motion/AddToJointGripperBuffer.cpp

+24
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,24 @@
1+
#include "abb_robotnode/RobotController.hpp"
2+
3+
SERVICE_CALLBACK_DEF(AddToJointGripperBuffer)
4+
{
5+
SERVICE_CHECK_EGM_OFF()
6+
7+
int randNumber = generateRandNumber();
8+
if(!prepareJoints(motionMsg, 37, randNumber, req.joints, req.gripperPos)) {
9+
res.success = false;
10+
res.msg = "Sent joints are not valid.";
11+
return true;
12+
}
13+
14+
if(sendMotion(randNumber)) {
15+
int ok, idCode;
16+
sscanf(motionReply, "%*d %d %d", &idCode, &ok);
17+
res.success = (bool) ok;
18+
res.msg = (res.success ? "Ok." : "Wrong answer from robot.");
19+
} else {
20+
res.success = false;
21+
res.msg = "No answer received.";
22+
}
23+
return true;
24+
}

‎src/services/Motion/ClearCartesianBuffer.cpp

+2
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,8 @@
22

33
SERVICE_CALLBACK_DEF(ClearCartesianBuffer)
44
{
5+
SERVICE_CHECK_EGM_OFF()
6+
57
// Send command to robot
68
int randNumber = generateRandNumber();
79
sprintf(motionMsg, "%.2d %.3d #", 31, randNumber);

‎src/services/Info/SetDefaults.cpp ‎src/services/Motion/SetDefaults.cpp

+3-11
Original file line numberDiff line numberDiff line change
@@ -80,16 +80,8 @@ int RobotController::setDefaults() {
8080

8181
SERVICE_CALLBACK_DEF(SetDefaults)
8282
{
83+
SERVICE_CHECK_EGM_OFF()
84+
8385
int result = setDefaults();
84-
if(result == 1) {
85-
res.success = true;
86-
res.msg = "Ok.";
87-
} else if(result == -1) {
88-
res.success = false;
89-
res.msg = "Wrong answer from robot.";
90-
} else {
91-
res.success = false;
92-
res.msg = "No answer received.";
93-
}
94-
return true;
86+
SERVICE_RESPONSE_FROM_RESULT()
9587
}

‎src/services/Info/SetIOSignal.cpp ‎src/services/Motion/SetIOSignal.cpp

+3-3
Original file line numberDiff line numberDiff line change
@@ -3,11 +3,11 @@
33
SERVICE_CALLBACK_DEF(SetIOSignal)
44
{
55
int randNumber = generateRandNumber();
6-
sprintf(infoMsg, "%.2d %.1d %.1d %.1d #", 11, randNumber, req.output_num, req.signal);
6+
sprintf(motionMsg, "%.2d %.1d %.1d %.1d #", 11, randNumber, req.output_num, req.signal);
77

8-
if(sendInfo(randNumber)) {
8+
if(sendMotion(randNumber)) {
99
int ok, idCode;
10-
sscanf(infoReply, "%*d %d %d", &idCode, &ok);
10+
sscanf(motionReply, "%*d %d %d", &idCode, &ok);
1111
if((bool) ok) {
1212
res.success = true;
1313
res.msg = "Ok.";

‎src/services/Info/SetInertia.cpp ‎src/services/Motion/SetInertia.cpp

+4-13
Original file line numberDiff line numberDiff line change
@@ -6,11 +6,11 @@ int RobotController::setInertia(double m, double cgx, double cgy, double cgz, do
66
return 1;
77

88
int randNumber = generateRandNumber();
9-
sprintf(infoMsg, "%.2d %.3d %+08.5lf %+08.1lf %+08.1lf %+08.1lf %+08.5lf %+08.5lf %+08.5lf #", 14, randNumber, m, cgx, cgy, cgz, ix, iy, iz);
9+
sprintf(motionMsg, "%.2d %.3d %+08.5lf %+08.1lf %+08.1lf %+08.1lf %+08.5lf %+08.5lf %+08.5lf #", 14, randNumber, m, cgx, cgy, cgz, ix, iy, iz);
1010

11-
if(sendInfo(randNumber)) {
11+
if(sendMotion(randNumber)) {
1212
int ok, idCode;
13-
sscanf(infoReply, "%*d %d %d", &idCode, &ok);
13+
sscanf(motionReply, "%*d %d %d", &idCode, &ok);
1414
if((bool) ok) {
1515
currentInertia[0] = m;
1616
currentInertia[1] = cgx;
@@ -29,15 +29,6 @@ int RobotController::setInertia(double m, double cgx, double cgy, double cgz, do
2929
SERVICE_CALLBACK_DEF(SetInertia)
3030
{
3131
int result = setInertia(req.m, req.cgx, req.cgy, req.cgz, req.ix, req.iy, req.iz);
32-
if(result == 1) {
33-
res.success = true;
34-
res.msg = "Ok.";
35-
} else if(result == -1) {
36-
res.success = false;
37-
res.msg = "Wrong answer from robot.";
38-
} else {
39-
res.success = false;
40-
res.msg = "No answer received.";
41-
}
32+
SERVICE_RESPONSE_FROM_RESULT()
4233
return true;
4334
}

‎src/services/Motion/SetJoints.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22

33
SERVICE_CALLBACK_DEF(SetJoints)
44
{
5-
// TODO: Validate joints!!
5+
// TODO: Validate joints
66
if(egmMode == EGM_JOINT_POS_AUTO) {
77
targetJointsMutex.lock();
88
targetJoints.header.seq++;

‎src/services/Motion/SetMaxAcceleration.cpp

+4-17
Original file line numberDiff line numberDiff line change
@@ -14,21 +14,8 @@ int RobotController::setMaxAcceleration(double acc, double deacc) {
1414

1515
SERVICE_CALLBACK_DEF(SetMaxAcceleration)
1616
{
17-
if(egmMode == EGM_OFF) {
18-
int result = setMaxAcceleration(req.acc, req.deacc);
19-
if(result == 1) {
20-
res.success = true;
21-
res.msg = "Ok.";
22-
} else if(result == -1) {
23-
res.success = false;
24-
res.msg = "Wrong answer from robot.";
25-
} else {
26-
res.success = false;
27-
res.msg = "No answer received.";
28-
}
29-
} else {
30-
res.success = false;
31-
res.msg = "EGM needs to be stopped.";
32-
}
33-
return true;
17+
SERVICE_CHECK_EGM_OFF()
18+
19+
int result = setMaxAcceleration(req.acc, req.deacc);
20+
SERVICE_RESPONSE_FROM_RESULT()
3421
}

0 commit comments

Comments
 (0)