Merge pull request #56 from cyberbotics/fix-enu-conversion

Fix ENU conversion
cyberbotics · Feb 2, 2021 · aa1a027 · aa1a027
2 parents 1d01a36 + 815f041
commit aa1a027
Show file tree

Hide file tree

Showing 13 changed files with 353 additions and 258 deletions.
diff --git a/launch/complete_test.launch b/launch/complete_test.launch
@@ -9,5 +9,5 @@
   </include>
 
   <arg name="auto_close" default="false" doc="Startup mode"/>
-  <node name="complete_test" pkg="webots_ros" type="complete_test" required="$(arg auto_close)"/>
+  <node name="complete_test" pkg="webots_ros" type="complete_test" output="screen" required="$(arg auto_close)"/>
 </launch>
diff --git a/package.xml b/package.xml
@@ -2,7 +2,7 @@
 <package>
   <name>webots_ros</name>
   <!-- Version matches Webots version this way: (major - 2018).minor.maintenance -->
-  <version>4.0.0</version>
+  <version>4.0.1</version>
   <description>The ROS package containing examples for interfacing ROS with the standard ROS controller of Webots</description>
 
   <url>http://wiki.ros.org/webots_ros</url>

diff --git a/plugins/robot_windows/info/info.html b/plugins/robot_windows/info/info.html
@@ -1,7 +1,27 @@
-<div>
-  <h2>Info window</h2>
+<html>
+
+<head>
+  <script>
+    window.onload = () => {
+      webots.window('info').receive = function (message, robot) {
+        if (message.startsWith('test')) {
+          document.querySelector("#text-display").innerHTML = message;
+          this.send("Answer: " + message, robot);
+        } else
+          console.log("Received unknown message for robot '" + robot + "': '" + message + "'");
+      }
+    }
+  </script>
+</head>
+
+<body>
   <div>
-    <p>Show message received from the controller:</p>
-    <span id='text-display'></span>
+    <h2>Info window</h2>
+    <div>
+      <p>Show message received from the controller:</p>
+      <span id='text-display'></span>
+    </div>
   </div>
-</div>
+</body>
+
+</html>
diff --git a/plugins/robot_windows/info/info.js b/plugins/robot_windows/info/info.js
diff --git a/src/complete_test.cpp b/src/complete_test.cpp
@@ -3535,3 +3535,4 @@ int main(int argc, char **argv) {
   printf("\nTest Completed\n");
   return 0;
 }
+
diff --git a/src/pioneer3at.cpp b/src/pioneer3at.cpp
@@ -21,7 +21,7 @@
 #include <sensor_msgs/Image.h>
 #include <sensor_msgs/Imu.h>
 #include <sensor_msgs/LaserScan.h>
-#include <geometry_msgs/Point.h>
+#include <geometry_msgs/PointStamped.h>
 #include <signal.h>
 #include <std_msgs/String.h>
 #include <tf/transform_broadcaster.h>
@@ -121,9 +121,9 @@ void broadcastTransform() {
   br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "base_link", "pioneer3at/Sick_LMS_291"));
 }
 
-void GPSCallback(const geometry_msgs::Point::ConstPtr &values) {
-  GPSValues[0] = values->x;
-  GPSValues[1] = values->y;
+void GPSCallback(const geometry_msgs::PointStamped::ConstPtr &values) {
+  GPSValues[0] = values->point.x;
+  GPSValues[1] = values->point.y;
   broadcastTransform();
 }
 

diff --git a/worlds/catch_the_bird.wbt b/worlds/catch_the_bird.wbt
@@ -1,19 +1,20 @@
 #VRML_SIM R2021a utf8
 WorldInfo {
-  coordinateSystem "NUE"
 }
 Viewpoint {
-  orientation -0.104674958073239 -0.9575576164575551 -0.26856389242876866 2.60605
-  position -1.59084 1.87221 -2.53085
+  orientation 0.8317372367594001 -0.2710211159782898 -0.4845211282098233 1.1840859377285968
+  position -1.4137324366994928 -2.042337130989267 1.4998046917960792
 }
 TexturedBackground {
 }
 TexturedBackgroundLight {
 }
 RectangleArena {
+  rotation 1 0 0 1.5707963267948966
 }
 Robot {
-  translation -0.1 0.09 0
+  translation -0.1 0 0.09
+  rotation 1 0 0 1.5707963267948966
   children [
     HingeJoint {
       jointParameters HingeJointParameters {
@@ -72,7 +73,8 @@ Robot {
   controller "ros"
 }
 Solid {
-  translation 0.3 0.1 0
+  translation 0.3 0 0.1
+  rotation 1 0 0 1.5707963267948966
   children [
     Solid {
       translation 0 -0.09 0

diff --git a/worlds/complete_test.wbt b/worlds/complete_test.wbt
@@ -1,20 +1,20 @@
 #VRML_SIM R2021a utf8
 WorldInfo {
-  coordinateSystem "NUE"
 }
 Viewpoint {
-  orientation -0.6194591910547989 0.7334082212488283 0.27996909047672397 0.888923
-  position 1.67801 1.75743 2.04642
+  orientation 0.8670407508271071 -0.2540217867377257 -0.4286178580830943 1.1991808309301875
+  position -1.2856898746073142 -2.3176378713303354 1.4855385133866121
 }
 TexturedBackground {
 }
 TexturedBackgroundLight {
 }
 DEF GROUND RectangleArena {
+  rotation 1 0 0 1.5707963267948966
 }
 Solid {
-  translation 0.182984 0.05 0.12186
-  rotation 0 -1 0 0.934348
+  translation 0.182984 -0.12186 0.05
+  rotation 0.814092969139429 -0.410641350571121 -0.410641350571121 1.775041980102555
   children [
     Shape {
       appearance PBRAppearance {
@@ -34,7 +34,8 @@ Solid {
   }
 }
 DEF CONE Solid {
-  translation 0.240484 0.1 -0.258217
+  translation 0.240484 0.258217 0.1
+  rotation 1 0 0 1.5707963267948966
   children [
     Shape {
       appearance PBRAppearance {
@@ -55,7 +56,8 @@ DEF CONE Solid {
   ]
 }
 DEF RADAR_TARGET Solid {
-  translation 0 0 -1.86
+  translation 0 1.86 0
+  rotation 1 0 0 1.5707963267948966
   children [
     Shape {
       appearance PBRAppearance {
@@ -73,7 +75,8 @@ DEF RADAR_TARGET Solid {
   radarCrossSection 1
 }
 Robot {
-  translation 0 0.1 0
+  translation 0 0 0.1
+  rotation 1 0 0 1.5707963267948966
   children [
     SliderJoint {
       device [

diff --git a/worlds/e-puck_line.wbt b/worlds/e-puck_line.wbt
@@ -1,16 +1,16 @@
 #VRML_SIM R2021a utf8
 WorldInfo {
-  coordinateSystem "NUE"
 }
 Viewpoint {
-  orientation 0.8817326747174138 0.4433808364310757 0.16112394055929466 5.50129
-  position -0.45881 0.996783 1.25076
+  orientation 0.886846591609518 -0.24544200950097636 -0.39148606989611184 1.2307403414482054
+  position -0.9554408008528787 -1.8017812565874936 0.9933059870384544
 }
 TexturedBackground {
 }
 TexturedBackgroundLight {
 }
 RectangleArena {
+  rotation 1 0 0 1.5707963267948966
   floorSize 0.9 0.9
   floorTileSize 0.9 0.9
   floorAppearance PBRAppearance {
@@ -25,7 +25,8 @@ RectangleArena {
   wallHeight 0.05
 }
 DEF OBSTACLE1 Solid {
-  translation 0.0871577 0.025 -0.192401
+  translation 0.0871577 0.192401 0.025
+  rotation 1 0 0 1.5707963267948966
   children [
     Shape {
       appearance PBRAppearance {
@@ -42,8 +43,8 @@ DEF OBSTACLE1 Solid {
   boundingObject USE BOX1
 }
 DEF OBSTACLE2 Solid {
-  translation -0.0677406 0.025 -0.0327153
-  rotation 0 1 0 4.08383
+  translation -0.0677406 0.0327153 0.025
+  rotation -0.338870904039348 0.665269310278009 0.665269310278009 3.795044776069445
   children [
     Shape {
       appearance PBRAppearance {
@@ -60,8 +61,8 @@ DEF OBSTACLE2 Solid {
   boundingObject USE BOX2
 }
 DEF OBSTACLE3 Solid {
-  translation 0.31759 0.025 0.250604
-  rotation 0 1 0 13.7261
+  translation 0.31759 -0.250604 0.025
+  rotation 0.733633125129165 0.48051141386715 0.48051141386715 1.875705136699914
   children [
     Shape {
       appearance PBRAppearance {
@@ -78,8 +79,8 @@ DEF OBSTACLE3 Solid {
   boundingObject USE BOX3
 }
 DEF EPUCK E-puck {
-  translation 0.080305 0 0.171886
-  rotation 0 1 0 1.5
+  translation 0.080305 -0.171886 0
+  rotation 0.604591835333834 0.563235613508089 0.563235613508089 2.054014631559264
   controller "ros"
   groundSensorsSlot [
     E-puckGroundSensors {

diff --git a/worlds/keyboard_teleop.wbt b/worlds/keyboard_teleop.wbt
@@ -1,30 +1,35 @@
 #VRML_SIM R2021a utf8
 WorldInfo {
-  coordinateSystem "NUE"
 }
 Viewpoint {
-  orientation 0.702730921913247 0.6961909226399652 0.146585983711513 5.45898
-  position -5.30367 5.63958 6.90061
+  orientation 0.6833738270619804 0.31755469396198066 0.6573881873211409 1.2306409582451472
+  position 6.9941003633455 -5.528115256749356 7.059051478396728
 }
 TexturedBackground {
 }
 TexturedBackgroundLight {
 }
 RectangleArena {
+  rotation 1 0 0 1.5707963267948966
   floorSize 4 4
   wallThickness 0.1
   wallHeight 0.5
 }
 CardboardBox {
-  translation 1.31407 0.3 -0.444198
+  translation 1.31407 0.444198 0.3
+  rotation 1 0 0 1.5707963267948966
 }
 CardboardBox {
-  translation 0.133486 0.3 -1.17773
+  translation 0.133486 1.17773 0.3
+  rotation 1 0 0 1.5707963267948966
   name "cardboard box(1)"
 }
 WoodenBox {
-  translation -0.797045 0.3 1.02408
+  translation -0.797045 -1.02408 0.3
+  rotation 1 0 0 1.5707963267948966
 }
 DEF PIONEER_3DX Pioneer3dx {
+  translation 0 0.0975 0.1
+  rotation 1 0 0 1.5707963267948966
   controller "ros"
 }
diff --git a/worlds/panoramic_view_recorder.wbt b/worlds/panoramic_view_recorder.wbt
@@ -1,6 +1,5 @@
 #VRML_SIM R2021a utf8
 WorldInfo {
-  coordinateSystem "NUE"
 }
 Viewpoint {
   orientation 0.6025650947976529 0.4034508611086422 0.688580212613019 1.542782453757079
@@ -11,10 +10,12 @@ TexturedBackground {
 TexturedBackgroundLight {
 }
 Robot {
+  rotation 1 0 0 1.5707963267948966
   controller "ros"
   supervisor TRUE
 }
 Floor {
+  rotation 1 0 0 1.5707963267948966
 }
 Nao {
   translation 0 0 0.33
Original file line number	Diff line number	Diff line change
Expand Up		@@ -3535,3 +3535,4 @@ int main(int argc, char **argv) {
		printf("\nTest Completed\n");
		return 0;
		}