Skip to content
GitLab
Projects
Groups
Snippets
Help
Loading...
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Open sidebar
Henrik Andreasson
harvest-automation
Commits
3538da8b
Commit
3538da8b
authored
Jul 23, 2020
by
Nicholas Shindler
Browse files
more cleaning up code
parent
bf7c6418
Changes
11
Hide whitespace changes
Inline
Side-by-side
Showing
11 changed files
with
81 additions
and
56 deletions
+81
-56
hv_bridge/libs/Controllers/arm.py
hv_bridge/libs/Controllers/arm.py
+1
-3
hv_bridge/libs/Controllers/drive.py
hv_bridge/libs/Controllers/drive.py
+2
-3
hv_bridge/libs/Controllers/gripper.py
hv_bridge/libs/Controllers/gripper.py
+3
-5
hv_bridge/libs/Resources/__init__.py
hv_bridge/libs/Resources/__init__.py
+1
-1
hv_bridge/libs/Resources/client.py
hv_bridge/libs/Resources/client.py
+18
-0
hv_bridge/libs/Resources/config.py
hv_bridge/libs/Resources/config.py
+23
-23
hv_bridge/libs/Resources/listener.py
hv_bridge/libs/Resources/listener.py
+1
-0
hv_bridge/libs/Resources/utils.py
hv_bridge/libs/Resources/utils.py
+2
-1
hv_bridge/libs/Resources/writer.py
hv_bridge/libs/Resources/writer.py
+8
-2
hv_bridge/scripts/hv_client.py
hv_bridge/scripts/hv_client.py
+20
-16
hv_bridge/scripts/main_node.py
hv_bridge/scripts/main_node.py
+2
-2
No files found.
hv_bridge/libs/Controllers/arm.py
View file @
3538da8b
...
...
@@ -14,12 +14,10 @@ class ArmController(Writer):
# Controller for the robot arm
# Extends the Setter class
###
def
__init__
(
self
):
def
__init__
(
self
,
hostname
):
# Create writer for this probe
# manip.armRequestedTargetPositionInPercent
Writer
.
__init__
(
self
,
[
"manip.armTargetPositionInPercent"
])
def
run
(
self
,
hostname
):
# runs a subscriber for setting the arm position.
rospy
.
Subscriber
(
"/"
+
hostname
+
"/set_arm_pos"
,
Float32
,
self
.
callback
)
...
...
hv_bridge/libs/Controllers/drive.py
View file @
3538da8b
...
...
@@ -18,12 +18,11 @@ class DriveController(Writer):
# extends the Writer parent class
# handles differential drive controll for the robot
###
def
__init__
(
self
):
def
__init__
(
self
,
hostname
):
Writer
.
__init__
(
self
,
[
"mob.driveControlWheelSpeeds.l"
,
"mob.driveControlWheelSpeeds.r"
]
)
def
run
(
self
,
hostname
):
# runs a subscriber for setting the wheel velocity.
rospy
.
Subscriber
(
"/"
+
hostname
+
"/cmd_vel"
,
Twist
,
self
.
callback
)
def
callback
(
self
,
cmd_msg
):
...
...
hv_bridge/libs/Controllers/gripper.py
View file @
3538da8b
...
...
@@ -14,10 +14,12 @@ class GripperController(Writer):
# Controller for the robot gripper
# Extends the Setter class
###
def
__init__
(
self
):
def
__init__
(
self
,
hostname
):
# Create writer for this probe
# manip.gripperRequestedTargetPositionInPercent
Writer
.
__init__
(
self
,
[
"manip.gripperTargetPositionInPercent"
])
# intializes the mp connection and runs a subscriber for setting the gripper position.
rospy
.
Subscriber
(
"/"
+
hostname
+
"/set_gripper_pos"
,
Float32
,
self
.
callback
)
def
callback
(
self
,
cmd_msg
):
# callback function - extends the setter.callback functionality
...
...
@@ -28,7 +30,3 @@ class GripperController(Writer):
elif
pos
<
0
:
pos
=
0
Writer
.
callback
(
self
,
pos
)
def
run
(
self
,
rospy
,
hostname
):
# intializes the mp connection and runs a subscriber for setting the gripper position.
rospy
.
Subscriber
(
"/"
+
hostname
+
"/set_gripper_pos"
,
Float32
,
self
.
callback
)
hv_bridge/libs/Resources/__init__.py
View file @
3538da8b
...
...
@@ -11,4 +11,4 @@ from writer import *
from
config
import
*
from
utils
import
*
from
listener
import
*
from
listener
import
*
hv_bridge/libs/Resources/client.py
View file @
3538da8b
...
...
@@ -172,6 +172,24 @@ class Client:
except
Exception
as
e
:
raise
e
def
lookup_probe_type
(
self
,
probe_id
):
# get the probe type
try
:
if
type
(
probe_id
)
!=
int
:
raise
ValueError
(
"probe id must be an int"
)
return
self
.
probe_defs
[
probe_id
][
1
]
except
Exception
as
e
:
raise
e
def
lookup_probe
(
self
,
t
):
# get the probe info
try
:
if
type
(
t
)
!=
int
:
raise
ValueError
(
"probe id must be an int"
)
return
(
self
.
probe_defs
[
t
][
0
],
self
.
probe_defs
[
t
][
1
])
except
Exception
as
e
:
raise
e
def
enable_probes
(
self
,
probes
):
# enables one or more probes.
# allow for singleton argument
...
...
hv_bridge/libs/Resources/config.py
View file @
3538da8b
...
...
@@ -22,20 +22,20 @@ MP_TLV_MISSED_DEBUG_MESSAGES = 15
MP_TLV_WRITE_PROBES
=
16
type_table
=
{
1
:
(
"uint8"
,
"<B"
),
# 1-byte unsigned integer
2
:
(
"int8"
,
"<b"
),
# 1-byte signed integer
3
:
(
"uint16"
,
"<H"
),
# 2-byte unsigned integer
4
:
(
"int16"
,
"<h"
),
# 2-byte signed integer
5
:
(
"uint32"
,
"<I"
),
# 4-byte unsigned integer
6
:
(
"int32"
,
"<i"
),
# 4-byte signed integer
7
:
(
"uint64"
,
"<Q"
),
# 8-byte unsigned integer
8
:
(
"int64"
,
"<q"
),
# 8-byte signed integer
9
:
(
"float"
,
"<f"
),
# 4-byte float (IEEE 754)
10
:
(
"double"
,
"<d"
),
# 8-byte float (IEEE 754)
11
:
(
"tlv"
,
""
),
# Variable-length TLV data
12
:
(
"bool"
,
"<B"
),
# 1-byte boolean integer (1 or 0)
13
:
(
"string"
,
""
),
# variable-size null-terminated char string
}
1
:
(
"uint8"
,
"<B"
),
# 1-byte unsigned integer
2
:
(
"int8"
,
"<b"
),
# 1-byte signed integer
3
:
(
"uint16"
,
"<H"
),
# 2-byte unsigned integer
4
:
(
"int16"
,
"<h"
),
# 2-byte signed integer
5
:
(
"uint32"
,
"<I"
),
# 4-byte unsigned integer
6
:
(
"int32"
,
"<i"
),
# 4-byte signed integer
7
:
(
"uint64"
,
"<Q"
),
# 8-byte unsigned integer
8
:
(
"int64"
,
"<q"
),
# 8-byte signed integer
9
:
(
"float"
,
"<f"
),
# 4-byte float (IEEE 754)
10
:
(
"double"
,
"<d"
),
# 8-byte float (IEEE 754)
11
:
(
"tlv"
,
""
),
# Variable-length TLV data
12
:
(
"bool"
,
"<B"
),
# 1-byte boolean integer (1 or 0)
13
:
(
"string"
,
""
),
# variable-size null-terminated char string
}
# Declare function types
...
...
@@ -63,18 +63,18 @@ AVOID_SRV = HV_Services.AVOID_SRV
# Probe names
START
=
"ui.fakeStartButtonNoSpacing"
#
1125
LIDAR_STANDBY
=
"vis.sick.useStandByMode"
#
2877
ARM_ENABLED
=
"manip.armEnableRequest"
#
1082
GRIPPER_ENABLED
=
"manip.gripperEnableRequest"
#
1083
DRIVE_ENABLED
=
"drive.enableRequest"
#
580
DRIVE_COMMAND
=
"mob.driveCommand.enable"
#
1220
ROB_SENSOR
=
"vis.robotSensor.enabled"
#
3100
LINE_SENSOR
=
"vis.lineSensor.enabled"
#
3171
START
=
"ui.fakeStartButtonNoSpacing"
#
1125
LIDAR_STANDBY
=
"vis.sick.useStandByMode"
#
2877
ARM_ENABLED
=
"manip.armEnableRequest"
#
1082
GRIPPER_ENABLED
=
"manip.gripperEnableRequest"
#
1083
DRIVE_ENABLED
=
"drive.enableRequest"
#
580
DRIVE_COMMAND
=
"mob.driveCommand.enable"
#
1220
ROB_SENSOR
=
"vis.robotSensor.enabled"
#
3100
LINE_SENSOR
=
"vis.lineSensor.enabled"
#
3171
ESTOP
=
"ui.fakeEstop"
FLAG_PULL
=
"estop.fakeFlagPulled"
BEEP
=
"ui.enableBeeping"
#
1120
BEEP
=
"ui.enableBeeping"
#
1120
HAS_POT
=
"manip.botGotPot"
...
...
hv_bridge/libs/Resources/listener.py
View file @
3538da8b
...
...
@@ -10,6 +10,7 @@ import sys
import
subprocess
import
rospy
class
listen
(
threading
.
Thread
):
# listener class for getting data from robot
def
__init__
(
self
,
q
,
client
):
...
...
hv_bridge/libs/Resources/utils.py
View file @
3538da8b
...
...
@@ -30,7 +30,8 @@ def twist_to_vel(twist):
def
resource_name
(
name
):
# formats a hostname to be legaly used in a ros pubisher/subscriber name
return
((
name
.
replace
(
"-"
,
""
)).
replace
(
"."
,
"_"
)).
lower
()
###
# functions that handle the formatting, reading and witing of tlv messages
# tlv is the structure of message that mp uses to communicate
...
...
hv_bridge/libs/Resources/writer.py
View file @
3538da8b
...
...
@@ -9,7 +9,13 @@ class Writer:
def
get_msgs
(
self
):
# collect the messages to send
return
self
.
msgs
m
=
self
.
msgs
self
.
msgs
=
[]
return
m
def
clear_msgs
(
self
):
# clear msg array
self
.
msgs
=
[]
def
callback
(
self
,
msg
):
# subscriber callback function
...
...
@@ -21,7 +27,7 @@ class Writer:
# each message coresponds to a probe (1 to 1)
if
len
(
msg
)
!=
len
(
self
.
probes
):
raise
Exception
(
"message size missmatch"
)
self
.
msgs
=
zip
(
self
.
probes
)
self
.
msgs
=
zip
(
self
.
probes
,
msg
)
else
:
# publish the same message to each of the probes
self
.
msgs
=
[(
c
,
msg
)
for
c
in
self
.
probes
]
hv_bridge/scripts/hv_client.py
View file @
3538da8b
...
...
@@ -36,13 +36,13 @@ class HVClient(Client):
name
=
resource_name
(
hostname
)
if
DRIVE_CMD
in
services
:
# enable drive controller
self
.
cmdr
.
append
(
DriveController
())
self
.
cmdr
.
append
(
DriveController
(
name
))
if
GRIPPER_CMD
in
services
:
# enable gripper controller
self
.
cmdr
.
append
(
GripperController
())
self
.
cmdr
.
append
(
GripperController
(
name
))
if
ARM_CMD
in
services
:
# enable arm controller
self
.
cmdr
.
append
(
ArmController
())
self
.
cmdr
.
append
(
ArmController
(
name
))
if
ODOM_LSTN
in
services
:
# enable default odometry listener
...
...
@@ -112,10 +112,10 @@ class HVClient(Client):
# sets the start to FALSE. this 'stops' the robot.
# sets flag pulled to true
# TODO: switch codes for enums
Client
.
write_probes
(
self
,[(
ESTOP
,
1
),
(
START
,
0
),
(
FLAG_PULL
,
1
)])
Client
.
write_probes
(
self
,
[(
ESTOP
,
1
),
(
START
,
0
),
(
FLAG_PULL
,
1
)])
print
"virtual e-stop triggered"
time
.
sleep
(
1
)
Client
.
write_probes
(
self
,[(
ESTOP
,
0
),
(
FLAG_PULL
,
0
)])
Client
.
write_probes
(
self
,
[(
ESTOP
,
0
),
(
FLAG_PULL
,
0
)])
print
str
(
self
.
hostname
)
+
" has stopped all running processes"
except
Exception
as
e
:
raise
e
...
...
@@ -142,25 +142,29 @@ class HVClient(Client):
)
msgs
=
get_msg_arr
([],
map
((
lambda
a
:
a
.
get_msgs
()),
self
.
cmdr
))
v
=
""
if
len
(
msgs
)
==
1
:
msgs
=
((
msgs
),)
if
len
(
msgs
)
==
0
:
return
print
msgs
print
len
(
msgs
)
for
(
probe
,
value
)
in
msgs
:
probe_id
=
Client
.
lookup_probe_id
(
self
,
probe
)
probe_type
=
self
.
probe_defs
[
probe_id
][
1
]
probe_id
=
Client
.
lookup_probe_id
(
self
,
probe
)
probe_type
=
Client
.
lookup_probe_type
(
self
,
probe_id
)
v
+=
make_tlv
(
probe_id
,
Client
.
encode_probe_value
(
self
,
probe_type
,
value
))
Client
.
send_message
(
self
,
make_tlv
(
MP_TLV_WRITE_PROBES
,
v
))
v
+=
make_tlv
(
probe_id
,
Client
.
encode_probe_value
(
self
,
probe_type
,
value
)
)
Client
.
send_message
(
self
,
make_tlv
(
MP_TLV_WRITE_PROBES
,
v
))
except
Exception
as
e
:
raise
e
rospy
.
logwarn
(
e
)
# raise e
def
read
(
self
,
t
,
v
):
# pass data to the correct child class
try
:
name
=
self
.
probe_defs
[
t
][
0
]
type
=
self
.
probe_defs
[
t
][
1
]
val
=
Client
.
decode_probe_data
(
self
,
type
,
v
)
(
pname
,
ptype
)
=
Client
.
lookup_probe
(
self
,
t
)
val
=
Client
.
decode_probe_data
(
self
,
ptype
,
v
)
[
serv
.
add
(
name
,
val
)
for
serv
in
self
.
lstnr
]
[
serv
.
add
(
p
name
,
val
)
for
serv
in
self
.
lstnr
]
except
Exception
as
e
:
raise
e
...
...
hv_bridge/scripts/main_node.py
View file @
3538da8b
...
...
@@ -87,7 +87,7 @@ if __name__ == "__main__":
"sys"
,
type
=
str
,
nargs
=
"*"
,
help
=
"System included variables from launch file (not set bu user)"
help
=
"System included variables from launch file (not set bu user)"
,
)
args
=
parser
.
parse_args
()
...
...
@@ -177,7 +177,7 @@ if __name__ == "__main__":
# default to 100. (mp runs at 200)
hz
=
100
if
hv_bot
.
hz
:
hz
=
hv_bot
.
hz
/
2
hz
=
hv_bot
.
hz
/
2
# Start the ROS main loop
rate
=
rospy
.
Rate
(
hz
)
...
...
Write
Preview
Markdown
is supported
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment