Newer
Older
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
#!/usr/bin/env python
"""Fake comm node dropper for simulation."""
import subprocess
import rospy
import tf2_ros
from tf2_geometry_msgs import *
from hw_msgs.msg import CommDropperStatus
from std_msgs.msg import Empty, Int16
from geometry_msgs.msg import PointStamped
status_msg = CommDropperStatus()
status_msg.status = [CommDropperStatus.UNKNOWN
for i in range(len(status_msg.status))]
result_pub = None
tf_buf = None
robot_name = None
def drop_cb(msg):
global status_msg
# Get the first available node index
try:
servo_id = status_msg.status.index(CommDropperStatus.LOADED)
except ValueError:
rospy.logwarn("No more radio mounted")
result_pub.publish(-1)
return
rospy.loginfo("Received drop command for servo %d", servo_id)
# Spawn gazebo model
try:
point = get_drop_position()
except Exception:
rospy.logerr("Robot pose is not available")
point = None
if point:
rospy.loginfo("Spawning radio model in gazebo")
model_name = 'radio_{}_{}'.format(robot_name, servo_id)
cmd_template = 'rosrun gazebo_ros spawn_model ' \
'-model "{model_name}" ' \
'-sdf -database Radio ' \
'-x "{x}" -y "{y}" -z "{z}" -R 1.57'
cmd = cmd_template.format(
model_name=model_name,
x=point.point.x,
y=point.point.y,
z=point.point.z,
)
rospy.loginfo("Executing: %s", cmd)
subprocess.check_call(cmd, shell=True)
# Update status
status_msg.header.stamp = rospy.Time.now()
status_msg.status[servo_id] = CommDropperStatus.EMPTY
# Report result
result_pub.publish(servo_id)
servo_id += 1
def get_drop_position():
point = PointStamped()
point.header.frame_id = robot_name + '/base_link'
point.point.x = -0.8
point.point.y = 0
point.point.z = -0.1
return tf_buf.transform(point, 'world', timeout=rospy.Duration(1))
def main():
global tf_buf, robot_name, result_pub
rospy.init_node('fake_comm_node_dropper')
robot_name = rospy.get_namespace().split('/')[1]
# Get loaded radios from config file
droppable_list = rospy.get_param('comm_node_manager/{}'.format(robot_name))
for droppable in droppable_list:
status_msg.status[droppable['servo_id']] = CommDropperStatus.LOADED
status_pub = rospy.Publisher('comm_dropper/status', CommDropperStatus)
result_pub = rospy.Publisher('comm_dropper/drop_result', Int16)
drop_pub = rospy.Subscriber('comm_dropper/drop', Empty, drop_cb)
tf_buf = tf2_ros.Buffer()
tf_listener = tf2_ros.TransformListener(tf_buf)
r = rospy.Rate(1)
while not rospy.is_shutdown():
status_pub.publish(status_msg)
r.sleep()
if __name__ == '__main__':
main()