-
Notifications
You must be signed in to change notification settings - Fork 368
/
ros2_simple.html
173 lines (149 loc) · 5.35 KB
/
ros2_simple.html
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
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
<!DOCTYPE html>
<html>
<head>
<meta charset="utf-8" />
<script src="https://unpkg.com/eventemitter3@latest/dist/eventemitter3.umd.min.js"></script>
<script src="../dist/RosLib.umd.cjs"></script>
<script>
// Connecting to ROS
// -----------------
var ros = new ROSLIB.Ros();
// If there is an error on the backend, an 'error' emit will be emitted.
ros.on('error', function(error) {
document.getElementById('connecting').style.display = 'none';
document.getElementById('connected').style.display = 'none';
document.getElementById('closed').style.display = 'none';
document.getElementById('error').style.display = 'inline';
console.log(error);
});
// Find out exactly when we made a connection.
ros.on('connection', function() {
console.log('Connection made!');
document.getElementById('connecting').style.display = 'none';
document.getElementById('error').style.display = 'none';
document.getElementById('closed').style.display = 'none';
document.getElementById('connected').style.display = 'inline';
});
ros.on('close', function() {
console.log('Connection closed.');
document.getElementById('connecting').style.display = 'none';
document.getElementById('connected').style.display = 'none';
document.getElementById('closed').style.display = 'inline';
});
// Create a connection to the rosbridge WebSocket server.
ros.connect('ws://localhost:9090');
// Publishing a Topic
// ------------------
// First, we create a Topic object with details of the topic's name and message type.
var cmdVel = new ROSLIB.Topic({
ros : ros,
name : '/cmd_vel',
messageType : 'geometry_msgs/Twist'
});
// Then we create the payload to be published. The object we pass in to ros.Message matches the
// fields defined in the geometry_msgs/Twist.msg definition.
var twist = {
linear : {
x : 0.1,
y : 0.2,
z : 0.3
},
angular : {
x : -0.1,
y : -0.2,
z : -0.3
}
};
// And finally, publish.
cmdVel.publish(twist);
// Subscribing to a Topic
// ----------------------
// Like when publishing a topic, we first create a Topic object with details of the topic's name
// and message type. Note that we can call publish or subscribe on the same topic object.
var listener = new ROSLIB.Topic({
ros : ros,
name : '/listener',
messageType : 'std_msgs/String'
});
// Then we add a callback to be called every time a message is published on this topic.
listener.subscribe(function(message) {
console.log('Received message on ' + listener.name + ': ' + message.data);
// If desired, we can unsubscribe from the topic as well.
listener.unsubscribe();
});
// Calling a service
// -----------------
// First, we create a Service client with details of the service's name and service type.
var addTwoIntsClient = new ROSLIB.Service({
ros : ros,
name : '/add_two_ints',
serviceType : 'example_interfaces/AddTwoInts'
});
// Then we create a Service Request. The object we pass in to ROSLIB.ServiceRequest matches the
// fields defined in the rospy_tutorials AddTwoInts.srv file.
var request = {
a : 1,
b : 2
};
// Finally, we call the /add_two_ints service and get back the results in the callback. The result
// is a ROSLIB.ServiceResponse object.
addTwoIntsClient.callService(request, function(result) {
console.log('Result for service call on ' + addTwoIntsClient.name + ': ' + result.sum);
});
// Advertising a Service
// ---------------------
// The Service object does double duty for both calling and advertising services
var setBoolServer = new ROSLIB.Service({
ros : ros,
name : '/set_bool',
serviceType : 'std_srvs/SetBool'
});
// Use the advertise() method to indicate that we want to provide this service
setBoolServer.advertise(function(request, response) {
console.log('Received service request on ' + setBoolServer.name + ': ' + request.data);
response['success'] = true;
response['message'] = 'Set successfully';
return true;
});
// Getting a param value
// ---------------------
// In ROS 2, params are set in the format 'node_name:param_name'
var favoriteColor = new ROSLIB.Param({
ros : ros,
name : '/add_two_ints_server:use_sim_time'
});
favoriteColor.get(function(value) {
console.log('The value of use_sim_time before setting is ' + value);
});
favoriteColor.set(true);
favoriteColor.get(function(value) {
console.log('The value of use_sim_time after setting is ' + value);
});
</script>
</head>
<body>
<h1>Simple roslib Example</h1>
<p>Run the following commands in the terminal then refresh this page. Check the JavaScript
console for the output.</p>
<ol>
<li><tt>ros2 topic pub /listener std_msgs/msg/String "{ data: 'hello world' }" </tt></li>
<li><tt>ros2 topic echo /cmd_vel</tt></li>
<li><tt>ros2 run demo_nodes_py add_two_ints_server</tt></li>
<li><tt>ros2 launch rosbridge_server rosbridge_websocket_launch.xml</tt></li>
</ol>
<div id="statusIndicator">
<p id="connecting">
Connecting to rosbridge...
</p>
<p id="connected" style="color:#00D600; display:none">
Connected
</p>
<p id="error" style="color:#FF0000; display:none">
Error in the backend!
</p>
<p id="closed" style="display:none">
Connection closed.
</p>
</div>
</body>
</html>