From a6705e16773e9cfef6dcc7d74ebe15839dc07b62 Mon Sep 17 00:00:00 2001 From: Stefania Pedrazzi Date: Thu, 17 Dec 2020 08:28:44 +0100 Subject: [PATCH 01/72] Update Webots version in world file header (#48) --- worlds/.catch_the_bird.wbproj | 2 +- worlds/.complete_test.wbproj | 2 +- worlds/.e-puck_line.wbproj | 2 +- worlds/.keyboard_teleop.wbproj | 2 +- worlds/.panoramic_view_recorder.wbproj | 2 +- worlds/.pioneer3at.wbproj | 2 +- worlds/.ros_python.wbproj | 2 +- worlds/catch_the_bird.wbt | 2 +- worlds/complete_test.wbt | 2 +- worlds/e-puck_line.wbt | 2 +- worlds/keyboard_teleop.wbt | 2 +- worlds/panoramic_view_recorder.wbt | 2 +- worlds/pioneer3at.wbt | 2 +- worlds/ros_python.wbt | 2 +- 14 files changed, 14 insertions(+), 14 deletions(-) diff --git a/worlds/.catch_the_bird.wbproj b/worlds/.catch_the_bird.wbproj index 61983e4..eb1d22b 100644 --- a/worlds/.catch_the_bird.wbproj +++ b/worlds/.catch_the_bird.wbproj @@ -1,4 +1,4 @@ -Webots Project File version R2021a +Webots Project File version R2021b perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000002fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000190000033d0000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff00000001000002ec0000025efc0200000001fb0000001400540065007800740045006400690074006f007200000000190000025e0000003f00ffffff00000003000004e20000005afc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000004e20000006900ffffff000004e20000015700000001000000020000000100000008fc00000000 simulationViewPerspectives: 000000ff00000001000000020000016c000002060100000002010000000101 sceneTreePerspectives: 000000ff0000000100000002000000c0000000fc0100000002010000000201 diff --git a/worlds/.complete_test.wbproj b/worlds/.complete_test.wbproj index efddce6..45f5eb4 100644 --- a/worlds/.complete_test.wbproj +++ b/worlds/.complete_test.wbproj @@ -1,4 +1,4 @@ -Webots Project File version R2021a +Webots Project File version R2021b perspectives: 000000ff00000000fd00000003000000000000018f00000482fc0100000004fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000190000033d0000000000000000fb0000002c0052006f0062006f0074003a002000660075006c006c005f00730075007000650072007600690073006f007201000000000000018f0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f007701000000000000018f0000005400fffffffb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000005400ffffff00000001000001ed000003a2fc0200000001fb0000001400540065007800740045006400690074006f00720100000000000003a20000003c00ffffff00000003000005aa000000dafc0100000001fb0000000e0043006f006e0073006f006c00650100000195000005aa0000005400ffffff000003b7000003a200000004000000040000000100000008fc00000000 simulationViewPerspectives: 000000ff000000010000000200000170000003490100000006010000000101 sceneTreePerspectives: 000000ff0000000100000002000000c0000000fc0100000006010000000201 diff --git a/worlds/.e-puck_line.wbproj b/worlds/.e-puck_line.wbproj index 5f9b1ce..a0361e0 100644 --- a/worlds/.e-puck_line.wbproj +++ b/worlds/.e-puck_line.wbproj @@ -1,4 +1,4 @@ -Webots Project File version R2021a +Webots Project File version R2021b perspectives: 000000ff00000000fd00000003000000000000016c0000033dfc0200000001fb00000012005300630065006e0065005400720065006500000000190000033d0000000000000000000000010000016200000309fc0200000001fb0000001400540065007800740045006400690074006f00720100000019000003090000003e00ffffff000000030000073f000000e2fc0100000001fb0000000e0043006f006e0073006f006c006501000000000000073f0000005a00ffffff000005d70000030900000004000000040000000100000008fc00000000 simulationViewPerspectives: 000000ff00000001000000020000016c000002850100000006010000000101 sceneTreePerspectives: 000000ff0000000100000002000000c0000000a80100000006010000000201 diff --git a/worlds/.keyboard_teleop.wbproj b/worlds/.keyboard_teleop.wbproj index 1289b98..2dff954 100644 --- a/worlds/.keyboard_teleop.wbproj +++ b/worlds/.keyboard_teleop.wbproj @@ -1,4 +1,4 @@ -Webots Project File version R2021a +Webots Project File version R2021b perspectives: 000000ff00000000fd00000003000000000000016c0000033dfc0200000001fb00000012005300630065006e0065005400720065006501000000190000033d000000000000000000000001000001e100000258fc0200000001fb0000001400540065007800740045006400690074006f00720100000019000002580000003e00ffffff000000030000073f00000193fc0100000001fb0000000e0043006f006e0073006f006c006501000000000000073f0000005a00ffffff000005580000025800000001000000020000000100000008fc00000000 simulationViewPerspectives: 000000ff00000001000000020000016c000002060100000006010000000101 sceneTreePerspectives: 000000ff0000000100000002000000c0000000a80100000006010000000201 diff --git a/worlds/.panoramic_view_recorder.wbproj b/worlds/.panoramic_view_recorder.wbproj index 3f4da1a..19379cb 100644 --- a/worlds/.panoramic_view_recorder.wbproj +++ b/worlds/.panoramic_view_recorder.wbproj @@ -1,4 +1,4 @@ -Webots Project File version R2021a +Webots Project File version R2021b perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000005400ffffff000000010000012e000003a2fc0200000001fb0000001400540065007800740045006400690074006f00720100000000000003a20000003c00ffffff000000030000073f000000dafc0100000001fb0000000e0043006f006e0073006f006c006501000000000000073f0000005400ffffff0000060b000003a200000001000000020000000100000008fc00000000 simulationViewPerspectives: 000000ff00000001000000020000016c000004a00100000006010000000101 sceneTreePerspectives: 000000ff0000000100000002000000c0000001120100000006010000000201 diff --git a/worlds/.pioneer3at.wbproj b/worlds/.pioneer3at.wbproj index 725746c..86294d3 100644 --- a/worlds/.pioneer3at.wbproj +++ b/worlds/.pioneer3at.wbproj @@ -1,4 +1,4 @@ -Webots Project File version R2021a +Webots Project File version R2021b perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000005400ffffff000000010000012e000002d9fc0200000001fb0000001400540065007800740045006400690074006f00720000000019000002d9000000a200ffffff000000030000073f000000bbfc0100000001fb0000000e0043006f006e0073006f006c006501000000000000073f0000005400ffffff0000073f000003c100000001000000020000000100000008fc00000000 simulationViewPerspectives: 000000ff00000001000000020000011e000004db0100000006010000000101 sceneTreePerspectives: 000000ff0000000100000002000000c0000001240100000006010000000201 diff --git a/worlds/.ros_python.wbproj b/worlds/.ros_python.wbproj index 158ad59..9022d0c 100644 --- a/worlds/.ros_python.wbproj +++ b/worlds/.ros_python.wbproj @@ -1,4 +1,4 @@ -Webots Project File version R2021a +Webots Project File version R2021b perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff00000001000001c900000375fc0200000001fb0000001400540065007800740045006400690074006f00720100000000000003750000008900ffffff000000030000073f00000093fc0100000001fb0000000e0043006f006e0073006f006c006501000000000000073f0000006900ffffff000005740000037500000001000000020000000100000008fc00000000 simulationViewPerspectives: 000000ff000000010000000200000100000005210100000002010000000100 sceneTreePerspectives: 000000ff0000000100000002000002aa000000fa0100000002010000000200 diff --git a/worlds/catch_the_bird.wbt b/worlds/catch_the_bird.wbt index ae55d5e..dd3c4d4 100644 --- a/worlds/catch_the_bird.wbt +++ b/worlds/catch_the_bird.wbt @@ -1,4 +1,4 @@ -#VRML_SIM R2021a utf8 +#VRML_SIM R2021b utf8 WorldInfo { } Viewpoint { diff --git a/worlds/complete_test.wbt b/worlds/complete_test.wbt index 6e4992a..3080d81 100644 --- a/worlds/complete_test.wbt +++ b/worlds/complete_test.wbt @@ -1,4 +1,4 @@ -#VRML_SIM R2021a utf8 +#VRML_SIM R2021b utf8 WorldInfo { } Viewpoint { diff --git a/worlds/e-puck_line.wbt b/worlds/e-puck_line.wbt index a65e11a..97007e2 100644 --- a/worlds/e-puck_line.wbt +++ b/worlds/e-puck_line.wbt @@ -1,4 +1,4 @@ -#VRML_SIM R2021a utf8 +#VRML_SIM R2021b utf8 WorldInfo { } Viewpoint { diff --git a/worlds/keyboard_teleop.wbt b/worlds/keyboard_teleop.wbt index d7a9f4a..afd0b69 100644 --- a/worlds/keyboard_teleop.wbt +++ b/worlds/keyboard_teleop.wbt @@ -1,4 +1,4 @@ -#VRML_SIM R2021a utf8 +#VRML_SIM R2021b utf8 WorldInfo { } Viewpoint { diff --git a/worlds/panoramic_view_recorder.wbt b/worlds/panoramic_view_recorder.wbt index f6e5f3d..4a48272 100644 --- a/worlds/panoramic_view_recorder.wbt +++ b/worlds/panoramic_view_recorder.wbt @@ -1,4 +1,4 @@ -#VRML_SIM R2021a utf8 +#VRML_SIM R2021b utf8 WorldInfo { } Viewpoint { diff --git a/worlds/pioneer3at.wbt b/worlds/pioneer3at.wbt index 53fb69c..8ca1183 100644 --- a/worlds/pioneer3at.wbt +++ b/worlds/pioneer3at.wbt @@ -1,4 +1,4 @@ -#VRML_SIM R2021a utf8 +#VRML_SIM R2021b utf8 WorldInfo { info [ "Pioneer 3AT robot (Adept MobileRobots)." diff --git a/worlds/ros_python.wbt b/worlds/ros_python.wbt index e47496b..99f9a22 100644 --- a/worlds/ros_python.wbt +++ b/worlds/ros_python.wbt @@ -1,4 +1,4 @@ -#VRML_SIM R2021a utf8 +#VRML_SIM R2021b utf8 WorldInfo { info [ "Simple Thymio II simulation controlled by a ROS node written in Python." From 4712dd316f9d7e2695f1a6d3fd9816f1a5b80a79 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Darko=20Luki=C4=87?= Date: Tue, 26 Jan 2021 17:19:45 +0100 Subject: [PATCH 02/72] Update package.xml --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index 16515a4..f77ed40 100644 --- a/package.xml +++ b/package.xml @@ -2,7 +2,7 @@ webots_ros - 3.0.0 + 4.1.0 The ROS package containing examples for interfacing ROS with the standard ROS controller of Webots http://wiki.ros.org/webots_ros From 1997cc6aeaa944727557d85f698bdd001ed700da Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Mon, 15 Feb 2021 15:42:28 +0100 Subject: [PATCH 03/72] Add message --- src/robot_information_parser.cpp | 4 +--- srv/node_get_string.srv | 3 +++ 2 files changed, 4 insertions(+), 3 deletions(-) create mode 100644 srv/node_get_string.srv diff --git a/src/robot_information_parser.cpp b/src/robot_information_parser.cpp index 7053891..4f803c8 100644 --- a/src/robot_information_parser.cpp +++ b/src/robot_information_parser.cpp @@ -90,10 +90,8 @@ int main(int argc, char **argv) { getTypeClient.call(getTypeSrv); if (getTypeSrv.response.value == 40) ROS_INFO("This controller is on a basic robot."); - else if (getTypeSrv.response.value == 41) - ROS_INFO("This controller is on a supervisor robot."); else - ROS_INFO("This controller is on a differential wheels robot."); + ROS_INFO("This controller is on a supervisor robot."); if (getModelClient.call(getModelSrv)) { if (!getModelSrv.response.value.empty()) diff --git a/srv/node_get_string.srv b/srv/node_get_string.srv new file mode 100644 index 0000000..2218a1a --- /dev/null +++ b/srv/node_get_string.srv @@ -0,0 +1,3 @@ +uint64 node +--- +string value From 8683de816e55b87344f33a140fac3cecca22008d Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Tue, 16 Feb 2021 13:01:35 +0100 Subject: [PATCH 04/72] Add node_export_string test --- CMakeLists.txt | 1 + src/complete_test.cpp | 590 +++++++++++++++++++++++++++--------------- 2 files changed, 386 insertions(+), 205 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ee87bdb..f6f8057 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -103,6 +103,7 @@ find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs sensor_msgs messag node_get_position.srv node_get_static_balance.srv node_get_status.srv + node_get_string.srv node_get_type.srv node_get_velocity.srv node_remove.srv diff --git a/src/complete_test.cpp b/src/complete_test.cpp index cd2678b..3ac0690 100644 --- a/src/complete_test.cpp +++ b/src/complete_test.cpp @@ -108,6 +108,7 @@ #include #include #include +#include #include #include #include @@ -150,64 +151,75 @@ static bool callbackCalled = false; ros::ServiceClient time_step_client; webots_ros::set_int time_step_srv; -void modelNameCallback(const std_msgs::String::ConstPtr &name) { +void modelNameCallback(const std_msgs::String::ConstPtr &name) +{ model_count++; model_list.push_back(name->data); ROS_INFO("Model #%d: %s.", model_count, model_list.back().c_str()); callbackCalled = true; } -void cameraCallback(const sensor_msgs::Image::ConstPtr &values) { +void cameraCallback(const sensor_msgs::Image::ConstPtr &values) +{ int i = 0; imageColor.resize(values->step * values->height); - for (std::vector::const_iterator it = values->data.begin(); it != values->data.end(); ++it) { + for (std::vector::const_iterator it = values->data.begin(); it != values->data.end(); ++it) + { imageColor[i] = *it; i++; } callbackCalled = true; } -void cameraRecognitionCallback(const webots_ros::RecognitionObject::ConstPtr &object) { +void cameraRecognitionCallback(const webots_ros::RecognitionObject::ConstPtr &object) +{ ROS_INFO("Camera recognition saw a '%s' (time: %d:%d).", object->model.c_str(), object->header.stamp.sec, object->header.stamp.nsec); callbackCalled = true; } -void segmentationCallback(const sensor_msgs::Image::ConstPtr &values) { +void segmentationCallback(const sensor_msgs::Image::ConstPtr &values) +{ ROS_INFO("Segmentation callback called."); int i = 0; imageColor.resize(values->step * values->height); - for (std::vector::const_iterator it = values->data.begin(); it != values->data.end(); ++it) { + for (std::vector::const_iterator it = values->data.begin(); it != values->data.end(); ++it) + { imageColor[i] = *it; i++; } callbackCalled = true; } -void joystickCallback(const webots_ros::Int8Stamped::ConstPtr &value) { +void joystickCallback(const webots_ros::Int8Stamped::ConstPtr &value) +{ ROS_INFO("Joystick button pressed: %d (time: %d:%d).", value->data, value->header.stamp.sec, value->header.stamp.nsec); callbackCalled = true; } -void keyboardCallback(const webots_ros::Int32Stamped::ConstPtr &value) { +void keyboardCallback(const webots_ros::Int32Stamped::ConstPtr &value) +{ ROS_INFO("Keyboard key pressed: %d (time: %d:%d).", value->data, value->header.stamp.sec, value->header.stamp.nsec); callbackCalled = true; } -void radarTargetsCallback(const webots_ros::RadarTarget::ConstPtr &target) { +void radarTargetsCallback(const webots_ros::RadarTarget::ConstPtr &target) +{ ROS_INFO("Received a radar target with distance=%lf received power=%lf speed=%lf azimuth=%lf (time: %d:%d).", target->distance, target->receivedPower, target->speed, target->azimuth, target->header.stamp.sec, target->header.stamp.nsec); callbackCalled = true; } -void radarTargetsNumberCallback(const webots_ros::Int8Stamped::ConstPtr &value) { +void radarTargetsNumberCallback(const webots_ros::Int8Stamped::ConstPtr &value) +{ ROS_INFO("Number of target seen by the radar: %d (time: %d:%d).", value->data, value->header.stamp.sec, value->header.stamp.nsec); callbackCalled = true; } -void rangeFinderCallback(const sensor_msgs::Image::ConstPtr &image) { +void rangeFinderCallback(const sensor_msgs::Image::ConstPtr &image) +{ int size = image->width * image->height; imageRangeFinder.resize(size); @@ -217,18 +229,21 @@ void rangeFinderCallback(const sensor_msgs::Image::ConstPtr &image) { callbackCalled = true; } -void lidarCallback(const sensor_msgs::Image::ConstPtr &image) { +void lidarCallback(const sensor_msgs::Image::ConstPtr &image) +{ callbackCalled = true; } -void connectorCallback(const webots_ros::Int8Stamped::ConstPtr &value) { +void connectorCallback(const webots_ros::Int8Stamped::ConstPtr &value) +{ connectorPresence = value->data; ROS_INFO("Connector presence: %d (time: %d:%d).", connectorPresence, value->header.stamp.sec, value->header.stamp.nsec); callbackCalled = true; } -void accelerometerCallback(const sensor_msgs::Imu::ConstPtr &values) { +void accelerometerCallback(const sensor_msgs::Imu::ConstPtr &values) +{ accelerometerValues[0] = values->linear_acceleration.x; accelerometerValues[1] = values->linear_acceleration.y; accelerometerValues[2] = values->linear_acceleration.z; @@ -238,12 +253,14 @@ void accelerometerCallback(const sensor_msgs::Imu::ConstPtr &values) { callbackCalled = true; } -void battery_sensorCallback(const webots_ros::Float64Stamped::ConstPtr &value) { +void battery_sensorCallback(const webots_ros::Float64Stamped::ConstPtr &value) +{ ROS_INFO("Battery level is %f (time: %d:%d).", value->data, value->header.stamp.sec, value->header.stamp.nsec); callbackCalled = true; } -void compassCallback(const sensor_msgs::MagneticField::ConstPtr &values) { +void compassCallback(const sensor_msgs::MagneticField::ConstPtr &values) +{ compassValues[0] = values->magnetic_field.x; compassValues[1] = values->magnetic_field.y; compassValues[2] = values->magnetic_field.z; @@ -253,12 +270,14 @@ void compassCallback(const sensor_msgs::MagneticField::ConstPtr &values) { callbackCalled = true; } -void distance_sensorCallback(const sensor_msgs::Range::ConstPtr &value) { +void distance_sensorCallback(const sensor_msgs::Range::ConstPtr &value) +{ ROS_INFO("Distance from object is %f (time: %d:%d).", value->range, value->header.stamp.sec, value->header.stamp.nsec); callbackCalled = true; } -void GPSCallback(const geometry_msgs::PointStamped::ConstPtr &values) { +void GPSCallback(const geometry_msgs::PointStamped::ConstPtr &values) +{ GPSValues[0] = values->point.x; GPSValues[1] = values->point.y; GPSValues[2] = values->point.z; @@ -268,12 +287,14 @@ void GPSCallback(const geometry_msgs::PointStamped::ConstPtr &values) { callbackCalled = true; } -void GPSSpeedCallback(const webots_ros::Float64Stamped::ConstPtr &value) { +void GPSSpeedCallback(const webots_ros::Float64Stamped::ConstPtr &value) +{ ROS_INFO("GPS speed is: %fkm/h (time: %d:%d).", value->data, value->header.stamp.sec, value->header.stamp.nsec); callbackCalled = true; } -void gyroCallback(const sensor_msgs::Imu::ConstPtr &values) { +void gyroCallback(const sensor_msgs::Imu::ConstPtr &values) +{ GyroValues[0] = values->angular_velocity.x; GyroValues[1] = values->angular_velocity.y; GyroValues[2] = values->angular_velocity.z; @@ -283,7 +304,8 @@ void gyroCallback(const sensor_msgs::Imu::ConstPtr &values) { callbackCalled = true; } -void inertialUnitCallback(const sensor_msgs::Imu::ConstPtr &values) { +void inertialUnitCallback(const sensor_msgs::Imu::ConstPtr &values) +{ inertialUnitValues[0] = values->orientation.x; inertialUnitValues[1] = values->orientation.y; inertialUnitValues[2] = values->orientation.z; @@ -295,32 +317,38 @@ void inertialUnitCallback(const sensor_msgs::Imu::ConstPtr &values) { callbackCalled = true; } -void lightSensorCallback(const sensor_msgs::Illuminance::ConstPtr &value) { +void lightSensorCallback(const sensor_msgs::Illuminance::ConstPtr &value) +{ ROS_INFO("Light intensity is %f.", value->illuminance); callbackCalled = true; } -void motorSensorCallback(const webots_ros::Float64Stamped::ConstPtr &value) { +void motorSensorCallback(const webots_ros::Float64Stamped::ConstPtr &value) +{ ROS_INFO("Motor sensor sent value %f.", value->data); callbackCalled = true; } -void positionSensorCallback(const webots_ros::Float64Stamped::ConstPtr &value) { +void positionSensorCallback(const webots_ros::Float64Stamped::ConstPtr &value) +{ ROS_INFO("Position sensor sent value %f (time: %d:%d).", value->data, value->header.stamp.sec, value->header.stamp.nsec); callbackCalled = true; } -void touchSensorCallback(const webots_ros::Float64Stamped::ConstPtr &value) { +void touchSensorCallback(const webots_ros::Float64Stamped::ConstPtr &value) +{ ROS_INFO("Touch sensor sent value %f (time: %d:%d).", value->data, value->header.stamp.sec, value->header.stamp.nsec); callbackCalled = true; } -void touchSensorBumperCallback(const webots_ros::BoolStamped::ConstPtr &value) { +void touchSensorBumperCallback(const webots_ros::BoolStamped::ConstPtr &value) +{ ROS_INFO("Touch sensor sent value %d (time: %d:%d).", value->data, value->header.stamp.sec, value->header.stamp.nsec); callbackCalled = true; } -void touchSensor3DCallback(const geometry_msgs::WrenchStamped::ConstPtr &values) { +void touchSensor3DCallback(const geometry_msgs::WrenchStamped::ConstPtr &values) +{ touchSensorValues[0] = values->wrench.force.x; touchSensorValues[1] = values->wrench.force.y; touchSensorValues[2] = values->wrench.force.z; @@ -330,13 +358,15 @@ void touchSensor3DCallback(const geometry_msgs::WrenchStamped::ConstPtr &values) callbackCalled = true; } -void receiverCallback(const webots_ros::StringStamped::ConstPtr &value) { +void receiverCallback(const webots_ros::StringStamped::ConstPtr &value) +{ char *message = const_cast(value->data.c_str()); ROS_INFO("Received a message %s.", message); callbackCalled = true; } -void quit(int sig) { +void quit(int sig) +{ time_step_srv.request.value = 0; time_step_client.call(time_step_srv); ROS_INFO("User stopped the 'complete_test' node."); @@ -344,7 +374,8 @@ void quit(int sig) { exit(0); } -int main(int argc, char **argv) { +int main(int argc, char **argv) +{ string model_name = "my_robot"; ros::init(argc, argv, "complete_test", ros::init_options::AnonymousName); @@ -353,7 +384,8 @@ int main(int argc, char **argv) { signal(SIGINT, quit); ros::Subscriber name_sub = n.subscribe("model_name", 100, modelNameCallback); - while (model_count == 0 || model_count < name_sub.getNumPublishers()) { + while (model_count == 0 || model_count < name_sub.getNumPublishers()) + { ros::spinOnce(); ros::spinOnce(); ros::spinOnce(); @@ -380,27 +412,31 @@ int main(int argc, char **argv) { ROS_ERROR("Failed to call service time_step to update robot's time step."); ros::ServiceClient get_number_of_devices_client = - n.serviceClient(model_name + "/robot/get_number_of_devices"); + n.serviceClient(model_name + "/robot/get_number_of_devices"); webots_ros::get_int get_number_of_devices_srv; - if (get_number_of_devices_client.call(get_number_of_devices_srv)) { + if (get_number_of_devices_client.call(get_number_of_devices_srv)) + { int number_of_devices = get_number_of_devices_srv.response.value; ROS_INFO("%s has %d devices.", model_name.c_str(), number_of_devices); - } else + } + else ROS_ERROR("Failed to call service get_number_of_devices."); get_number_of_devices_client.shutdown(); time_step_client.call(time_step_srv); ros::ServiceClient device_list_client = - n.serviceClient(model_name + "/robot/get_device_list"); + n.serviceClient(model_name + "/robot/get_device_list"); webots_ros::robot_get_device_list device_list_srv; - if (device_list_client.call(device_list_srv)) { + if (device_list_client.call(device_list_srv)) + { device_list = device_list_srv.response.list; for (unsigned int i = 0; i < device_list.size(); i++) ROS_INFO("Device [%d]: %s.", i, device_list[i].c_str()); - } else + } + else ROS_ERROR("Failed to call service device_list."); device_list_client.shutdown(); @@ -410,39 +446,45 @@ int main(int argc, char **argv) { webots_ros::get_urdf urdf_srv; urdf_srv.request.prefix = "unique_robot_prefix_name_"; - if (urdf_client.call(urdf_srv)) { + if (urdf_client.call(urdf_srv)) + { std::string urdf = urdf_srv.response.value; if (urdf.find(urdf_srv.request.prefix) == std::string::npos) ROS_ERROR("Invalid response from get_urdf."); else ROS_INFO("URDF has been successfully obtained."); - } else + } + else ROS_ERROR("Failed to call service get_urdf."); urdf_client.shutdown(); time_step_client.call(time_step_srv); ros::ServiceClient get_basic_time_step_client = - n.serviceClient(model_name + "/robot/get_basic_time_step"); + n.serviceClient(model_name + "/robot/get_basic_time_step"); webots_ros::get_float get_basic_time_step_srv; - if (get_basic_time_step_client.call(get_basic_time_step_srv)) { + if (get_basic_time_step_client.call(get_basic_time_step_srv)) + { double basic_time_step = get_basic_time_step_srv.response.value; ROS_INFO("%s has a basic time step of %f.", model_name.c_str(), basic_time_step); - } else + } + else ROS_ERROR("Failed to call service get_basic_time_step."); get_basic_time_step_client.shutdown(); time_step_client.call(time_step_srv); ros::ServiceClient robot_get_custom_data_client = - n.serviceClient(model_name + "/robot/get_custom_data"); + n.serviceClient(model_name + "/robot/get_custom_data"); webots_ros::get_string robot_get_custom_data_srv; - if (robot_get_custom_data_client.call(robot_get_custom_data_srv)) { + if (robot_get_custom_data_client.call(robot_get_custom_data_srv)) + { data = robot_get_custom_data_srv.response.value; ROS_INFO("Data of %s is %s.", model_name.c_str(), data.c_str()); - } else + } + else ROS_ERROR("Failed to call service robot_get_custom_data."); robot_get_custom_data_client.shutdown(); @@ -451,10 +493,12 @@ int main(int argc, char **argv) { ros::ServiceClient get_mode_client = n.serviceClient(model_name + "/robot/get_mode"); webots_ros::get_int get_mode_srv; - if (get_mode_client.call(get_mode_srv)) { + if (get_mode_client.call(get_mode_srv)) + { mode = get_mode_srv.response.value; ROS_INFO("Mode of %s is %d.", model_name.c_str(), mode); - } else + } + else ROS_ERROR("Failed to call service get_mode."); get_mode_client.shutdown(); @@ -463,10 +507,12 @@ int main(int argc, char **argv) { ros::ServiceClient get_model_client = n.serviceClient(model_name + "/robot/get_model"); webots_ros::get_string get_model_srv; - if (get_model_client.call(get_model_srv)) { + if (get_model_client.call(get_model_srv)) + { model = get_model_srv.response.value; ROS_INFO("Model of %s is %s.", model_name.c_str(), model.c_str()); - } else + } + else ROS_ERROR("Failed to call service get_model."); get_model_client.shutdown(); @@ -475,10 +521,12 @@ int main(int argc, char **argv) { ros::ServiceClient get_project_path_client = n.serviceClient(model_name + "/robot/get_project_path"); webots_ros::get_string get_project_path_srv; - if (get_project_path_client.call(get_project_path_srv)) { + if (get_project_path_client.call(get_project_path_srv)) + { path = get_project_path_srv.response.value; ROS_INFO("World path of %s is %s.", model_name.c_str(), path.c_str()); - } else + } + else ROS_ERROR("Failed to call service get_project_path."); get_project_path_client.shutdown(); @@ -488,10 +536,12 @@ int main(int argc, char **argv) { ros::ServiceClient get_world_path_client = n.serviceClient(model_name + "/robot/get_world_path"); webots_ros::get_string get_world_path_srv; - if (get_world_path_client.call(get_world_path_srv)) { + if (get_world_path_client.call(get_world_path_srv)) + { path = get_world_path_srv.response.value; ROS_INFO("Project path of %s is %s.", model_name.c_str(), path.c_str()); - } else + } + else ROS_ERROR("Failed to call service get_project_path."); get_world_path_client.shutdown(); @@ -500,28 +550,32 @@ int main(int argc, char **argv) { ros::ServiceClient get_supervisor_client = n.serviceClient(model_name + "/robot/get_supervisor"); webots_ros::get_bool get_supervisor_srv; - if (get_supervisor_client.call(get_supervisor_srv)) { + if (get_supervisor_client.call(get_supervisor_srv)) + { if (get_supervisor_srv.response.value) ROS_INFO("%s is a supervisor.", model_name.c_str()); else ROS_ERROR("%s isn't a supervisor.", model_name.c_str()); - } else + } + else ROS_ERROR("Failed to call service get_synchronization."); get_supervisor_client.shutdown(); time_step_client.call(time_step_srv); ros::ServiceClient get_synchronization_client = - n.serviceClient(model_name + "/robot/get_synchronization"); + n.serviceClient(model_name + "/robot/get_synchronization"); webots_ros::get_bool get_synchronization_srv; - if (get_synchronization_client.call(get_synchronization_srv)) { + if (get_synchronization_client.call(get_synchronization_srv)) + { bool synchronization = get_synchronization_srv.response.value; if (synchronization) ROS_INFO("%s is sync.", model_name.c_str()); else ROS_INFO("%s isn't sync.", model_name.c_str()); - } else + } + else ROS_ERROR("Failed to call service get_synchronization."); get_synchronization_client.shutdown(); @@ -530,10 +584,12 @@ int main(int argc, char **argv) { ros::ServiceClient get_time_client = n.serviceClient(model_name + "/robot/get_time"); webots_ros::get_float get_time_srv; - if (get_time_client.call(get_time_srv)) { + if (get_time_client.call(get_time_srv)) + { double time = get_time_srv.response.value; ROS_INFO("Time for %s is %f.", model_name.c_str(), time); - } else + } + else ROS_ERROR("Failed to call service get_time."); get_time_client.shutdown(); @@ -542,24 +598,28 @@ int main(int argc, char **argv) { ros::ServiceClient get_type_client = n.serviceClient(model_name + "/robot/get_type"); webots_ros::get_int get_type_srv; - if (get_type_client.call(get_type_srv)) { + if (get_type_client.call(get_type_srv)) + { int type = get_type_srv.response.value; ROS_INFO("Type of %s is %d.", model_name.c_str(), type); - } else + } + else ROS_ERROR("Failed to call service get_type."); get_type_client.shutdown(); time_step_client.call(time_step_srv); ros::ServiceClient robot_set_custom_data_client = - n.serviceClient(model_name + "/robot/set_custom_data"); + n.serviceClient(model_name + "/robot/set_custom_data"); webots_ros::set_string robot_set_custom_data_srv; robot_set_custom_data_srv.request.value = "OVERWRITTEN"; - if (robot_set_custom_data_client.call(robot_set_custom_data_srv)) { + if (robot_set_custom_data_client.call(robot_set_custom_data_srv)) + { if (robot_set_custom_data_srv.response.success) ROS_INFO("Data of %s has been set to %s.", model_name.c_str(), data.c_str()); - } else + } + else ROS_ERROR("Failed to call service robot_set_custom_data."); robot_set_custom_data_client.shutdown(); @@ -569,10 +629,12 @@ int main(int argc, char **argv) { webots_ros::robot_set_mode set_mode_srv; set_mode_srv.request.mode = mode; - if (set_mode_client.call(set_mode_srv)) { + if (set_mode_client.call(set_mode_srv)) + { if (set_mode_srv.response.success == 1) ROS_INFO("Mode of %s has been set to %d.", model_name.c_str(), mode); - } else + } + else ROS_ERROR("Failed to call service set_mode."); set_mode_client.shutdown(); @@ -583,24 +645,27 @@ int main(int argc, char **argv) { ros::Subscriber sub_keyboard; enable_keyboard_srv.request.value = 32; - if (enable_keyboard_client.call(enable_keyboard_srv) && enable_keyboard_srv.response.success) { + if (enable_keyboard_client.call(enable_keyboard_srv) && enable_keyboard_srv.response.success) + { ROS_INFO("Keyboard of %s has been enabled.", model_name.c_str()); sub_keyboard = n.subscribe(model_name + "/keyboard/key", 1, keyboardCallback); ROS_INFO("Topics for keyboard initialized."); callbackCalled = false; - while (sub_keyboard.getNumPublishers() == 0 && !callbackCalled) { + while (sub_keyboard.getNumPublishers() == 0 && !callbackCalled) + { ros::spinOnce(); time_step_client.call(time_step_srv); } ROS_INFO("Topics for keyboard connected."); - } else + } + else ROS_ERROR("Failed to enable keyboard."); ros::ServiceClient wait_for_user_input_event_client = - n.serviceClient(model_name + "/robot/wait_for_user_input_event"); + n.serviceClient(model_name + "/robot/wait_for_user_input_event"); webots_ros::robot_wait_for_user_input_event wait_for_user_input_event_srv; - wait_for_user_input_event_srv.request.eventType = 4; // WB_EVENT_KEYBOARD + wait_for_user_input_event_srv.request.eventType = 4; // WB_EVENT_KEYBOARD wait_for_user_input_event_srv.request.timeout = 20; if (wait_for_user_input_event_client.call(wait_for_user_input_event_srv)) ROS_INFO("Detected user input event: %d.", wait_for_user_input_event_srv.response.event); @@ -632,14 +697,16 @@ int main(int argc, char **argv) { // brake_get_motor_name ros::ServiceClient brake_get_motor_name_client = - n.serviceClient(model_name + "/my_brake/get_motor_name"); + n.serviceClient(model_name + "/my_brake/get_motor_name"); webots_ros::get_string brake_get_motor_name_srv; - if (brake_get_motor_name_client.call(brake_get_motor_name_srv)) { + if (brake_get_motor_name_client.call(brake_get_motor_name_srv)) + { ROS_INFO("Linear motor name returned from Brake API %s.", brake_get_motor_name_srv.response.value.c_str()); if (brake_get_motor_name_srv.response.value.compare("linear_motor") != 0) ROS_ERROR("Failed to call service brake_get_motor_name: received '%s' instead of 'linear_motor'", brake_get_motor_name_srv.response.value.c_str()); - } else + } + else ROS_ERROR("Failed to call service brake_get_motor_name."); brake_get_motor_name_client.shutdown(); @@ -682,17 +749,21 @@ int main(int argc, char **argv) { enable_camera_client = n.serviceClient(model_name + "/camera/enable"); camera_srv.request.value = TIME_STEP; - if (enable_camera_client.call(camera_srv) && camera_srv.response.success) { + if (enable_camera_client.call(camera_srv) && camera_srv.response.success) + { ROS_INFO("Camera enabled."); sub_camera_color = n.subscribe(model_name + "/camera/image", 1, cameraCallback); ROS_INFO("Topic for camera color initialized."); callbackCalled = false; - while (sub_camera_color.getNumPublishers() == 0 && !callbackCalled) { + while (sub_camera_color.getNumPublishers() == 0 && !callbackCalled) + { ros::spinOnce(); time_step_client.call(time_step_srv); } ROS_INFO("Topic for camera color connected."); - } else { + } + else + { if (camera_srv.response.success == -1) ROS_ERROR("Sampling period is not valid."); ROS_ERROR("Failed to enable camera."); @@ -790,17 +861,21 @@ int main(int argc, char **argv) { enable_camera_recognition_client = n.serviceClient(model_name + "/camera/recognition_enable"); camera_recognition_srv.request.value = TIME_STEP; - if (enable_camera_recognition_client.call(camera_recognition_srv) && camera_recognition_srv.response.success) { + if (enable_camera_recognition_client.call(camera_recognition_srv) && camera_recognition_srv.response.success) + { ROS_INFO("Camera recognition enabled."); sub_camera_recognition = n.subscribe(model_name + "/camera/recognition_objects", 1, cameraRecognitionCallback); ROS_INFO("Topic for camera recognition initialized."); callbackCalled = false; - while (sub_camera_recognition.getNumPublishers() == 0 && !callbackCalled) { + while (sub_camera_recognition.getNumPublishers() == 0 && !callbackCalled) + { ros::spinOnce(); time_step_client.call(time_step_srv); } ROS_INFO("Topic for camera recognition connected."); - } else { + } + else + { if (camera_recognition_srv.response.success == -1) ROS_ERROR("Sampling period is not valid."); ROS_ERROR("Failed to enable camera recognition."); @@ -813,24 +888,27 @@ int main(int argc, char **argv) { // camera recognition segmentation is enabled ros::ServiceClient has_segmentation_client = - n.serviceClient(model_name + "/camera/recognition_has_segmentation"); + n.serviceClient(model_name + "/camera/recognition_has_segmentation"); webots_ros::get_bool has_segmentation_srv; - if (has_segmentation_client.call(has_segmentation_srv)) { + if (has_segmentation_client.call(has_segmentation_srv)) + { if (has_segmentation_srv.response.value) ROS_INFO("Camera recognition segmentation field is TRUE."); else ROS_INFO("Camera recognition segmentation field is FALSE."); - } else + } + else ROS_ERROR("Failed to get segmentation field value."); has_segmentation_client.shutdown(); // camera recognition enable segmentation ros::ServiceClient enable_segmentation_client = - n.serviceClient(model_name + "/camera/recognition_enable_segmentation"); + n.serviceClient(model_name + "/camera/recognition_enable_segmentation"); webots_ros::get_bool enable_segmentation_srv; if (enable_segmentation_client.call(enable_segmentation_srv) && enable_segmentation_srv.response.value) ROS_INFO("Segmentation correctly available."); - else { + else + { if (!enable_segmentation_srv.response.value) ROS_ERROR("Segmentation value could not be retrieved correctly."); ROS_ERROR("Failed to retrieve segmentation value."); @@ -841,11 +919,12 @@ int main(int argc, char **argv) { time_step_client.call(time_step_srv); ros::ServiceClient is_segmentation_enabled_client = - n.serviceClient(model_name + "/camera/recognition_is_segmentation_enabled"); + n.serviceClient(model_name + "/camera/recognition_is_segmentation_enabled"); webots_ros::get_bool is_segmentation_enabled_srv; if (is_segmentation_enabled_client.call(is_segmentation_enabled_srv) && is_segmentation_enabled_srv.response.value) ROS_INFO("Segmentation correctly enabled."); - else { + else + { if (!enable_segmentation_srv.response.value) ROS_ERROR("Failed to enable segmentation."); ROS_ERROR("Failed to query segmentation enabled status."); @@ -856,10 +935,11 @@ int main(int argc, char **argv) { time_step_client.call(time_step_srv); ros::Subscriber sub_segmentation = - n.subscribe(model_name + "/camera/recognition_segmentation_image", 1, segmentationCallback); + n.subscribe(model_name + "/camera/recognition_segmentation_image", 1, segmentationCallback); ROS_INFO("Topic for camera recognition segmentation initialized."); callbackCalled = false; - while (sub_segmentation.getNumPublishers() == 0 && !callbackCalled) { + while (sub_segmentation.getNumPublishers() == 0 && !callbackCalled) + { ros::spinOnce(); time_step_client.call(time_step_srv); } @@ -873,7 +953,7 @@ int main(int argc, char **argv) { // camera recognition save segmentation image ros::ServiceClient save_segmentation_image_client = - n.serviceClient(model_name + "/camera/recognition_save_segmentation_image"); + n.serviceClient(model_name + "/camera/recognition_save_segmentation_image"); webots_ros::save_image save_segmentation_image_srv; save_segmentation_image_srv.request.filename = std::string(getenv("HOME")) + std::string("/test_image_segmentation.png"); save_segmentation_image_srv.request.quality = 100; @@ -884,11 +964,12 @@ int main(int argc, char **argv) { // camera recognition disable segmentation ros::ServiceClient disable_segmentation_client = - n.serviceClient(model_name + "/camera/recognition_disable_segmentation"); + n.serviceClient(model_name + "/camera/recognition_disable_segmentation"); webots_ros::get_bool disable_segmentation_srv; if (disable_segmentation_client.call(disable_segmentation_srv) && disable_segmentation_srv.response.value) ROS_INFO("Segmentation correctly disabled."); - else { + else + { if (!disable_segmentation_srv.response.value) ROS_ERROR("Segmentation value could not be disabled."); ROS_ERROR("Failed to set segmentation."); @@ -952,14 +1033,18 @@ int main(int argc, char **argv) { set_accelerometer_client = n.serviceClient(model_name + "/accelerometer/enable"); accelerometer_srv.request.value = 64; - if (set_accelerometer_client.call(accelerometer_srv) && accelerometer_srv.response.success) { + if (set_accelerometer_client.call(accelerometer_srv) && accelerometer_srv.response.success) + { sub_accelerometer_64 = n.subscribe(model_name + "/accelerometer/values", 1, accelerometerCallback); callbackCalled = false; - while (sub_accelerometer_64.getNumPublishers() == 0 && !callbackCalled) { + while (sub_accelerometer_64.getNumPublishers() == 0 && !callbackCalled) + { ros::spinOnce(); time_step_client.call(time_step_srv); } - } else { + } + else + { if (accelerometer_srv.response.success == -1) ROS_ERROR("Sampling period is not valid."); ROS_ERROR("Failed to enable accelerometer."); @@ -973,19 +1058,23 @@ int main(int argc, char **argv) { webots_ros::get_int sampling_period_accelerometer_srv; sampling_period_accelerometer_client = - n.serviceClient(model_name + "/accelerometer/get_sampling_period"); + n.serviceClient(model_name + "/accelerometer/get_sampling_period"); sampling_period_accelerometer_client.call(sampling_period_accelerometer_srv); ROS_INFO("Accelerometer is enabled with a sampling period of %d.", sampling_period_accelerometer_srv.response.value); accelerometer_srv.request.value = 32; - if (set_accelerometer_client.call(accelerometer_srv) && accelerometer_srv.response.success) { + if (set_accelerometer_client.call(accelerometer_srv) && accelerometer_srv.response.success) + { sub_accelerometer_32 = n.subscribe(model_name + "/accelerometer/values", 1, accelerometerCallback); callbackCalled = false; - while (sub_accelerometer_32.getNumPublishers() == 0 && !callbackCalled) { + while (sub_accelerometer_32.getNumPublishers() == 0 && !callbackCalled) + { ros::spinOnce(); time_step_client.call(time_step_srv); } - } else { + } + else + { if (accelerometer_srv.response.success == -1) ROS_ERROR("Sampling period is not valid."); ROS_ERROR("Failed to enable accelerometer."); @@ -995,7 +1084,7 @@ int main(int argc, char **argv) { ros::ServiceClient lookup_table_accelerometer_client; webots_ros::get_float_array lookup_table_accelerometer_srv; lookup_table_accelerometer_client = - n.serviceClient(model_name + "/accelerometer/get_lookup_table"); + n.serviceClient(model_name + "/accelerometer/get_lookup_table"); if (lookup_table_accelerometer_client.call(lookup_table_accelerometer_srv)) ROS_INFO("Accelerometer lookup table size = %lu.", lookup_table_accelerometer_srv.response.value.size()); else @@ -1034,18 +1123,22 @@ int main(int argc, char **argv) { ros::ServiceClient sampling_period_battery_sensor_client; webots_ros::get_int sampling_period_battery_sensor_srv; sampling_period_battery_sensor_client = - n.serviceClient(model_name + "/battery_sensor/get_sampling_period"); + n.serviceClient(model_name + "/battery_sensor/get_sampling_period"); battery_sensor_srv.request.value = 32; - if (set_battery_sensor_client.call(battery_sensor_srv) && battery_sensor_srv.response.success) { + if (set_battery_sensor_client.call(battery_sensor_srv) && battery_sensor_srv.response.success) + { ROS_INFO("Battery_sensor enabled."); sub_battery_sensor_32 = n.subscribe(model_name + "/battery_sensor/value", 1, battery_sensorCallback); callbackCalled = false; - while (sub_battery_sensor_32.getNumPublishers() == 0 && !callbackCalled) { + while (sub_battery_sensor_32.getNumPublishers() == 0 && !callbackCalled) + { ros::spinOnce(); time_step_client.call(time_step_srv); } - } else { + } + else + { if (!battery_sensor_srv.response.success) ROS_ERROR("Sampling period is not valid."); ROS_ERROR("Failed to enable battery_sensor."); @@ -1086,15 +1179,19 @@ int main(int argc, char **argv) { sampling_period_compass_client = n.serviceClient(model_name + "/compass/get_sampling_period"); compass_srv.request.value = 32; - if (set_compass_client.call(compass_srv) && compass_srv.response.success == 1) { + if (set_compass_client.call(compass_srv) && compass_srv.response.success == 1) + { ROS_INFO("Compass enabled."); sub_compass_32 = n.subscribe(model_name + "/compass/values", 1, compassCallback); callbackCalled = false; - while (sub_compass_32.getNumPublishers() == 0 && !callbackCalled) { + while (sub_compass_32.getNumPublishers() == 0 && !callbackCalled) + { ros::spinOnce(); time_step_client.call(time_step_srv); } - } else { + } + else + { if (compass_srv.response.success == -1) ROS_ERROR("Sampling period is not valid."); ROS_ERROR("Failed to enable compass."); @@ -1142,15 +1239,19 @@ int main(int argc, char **argv) { connector_enable_presence_client = n.serviceClient(model_name + "/connector/presence_sensor/enable"); connector_srv.request.value = 32; - if (connector_enable_presence_client.call(connector_srv) && connector_srv.response.success) { + if (connector_enable_presence_client.call(connector_srv) && connector_srv.response.success) + { ROS_INFO("Connector's presence sensor enabled."); sub_connector = n.subscribe(model_name + "/connector/presence", 1, connectorCallback); callbackCalled = false; - while (sub_connector.getNumPublishers() == 0 && !callbackCalled) { + while (sub_connector.getNumPublishers() == 0 && !callbackCalled) + { ros::spinOnce(); time_step_client.call(time_step_srv); } - } else { + } + else + { if (!connector_srv.response.success) ROS_ERROR("Sampling period is not valid."); ROS_ERROR("Failed to enable connector's presence sensor."); @@ -1166,7 +1267,8 @@ int main(int argc, char **argv) { connector_srv.request.value = 0; if (connector_enable_presence_client.call(connector_srv) && connector_srv.response.success) ROS_INFO("Connector's presence sensor disabled."); - else { + else + { if (!connector_srv.response.success) ROS_ERROR("Sampling period is not valid."); ROS_ERROR("Failed to disable connector's presence sensor."); @@ -1549,7 +1651,7 @@ int main(int argc, char **argv) { ros::ServiceClient sampling_period_distance_sensor_client; webots_ros::get_int sampling_period_distance_sensor_srv; sampling_period_distance_sensor_client = - n.serviceClient(model_name + "/distance_sensor/get_sampling_period"); + n.serviceClient(model_name + "/distance_sensor/get_sampling_period"); ros::ServiceClient min_value_distance_sensor_client; webots_ros::get_float min_value_distance_sensor_srv; @@ -1581,7 +1683,7 @@ int main(int argc, char **argv) { ros::ServiceClient lookup_table_distance_sensor_client; webots_ros::get_float_array lookup_table_distance_sensor_srv; lookup_table_distance_sensor_client = - n.serviceClient(model_name + "/distance_sensor/get_lookup_table"); + n.serviceClient(model_name + "/distance_sensor/get_lookup_table"); if (lookup_table_distance_sensor_client.call(lookup_table_distance_sensor_srv)) ROS_INFO("Distance_sensor lookup table size = %lu.", lookup_table_distance_sensor_srv.response.value.size()); else @@ -1592,15 +1694,19 @@ int main(int argc, char **argv) { lookup_table_distance_sensor_client.shutdown(); distance_sensor_srv.request.value = 32; - if (set_distance_sensor_client.call(distance_sensor_srv) && distance_sensor_srv.response.success) { + if (set_distance_sensor_client.call(distance_sensor_srv) && distance_sensor_srv.response.success) + { ROS_INFO("Distance_sensor enabled."); sub_distance_sensor_32 = n.subscribe(model_name + "/distance_sensor/value", 1, distance_sensorCallback); callbackCalled = false; - while (sub_distance_sensor_32.getNumPublishers() == 0 && !callbackCalled) { + while (sub_distance_sensor_32.getNumPublishers() == 0 && !callbackCalled) + { ros::spinOnce(); time_step_client.call(time_step_srv); } - } else { + } + else + { if (!distance_sensor_srv.response.success) ROS_ERROR("Sampling period is not valid."); ROS_ERROR("Failed to enable distance_sensor."); @@ -1718,11 +1824,13 @@ int main(int argc, char **argv) { sampling_period_GPS_client = n.serviceClient(model_name + "/gps/get_sampling_period"); GPS_srv.request.value = 32; - if (set_GPS_client.call(GPS_srv) && GPS_srv.response.success) { + if (set_GPS_client.call(GPS_srv) && GPS_srv.response.success) + { ROS_INFO("GPS enabled."); sub_GPS_32 = n.subscribe(model_name + "/gps/values", 1, GPSCallback); callbackCalled = false; - while (sub_GPS_32.getNumPublishers() == 0 && !callbackCalled) { + while (sub_GPS_32.getNumPublishers() == 0 && !callbackCalled) + { ros::spinOnce(); time_step_client.call(time_step_srv); } @@ -1731,11 +1839,14 @@ int main(int argc, char **argv) { sub_GPS_speed = n.subscribe(model_name + "/gps/speed", 1, GPSSpeedCallback); callbackCalled = false; - while (sub_GPS_speed.getNumPublishers() == 0 && !callbackCalled) { + while (sub_GPS_speed.getNumPublishers() == 0 && !callbackCalled) + { ros::spinOnce(); time_step_client.call(time_step_srv); } - } else { + } + else + { if (!GPS_srv.response.success) ROS_ERROR("Sampling period is not valid."); ROS_ERROR("Failed to enable GPS."); @@ -1779,15 +1890,19 @@ int main(int argc, char **argv) { sampling_period_gyro_client = n.serviceClient(model_name + "/gyro/get_sampling_period"); gyro_srv.request.value = 32; - if (set_gyro_client.call(gyro_srv) && gyro_srv.response.success) { + if (set_gyro_client.call(gyro_srv) && gyro_srv.response.success) + { ROS_INFO("Gyro enabled."); sub_gyro_32 = n.subscribe(model_name + "/gyro/values", 1, gyroCallback); callbackCalled = false; - while (sub_gyro_32.getNumPublishers() == 0 && !callbackCalled) { + while (sub_gyro_32.getNumPublishers() == 0 && !callbackCalled) + { ros::spinOnce(); time_step_client.call(time_step_srv); } - } else { + } + else + { if (!gyro_srv.response.success) ROS_ERROR("Sampling period is not valid."); ROS_ERROR("Failed to enable gyro."); @@ -1832,18 +1947,22 @@ int main(int argc, char **argv) { ros::ServiceClient sampling_period_inertial_unit_client; webots_ros::get_int sampling_period_inertial_unit_srv; sampling_period_inertial_unit_client = - n.serviceClient(model_name + "/inertial_unit/get_sampling_period"); + n.serviceClient(model_name + "/inertial_unit/get_sampling_period"); inertial_unit_srv.request.value = 32; - if (set_inertial_unit_client.call(inertial_unit_srv) && inertial_unit_srv.response.success) { + if (set_inertial_unit_client.call(inertial_unit_srv) && inertial_unit_srv.response.success) + { ROS_INFO("Inertial_unit enabled."); sub_inertial_unit_32 = n.subscribe(model_name + "/inertial_unit/quaternion", 1, inertialUnitCallback); callbackCalled = false; - while (sub_inertial_unit_32.getNumPublishers() == 0 && !callbackCalled) { + while (sub_inertial_unit_32.getNumPublishers() == 0 && !callbackCalled) + { ros::spinOnce(); time_step_client.call(time_step_srv); } - } else { + } + else + { if (!inertial_unit_srv.response.success) ROS_ERROR("Sampling period is not valid."); ROS_ERROR("Failed to enable inertial_unit."); @@ -1855,7 +1974,7 @@ int main(int argc, char **argv) { ros::ServiceClient noise_inertial_unit_client; webots_ros::get_float noise_inertial_unit_srv; noise_inertial_unit_client = - n.serviceClient(model_name + "/inertial_unit/get_noise"); + n.serviceClient(model_name + "/inertial_unit/get_noise"); if (noise_inertial_unit_client.call(noise_inertial_unit_srv)) ROS_INFO("Noise value is %f.", noise_inertial_unit_srv.response.value); else @@ -1885,18 +2004,21 @@ int main(int argc, char **argv) { ros::Subscriber sub_joystick; enable_joystick_srv.request.value = 32; - if (enable_joystick_client.call(enable_joystick_srv) && enable_joystick_srv.response.success) { + if (enable_joystick_client.call(enable_joystick_srv) && enable_joystick_srv.response.success) + { ROS_INFO("Joystick of %s has been enabled.", model_name.c_str()); sub_joystick = n.subscribe(model_name + "/joystick/pressed_button", 1, joystickCallback); callbackCalled = false; ROS_INFO("Topics for joystick initialized."); - while (sub_joystick.getNumPublishers() == 0 && !callbackCalled) { + while (sub_joystick.getNumPublishers() == 0 && !callbackCalled) + { ros::spinOnce(); time_step_client.call(time_step_srv); } ROS_INFO("Topics for joystick connected."); - } else + } + else ROS_ERROR("Failed to enable joystick."); sub_joystick.shutdown(); @@ -1958,18 +2080,22 @@ int main(int argc, char **argv) { set_lidar_client = n.serviceClient(model_name + "/lidar/enable"); lidar_srv.request.value = TIME_STEP; - if (set_lidar_client.call(lidar_srv) && lidar_srv.response.success) { + if (set_lidar_client.call(lidar_srv) && lidar_srv.response.success) + { ROS_INFO("Lidar enabled."); sub_lidar = n.subscribe(model_name + "/lidar/range_image", 1, lidarCallback); callbackCalled = false; ROS_INFO("Topic for lidar initialized."); - while (sub_lidar.getNumPublishers() == 0 && !callbackCalled) { + while (sub_lidar.getNumPublishers() == 0 && !callbackCalled) + { ros::spinOnce(); time_step_client.call(time_step_srv); } ROS_INFO("Topic for lidar color connected."); - } else { + } + else + { if (!lidar_srv.response.success) ROS_ERROR("Sampling period is not valid."); ROS_ERROR("Failed to enable lidar."); @@ -2023,15 +2149,19 @@ int main(int argc, char **argv) { sampling_period_light_sensor_client = n.serviceClient(model_name + "/light_sensor/get_sampling_period"); light_sensor_srv.request.value = 32; - if (set_light_sensor_client.call(light_sensor_srv) && light_sensor_srv.response.success) { + if (set_light_sensor_client.call(light_sensor_srv) && light_sensor_srv.response.success) + { ROS_INFO("Light_sensor enabled."); sub_light_sensor_32 = n.subscribe(model_name + "/light_sensor/value", 1, lightSensorCallback); callbackCalled = false; - while (sub_light_sensor_32.getNumPublishers() == 0 && !callbackCalled) { + while (sub_light_sensor_32.getNumPublishers() == 0 && !callbackCalled) + { ros::spinOnce(); time_step_client.call(time_step_srv); } - } else { + } + else + { if (!light_sensor_srv.response.success) ROS_ERROR("Sampling period is not valid."); ROS_ERROR("Failed to enable light_sensor."); @@ -2043,7 +2173,7 @@ int main(int argc, char **argv) { ros::ServiceClient lookup_table_light_sensor_client; webots_ros::get_float_array lookup_table_light_sensor_srv; lookup_table_light_sensor_client = - n.serviceClient(model_name + "/light_sensor/get_lookup_table"); + n.serviceClient(model_name + "/light_sensor/get_lookup_table"); if (lookup_table_light_sensor_client.call(lookup_table_light_sensor_srv)) ROS_INFO("Light sensor lookup table size = %lu.", lookup_table_light_sensor_srv.response.value.size()); else @@ -2092,12 +2222,14 @@ int main(int argc, char **argv) { ros::ServiceClient motor_get_brake_name_client; webots_ros::get_string motor_get_brake_name_srv; motor_get_brake_name_client = n.serviceClient(model_name + "/linear_motor/get_brake_name"); - if (motor_get_brake_name_client.call(motor_get_brake_name_srv)) { + if (motor_get_brake_name_client.call(motor_get_brake_name_srv)) + { ROS_INFO("Brake name returned from Motor API: %s.", motor_get_brake_name_srv.response.value.c_str()); if (motor_get_brake_name_srv.response.value.compare("my_brake") != 0) ROS_ERROR("Failed to call service motor_get_brake_name: received '%s' instead of 'my_brake'", motor_get_brake_name_srv.response.value.c_str()); - } else + } + else ROS_ERROR("Failed to call service motor_get_brake_name."); motor_get_brake_name_client.shutdown(); @@ -2198,7 +2330,7 @@ int main(int argc, char **argv) { ros::ServiceClient set_linear_control_pid_client; webots_ros::motor_set_control_pid set_linear_control_pid_srv; set_linear_control_pid_client = - n.serviceClient(model_name + "/linear_motor/set_control_pid"); + n.serviceClient(model_name + "/linear_motor/set_control_pid"); set_linear_control_pid_srv.request.controlp = 1; if (set_linear_control_pid_client.call(set_linear_control_pid_srv) && set_linear_control_pid_srv.response.success == 1) @@ -2341,23 +2473,27 @@ int main(int argc, char **argv) { webots_ros::set_int motor_feedback_srv; ros::Subscriber sub_motor_feedback_32; set_motor_feedback_client = - n.serviceClient(model_name + "/rotational_motor/torque_feedback_sensor/enable"); + n.serviceClient(model_name + "/rotational_motor/torque_feedback_sensor/enable"); ros::ServiceClient sampling_period_motor_feedback_client; webots_ros::get_int sampling_period_motor_feedback_srv; sampling_period_motor_feedback_client = - n.serviceClient(model_name + "/rotational_motor/torque_feedback_sensor/get_sampling_period"); + n.serviceClient(model_name + "/rotational_motor/torque_feedback_sensor/get_sampling_period"); motor_feedback_srv.request.value = 32; - if (set_motor_feedback_client.call(motor_feedback_srv) && motor_feedback_srv.response.success) { + if (set_motor_feedback_client.call(motor_feedback_srv) && motor_feedback_srv.response.success) + { ROS_INFO("Motor feedback enabled."); sub_motor_feedback_32 = n.subscribe(model_name + "/rotational_motor/torque_feedback", 1, motorSensorCallback); callbackCalled = false; - while (sub_motor_feedback_32.getNumPublishers() == 0 && !callbackCalled) { + while (sub_motor_feedback_32.getNumPublishers() == 0 && !callbackCalled) + { ros::spinOnce(); time_step_client.call(time_step_srv); } - } else { + } + else + { if (!motor_feedback_srv.response.success) ROS_ERROR("Sampling period is not valid."); ROS_ERROR("Failed to enable motor_feedback."); @@ -2424,18 +2560,22 @@ int main(int argc, char **argv) { ros::ServiceClient sampling_period_position_sensor_client; webots_ros::get_int sampling_period_position_sensor_srv; sampling_period_position_sensor_client = - n.serviceClient(model_name + "/position_sensor/get_sampling_period"); + n.serviceClient(model_name + "/position_sensor/get_sampling_period"); position_sensor_srv.request.value = 32; - if (set_position_sensor_client.call(position_sensor_srv) && position_sensor_srv.response.success) { + if (set_position_sensor_client.call(position_sensor_srv) && position_sensor_srv.response.success) + { ROS_INFO("Position_sensor enabled."); sub_position_sensor_32 = n.subscribe(model_name + "/position_sensor/value", 1, positionSensorCallback); callbackCalled = false; - while (sub_position_sensor_32.getNumPublishers() == 0 && !callbackCalled) { + while (sub_position_sensor_32.getNumPublishers() == 0 && !callbackCalled) + { ros::spinOnce(); time_step_client.call(time_step_srv); } - } else { + } + else + { if (!position_sensor_srv.response.success) ROS_ERROR("Sampling period is not valid."); ROS_ERROR("Failed to enable position_sensor."); @@ -2480,19 +2620,23 @@ int main(int argc, char **argv) { set_radar_client = n.serviceClient(model_name + "/radar/enable"); radar_srv.request.value = TIME_STEP; - if (set_radar_client.call(radar_srv) && radar_srv.response.success) { + if (set_radar_client.call(radar_srv) && radar_srv.response.success) + { ROS_INFO("Radar enabled."); sub_radar_target = n.subscribe(model_name + "/radar/targets", 1, radarTargetsCallback); sub_radar_target_number = n.subscribe(model_name + "/radar/number_of_targets", 1, radarTargetsNumberCallback); callbackCalled = false; ROS_INFO("Topics for radar initialized."); - while (sub_radar_target.getNumPublishers() == 0 && sub_radar_target_number.getNumPublishers() == 0 && !callbackCalled) { + while (sub_radar_target.getNumPublishers() == 0 && sub_radar_target_number.getNumPublishers() == 0 && !callbackCalled) + { ros::spinOnce(); time_step_client.call(time_step_srv); } ROS_INFO("Topics for radar connected."); - } else { + } + else + { if (!radar_srv.response.success) ROS_ERROR("Sampling period is not valid."); ROS_ERROR("Failed to enable radar."); @@ -2507,12 +2651,14 @@ int main(int argc, char **argv) { // get max and min range ros::ServiceClient radar_range_client = n.serviceClient(model_name + "/radar/get_max_range"); webots_ros::get_float radar_get_max_range_srv; - if (radar_range_client.call(radar_get_max_range_srv)) { + if (radar_range_client.call(radar_get_max_range_srv)) + { if (radar_get_max_range_srv.response.value == 2.0) ROS_INFO("Received correct radar max range."); else ROS_ERROR("Received wrong radar max range."); - } else + } + else ROS_ERROR("Failed to call service radar_get_max_range."); radar_range_client.shutdown(); @@ -2520,12 +2666,14 @@ int main(int argc, char **argv) { radar_range_client = n.serviceClient(model_name + "/radar/get_min_range"); webots_ros::get_float radar_get_min_range_srv; - if (radar_range_client.call(radar_get_min_range_srv)) { + if (radar_range_client.call(radar_get_min_range_srv)) + { if (radar_get_min_range_srv.response.value == 1.0) ROS_INFO("Received correct radar min range."); else ROS_ERROR("Received wrong radar min range."); - } else + } + else ROS_ERROR("Failed to call service radar_get_min_range."); radar_range_client.shutdown(); @@ -2544,18 +2692,22 @@ int main(int argc, char **argv) { set_range_finder_client = n.serviceClient(model_name + "/range_finder/enable"); range_finder_srv.request.value = TIME_STEP; - if (set_range_finder_client.call(range_finder_srv) && range_finder_srv.response.success) { + if (set_range_finder_client.call(range_finder_srv) && range_finder_srv.response.success) + { ROS_INFO("Range-finder enabled."); sub_range_finder_color = n.subscribe(model_name + "/range_finder/range_image", 1, rangeFinderCallback); callbackCalled = false; ROS_INFO("Topic for range-finder initialized."); - while (sub_range_finder_color.getNumPublishers() == 0 && !callbackCalled) { + while (sub_range_finder_color.getNumPublishers() == 0 && !callbackCalled) + { ros::spinOnce(); time_step_client.call(time_step_srv); } ROS_INFO("Topic for range-finder connected."); - } else { + } + else + { if (!range_finder_srv.response.success) ROS_ERROR("Sampling period is not valid."); ROS_ERROR("Failed to enable range-finder."); @@ -2571,10 +2723,10 @@ int main(int argc, char **argv) { webots_ros::range_finder_get_info get_range_finder_info_srv; if (get_info_client.call(get_range_finder_info_srv)) ROS_INFO( - "Range-finder of %s has a width of %d, a height of %d, a field of view of %f, a min range of %f and a max range of %f.", - model_name.c_str(), get_range_finder_info_srv.response.width, get_range_finder_info_srv.response.height, - get_range_finder_info_srv.response.Fov, get_range_finder_info_srv.response.minRange, - get_range_finder_info_srv.response.maxRange); + "Range-finder of %s has a width of %d, a height of %d, a field of view of %f, a min range of %f and a max range of %f.", + model_name.c_str(), get_range_finder_info_srv.response.width, get_range_finder_info_srv.response.height, + get_range_finder_info_srv.response.Fov, get_range_finder_info_srv.response.minRange, + get_range_finder_info_srv.response.maxRange); else ROS_ERROR("Failed to call service range_finder_get_info."); @@ -2611,15 +2763,19 @@ int main(int argc, char **argv) { sampling_period_receiver_client = n.serviceClient(model_name + "/receiver/get_sampling_period"); receiver_srv.request.value = 32; - if (set_receiver_client.call(receiver_srv) && receiver_srv.response.success) { + if (set_receiver_client.call(receiver_srv) && receiver_srv.response.success) + { ROS_INFO("Receiver enabled."); sub_receiver_32 = n.subscribe(model_name + "/receiver/data", 1, receiverCallback); callbackCalled = false; - while (sub_receiver_32.getNumPublishers() == 0 && !callbackCalled) { + while (sub_receiver_32.getNumPublishers() == 0 && !callbackCalled) + { ros::spinOnce(); time_step_client.call(time_step_srv); } - } else { + } + else + { if (!receiver_srv.response.success) ROS_ERROR("Sampling period is not valid."); ROS_ERROR("Failed to enable receiver."); @@ -2708,7 +2864,7 @@ int main(int argc, char **argv) { ros::ServiceClient receiver_get_emitter_direction_client; webots_ros::receiver_get_emitter_direction receiver_get_emitter_direction_srv; receiver_get_emitter_direction_client = - n.serviceClient(model_name + "/receiver/get_emitter_direction"); + n.serviceClient(model_name + "/receiver/get_emitter_direction"); receiver_get_emitter_direction_client.call(receiver_get_emitter_direction_srv); if (receiver_get_emitter_direction_srv.response.direction[0] != 0 || @@ -2769,7 +2925,8 @@ int main(int argc, char **argv) { time_step_client.call(time_step_srv); touch_sensor_srv.request.value = 32; - if (set_touch_sensor_client.call(touch_sensor_srv) && touch_sensor_srv.response.success) { + if (set_touch_sensor_client.call(touch_sensor_srv) && touch_sensor_srv.response.success) + { ROS_INFO("Touch_sensor enabled."); if (touch_sensor_get_type_srv.response.value == 0) sub_touch_sensor_32 = n.subscribe(model_name + "/touch_sensor/value", 1, touchSensorBumperCallback); @@ -2778,11 +2935,14 @@ int main(int argc, char **argv) { else sub_touch_sensor_32 = n.subscribe(model_name + "/touch_sensor/values", 1, touchSensor3DCallback); callbackCalled = false; - while (sub_touch_sensor_32.getNumPublishers() == 0 && !callbackCalled) { + while (sub_touch_sensor_32.getNumPublishers() == 0 && !callbackCalled) + { ros::spinOnce(); time_step_client.call(time_step_srv); } - } else { + } + else + { if (!touch_sensor_srv.response.success) ROS_ERROR("Sampling period is not valid."); ROS_ERROR("Failed to enable touch_sensor."); @@ -2794,7 +2954,7 @@ int main(int argc, char **argv) { ros::ServiceClient lookup_table_touch_sensor_client; webots_ros::get_float_array lookup_table_touch_sensor_srv; lookup_table_touch_sensor_client = - n.serviceClient(model_name + "/touch_sensor/get_lookup_table"); + n.serviceClient(model_name + "/touch_sensor/get_lookup_table"); if (lookup_table_touch_sensor_client.call(lookup_table_touch_sensor_srv)) ROS_INFO("Touch sensor lookup table size = %lu.", lookup_table_touch_sensor_srv.response.value.size()); else @@ -2824,7 +2984,7 @@ int main(int argc, char **argv) { ros::ServiceClient supervisor_simulation_reset_physics_client; webots_ros::get_bool supervisor_simulation_reset_physics_srv; supervisor_simulation_reset_physics_client = - n.serviceClient(model_name + "/supervisor/simulation_reset_physics"); + n.serviceClient(model_name + "/supervisor/simulation_reset_physics"); if (supervisor_simulation_reset_physics_client.call(supervisor_simulation_reset_physics_srv) && supervisor_simulation_reset_physics_srv.response.value) @@ -2893,15 +3053,17 @@ int main(int argc, char **argv) { ros::ServiceClient supervisor_get_from_def_client; webots_ros::supervisor_get_from_def supervisor_get_from_def_srv; supervisor_get_from_def_client = - n.serviceClient(model_name + "/supervisor/get_from_def"); + n.serviceClient(model_name + "/supervisor/get_from_def"); supervisor_get_from_def_srv.request.name = "TEST"; supervisor_get_from_def_client.call(supervisor_get_from_def_srv); uint64_t from_def_node = 0; - if (supervisor_get_from_def_srv.response.node != 0) { + if (supervisor_get_from_def_srv.response.node != 0) + { ROS_INFO("Got from DEF node: %ld.", supervisor_get_from_def_srv.response.node); from_def_node = supervisor_get_from_def_srv.response.node; - } else + } + else ROS_ERROR("Could not get node from DEF."); time_step_client.call(time_step_srv); @@ -2920,7 +3082,7 @@ int main(int argc, char **argv) { ros::ServiceClient supervisor_node_get_type_name_client; webots_ros::node_get_name supervisor_node_get_type_name_srv; supervisor_node_get_type_name_client = - n.serviceClient(model_name + "/supervisor/node/get_type_name"); + n.serviceClient(model_name + "/supervisor/node/get_type_name"); supervisor_node_get_type_name_srv.request.node = from_def_node; supervisor_node_get_type_name_client.call(supervisor_node_get_type_name_srv); @@ -2937,10 +3099,12 @@ int main(int argc, char **argv) { supervisor_get_from_def_srv.request.name = "GROUND"; supervisor_get_from_def_client.call(supervisor_get_from_def_srv); uint64_t ground_node = 0; - if (supervisor_get_from_def_srv.response.node != 0) { + if (supervisor_get_from_def_srv.response.node != 0) + { ROS_INFO("Got from DEF GROUND node: %ld.", supervisor_get_from_def_srv.response.node); ground_node = supervisor_get_from_def_srv.response.node; - } else + } + else ROS_ERROR("Could not get node from DEF GROUND."); supervisor_node_get_type_name_srv.request.node = ground_node; @@ -2952,7 +3116,7 @@ int main(int argc, char **argv) { ros::ServiceClient supervisor_node_get_base_type_name_client; webots_ros::node_get_name supervisor_node_get_base_type_name_srv; supervisor_node_get_base_type_name_client = - n.serviceClient(model_name + "/supervisor/node/get_base_type_name"); + n.serviceClient(model_name + "/supervisor/node/get_base_type_name"); supervisor_node_get_base_type_name_srv.request.node = ground_node; supervisor_node_get_base_type_name_client.call(supervisor_node_get_base_type_name_srv); ROS_INFO("Got base type name of GROUND node: %s.", supervisor_node_get_base_type_name_srv.response.name.c_str()); @@ -2973,7 +3137,7 @@ int main(int argc, char **argv) { ros::ServiceClient supervisor_node_get_position_client; webots_ros::node_get_position supervisor_node_get_position_srv; supervisor_node_get_position_client = - n.serviceClient(model_name + "/supervisor/node/get_position"); + n.serviceClient(model_name + "/supervisor/node/get_position"); supervisor_node_get_position_srv.request.node = from_def_node; supervisor_node_get_position_client.call(supervisor_node_get_position_srv); @@ -2986,14 +3150,14 @@ int main(int argc, char **argv) { ros::ServiceClient supervisor_node_get_orientation_client; webots_ros::node_get_orientation supervisor_node_get_orientation_srv; supervisor_node_get_orientation_client = - n.serviceClient(model_name + "/supervisor/node/get_orientation"); + n.serviceClient(model_name + "/supervisor/node/get_orientation"); supervisor_node_get_orientation_srv.request.node = from_def_node; supervisor_node_get_orientation_client.call(supervisor_node_get_orientation_srv); ROS_INFO( - "From_def orientation quaternion is:\nw=%f x=%f y=%f z=%f.", supervisor_node_get_orientation_srv.response.orientation.w, - supervisor_node_get_orientation_srv.response.orientation.x, supervisor_node_get_orientation_srv.response.orientation.y, - supervisor_node_get_orientation_srv.response.orientation.z); + "From_def orientation quaternion is:\nw=%f x=%f y=%f z=%f.", supervisor_node_get_orientation_srv.response.orientation.w, + supervisor_node_get_orientation_srv.response.orientation.x, supervisor_node_get_orientation_srv.response.orientation.y, + supervisor_node_get_orientation_srv.response.orientation.z); supervisor_node_get_orientation_client.shutdown(); time_step_client.call(time_step_srv); @@ -3001,7 +3165,7 @@ int main(int argc, char **argv) { ros::ServiceClient supervisor_node_get_center_of_mass_client; webots_ros::node_get_center_of_mass supervisor_node_get_center_of_mass_srv; supervisor_node_get_center_of_mass_client = - n.serviceClient(model_name + "/supervisor/node/get_center_of_mass"); + n.serviceClient(model_name + "/supervisor/node/get_center_of_mass"); supervisor_node_get_center_of_mass_srv.request.node = from_def_node; supervisor_node_get_center_of_mass_client.call(supervisor_node_get_center_of_mass_srv); @@ -3016,7 +3180,7 @@ int main(int argc, char **argv) { ros::ServiceClient supervisor_node_get_number_of_contact_points_client; webots_ros::node_get_number_of_contact_points supervisor_node_get_number_of_contact_points_srv; supervisor_node_get_number_of_contact_points_client = n.serviceClient( - model_name + "/supervisor/node/get_number_of_contact_points"); + model_name + "/supervisor/node/get_number_of_contact_points"); supervisor_node_get_number_of_contact_points_srv.request.node = from_def_node; supervisor_node_get_number_of_contact_points_srv.request.includeDescendants = false; @@ -3030,7 +3194,7 @@ int main(int argc, char **argv) { ros::ServiceClient supervisor_node_get_contact_point_client; webots_ros::node_get_contact_point supervisor_node_get_contact_point_srv; supervisor_node_get_contact_point_client = - n.serviceClient(model_name + "/supervisor/node/get_contact_point"); + n.serviceClient(model_name + "/supervisor/node/get_contact_point"); supervisor_node_get_contact_point_srv.request.node = from_def_node; supervisor_node_get_contact_point_srv.request.index = 0; @@ -3044,7 +3208,7 @@ int main(int argc, char **argv) { ros::ServiceClient supervisor_node_get_contact_point_node_client; webots_ros::node_get_contact_point_node supervisor_node_get_contact_point_node_srv; supervisor_node_get_contact_point_node_client = - n.serviceClient(model_name + "/supervisor/node/get_contact_point_node"); + n.serviceClient(model_name + "/supervisor/node/get_contact_point_node"); supervisor_node_get_contact_point_node_srv.request.node = from_def_node; supervisor_node_get_contact_point_node_srv.request.index = 0; @@ -3059,7 +3223,7 @@ int main(int argc, char **argv) { ros::ServiceClient supervisor_node_get_static_balance_client; webots_ros::node_get_static_balance supervisor_node_get_static_balance_srv; supervisor_node_get_static_balance_client = - n.serviceClient(model_name + "/supervisor/node/get_static_balance"); + n.serviceClient(model_name + "/supervisor/node/get_static_balance"); supervisor_node_get_static_balance_srv.request.node = from_def_node; supervisor_node_get_static_balance_client.call(supervisor_node_get_static_balance_srv); @@ -3071,7 +3235,7 @@ int main(int argc, char **argv) { // test reset_physics // if the node isn't a top Solid webots will throw a warning but still return true to ros ros::ServiceClient supervisor_node_reset_physics_client = - n.serviceClient(model_name + "/supervisor/node/reset_physics"); + n.serviceClient(model_name + "/supervisor/node/reset_physics"); webots_ros::node_reset_functions supervisor_node_reset_physics_srv; supervisor_node_reset_physics_srv.request.node = from_def_node; @@ -3086,7 +3250,7 @@ int main(int argc, char **argv) { // test restart_controller ros::ServiceClient supervisor_node_restart_controller_client = - n.serviceClient(model_name + "/supervisor/node/restart_controller"); + n.serviceClient(model_name + "/supervisor/node/restart_controller"); webots_ros::node_reset_functions supervisor_node_restart_controller_srv; supervisor_node_restart_controller_srv.request.node = from_def_node; @@ -3126,7 +3290,7 @@ int main(int argc, char **argv) { ros::ServiceClient supervisor_field_get_type_name_client; webots_ros::field_get_type_name supervisor_field_get_type_name_srv; supervisor_field_get_type_name_client = - n.serviceClient(model_name + "/supervisor/field/get_type_name"); + n.serviceClient(model_name + "/supervisor/field/get_type_name"); supervisor_field_get_type_name_srv.request.field = field; supervisor_field_get_type_name_client.call(supervisor_field_get_type_name_srv); @@ -3157,7 +3321,7 @@ int main(int argc, char **argv) { ros::ServiceClient supervisor_field_set_string_client; webots_ros::field_set_string supervisor_field_set_string_srv; supervisor_field_set_string_client = - n.serviceClient(model_name + "/supervisor/field/set_string"); + n.serviceClient(model_name + "/supervisor/field/set_string"); supervisor_field_set_string_srv.request.field = field; supervisor_field_set_string_srv.request.value = "solid_test"; @@ -3173,7 +3337,7 @@ int main(int argc, char **argv) { ros::ServiceClient supervisor_field_get_string_client; webots_ros::field_get_string supervisor_field_get_string_srv; supervisor_field_get_string_client = - n.serviceClient(model_name + "/supervisor/field/get_string"); + n.serviceClient(model_name + "/supervisor/field/get_string"); supervisor_field_get_string_srv.request.field = field; supervisor_field_get_string_client.call(supervisor_field_get_string_srv); @@ -3204,10 +3368,12 @@ int main(int argc, char **argv) { supervisor_get_from_def_srv.request.proto = 0; supervisor_get_from_def_client.call(supervisor_get_from_def_srv); uint64_t cone_node = 0; - if (supervisor_get_from_def_srv.response.node != 0) { + if (supervisor_get_from_def_srv.response.node != 0) + { ROS_INFO("Got CONE node from DEF: %lu.", supervisor_get_from_def_srv.response.node); cone_node = supervisor_get_from_def_srv.response.node; - } else + } + else ROS_ERROR("could not get CONE node from DEF."); supervisor_node_get_type_name_client.shutdown(); @@ -3247,7 +3413,7 @@ int main(int argc, char **argv) { ros::ServiceClient supervisor_get_from_device_client; webots_ros::supervisor_get_from_string supervisor_get_from_device_srv; supervisor_get_from_device_client = - n.serviceClient(model_name + "/supervisor/get_from_device"); + n.serviceClient(model_name + "/supervisor/get_from_device"); supervisor_get_from_device_srv.request.value = "compass"; supervisor_get_from_device_client.call(supervisor_get_from_device_srv); uint64_t compass_node_from_device = supervisor_get_from_device_srv.response.node; @@ -3295,7 +3461,7 @@ int main(int argc, char **argv) { ros::ServiceClient node_add_force_or_torque_client; webots_ros::node_add_force_or_torque node_add_force_or_torque_srv; node_add_force_or_torque_client = - n.serviceClient(model_name + "/supervisor/node/add_torque"); + n.serviceClient(model_name + "/supervisor/node/add_torque"); node_add_force_or_torque_srv.request.node = cone_node; node_add_force_or_torque_srv.request.force.x = 0.0; node_add_force_or_torque_srv.request.force.y = 0.0; @@ -3313,7 +3479,7 @@ int main(int argc, char **argv) { ros::ServiceClient node_add_force_with_offset_client; webots_ros::node_add_force_with_offset node_add_force_with_offset_srv; node_add_force_with_offset_client = - n.serviceClient(model_name + "/supervisor/node/add_force_with_offset"); + n.serviceClient(model_name + "/supervisor/node/add_force_with_offset"); node_add_force_with_offset_srv.request.node = cone_node; node_add_force_with_offset_srv.request.force.x = 0.0; node_add_force_with_offset_srv.request.force.y = 0.0; @@ -3335,7 +3501,7 @@ int main(int argc, char **argv) { ros::ServiceClient node_get_parent_node_client; webots_ros::node_get_parent_node node_get_parent_node_srv; node_get_parent_node_client = - n.serviceClient(model_name + "/supervisor/node/get_parent_node"); + n.serviceClient(model_name + "/supervisor/node/get_parent_node"); node_get_parent_node_srv.request.node = cone_node; node_get_parent_node_client.call(node_get_parent_node_srv); if (node_get_parent_node_srv.response.node == root_node) @@ -3363,7 +3529,7 @@ int main(int argc, char **argv) { ros::ServiceClient supervisor_movie_start_client; webots_ros::supervisor_movie_start_recording supervisor_movie_start_srv; supervisor_movie_start_client = - n.serviceClient(model_name + "/supervisor/movie_start_recording"); + n.serviceClient(model_name + "/supervisor/movie_start_recording"); supervisor_movie_start_srv.request.filename = std::string(getenv("HOME")) + std::string("/movie_test.mp4"); supervisor_movie_start_srv.request.width = 480; @@ -3439,6 +3605,21 @@ int main(int argc, char **argv) { remove_node_client.shutdown(); time_step_client.call(time_step_srv); + // node_export_string + ros::ServiceClient node_export_string_client; + webots_ros::node_get_string node_export_string_srv; + node_export_string_client = n.serviceClient(model_name + "/supervisor/node/export_string"); + node_export_string_srv.request.node = root_node; + node_export_string_client.call(node_export_string_srv); + std::string export_string_result = node_export_string_srv.response.value; + if (!export_string_result.find("WorldInfo {") != std::string::npos) + ROS_INFO("Node exported successfully."); + else + ROS_ERROR("Failed to call service node_export_string."); + + node_export_string_client.shutdown(); + time_step_client.call(time_step_srv); + // html robot window ros::ServiceClient wwi_send_client; wwi_send_client = n.serviceClient(model_name + "/robot/wwi_send_text"); @@ -3468,7 +3649,7 @@ int main(int argc, char **argv) { ros::ServiceClient virtual_reality_headset_client; webots_ros::get_bool supervisor_virtual_reality_headset_is_used_srv; virtual_reality_headset_client = - n.serviceClient(model_name + "/supervisor/vitual_reality_headset_is_used"); + n.serviceClient(model_name + "/supervisor/vitual_reality_headset_is_used"); virtual_reality_headset_client.call(supervisor_virtual_reality_headset_is_used_srv); bool used = supervisor_virtual_reality_headset_is_used_srv.response.value; // to test this service we assume no virtual reality headset is connected @@ -3535,4 +3716,3 @@ int main(int argc, char **argv) { printf("\nTest Completed\n"); return 0; } - From 9cd1b36fb224758a20dda6a4fce8acf022f73b91 Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Tue, 16 Feb 2021 16:49:28 +0100 Subject: [PATCH 05/72] Change data type --- src/complete_test.cpp | 5 +++-- srv/node_set_bool.srv | 4 ++++ 2 files changed, 7 insertions(+), 2 deletions(-) create mode 100644 srv/node_set_bool.srv diff --git a/src/complete_test.cpp b/src/complete_test.cpp index 3ac0690..81dd0dd 100644 --- a/src/complete_test.cpp +++ b/src/complete_test.cpp @@ -3235,10 +3235,11 @@ int main(int argc, char **argv) // test reset_physics // if the node isn't a top Solid webots will throw a warning but still return true to ros ros::ServiceClient supervisor_node_reset_physics_client = - n.serviceClient(model_name + "/supervisor/node/reset_physics"); - webots_ros::node_reset_functions supervisor_node_reset_physics_srv; + n.serviceClient(model_name + "/supervisor/node/reset_physics"); + webots_ros::node_set_bool supervisor_node_reset_physics_srv; supervisor_node_reset_physics_srv.request.node = from_def_node; + supervisor_node_reset_physics_srv.request.value = true; if (supervisor_node_reset_physics_client.call(supervisor_node_reset_physics_srv) && supervisor_node_reset_physics_srv.response.success == 1) ROS_INFO("Node physics has been reset successfully."); diff --git a/srv/node_set_bool.srv b/srv/node_set_bool.srv new file mode 100644 index 0000000..44cecaa --- /dev/null +++ b/srv/node_set_bool.srv @@ -0,0 +1,4 @@ +uint64 node +bool value +--- +bool success From ea1391a68ad63c9cdf347ea553f4c6fa713d57fe Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Tue, 16 Feb 2021 16:52:17 +0100 Subject: [PATCH 06/72] Change name --- src/complete_test.cpp | 2 +- srv/{node_set_bool.srv => node_reset_physics.srv} | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) rename srv/{node_set_bool.srv => node_reset_physics.srv} (65%) diff --git a/src/complete_test.cpp b/src/complete_test.cpp index 81dd0dd..d8c8cf4 100644 --- a/src/complete_test.cpp +++ b/src/complete_test.cpp @@ -3239,7 +3239,7 @@ int main(int argc, char **argv) webots_ros::node_set_bool supervisor_node_reset_physics_srv; supervisor_node_reset_physics_srv.request.node = from_def_node; - supervisor_node_reset_physics_srv.request.value = true; + supervisor_node_reset_physics_srv.request.recursive = true; if (supervisor_node_reset_physics_client.call(supervisor_node_reset_physics_srv) && supervisor_node_reset_physics_srv.response.success == 1) ROS_INFO("Node physics has been reset successfully."); diff --git a/srv/node_set_bool.srv b/srv/node_reset_physics.srv similarity index 65% rename from srv/node_set_bool.srv rename to srv/node_reset_physics.srv index 44cecaa..2d11b80 100644 --- a/srv/node_set_bool.srv +++ b/srv/node_reset_physics.srv @@ -1,4 +1,4 @@ uint64 node -bool value +bool recursive --- bool success From a696d898602745b22d4982c8c0f649ab9e8bdf5d Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Tue, 16 Feb 2021 16:56:36 +0100 Subject: [PATCH 07/72] add to cmake --- CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index f6f8057..f7b57d8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -108,6 +108,7 @@ find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs sensor_msgs messag node_get_velocity.srv node_remove.srv node_reset_functions.srv + node_reset_physics.srv node_move_viewpoint.srv node_set_visibility.srv node_set_velocity.srv From 117842f0d50afd35917aa41d0d8fe38867fec556 Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Tue, 16 Feb 2021 17:17:00 +0100 Subject: [PATCH 08/72] Update --- src/complete_test.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/complete_test.cpp b/src/complete_test.cpp index d8c8cf4..3a19e27 100644 --- a/src/complete_test.cpp +++ b/src/complete_test.cpp @@ -112,6 +112,7 @@ #include #include #include +#include #include #include #include @@ -3235,8 +3236,8 @@ int main(int argc, char **argv) // test reset_physics // if the node isn't a top Solid webots will throw a warning but still return true to ros ros::ServiceClient supervisor_node_reset_physics_client = - n.serviceClient(model_name + "/supervisor/node/reset_physics"); - webots_ros::node_set_bool supervisor_node_reset_physics_srv; + n.serviceClient(model_name + "/supervisor/node/reset_physics"); + webots_ros::node_reset_physics supervisor_node_reset_physics_srv; supervisor_node_reset_physics_srv.request.node = from_def_node; supervisor_node_reset_physics_srv.request.recursive = true; From 791a0c57057f85b98712a7132ebbb19e1ed9877c Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Tue, 16 Feb 2021 18:26:23 +0100 Subject: [PATCH 09/72] Add clang --- .clang-format | 88 +++++++ src/complete_test.cpp | 578 +++++++++++++++--------------------------- 2 files changed, 294 insertions(+), 372 deletions(-) create mode 100644 .clang-format diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..bc49dbc --- /dev/null +++ b/.clang-format @@ -0,0 +1,88 @@ +--- +Language: Cpp +AccessModifierOffset: -2 +AlignAfterOpenBracket: Align +AlignEscapedNewlines: Left +AlignOperands: true +AllowAllConstructorInitializersOnNextLine: false +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortBlocksOnASingleLine: false +AllowShortCaseLabelsOnASingleLine: false +AllowShortFunctionsOnASingleLine: InlineOnly +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: false +AlwaysBreakTemplateDeclarations: false +BinPackArguments: true +BinPackParameters: true +BreakBeforeBinaryOperators: None +BreakBeforeBraces: Attach +BreakBeforeInheritanceComma: false +BreakBeforeTernaryOperators: false +BreakConstructorInitializersBeforeComma: false +BreakConstructorInitializers: AfterColon +BreakStringLiterals: true +ColumnLimit: 128 +CommentPragmas: '^ IWYU pragma:' +CompactNamespaces: false +ConstructorInitializerAllOnOneLineOrOnePerLine: true +ConstructorInitializerIndentWidth: 2 +ContinuationIndentWidth: 2 +Cpp11BracedListStyle: true +DerivePointerAlignment: false +DisableFormat: false +ExperimentalAutoDetectBinPacking: false +FixNamespaceComments: true +ForEachMacros: + - foreach + - Q_FOREACH + - BOOST_FOREACH +IncludeCategories: + - Regex: '^<.*\.h>' + Priority: 1 + - Regex: '^<.*' + Priority: 2 + - Regex: '.*' + Priority: 3 +IncludeIsMainRegex: '([-_](test|unittest))?$' +IndentCaseLabels: true +IndentWidth: 2 +IndentWrappedFunctionNames: true +JavaScriptQuotes: Single +JavaScriptWrapImports: true +KeepEmptyLinesAtTheStartOfBlocks: false +MacroBlockBegin: '' +MacroBlockEnd: '' +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: All +ObjCBlockIndentWidth: 2 +ObjCSpaceAfterProperty: false +ObjCSpaceBeforeProtocolList: false +PenaltyBreakAssignment: 2 +PenaltyBreakBeforeFirstCallParameter: 1 +PenaltyBreakComment: 300 +PenaltyBreakFirstLessLess: 120 +PenaltyBreakString: 1000 +PenaltyExcessCharacter: 1000000 +PenaltyReturnTypeOnItsOwnLine: 200 +PointerAlignment: Right +ReflowComments: true +SortIncludes: true +SortUsingDeclarations: true +SpaceAfterCStyleCast: false +SpaceAfterTemplateKeyword: false +SpaceBeforeAssignmentOperators: true +SpaceBeforeParens: ControlStatements +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 2 +SpacesInAngles: false +SpacesInContainerLiterals: false +SpacesInCStyleCastParentheses: false +SpacesInParentheses: false +SpacesInSquareBrackets: false +Standard: Cpp11 +TabWidth: 8 +UseTab: Never +... diff --git a/src/complete_test.cpp b/src/complete_test.cpp index 3a19e27..68dc2f4 100644 --- a/src/complete_test.cpp +++ b/src/complete_test.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include +#include #include #include #include @@ -107,8 +107,8 @@ #include #include #include -#include #include +#include #include #include #include @@ -152,75 +152,64 @@ static bool callbackCalled = false; ros::ServiceClient time_step_client; webots_ros::set_int time_step_srv; -void modelNameCallback(const std_msgs::String::ConstPtr &name) -{ +void modelNameCallback(const std_msgs::String::ConstPtr &name) { model_count++; model_list.push_back(name->data); ROS_INFO("Model #%d: %s.", model_count, model_list.back().c_str()); callbackCalled = true; } -void cameraCallback(const sensor_msgs::Image::ConstPtr &values) -{ +void cameraCallback(const sensor_msgs::Image::ConstPtr &values) { int i = 0; imageColor.resize(values->step * values->height); - for (std::vector::const_iterator it = values->data.begin(); it != values->data.end(); ++it) - { + for (std::vector::const_iterator it = values->data.begin(); it != values->data.end(); ++it) { imageColor[i] = *it; i++; } callbackCalled = true; } -void cameraRecognitionCallback(const webots_ros::RecognitionObject::ConstPtr &object) -{ +void cameraRecognitionCallback(const webots_ros::RecognitionObject::ConstPtr &object) { ROS_INFO("Camera recognition saw a '%s' (time: %d:%d).", object->model.c_str(), object->header.stamp.sec, object->header.stamp.nsec); callbackCalled = true; } -void segmentationCallback(const sensor_msgs::Image::ConstPtr &values) -{ +void segmentationCallback(const sensor_msgs::Image::ConstPtr &values) { ROS_INFO("Segmentation callback called."); int i = 0; imageColor.resize(values->step * values->height); - for (std::vector::const_iterator it = values->data.begin(); it != values->data.end(); ++it) - { + for (std::vector::const_iterator it = values->data.begin(); it != values->data.end(); ++it) { imageColor[i] = *it; i++; } callbackCalled = true; } -void joystickCallback(const webots_ros::Int8Stamped::ConstPtr &value) -{ +void joystickCallback(const webots_ros::Int8Stamped::ConstPtr &value) { ROS_INFO("Joystick button pressed: %d (time: %d:%d).", value->data, value->header.stamp.sec, value->header.stamp.nsec); callbackCalled = true; } -void keyboardCallback(const webots_ros::Int32Stamped::ConstPtr &value) -{ +void keyboardCallback(const webots_ros::Int32Stamped::ConstPtr &value) { ROS_INFO("Keyboard key pressed: %d (time: %d:%d).", value->data, value->header.stamp.sec, value->header.stamp.nsec); callbackCalled = true; } -void radarTargetsCallback(const webots_ros::RadarTarget::ConstPtr &target) -{ +void radarTargetsCallback(const webots_ros::RadarTarget::ConstPtr &target) { ROS_INFO("Received a radar target with distance=%lf received power=%lf speed=%lf azimuth=%lf (time: %d:%d).", target->distance, target->receivedPower, target->speed, target->azimuth, target->header.stamp.sec, target->header.stamp.nsec); callbackCalled = true; } -void radarTargetsNumberCallback(const webots_ros::Int8Stamped::ConstPtr &value) -{ +void radarTargetsNumberCallback(const webots_ros::Int8Stamped::ConstPtr &value) { ROS_INFO("Number of target seen by the radar: %d (time: %d:%d).", value->data, value->header.stamp.sec, value->header.stamp.nsec); callbackCalled = true; } -void rangeFinderCallback(const sensor_msgs::Image::ConstPtr &image) -{ +void rangeFinderCallback(const sensor_msgs::Image::ConstPtr &image) { int size = image->width * image->height; imageRangeFinder.resize(size); @@ -230,21 +219,18 @@ void rangeFinderCallback(const sensor_msgs::Image::ConstPtr &image) callbackCalled = true; } -void lidarCallback(const sensor_msgs::Image::ConstPtr &image) -{ +void lidarCallback(const sensor_msgs::Image::ConstPtr &image) { callbackCalled = true; } -void connectorCallback(const webots_ros::Int8Stamped::ConstPtr &value) -{ +void connectorCallback(const webots_ros::Int8Stamped::ConstPtr &value) { connectorPresence = value->data; ROS_INFO("Connector presence: %d (time: %d:%d).", connectorPresence, value->header.stamp.sec, value->header.stamp.nsec); callbackCalled = true; } -void accelerometerCallback(const sensor_msgs::Imu::ConstPtr &values) -{ +void accelerometerCallback(const sensor_msgs::Imu::ConstPtr &values) { accelerometerValues[0] = values->linear_acceleration.x; accelerometerValues[1] = values->linear_acceleration.y; accelerometerValues[2] = values->linear_acceleration.z; @@ -254,14 +240,12 @@ void accelerometerCallback(const sensor_msgs::Imu::ConstPtr &values) callbackCalled = true; } -void battery_sensorCallback(const webots_ros::Float64Stamped::ConstPtr &value) -{ +void battery_sensorCallback(const webots_ros::Float64Stamped::ConstPtr &value) { ROS_INFO("Battery level is %f (time: %d:%d).", value->data, value->header.stamp.sec, value->header.stamp.nsec); callbackCalled = true; } -void compassCallback(const sensor_msgs::MagneticField::ConstPtr &values) -{ +void compassCallback(const sensor_msgs::MagneticField::ConstPtr &values) { compassValues[0] = values->magnetic_field.x; compassValues[1] = values->magnetic_field.y; compassValues[2] = values->magnetic_field.z; @@ -271,14 +255,12 @@ void compassCallback(const sensor_msgs::MagneticField::ConstPtr &values) callbackCalled = true; } -void distance_sensorCallback(const sensor_msgs::Range::ConstPtr &value) -{ +void distance_sensorCallback(const sensor_msgs::Range::ConstPtr &value) { ROS_INFO("Distance from object is %f (time: %d:%d).", value->range, value->header.stamp.sec, value->header.stamp.nsec); callbackCalled = true; } -void GPSCallback(const geometry_msgs::PointStamped::ConstPtr &values) -{ +void GPSCallback(const geometry_msgs::PointStamped::ConstPtr &values) { GPSValues[0] = values->point.x; GPSValues[1] = values->point.y; GPSValues[2] = values->point.z; @@ -288,14 +270,12 @@ void GPSCallback(const geometry_msgs::PointStamped::ConstPtr &values) callbackCalled = true; } -void GPSSpeedCallback(const webots_ros::Float64Stamped::ConstPtr &value) -{ +void GPSSpeedCallback(const webots_ros::Float64Stamped::ConstPtr &value) { ROS_INFO("GPS speed is: %fkm/h (time: %d:%d).", value->data, value->header.stamp.sec, value->header.stamp.nsec); callbackCalled = true; } -void gyroCallback(const sensor_msgs::Imu::ConstPtr &values) -{ +void gyroCallback(const sensor_msgs::Imu::ConstPtr &values) { GyroValues[0] = values->angular_velocity.x; GyroValues[1] = values->angular_velocity.y; GyroValues[2] = values->angular_velocity.z; @@ -305,8 +285,7 @@ void gyroCallback(const sensor_msgs::Imu::ConstPtr &values) callbackCalled = true; } -void inertialUnitCallback(const sensor_msgs::Imu::ConstPtr &values) -{ +void inertialUnitCallback(const sensor_msgs::Imu::ConstPtr &values) { inertialUnitValues[0] = values->orientation.x; inertialUnitValues[1] = values->orientation.y; inertialUnitValues[2] = values->orientation.z; @@ -318,38 +297,32 @@ void inertialUnitCallback(const sensor_msgs::Imu::ConstPtr &values) callbackCalled = true; } -void lightSensorCallback(const sensor_msgs::Illuminance::ConstPtr &value) -{ +void lightSensorCallback(const sensor_msgs::Illuminance::ConstPtr &value) { ROS_INFO("Light intensity is %f.", value->illuminance); callbackCalled = true; } -void motorSensorCallback(const webots_ros::Float64Stamped::ConstPtr &value) -{ +void motorSensorCallback(const webots_ros::Float64Stamped::ConstPtr &value) { ROS_INFO("Motor sensor sent value %f.", value->data); callbackCalled = true; } -void positionSensorCallback(const webots_ros::Float64Stamped::ConstPtr &value) -{ +void positionSensorCallback(const webots_ros::Float64Stamped::ConstPtr &value) { ROS_INFO("Position sensor sent value %f (time: %d:%d).", value->data, value->header.stamp.sec, value->header.stamp.nsec); callbackCalled = true; } -void touchSensorCallback(const webots_ros::Float64Stamped::ConstPtr &value) -{ +void touchSensorCallback(const webots_ros::Float64Stamped::ConstPtr &value) { ROS_INFO("Touch sensor sent value %f (time: %d:%d).", value->data, value->header.stamp.sec, value->header.stamp.nsec); callbackCalled = true; } -void touchSensorBumperCallback(const webots_ros::BoolStamped::ConstPtr &value) -{ +void touchSensorBumperCallback(const webots_ros::BoolStamped::ConstPtr &value) { ROS_INFO("Touch sensor sent value %d (time: %d:%d).", value->data, value->header.stamp.sec, value->header.stamp.nsec); callbackCalled = true; } -void touchSensor3DCallback(const geometry_msgs::WrenchStamped::ConstPtr &values) -{ +void touchSensor3DCallback(const geometry_msgs::WrenchStamped::ConstPtr &values) { touchSensorValues[0] = values->wrench.force.x; touchSensorValues[1] = values->wrench.force.y; touchSensorValues[2] = values->wrench.force.z; @@ -359,15 +332,13 @@ void touchSensor3DCallback(const geometry_msgs::WrenchStamped::ConstPtr &values) callbackCalled = true; } -void receiverCallback(const webots_ros::StringStamped::ConstPtr &value) -{ +void receiverCallback(const webots_ros::StringStamped::ConstPtr &value) { char *message = const_cast(value->data.c_str()); ROS_INFO("Received a message %s.", message); callbackCalled = true; } -void quit(int sig) -{ +void quit(int sig) { time_step_srv.request.value = 0; time_step_client.call(time_step_srv); ROS_INFO("User stopped the 'complete_test' node."); @@ -375,8 +346,7 @@ void quit(int sig) exit(0); } -int main(int argc, char **argv) -{ +int main(int argc, char **argv) { string model_name = "my_robot"; ros::init(argc, argv, "complete_test", ros::init_options::AnonymousName); @@ -385,8 +355,7 @@ int main(int argc, char **argv) signal(SIGINT, quit); ros::Subscriber name_sub = n.subscribe("model_name", 100, modelNameCallback); - while (model_count == 0 || model_count < name_sub.getNumPublishers()) - { + while (model_count == 0 || model_count < name_sub.getNumPublishers()) { ros::spinOnce(); ros::spinOnce(); ros::spinOnce(); @@ -413,31 +382,27 @@ int main(int argc, char **argv) ROS_ERROR("Failed to call service time_step to update robot's time step."); ros::ServiceClient get_number_of_devices_client = - n.serviceClient(model_name + "/robot/get_number_of_devices"); + n.serviceClient(model_name + "/robot/get_number_of_devices"); webots_ros::get_int get_number_of_devices_srv; - if (get_number_of_devices_client.call(get_number_of_devices_srv)) - { + if (get_number_of_devices_client.call(get_number_of_devices_srv)) { int number_of_devices = get_number_of_devices_srv.response.value; ROS_INFO("%s has %d devices.", model_name.c_str(), number_of_devices); - } - else + } else ROS_ERROR("Failed to call service get_number_of_devices."); get_number_of_devices_client.shutdown(); time_step_client.call(time_step_srv); ros::ServiceClient device_list_client = - n.serviceClient(model_name + "/robot/get_device_list"); + n.serviceClient(model_name + "/robot/get_device_list"); webots_ros::robot_get_device_list device_list_srv; - if (device_list_client.call(device_list_srv)) - { + if (device_list_client.call(device_list_srv)) { device_list = device_list_srv.response.list; for (unsigned int i = 0; i < device_list.size(); i++) ROS_INFO("Device [%d]: %s.", i, device_list[i].c_str()); - } - else + } else ROS_ERROR("Failed to call service device_list."); device_list_client.shutdown(); @@ -447,45 +412,39 @@ int main(int argc, char **argv) webots_ros::get_urdf urdf_srv; urdf_srv.request.prefix = "unique_robot_prefix_name_"; - if (urdf_client.call(urdf_srv)) - { + if (urdf_client.call(urdf_srv)) { std::string urdf = urdf_srv.response.value; if (urdf.find(urdf_srv.request.prefix) == std::string::npos) ROS_ERROR("Invalid response from get_urdf."); else ROS_INFO("URDF has been successfully obtained."); - } - else + } else ROS_ERROR("Failed to call service get_urdf."); urdf_client.shutdown(); time_step_client.call(time_step_srv); ros::ServiceClient get_basic_time_step_client = - n.serviceClient(model_name + "/robot/get_basic_time_step"); + n.serviceClient(model_name + "/robot/get_basic_time_step"); webots_ros::get_float get_basic_time_step_srv; - if (get_basic_time_step_client.call(get_basic_time_step_srv)) - { + if (get_basic_time_step_client.call(get_basic_time_step_srv)) { double basic_time_step = get_basic_time_step_srv.response.value; ROS_INFO("%s has a basic time step of %f.", model_name.c_str(), basic_time_step); - } - else + } else ROS_ERROR("Failed to call service get_basic_time_step."); get_basic_time_step_client.shutdown(); time_step_client.call(time_step_srv); ros::ServiceClient robot_get_custom_data_client = - n.serviceClient(model_name + "/robot/get_custom_data"); + n.serviceClient(model_name + "/robot/get_custom_data"); webots_ros::get_string robot_get_custom_data_srv; - if (robot_get_custom_data_client.call(robot_get_custom_data_srv)) - { + if (robot_get_custom_data_client.call(robot_get_custom_data_srv)) { data = robot_get_custom_data_srv.response.value; ROS_INFO("Data of %s is %s.", model_name.c_str(), data.c_str()); - } - else + } else ROS_ERROR("Failed to call service robot_get_custom_data."); robot_get_custom_data_client.shutdown(); @@ -494,12 +453,10 @@ int main(int argc, char **argv) ros::ServiceClient get_mode_client = n.serviceClient(model_name + "/robot/get_mode"); webots_ros::get_int get_mode_srv; - if (get_mode_client.call(get_mode_srv)) - { + if (get_mode_client.call(get_mode_srv)) { mode = get_mode_srv.response.value; ROS_INFO("Mode of %s is %d.", model_name.c_str(), mode); - } - else + } else ROS_ERROR("Failed to call service get_mode."); get_mode_client.shutdown(); @@ -508,12 +465,10 @@ int main(int argc, char **argv) ros::ServiceClient get_model_client = n.serviceClient(model_name + "/robot/get_model"); webots_ros::get_string get_model_srv; - if (get_model_client.call(get_model_srv)) - { + if (get_model_client.call(get_model_srv)) { model = get_model_srv.response.value; ROS_INFO("Model of %s is %s.", model_name.c_str(), model.c_str()); - } - else + } else ROS_ERROR("Failed to call service get_model."); get_model_client.shutdown(); @@ -522,12 +477,10 @@ int main(int argc, char **argv) ros::ServiceClient get_project_path_client = n.serviceClient(model_name + "/robot/get_project_path"); webots_ros::get_string get_project_path_srv; - if (get_project_path_client.call(get_project_path_srv)) - { + if (get_project_path_client.call(get_project_path_srv)) { path = get_project_path_srv.response.value; ROS_INFO("World path of %s is %s.", model_name.c_str(), path.c_str()); - } - else + } else ROS_ERROR("Failed to call service get_project_path."); get_project_path_client.shutdown(); @@ -537,12 +490,10 @@ int main(int argc, char **argv) ros::ServiceClient get_world_path_client = n.serviceClient(model_name + "/robot/get_world_path"); webots_ros::get_string get_world_path_srv; - if (get_world_path_client.call(get_world_path_srv)) - { + if (get_world_path_client.call(get_world_path_srv)) { path = get_world_path_srv.response.value; ROS_INFO("Project path of %s is %s.", model_name.c_str(), path.c_str()); - } - else + } else ROS_ERROR("Failed to call service get_project_path."); get_world_path_client.shutdown(); @@ -551,32 +502,28 @@ int main(int argc, char **argv) ros::ServiceClient get_supervisor_client = n.serviceClient(model_name + "/robot/get_supervisor"); webots_ros::get_bool get_supervisor_srv; - if (get_supervisor_client.call(get_supervisor_srv)) - { + if (get_supervisor_client.call(get_supervisor_srv)) { if (get_supervisor_srv.response.value) ROS_INFO("%s is a supervisor.", model_name.c_str()); else ROS_ERROR("%s isn't a supervisor.", model_name.c_str()); - } - else + } else ROS_ERROR("Failed to call service get_synchronization."); get_supervisor_client.shutdown(); time_step_client.call(time_step_srv); ros::ServiceClient get_synchronization_client = - n.serviceClient(model_name + "/robot/get_synchronization"); + n.serviceClient(model_name + "/robot/get_synchronization"); webots_ros::get_bool get_synchronization_srv; - if (get_synchronization_client.call(get_synchronization_srv)) - { + if (get_synchronization_client.call(get_synchronization_srv)) { bool synchronization = get_synchronization_srv.response.value; if (synchronization) ROS_INFO("%s is sync.", model_name.c_str()); else ROS_INFO("%s isn't sync.", model_name.c_str()); - } - else + } else ROS_ERROR("Failed to call service get_synchronization."); get_synchronization_client.shutdown(); @@ -585,12 +532,10 @@ int main(int argc, char **argv) ros::ServiceClient get_time_client = n.serviceClient(model_name + "/robot/get_time"); webots_ros::get_float get_time_srv; - if (get_time_client.call(get_time_srv)) - { + if (get_time_client.call(get_time_srv)) { double time = get_time_srv.response.value; ROS_INFO("Time for %s is %f.", model_name.c_str(), time); - } - else + } else ROS_ERROR("Failed to call service get_time."); get_time_client.shutdown(); @@ -599,28 +544,24 @@ int main(int argc, char **argv) ros::ServiceClient get_type_client = n.serviceClient(model_name + "/robot/get_type"); webots_ros::get_int get_type_srv; - if (get_type_client.call(get_type_srv)) - { + if (get_type_client.call(get_type_srv)) { int type = get_type_srv.response.value; ROS_INFO("Type of %s is %d.", model_name.c_str(), type); - } - else + } else ROS_ERROR("Failed to call service get_type."); get_type_client.shutdown(); time_step_client.call(time_step_srv); ros::ServiceClient robot_set_custom_data_client = - n.serviceClient(model_name + "/robot/set_custom_data"); + n.serviceClient(model_name + "/robot/set_custom_data"); webots_ros::set_string robot_set_custom_data_srv; robot_set_custom_data_srv.request.value = "OVERWRITTEN"; - if (robot_set_custom_data_client.call(robot_set_custom_data_srv)) - { + if (robot_set_custom_data_client.call(robot_set_custom_data_srv)) { if (robot_set_custom_data_srv.response.success) ROS_INFO("Data of %s has been set to %s.", model_name.c_str(), data.c_str()); - } - else + } else ROS_ERROR("Failed to call service robot_set_custom_data."); robot_set_custom_data_client.shutdown(); @@ -630,12 +571,10 @@ int main(int argc, char **argv) webots_ros::robot_set_mode set_mode_srv; set_mode_srv.request.mode = mode; - if (set_mode_client.call(set_mode_srv)) - { + if (set_mode_client.call(set_mode_srv)) { if (set_mode_srv.response.success == 1) ROS_INFO("Mode of %s has been set to %d.", model_name.c_str(), mode); - } - else + } else ROS_ERROR("Failed to call service set_mode."); set_mode_client.shutdown(); @@ -646,27 +585,24 @@ int main(int argc, char **argv) ros::Subscriber sub_keyboard; enable_keyboard_srv.request.value = 32; - if (enable_keyboard_client.call(enable_keyboard_srv) && enable_keyboard_srv.response.success) - { + if (enable_keyboard_client.call(enable_keyboard_srv) && enable_keyboard_srv.response.success) { ROS_INFO("Keyboard of %s has been enabled.", model_name.c_str()); sub_keyboard = n.subscribe(model_name + "/keyboard/key", 1, keyboardCallback); ROS_INFO("Topics for keyboard initialized."); callbackCalled = false; - while (sub_keyboard.getNumPublishers() == 0 && !callbackCalled) - { + while (sub_keyboard.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } ROS_INFO("Topics for keyboard connected."); - } - else + } else ROS_ERROR("Failed to enable keyboard."); ros::ServiceClient wait_for_user_input_event_client = - n.serviceClient(model_name + "/robot/wait_for_user_input_event"); + n.serviceClient(model_name + "/robot/wait_for_user_input_event"); webots_ros::robot_wait_for_user_input_event wait_for_user_input_event_srv; - wait_for_user_input_event_srv.request.eventType = 4; // WB_EVENT_KEYBOARD + wait_for_user_input_event_srv.request.eventType = 4; // WB_EVENT_KEYBOARD wait_for_user_input_event_srv.request.timeout = 20; if (wait_for_user_input_event_client.call(wait_for_user_input_event_srv)) ROS_INFO("Detected user input event: %d.", wait_for_user_input_event_srv.response.event); @@ -698,16 +634,14 @@ int main(int argc, char **argv) // brake_get_motor_name ros::ServiceClient brake_get_motor_name_client = - n.serviceClient(model_name + "/my_brake/get_motor_name"); + n.serviceClient(model_name + "/my_brake/get_motor_name"); webots_ros::get_string brake_get_motor_name_srv; - if (brake_get_motor_name_client.call(brake_get_motor_name_srv)) - { + if (brake_get_motor_name_client.call(brake_get_motor_name_srv)) { ROS_INFO("Linear motor name returned from Brake API %s.", brake_get_motor_name_srv.response.value.c_str()); if (brake_get_motor_name_srv.response.value.compare("linear_motor") != 0) ROS_ERROR("Failed to call service brake_get_motor_name: received '%s' instead of 'linear_motor'", brake_get_motor_name_srv.response.value.c_str()); - } - else + } else ROS_ERROR("Failed to call service brake_get_motor_name."); brake_get_motor_name_client.shutdown(); @@ -750,21 +684,17 @@ int main(int argc, char **argv) enable_camera_client = n.serviceClient(model_name + "/camera/enable"); camera_srv.request.value = TIME_STEP; - if (enable_camera_client.call(camera_srv) && camera_srv.response.success) - { + if (enable_camera_client.call(camera_srv) && camera_srv.response.success) { ROS_INFO("Camera enabled."); sub_camera_color = n.subscribe(model_name + "/camera/image", 1, cameraCallback); ROS_INFO("Topic for camera color initialized."); callbackCalled = false; - while (sub_camera_color.getNumPublishers() == 0 && !callbackCalled) - { + while (sub_camera_color.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } ROS_INFO("Topic for camera color connected."); - } - else - { + } else { if (camera_srv.response.success == -1) ROS_ERROR("Sampling period is not valid."); ROS_ERROR("Failed to enable camera."); @@ -862,21 +792,17 @@ int main(int argc, char **argv) enable_camera_recognition_client = n.serviceClient(model_name + "/camera/recognition_enable"); camera_recognition_srv.request.value = TIME_STEP; - if (enable_camera_recognition_client.call(camera_recognition_srv) && camera_recognition_srv.response.success) - { + if (enable_camera_recognition_client.call(camera_recognition_srv) && camera_recognition_srv.response.success) { ROS_INFO("Camera recognition enabled."); sub_camera_recognition = n.subscribe(model_name + "/camera/recognition_objects", 1, cameraRecognitionCallback); ROS_INFO("Topic for camera recognition initialized."); callbackCalled = false; - while (sub_camera_recognition.getNumPublishers() == 0 && !callbackCalled) - { + while (sub_camera_recognition.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } ROS_INFO("Topic for camera recognition connected."); - } - else - { + } else { if (camera_recognition_srv.response.success == -1) ROS_ERROR("Sampling period is not valid."); ROS_ERROR("Failed to enable camera recognition."); @@ -889,27 +815,24 @@ int main(int argc, char **argv) // camera recognition segmentation is enabled ros::ServiceClient has_segmentation_client = - n.serviceClient(model_name + "/camera/recognition_has_segmentation"); + n.serviceClient(model_name + "/camera/recognition_has_segmentation"); webots_ros::get_bool has_segmentation_srv; - if (has_segmentation_client.call(has_segmentation_srv)) - { + if (has_segmentation_client.call(has_segmentation_srv)) { if (has_segmentation_srv.response.value) ROS_INFO("Camera recognition segmentation field is TRUE."); else ROS_INFO("Camera recognition segmentation field is FALSE."); - } - else + } else ROS_ERROR("Failed to get segmentation field value."); has_segmentation_client.shutdown(); // camera recognition enable segmentation ros::ServiceClient enable_segmentation_client = - n.serviceClient(model_name + "/camera/recognition_enable_segmentation"); + n.serviceClient(model_name + "/camera/recognition_enable_segmentation"); webots_ros::get_bool enable_segmentation_srv; if (enable_segmentation_client.call(enable_segmentation_srv) && enable_segmentation_srv.response.value) ROS_INFO("Segmentation correctly available."); - else - { + else { if (!enable_segmentation_srv.response.value) ROS_ERROR("Segmentation value could not be retrieved correctly."); ROS_ERROR("Failed to retrieve segmentation value."); @@ -920,12 +843,11 @@ int main(int argc, char **argv) time_step_client.call(time_step_srv); ros::ServiceClient is_segmentation_enabled_client = - n.serviceClient(model_name + "/camera/recognition_is_segmentation_enabled"); + n.serviceClient(model_name + "/camera/recognition_is_segmentation_enabled"); webots_ros::get_bool is_segmentation_enabled_srv; if (is_segmentation_enabled_client.call(is_segmentation_enabled_srv) && is_segmentation_enabled_srv.response.value) ROS_INFO("Segmentation correctly enabled."); - else - { + else { if (!enable_segmentation_srv.response.value) ROS_ERROR("Failed to enable segmentation."); ROS_ERROR("Failed to query segmentation enabled status."); @@ -936,11 +858,10 @@ int main(int argc, char **argv) time_step_client.call(time_step_srv); ros::Subscriber sub_segmentation = - n.subscribe(model_name + "/camera/recognition_segmentation_image", 1, segmentationCallback); + n.subscribe(model_name + "/camera/recognition_segmentation_image", 1, segmentationCallback); ROS_INFO("Topic for camera recognition segmentation initialized."); callbackCalled = false; - while (sub_segmentation.getNumPublishers() == 0 && !callbackCalled) - { + while (sub_segmentation.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } @@ -954,7 +875,7 @@ int main(int argc, char **argv) // camera recognition save segmentation image ros::ServiceClient save_segmentation_image_client = - n.serviceClient(model_name + "/camera/recognition_save_segmentation_image"); + n.serviceClient(model_name + "/camera/recognition_save_segmentation_image"); webots_ros::save_image save_segmentation_image_srv; save_segmentation_image_srv.request.filename = std::string(getenv("HOME")) + std::string("/test_image_segmentation.png"); save_segmentation_image_srv.request.quality = 100; @@ -965,12 +886,11 @@ int main(int argc, char **argv) // camera recognition disable segmentation ros::ServiceClient disable_segmentation_client = - n.serviceClient(model_name + "/camera/recognition_disable_segmentation"); + n.serviceClient(model_name + "/camera/recognition_disable_segmentation"); webots_ros::get_bool disable_segmentation_srv; if (disable_segmentation_client.call(disable_segmentation_srv) && disable_segmentation_srv.response.value) ROS_INFO("Segmentation correctly disabled."); - else - { + else { if (!disable_segmentation_srv.response.value) ROS_ERROR("Segmentation value could not be disabled."); ROS_ERROR("Failed to set segmentation."); @@ -1034,18 +954,14 @@ int main(int argc, char **argv) set_accelerometer_client = n.serviceClient(model_name + "/accelerometer/enable"); accelerometer_srv.request.value = 64; - if (set_accelerometer_client.call(accelerometer_srv) && accelerometer_srv.response.success) - { + if (set_accelerometer_client.call(accelerometer_srv) && accelerometer_srv.response.success) { sub_accelerometer_64 = n.subscribe(model_name + "/accelerometer/values", 1, accelerometerCallback); callbackCalled = false; - while (sub_accelerometer_64.getNumPublishers() == 0 && !callbackCalled) - { + while (sub_accelerometer_64.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } - } - else - { + } else { if (accelerometer_srv.response.success == -1) ROS_ERROR("Sampling period is not valid."); ROS_ERROR("Failed to enable accelerometer."); @@ -1059,23 +975,19 @@ int main(int argc, char **argv) webots_ros::get_int sampling_period_accelerometer_srv; sampling_period_accelerometer_client = - n.serviceClient(model_name + "/accelerometer/get_sampling_period"); + n.serviceClient(model_name + "/accelerometer/get_sampling_period"); sampling_period_accelerometer_client.call(sampling_period_accelerometer_srv); ROS_INFO("Accelerometer is enabled with a sampling period of %d.", sampling_period_accelerometer_srv.response.value); accelerometer_srv.request.value = 32; - if (set_accelerometer_client.call(accelerometer_srv) && accelerometer_srv.response.success) - { + if (set_accelerometer_client.call(accelerometer_srv) && accelerometer_srv.response.success) { sub_accelerometer_32 = n.subscribe(model_name + "/accelerometer/values", 1, accelerometerCallback); callbackCalled = false; - while (sub_accelerometer_32.getNumPublishers() == 0 && !callbackCalled) - { + while (sub_accelerometer_32.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } - } - else - { + } else { if (accelerometer_srv.response.success == -1) ROS_ERROR("Sampling period is not valid."); ROS_ERROR("Failed to enable accelerometer."); @@ -1085,7 +997,7 @@ int main(int argc, char **argv) ros::ServiceClient lookup_table_accelerometer_client; webots_ros::get_float_array lookup_table_accelerometer_srv; lookup_table_accelerometer_client = - n.serviceClient(model_name + "/accelerometer/get_lookup_table"); + n.serviceClient(model_name + "/accelerometer/get_lookup_table"); if (lookup_table_accelerometer_client.call(lookup_table_accelerometer_srv)) ROS_INFO("Accelerometer lookup table size = %lu.", lookup_table_accelerometer_srv.response.value.size()); else @@ -1124,22 +1036,18 @@ int main(int argc, char **argv) ros::ServiceClient sampling_period_battery_sensor_client; webots_ros::get_int sampling_period_battery_sensor_srv; sampling_period_battery_sensor_client = - n.serviceClient(model_name + "/battery_sensor/get_sampling_period"); + n.serviceClient(model_name + "/battery_sensor/get_sampling_period"); battery_sensor_srv.request.value = 32; - if (set_battery_sensor_client.call(battery_sensor_srv) && battery_sensor_srv.response.success) - { + if (set_battery_sensor_client.call(battery_sensor_srv) && battery_sensor_srv.response.success) { ROS_INFO("Battery_sensor enabled."); sub_battery_sensor_32 = n.subscribe(model_name + "/battery_sensor/value", 1, battery_sensorCallback); callbackCalled = false; - while (sub_battery_sensor_32.getNumPublishers() == 0 && !callbackCalled) - { + while (sub_battery_sensor_32.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } - } - else - { + } else { if (!battery_sensor_srv.response.success) ROS_ERROR("Sampling period is not valid."); ROS_ERROR("Failed to enable battery_sensor."); @@ -1180,19 +1088,15 @@ int main(int argc, char **argv) sampling_period_compass_client = n.serviceClient(model_name + "/compass/get_sampling_period"); compass_srv.request.value = 32; - if (set_compass_client.call(compass_srv) && compass_srv.response.success == 1) - { + if (set_compass_client.call(compass_srv) && compass_srv.response.success == 1) { ROS_INFO("Compass enabled."); sub_compass_32 = n.subscribe(model_name + "/compass/values", 1, compassCallback); callbackCalled = false; - while (sub_compass_32.getNumPublishers() == 0 && !callbackCalled) - { + while (sub_compass_32.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } - } - else - { + } else { if (compass_srv.response.success == -1) ROS_ERROR("Sampling period is not valid."); ROS_ERROR("Failed to enable compass."); @@ -1240,19 +1144,15 @@ int main(int argc, char **argv) connector_enable_presence_client = n.serviceClient(model_name + "/connector/presence_sensor/enable"); connector_srv.request.value = 32; - if (connector_enable_presence_client.call(connector_srv) && connector_srv.response.success) - { + if (connector_enable_presence_client.call(connector_srv) && connector_srv.response.success) { ROS_INFO("Connector's presence sensor enabled."); sub_connector = n.subscribe(model_name + "/connector/presence", 1, connectorCallback); callbackCalled = false; - while (sub_connector.getNumPublishers() == 0 && !callbackCalled) - { + while (sub_connector.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } - } - else - { + } else { if (!connector_srv.response.success) ROS_ERROR("Sampling period is not valid."); ROS_ERROR("Failed to enable connector's presence sensor."); @@ -1268,8 +1168,7 @@ int main(int argc, char **argv) connector_srv.request.value = 0; if (connector_enable_presence_client.call(connector_srv) && connector_srv.response.success) ROS_INFO("Connector's presence sensor disabled."); - else - { + else { if (!connector_srv.response.success) ROS_ERROR("Sampling period is not valid."); ROS_ERROR("Failed to disable connector's presence sensor."); @@ -1652,7 +1551,7 @@ int main(int argc, char **argv) ros::ServiceClient sampling_period_distance_sensor_client; webots_ros::get_int sampling_period_distance_sensor_srv; sampling_period_distance_sensor_client = - n.serviceClient(model_name + "/distance_sensor/get_sampling_period"); + n.serviceClient(model_name + "/distance_sensor/get_sampling_period"); ros::ServiceClient min_value_distance_sensor_client; webots_ros::get_float min_value_distance_sensor_srv; @@ -1684,7 +1583,7 @@ int main(int argc, char **argv) ros::ServiceClient lookup_table_distance_sensor_client; webots_ros::get_float_array lookup_table_distance_sensor_srv; lookup_table_distance_sensor_client = - n.serviceClient(model_name + "/distance_sensor/get_lookup_table"); + n.serviceClient(model_name + "/distance_sensor/get_lookup_table"); if (lookup_table_distance_sensor_client.call(lookup_table_distance_sensor_srv)) ROS_INFO("Distance_sensor lookup table size = %lu.", lookup_table_distance_sensor_srv.response.value.size()); else @@ -1695,19 +1594,15 @@ int main(int argc, char **argv) lookup_table_distance_sensor_client.shutdown(); distance_sensor_srv.request.value = 32; - if (set_distance_sensor_client.call(distance_sensor_srv) && distance_sensor_srv.response.success) - { + if (set_distance_sensor_client.call(distance_sensor_srv) && distance_sensor_srv.response.success) { ROS_INFO("Distance_sensor enabled."); sub_distance_sensor_32 = n.subscribe(model_name + "/distance_sensor/value", 1, distance_sensorCallback); callbackCalled = false; - while (sub_distance_sensor_32.getNumPublishers() == 0 && !callbackCalled) - { + while (sub_distance_sensor_32.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } - } - else - { + } else { if (!distance_sensor_srv.response.success) ROS_ERROR("Sampling period is not valid."); ROS_ERROR("Failed to enable distance_sensor."); @@ -1825,13 +1720,11 @@ int main(int argc, char **argv) sampling_period_GPS_client = n.serviceClient(model_name + "/gps/get_sampling_period"); GPS_srv.request.value = 32; - if (set_GPS_client.call(GPS_srv) && GPS_srv.response.success) - { + if (set_GPS_client.call(GPS_srv) && GPS_srv.response.success) { ROS_INFO("GPS enabled."); sub_GPS_32 = n.subscribe(model_name + "/gps/values", 1, GPSCallback); callbackCalled = false; - while (sub_GPS_32.getNumPublishers() == 0 && !callbackCalled) - { + while (sub_GPS_32.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } @@ -1840,14 +1733,11 @@ int main(int argc, char **argv) sub_GPS_speed = n.subscribe(model_name + "/gps/speed", 1, GPSSpeedCallback); callbackCalled = false; - while (sub_GPS_speed.getNumPublishers() == 0 && !callbackCalled) - { + while (sub_GPS_speed.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } - } - else - { + } else { if (!GPS_srv.response.success) ROS_ERROR("Sampling period is not valid."); ROS_ERROR("Failed to enable GPS."); @@ -1891,19 +1781,15 @@ int main(int argc, char **argv) sampling_period_gyro_client = n.serviceClient(model_name + "/gyro/get_sampling_period"); gyro_srv.request.value = 32; - if (set_gyro_client.call(gyro_srv) && gyro_srv.response.success) - { + if (set_gyro_client.call(gyro_srv) && gyro_srv.response.success) { ROS_INFO("Gyro enabled."); sub_gyro_32 = n.subscribe(model_name + "/gyro/values", 1, gyroCallback); callbackCalled = false; - while (sub_gyro_32.getNumPublishers() == 0 && !callbackCalled) - { + while (sub_gyro_32.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } - } - else - { + } else { if (!gyro_srv.response.success) ROS_ERROR("Sampling period is not valid."); ROS_ERROR("Failed to enable gyro."); @@ -1948,22 +1834,18 @@ int main(int argc, char **argv) ros::ServiceClient sampling_period_inertial_unit_client; webots_ros::get_int sampling_period_inertial_unit_srv; sampling_period_inertial_unit_client = - n.serviceClient(model_name + "/inertial_unit/get_sampling_period"); + n.serviceClient(model_name + "/inertial_unit/get_sampling_period"); inertial_unit_srv.request.value = 32; - if (set_inertial_unit_client.call(inertial_unit_srv) && inertial_unit_srv.response.success) - { + if (set_inertial_unit_client.call(inertial_unit_srv) && inertial_unit_srv.response.success) { ROS_INFO("Inertial_unit enabled."); sub_inertial_unit_32 = n.subscribe(model_name + "/inertial_unit/quaternion", 1, inertialUnitCallback); callbackCalled = false; - while (sub_inertial_unit_32.getNumPublishers() == 0 && !callbackCalled) - { + while (sub_inertial_unit_32.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } - } - else - { + } else { if (!inertial_unit_srv.response.success) ROS_ERROR("Sampling period is not valid."); ROS_ERROR("Failed to enable inertial_unit."); @@ -1974,8 +1856,7 @@ int main(int argc, char **argv) ros::ServiceClient noise_inertial_unit_client; webots_ros::get_float noise_inertial_unit_srv; - noise_inertial_unit_client = - n.serviceClient(model_name + "/inertial_unit/get_noise"); + noise_inertial_unit_client = n.serviceClient(model_name + "/inertial_unit/get_noise"); if (noise_inertial_unit_client.call(noise_inertial_unit_srv)) ROS_INFO("Noise value is %f.", noise_inertial_unit_srv.response.value); else @@ -2005,21 +1886,18 @@ int main(int argc, char **argv) ros::Subscriber sub_joystick; enable_joystick_srv.request.value = 32; - if (enable_joystick_client.call(enable_joystick_srv) && enable_joystick_srv.response.success) - { + if (enable_joystick_client.call(enable_joystick_srv) && enable_joystick_srv.response.success) { ROS_INFO("Joystick of %s has been enabled.", model_name.c_str()); sub_joystick = n.subscribe(model_name + "/joystick/pressed_button", 1, joystickCallback); callbackCalled = false; ROS_INFO("Topics for joystick initialized."); - while (sub_joystick.getNumPublishers() == 0 && !callbackCalled) - { + while (sub_joystick.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } ROS_INFO("Topics for joystick connected."); - } - else + } else ROS_ERROR("Failed to enable joystick."); sub_joystick.shutdown(); @@ -2081,22 +1959,18 @@ int main(int argc, char **argv) set_lidar_client = n.serviceClient(model_name + "/lidar/enable"); lidar_srv.request.value = TIME_STEP; - if (set_lidar_client.call(lidar_srv) && lidar_srv.response.success) - { + if (set_lidar_client.call(lidar_srv) && lidar_srv.response.success) { ROS_INFO("Lidar enabled."); sub_lidar = n.subscribe(model_name + "/lidar/range_image", 1, lidarCallback); callbackCalled = false; ROS_INFO("Topic for lidar initialized."); - while (sub_lidar.getNumPublishers() == 0 && !callbackCalled) - { + while (sub_lidar.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } ROS_INFO("Topic for lidar color connected."); - } - else - { + } else { if (!lidar_srv.response.success) ROS_ERROR("Sampling period is not valid."); ROS_ERROR("Failed to enable lidar."); @@ -2150,19 +2024,15 @@ int main(int argc, char **argv) sampling_period_light_sensor_client = n.serviceClient(model_name + "/light_sensor/get_sampling_period"); light_sensor_srv.request.value = 32; - if (set_light_sensor_client.call(light_sensor_srv) && light_sensor_srv.response.success) - { + if (set_light_sensor_client.call(light_sensor_srv) && light_sensor_srv.response.success) { ROS_INFO("Light_sensor enabled."); sub_light_sensor_32 = n.subscribe(model_name + "/light_sensor/value", 1, lightSensorCallback); callbackCalled = false; - while (sub_light_sensor_32.getNumPublishers() == 0 && !callbackCalled) - { + while (sub_light_sensor_32.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } - } - else - { + } else { if (!light_sensor_srv.response.success) ROS_ERROR("Sampling period is not valid."); ROS_ERROR("Failed to enable light_sensor."); @@ -2174,7 +2044,7 @@ int main(int argc, char **argv) ros::ServiceClient lookup_table_light_sensor_client; webots_ros::get_float_array lookup_table_light_sensor_srv; lookup_table_light_sensor_client = - n.serviceClient(model_name + "/light_sensor/get_lookup_table"); + n.serviceClient(model_name + "/light_sensor/get_lookup_table"); if (lookup_table_light_sensor_client.call(lookup_table_light_sensor_srv)) ROS_INFO("Light sensor lookup table size = %lu.", lookup_table_light_sensor_srv.response.value.size()); else @@ -2223,14 +2093,12 @@ int main(int argc, char **argv) ros::ServiceClient motor_get_brake_name_client; webots_ros::get_string motor_get_brake_name_srv; motor_get_brake_name_client = n.serviceClient(model_name + "/linear_motor/get_brake_name"); - if (motor_get_brake_name_client.call(motor_get_brake_name_srv)) - { + if (motor_get_brake_name_client.call(motor_get_brake_name_srv)) { ROS_INFO("Brake name returned from Motor API: %s.", motor_get_brake_name_srv.response.value.c_str()); if (motor_get_brake_name_srv.response.value.compare("my_brake") != 0) ROS_ERROR("Failed to call service motor_get_brake_name: received '%s' instead of 'my_brake'", motor_get_brake_name_srv.response.value.c_str()); - } - else + } else ROS_ERROR("Failed to call service motor_get_brake_name."); motor_get_brake_name_client.shutdown(); @@ -2331,7 +2199,7 @@ int main(int argc, char **argv) ros::ServiceClient set_linear_control_pid_client; webots_ros::motor_set_control_pid set_linear_control_pid_srv; set_linear_control_pid_client = - n.serviceClient(model_name + "/linear_motor/set_control_pid"); + n.serviceClient(model_name + "/linear_motor/set_control_pid"); set_linear_control_pid_srv.request.controlp = 1; if (set_linear_control_pid_client.call(set_linear_control_pid_srv) && set_linear_control_pid_srv.response.success == 1) @@ -2474,27 +2342,23 @@ int main(int argc, char **argv) webots_ros::set_int motor_feedback_srv; ros::Subscriber sub_motor_feedback_32; set_motor_feedback_client = - n.serviceClient(model_name + "/rotational_motor/torque_feedback_sensor/enable"); + n.serviceClient(model_name + "/rotational_motor/torque_feedback_sensor/enable"); ros::ServiceClient sampling_period_motor_feedback_client; webots_ros::get_int sampling_period_motor_feedback_srv; sampling_period_motor_feedback_client = - n.serviceClient(model_name + "/rotational_motor/torque_feedback_sensor/get_sampling_period"); + n.serviceClient(model_name + "/rotational_motor/torque_feedback_sensor/get_sampling_period"); motor_feedback_srv.request.value = 32; - if (set_motor_feedback_client.call(motor_feedback_srv) && motor_feedback_srv.response.success) - { + if (set_motor_feedback_client.call(motor_feedback_srv) && motor_feedback_srv.response.success) { ROS_INFO("Motor feedback enabled."); sub_motor_feedback_32 = n.subscribe(model_name + "/rotational_motor/torque_feedback", 1, motorSensorCallback); callbackCalled = false; - while (sub_motor_feedback_32.getNumPublishers() == 0 && !callbackCalled) - { + while (sub_motor_feedback_32.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } - } - else - { + } else { if (!motor_feedback_srv.response.success) ROS_ERROR("Sampling period is not valid."); ROS_ERROR("Failed to enable motor_feedback."); @@ -2561,22 +2425,18 @@ int main(int argc, char **argv) ros::ServiceClient sampling_period_position_sensor_client; webots_ros::get_int sampling_period_position_sensor_srv; sampling_period_position_sensor_client = - n.serviceClient(model_name + "/position_sensor/get_sampling_period"); + n.serviceClient(model_name + "/position_sensor/get_sampling_period"); position_sensor_srv.request.value = 32; - if (set_position_sensor_client.call(position_sensor_srv) && position_sensor_srv.response.success) - { + if (set_position_sensor_client.call(position_sensor_srv) && position_sensor_srv.response.success) { ROS_INFO("Position_sensor enabled."); sub_position_sensor_32 = n.subscribe(model_name + "/position_sensor/value", 1, positionSensorCallback); callbackCalled = false; - while (sub_position_sensor_32.getNumPublishers() == 0 && !callbackCalled) - { + while (sub_position_sensor_32.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } - } - else - { + } else { if (!position_sensor_srv.response.success) ROS_ERROR("Sampling period is not valid."); ROS_ERROR("Failed to enable position_sensor."); @@ -2621,23 +2481,19 @@ int main(int argc, char **argv) set_radar_client = n.serviceClient(model_name + "/radar/enable"); radar_srv.request.value = TIME_STEP; - if (set_radar_client.call(radar_srv) && radar_srv.response.success) - { + if (set_radar_client.call(radar_srv) && radar_srv.response.success) { ROS_INFO("Radar enabled."); sub_radar_target = n.subscribe(model_name + "/radar/targets", 1, radarTargetsCallback); sub_radar_target_number = n.subscribe(model_name + "/radar/number_of_targets", 1, radarTargetsNumberCallback); callbackCalled = false; ROS_INFO("Topics for radar initialized."); - while (sub_radar_target.getNumPublishers() == 0 && sub_radar_target_number.getNumPublishers() == 0 && !callbackCalled) - { + while (sub_radar_target.getNumPublishers() == 0 && sub_radar_target_number.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } ROS_INFO("Topics for radar connected."); - } - else - { + } else { if (!radar_srv.response.success) ROS_ERROR("Sampling period is not valid."); ROS_ERROR("Failed to enable radar."); @@ -2652,14 +2508,12 @@ int main(int argc, char **argv) // get max and min range ros::ServiceClient radar_range_client = n.serviceClient(model_name + "/radar/get_max_range"); webots_ros::get_float radar_get_max_range_srv; - if (radar_range_client.call(radar_get_max_range_srv)) - { + if (radar_range_client.call(radar_get_max_range_srv)) { if (radar_get_max_range_srv.response.value == 2.0) ROS_INFO("Received correct radar max range."); else ROS_ERROR("Received wrong radar max range."); - } - else + } else ROS_ERROR("Failed to call service radar_get_max_range."); radar_range_client.shutdown(); @@ -2667,14 +2521,12 @@ int main(int argc, char **argv) radar_range_client = n.serviceClient(model_name + "/radar/get_min_range"); webots_ros::get_float radar_get_min_range_srv; - if (radar_range_client.call(radar_get_min_range_srv)) - { + if (radar_range_client.call(radar_get_min_range_srv)) { if (radar_get_min_range_srv.response.value == 1.0) ROS_INFO("Received correct radar min range."); else ROS_ERROR("Received wrong radar min range."); - } - else + } else ROS_ERROR("Failed to call service radar_get_min_range."); radar_range_client.shutdown(); @@ -2693,22 +2545,18 @@ int main(int argc, char **argv) set_range_finder_client = n.serviceClient(model_name + "/range_finder/enable"); range_finder_srv.request.value = TIME_STEP; - if (set_range_finder_client.call(range_finder_srv) && range_finder_srv.response.success) - { + if (set_range_finder_client.call(range_finder_srv) && range_finder_srv.response.success) { ROS_INFO("Range-finder enabled."); sub_range_finder_color = n.subscribe(model_name + "/range_finder/range_image", 1, rangeFinderCallback); callbackCalled = false; ROS_INFO("Topic for range-finder initialized."); - while (sub_range_finder_color.getNumPublishers() == 0 && !callbackCalled) - { + while (sub_range_finder_color.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } ROS_INFO("Topic for range-finder connected."); - } - else - { + } else { if (!range_finder_srv.response.success) ROS_ERROR("Sampling period is not valid."); ROS_ERROR("Failed to enable range-finder."); @@ -2724,10 +2572,10 @@ int main(int argc, char **argv) webots_ros::range_finder_get_info get_range_finder_info_srv; if (get_info_client.call(get_range_finder_info_srv)) ROS_INFO( - "Range-finder of %s has a width of %d, a height of %d, a field of view of %f, a min range of %f and a max range of %f.", - model_name.c_str(), get_range_finder_info_srv.response.width, get_range_finder_info_srv.response.height, - get_range_finder_info_srv.response.Fov, get_range_finder_info_srv.response.minRange, - get_range_finder_info_srv.response.maxRange); + "Range-finder of %s has a width of %d, a height of %d, a field of view of %f, a min range of %f and a max range of %f.", + model_name.c_str(), get_range_finder_info_srv.response.width, get_range_finder_info_srv.response.height, + get_range_finder_info_srv.response.Fov, get_range_finder_info_srv.response.minRange, + get_range_finder_info_srv.response.maxRange); else ROS_ERROR("Failed to call service range_finder_get_info."); @@ -2764,19 +2612,15 @@ int main(int argc, char **argv) sampling_period_receiver_client = n.serviceClient(model_name + "/receiver/get_sampling_period"); receiver_srv.request.value = 32; - if (set_receiver_client.call(receiver_srv) && receiver_srv.response.success) - { + if (set_receiver_client.call(receiver_srv) && receiver_srv.response.success) { ROS_INFO("Receiver enabled."); sub_receiver_32 = n.subscribe(model_name + "/receiver/data", 1, receiverCallback); callbackCalled = false; - while (sub_receiver_32.getNumPublishers() == 0 && !callbackCalled) - { + while (sub_receiver_32.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } - } - else - { + } else { if (!receiver_srv.response.success) ROS_ERROR("Sampling period is not valid."); ROS_ERROR("Failed to enable receiver."); @@ -2865,7 +2709,7 @@ int main(int argc, char **argv) ros::ServiceClient receiver_get_emitter_direction_client; webots_ros::receiver_get_emitter_direction receiver_get_emitter_direction_srv; receiver_get_emitter_direction_client = - n.serviceClient(model_name + "/receiver/get_emitter_direction"); + n.serviceClient(model_name + "/receiver/get_emitter_direction"); receiver_get_emitter_direction_client.call(receiver_get_emitter_direction_srv); if (receiver_get_emitter_direction_srv.response.direction[0] != 0 || @@ -2926,8 +2770,7 @@ int main(int argc, char **argv) time_step_client.call(time_step_srv); touch_sensor_srv.request.value = 32; - if (set_touch_sensor_client.call(touch_sensor_srv) && touch_sensor_srv.response.success) - { + if (set_touch_sensor_client.call(touch_sensor_srv) && touch_sensor_srv.response.success) { ROS_INFO("Touch_sensor enabled."); if (touch_sensor_get_type_srv.response.value == 0) sub_touch_sensor_32 = n.subscribe(model_name + "/touch_sensor/value", 1, touchSensorBumperCallback); @@ -2936,14 +2779,11 @@ int main(int argc, char **argv) else sub_touch_sensor_32 = n.subscribe(model_name + "/touch_sensor/values", 1, touchSensor3DCallback); callbackCalled = false; - while (sub_touch_sensor_32.getNumPublishers() == 0 && !callbackCalled) - { + while (sub_touch_sensor_32.getNumPublishers() == 0 && !callbackCalled) { ros::spinOnce(); time_step_client.call(time_step_srv); } - } - else - { + } else { if (!touch_sensor_srv.response.success) ROS_ERROR("Sampling period is not valid."); ROS_ERROR("Failed to enable touch_sensor."); @@ -2955,7 +2795,7 @@ int main(int argc, char **argv) ros::ServiceClient lookup_table_touch_sensor_client; webots_ros::get_float_array lookup_table_touch_sensor_srv; lookup_table_touch_sensor_client = - n.serviceClient(model_name + "/touch_sensor/get_lookup_table"); + n.serviceClient(model_name + "/touch_sensor/get_lookup_table"); if (lookup_table_touch_sensor_client.call(lookup_table_touch_sensor_srv)) ROS_INFO("Touch sensor lookup table size = %lu.", lookup_table_touch_sensor_srv.response.value.size()); else @@ -2985,7 +2825,7 @@ int main(int argc, char **argv) ros::ServiceClient supervisor_simulation_reset_physics_client; webots_ros::get_bool supervisor_simulation_reset_physics_srv; supervisor_simulation_reset_physics_client = - n.serviceClient(model_name + "/supervisor/simulation_reset_physics"); + n.serviceClient(model_name + "/supervisor/simulation_reset_physics"); if (supervisor_simulation_reset_physics_client.call(supervisor_simulation_reset_physics_srv) && supervisor_simulation_reset_physics_srv.response.value) @@ -3054,17 +2894,15 @@ int main(int argc, char **argv) ros::ServiceClient supervisor_get_from_def_client; webots_ros::supervisor_get_from_def supervisor_get_from_def_srv; supervisor_get_from_def_client = - n.serviceClient(model_name + "/supervisor/get_from_def"); + n.serviceClient(model_name + "/supervisor/get_from_def"); supervisor_get_from_def_srv.request.name = "TEST"; supervisor_get_from_def_client.call(supervisor_get_from_def_srv); uint64_t from_def_node = 0; - if (supervisor_get_from_def_srv.response.node != 0) - { + if (supervisor_get_from_def_srv.response.node != 0) { ROS_INFO("Got from DEF node: %ld.", supervisor_get_from_def_srv.response.node); from_def_node = supervisor_get_from_def_srv.response.node; - } - else + } else ROS_ERROR("Could not get node from DEF."); time_step_client.call(time_step_srv); @@ -3083,7 +2921,7 @@ int main(int argc, char **argv) ros::ServiceClient supervisor_node_get_type_name_client; webots_ros::node_get_name supervisor_node_get_type_name_srv; supervisor_node_get_type_name_client = - n.serviceClient(model_name + "/supervisor/node/get_type_name"); + n.serviceClient(model_name + "/supervisor/node/get_type_name"); supervisor_node_get_type_name_srv.request.node = from_def_node; supervisor_node_get_type_name_client.call(supervisor_node_get_type_name_srv); @@ -3100,12 +2938,10 @@ int main(int argc, char **argv) supervisor_get_from_def_srv.request.name = "GROUND"; supervisor_get_from_def_client.call(supervisor_get_from_def_srv); uint64_t ground_node = 0; - if (supervisor_get_from_def_srv.response.node != 0) - { + if (supervisor_get_from_def_srv.response.node != 0) { ROS_INFO("Got from DEF GROUND node: %ld.", supervisor_get_from_def_srv.response.node); ground_node = supervisor_get_from_def_srv.response.node; - } - else + } else ROS_ERROR("Could not get node from DEF GROUND."); supervisor_node_get_type_name_srv.request.node = ground_node; @@ -3117,7 +2953,7 @@ int main(int argc, char **argv) ros::ServiceClient supervisor_node_get_base_type_name_client; webots_ros::node_get_name supervisor_node_get_base_type_name_srv; supervisor_node_get_base_type_name_client = - n.serviceClient(model_name + "/supervisor/node/get_base_type_name"); + n.serviceClient(model_name + "/supervisor/node/get_base_type_name"); supervisor_node_get_base_type_name_srv.request.node = ground_node; supervisor_node_get_base_type_name_client.call(supervisor_node_get_base_type_name_srv); ROS_INFO("Got base type name of GROUND node: %s.", supervisor_node_get_base_type_name_srv.response.name.c_str()); @@ -3138,7 +2974,7 @@ int main(int argc, char **argv) ros::ServiceClient supervisor_node_get_position_client; webots_ros::node_get_position supervisor_node_get_position_srv; supervisor_node_get_position_client = - n.serviceClient(model_name + "/supervisor/node/get_position"); + n.serviceClient(model_name + "/supervisor/node/get_position"); supervisor_node_get_position_srv.request.node = from_def_node; supervisor_node_get_position_client.call(supervisor_node_get_position_srv); @@ -3151,14 +2987,14 @@ int main(int argc, char **argv) ros::ServiceClient supervisor_node_get_orientation_client; webots_ros::node_get_orientation supervisor_node_get_orientation_srv; supervisor_node_get_orientation_client = - n.serviceClient(model_name + "/supervisor/node/get_orientation"); + n.serviceClient(model_name + "/supervisor/node/get_orientation"); supervisor_node_get_orientation_srv.request.node = from_def_node; supervisor_node_get_orientation_client.call(supervisor_node_get_orientation_srv); ROS_INFO( - "From_def orientation quaternion is:\nw=%f x=%f y=%f z=%f.", supervisor_node_get_orientation_srv.response.orientation.w, - supervisor_node_get_orientation_srv.response.orientation.x, supervisor_node_get_orientation_srv.response.orientation.y, - supervisor_node_get_orientation_srv.response.orientation.z); + "From_def orientation quaternion is:\nw=%f x=%f y=%f z=%f.", supervisor_node_get_orientation_srv.response.orientation.w, + supervisor_node_get_orientation_srv.response.orientation.x, supervisor_node_get_orientation_srv.response.orientation.y, + supervisor_node_get_orientation_srv.response.orientation.z); supervisor_node_get_orientation_client.shutdown(); time_step_client.call(time_step_srv); @@ -3166,7 +3002,7 @@ int main(int argc, char **argv) ros::ServiceClient supervisor_node_get_center_of_mass_client; webots_ros::node_get_center_of_mass supervisor_node_get_center_of_mass_srv; supervisor_node_get_center_of_mass_client = - n.serviceClient(model_name + "/supervisor/node/get_center_of_mass"); + n.serviceClient(model_name + "/supervisor/node/get_center_of_mass"); supervisor_node_get_center_of_mass_srv.request.node = from_def_node; supervisor_node_get_center_of_mass_client.call(supervisor_node_get_center_of_mass_srv); @@ -3181,7 +3017,7 @@ int main(int argc, char **argv) ros::ServiceClient supervisor_node_get_number_of_contact_points_client; webots_ros::node_get_number_of_contact_points supervisor_node_get_number_of_contact_points_srv; supervisor_node_get_number_of_contact_points_client = n.serviceClient( - model_name + "/supervisor/node/get_number_of_contact_points"); + model_name + "/supervisor/node/get_number_of_contact_points"); supervisor_node_get_number_of_contact_points_srv.request.node = from_def_node; supervisor_node_get_number_of_contact_points_srv.request.includeDescendants = false; @@ -3195,7 +3031,7 @@ int main(int argc, char **argv) ros::ServiceClient supervisor_node_get_contact_point_client; webots_ros::node_get_contact_point supervisor_node_get_contact_point_srv; supervisor_node_get_contact_point_client = - n.serviceClient(model_name + "/supervisor/node/get_contact_point"); + n.serviceClient(model_name + "/supervisor/node/get_contact_point"); supervisor_node_get_contact_point_srv.request.node = from_def_node; supervisor_node_get_contact_point_srv.request.index = 0; @@ -3209,7 +3045,7 @@ int main(int argc, char **argv) ros::ServiceClient supervisor_node_get_contact_point_node_client; webots_ros::node_get_contact_point_node supervisor_node_get_contact_point_node_srv; supervisor_node_get_contact_point_node_client = - n.serviceClient(model_name + "/supervisor/node/get_contact_point_node"); + n.serviceClient(model_name + "/supervisor/node/get_contact_point_node"); supervisor_node_get_contact_point_node_srv.request.node = from_def_node; supervisor_node_get_contact_point_node_srv.request.index = 0; @@ -3224,7 +3060,7 @@ int main(int argc, char **argv) ros::ServiceClient supervisor_node_get_static_balance_client; webots_ros::node_get_static_balance supervisor_node_get_static_balance_srv; supervisor_node_get_static_balance_client = - n.serviceClient(model_name + "/supervisor/node/get_static_balance"); + n.serviceClient(model_name + "/supervisor/node/get_static_balance"); supervisor_node_get_static_balance_srv.request.node = from_def_node; supervisor_node_get_static_balance_client.call(supervisor_node_get_static_balance_srv); @@ -3236,7 +3072,7 @@ int main(int argc, char **argv) // test reset_physics // if the node isn't a top Solid webots will throw a warning but still return true to ros ros::ServiceClient supervisor_node_reset_physics_client = - n.serviceClient(model_name + "/supervisor/node/reset_physics"); + n.serviceClient(model_name + "/supervisor/node/reset_physics"); webots_ros::node_reset_physics supervisor_node_reset_physics_srv; supervisor_node_reset_physics_srv.request.node = from_def_node; @@ -3252,7 +3088,7 @@ int main(int argc, char **argv) // test restart_controller ros::ServiceClient supervisor_node_restart_controller_client = - n.serviceClient(model_name + "/supervisor/node/restart_controller"); + n.serviceClient(model_name + "/supervisor/node/restart_controller"); webots_ros::node_reset_functions supervisor_node_restart_controller_srv; supervisor_node_restart_controller_srv.request.node = from_def_node; @@ -3292,7 +3128,7 @@ int main(int argc, char **argv) ros::ServiceClient supervisor_field_get_type_name_client; webots_ros::field_get_type_name supervisor_field_get_type_name_srv; supervisor_field_get_type_name_client = - n.serviceClient(model_name + "/supervisor/field/get_type_name"); + n.serviceClient(model_name + "/supervisor/field/get_type_name"); supervisor_field_get_type_name_srv.request.field = field; supervisor_field_get_type_name_client.call(supervisor_field_get_type_name_srv); @@ -3323,7 +3159,7 @@ int main(int argc, char **argv) ros::ServiceClient supervisor_field_set_string_client; webots_ros::field_set_string supervisor_field_set_string_srv; supervisor_field_set_string_client = - n.serviceClient(model_name + "/supervisor/field/set_string"); + n.serviceClient(model_name + "/supervisor/field/set_string"); supervisor_field_set_string_srv.request.field = field; supervisor_field_set_string_srv.request.value = "solid_test"; @@ -3339,7 +3175,7 @@ int main(int argc, char **argv) ros::ServiceClient supervisor_field_get_string_client; webots_ros::field_get_string supervisor_field_get_string_srv; supervisor_field_get_string_client = - n.serviceClient(model_name + "/supervisor/field/get_string"); + n.serviceClient(model_name + "/supervisor/field/get_string"); supervisor_field_get_string_srv.request.field = field; supervisor_field_get_string_client.call(supervisor_field_get_string_srv); @@ -3370,12 +3206,10 @@ int main(int argc, char **argv) supervisor_get_from_def_srv.request.proto = 0; supervisor_get_from_def_client.call(supervisor_get_from_def_srv); uint64_t cone_node = 0; - if (supervisor_get_from_def_srv.response.node != 0) - { + if (supervisor_get_from_def_srv.response.node != 0) { ROS_INFO("Got CONE node from DEF: %lu.", supervisor_get_from_def_srv.response.node); cone_node = supervisor_get_from_def_srv.response.node; - } - else + } else ROS_ERROR("could not get CONE node from DEF."); supervisor_node_get_type_name_client.shutdown(); @@ -3415,7 +3249,7 @@ int main(int argc, char **argv) ros::ServiceClient supervisor_get_from_device_client; webots_ros::supervisor_get_from_string supervisor_get_from_device_srv; supervisor_get_from_device_client = - n.serviceClient(model_name + "/supervisor/get_from_device"); + n.serviceClient(model_name + "/supervisor/get_from_device"); supervisor_get_from_device_srv.request.value = "compass"; supervisor_get_from_device_client.call(supervisor_get_from_device_srv); uint64_t compass_node_from_device = supervisor_get_from_device_srv.response.node; @@ -3463,7 +3297,7 @@ int main(int argc, char **argv) ros::ServiceClient node_add_force_or_torque_client; webots_ros::node_add_force_or_torque node_add_force_or_torque_srv; node_add_force_or_torque_client = - n.serviceClient(model_name + "/supervisor/node/add_torque"); + n.serviceClient(model_name + "/supervisor/node/add_torque"); node_add_force_or_torque_srv.request.node = cone_node; node_add_force_or_torque_srv.request.force.x = 0.0; node_add_force_or_torque_srv.request.force.y = 0.0; @@ -3481,7 +3315,7 @@ int main(int argc, char **argv) ros::ServiceClient node_add_force_with_offset_client; webots_ros::node_add_force_with_offset node_add_force_with_offset_srv; node_add_force_with_offset_client = - n.serviceClient(model_name + "/supervisor/node/add_force_with_offset"); + n.serviceClient(model_name + "/supervisor/node/add_force_with_offset"); node_add_force_with_offset_srv.request.node = cone_node; node_add_force_with_offset_srv.request.force.x = 0.0; node_add_force_with_offset_srv.request.force.y = 0.0; @@ -3503,7 +3337,7 @@ int main(int argc, char **argv) ros::ServiceClient node_get_parent_node_client; webots_ros::node_get_parent_node node_get_parent_node_srv; node_get_parent_node_client = - n.serviceClient(model_name + "/supervisor/node/get_parent_node"); + n.serviceClient(model_name + "/supervisor/node/get_parent_node"); node_get_parent_node_srv.request.node = cone_node; node_get_parent_node_client.call(node_get_parent_node_srv); if (node_get_parent_node_srv.response.node == root_node) @@ -3531,7 +3365,7 @@ int main(int argc, char **argv) ros::ServiceClient supervisor_movie_start_client; webots_ros::supervisor_movie_start_recording supervisor_movie_start_srv; supervisor_movie_start_client = - n.serviceClient(model_name + "/supervisor/movie_start_recording"); + n.serviceClient(model_name + "/supervisor/movie_start_recording"); supervisor_movie_start_srv.request.filename = std::string(getenv("HOME")) + std::string("/movie_test.mp4"); supervisor_movie_start_srv.request.width = 480; @@ -3651,7 +3485,7 @@ int main(int argc, char **argv) ros::ServiceClient virtual_reality_headset_client; webots_ros::get_bool supervisor_virtual_reality_headset_is_used_srv; virtual_reality_headset_client = - n.serviceClient(model_name + "/supervisor/vitual_reality_headset_is_used"); + n.serviceClient(model_name + "/supervisor/vitual_reality_headset_is_used"); virtual_reality_headset_client.call(supervisor_virtual_reality_headset_is_used_srv); bool used = supervisor_virtual_reality_headset_is_used_srv.response.value; // to test this service we assume no virtual reality headset is connected From 2726055aec85f570e284215f68c221d5de7bb8a2 Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Tue, 16 Feb 2021 20:24:31 +0100 Subject: [PATCH 10/72] Delete --- .clang-format | 88 --------------------------------------------------- 1 file changed, 88 deletions(-) delete mode 100644 .clang-format diff --git a/.clang-format b/.clang-format deleted file mode 100644 index bc49dbc..0000000 --- a/.clang-format +++ /dev/null @@ -1,88 +0,0 @@ ---- -Language: Cpp -AccessModifierOffset: -2 -AlignAfterOpenBracket: Align -AlignEscapedNewlines: Left -AlignOperands: true -AllowAllConstructorInitializersOnNextLine: false -AllowAllParametersOfDeclarationOnNextLine: false -AllowShortBlocksOnASingleLine: false -AllowShortCaseLabelsOnASingleLine: false -AllowShortFunctionsOnASingleLine: InlineOnly -AllowShortIfStatementsOnASingleLine: false -AllowShortLoopsOnASingleLine: false -AlwaysBreakAfterDefinitionReturnType: None -AlwaysBreakAfterReturnType: None -AlwaysBreakBeforeMultilineStrings: false -AlwaysBreakTemplateDeclarations: false -BinPackArguments: true -BinPackParameters: true -BreakBeforeBinaryOperators: None -BreakBeforeBraces: Attach -BreakBeforeInheritanceComma: false -BreakBeforeTernaryOperators: false -BreakConstructorInitializersBeforeComma: false -BreakConstructorInitializers: AfterColon -BreakStringLiterals: true -ColumnLimit: 128 -CommentPragmas: '^ IWYU pragma:' -CompactNamespaces: false -ConstructorInitializerAllOnOneLineOrOnePerLine: true -ConstructorInitializerIndentWidth: 2 -ContinuationIndentWidth: 2 -Cpp11BracedListStyle: true -DerivePointerAlignment: false -DisableFormat: false -ExperimentalAutoDetectBinPacking: false -FixNamespaceComments: true -ForEachMacros: - - foreach - - Q_FOREACH - - BOOST_FOREACH -IncludeCategories: - - Regex: '^<.*\.h>' - Priority: 1 - - Regex: '^<.*' - Priority: 2 - - Regex: '.*' - Priority: 3 -IncludeIsMainRegex: '([-_](test|unittest))?$' -IndentCaseLabels: true -IndentWidth: 2 -IndentWrappedFunctionNames: true -JavaScriptQuotes: Single -JavaScriptWrapImports: true -KeepEmptyLinesAtTheStartOfBlocks: false -MacroBlockBegin: '' -MacroBlockEnd: '' -MaxEmptyLinesToKeep: 1 -NamespaceIndentation: All -ObjCBlockIndentWidth: 2 -ObjCSpaceAfterProperty: false -ObjCSpaceBeforeProtocolList: false -PenaltyBreakAssignment: 2 -PenaltyBreakBeforeFirstCallParameter: 1 -PenaltyBreakComment: 300 -PenaltyBreakFirstLessLess: 120 -PenaltyBreakString: 1000 -PenaltyExcessCharacter: 1000000 -PenaltyReturnTypeOnItsOwnLine: 200 -PointerAlignment: Right -ReflowComments: true -SortIncludes: true -SortUsingDeclarations: true -SpaceAfterCStyleCast: false -SpaceAfterTemplateKeyword: false -SpaceBeforeAssignmentOperators: true -SpaceBeforeParens: ControlStatements -SpaceInEmptyParentheses: false -SpacesBeforeTrailingComments: 2 -SpacesInAngles: false -SpacesInContainerLiterals: false -SpacesInCStyleCastParentheses: false -SpacesInParentheses: false -SpacesInSquareBrackets: false -Standard: Cpp11 -TabWidth: 8 -UseTab: Never -... From 7ada854c4a08db1b245339e9c58e89a8141ef98b Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Tue, 16 Feb 2021 22:02:42 +0100 Subject: [PATCH 11/72] Add node_set_string --- CMakeLists.txt | 1 + srv/node_set_string.srv | 4 ++++ 2 files changed, 5 insertions(+) create mode 100644 srv/node_set_string.srv diff --git a/CMakeLists.txt b/CMakeLists.txt index f7b57d8..6c4f767 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -104,6 +104,7 @@ find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs sensor_msgs messag node_get_static_balance.srv node_get_status.srv node_get_string.srv + node_set_string.srv node_get_type.srv node_get_velocity.srv node_remove.srv diff --git a/srv/node_set_string.srv b/srv/node_set_string.srv new file mode 100644 index 0000000..44068dc --- /dev/null +++ b/srv/node_set_string.srv @@ -0,0 +1,4 @@ +uint64 node +string state_name +--- +int32 success From fbb51fefba99882219b1e9a002d1b674b32e2b78 Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Tue, 16 Feb 2021 22:17:12 +0100 Subject: [PATCH 12/72] Add save/load state test --- src/complete_test.cpp | 33 +++++++++++++++++++++++++++++++++ 1 file changed, 33 insertions(+) diff --git a/src/complete_test.cpp b/src/complete_test.cpp index 68dc2f4..8d601a8 100644 --- a/src/complete_test.cpp +++ b/src/complete_test.cpp @@ -113,6 +113,7 @@ #include #include #include +#include #include #include #include @@ -3086,6 +3087,38 @@ int main(int argc, char **argv) { supervisor_node_reset_physics_client.shutdown(); time_step_client.call(time_step_srv); + // test node_save_state + ros::ServiceClient supervisor_node_save_state_client = + n.serviceClient(model_name + "/supervisor/node/save_state"); + webots_ros::node_set_string supervisor_node_save_state_srv; + + supervisor_node_save_state_srv.request.node = from_def_node; + supervisor_node_save_state_srv.request.state_name = "dummy_state"; + if (supervisor_node_save_state_client.call(supervisor_node_save_state_srv) && + supervisor_node_save_state_srv.response.success == 1) + ROS_INFO("Node state has been saved successfully."); + else + ROS_ERROR("Failed to call service node_save_state."); + + supervisor_node_save_state_client.shutdown(); + time_step_client.call(time_step_srv); + + // test node_load_state + ros::ServiceClient supervisor_node_load_state_client = + n.serviceClient(model_name + "/supervisor/node/load_state"); + webots_ros::node_set_string supervisor_node_load_state_srv; + + supervisor_node_load_state_srv.request.node = from_def_node; + supervisor_node_load_state_srv.request.state_name = "dummy_state"; + if (supervisor_node_load_state_client.call(supervisor_node_load_state_srv) && + supervisor_node_load_state_srv.response.success == 1) + ROS_INFO("Node state has been loaded successfully."); + else + ROS_ERROR("Failed to call service node_load_state."); + + supervisor_node_load_state_client.shutdown(); + time_step_client.call(time_step_srv); + // test restart_controller ros::ServiceClient supervisor_node_restart_controller_client = n.serviceClient(model_name + "/supervisor/node/restart_controller"); From 90593eeac011f990dae715e4f4641636c0a21bde Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Wed, 17 Feb 2021 11:34:27 +0100 Subject: [PATCH 13/72] Revert physics reset --- CMakeLists.txt | 1 - src/complete_test.cpp | 6 ++---- srv/node_reset_physics.srv | 4 ---- 3 files changed, 2 insertions(+), 9 deletions(-) delete mode 100644 srv/node_reset_physics.srv diff --git a/CMakeLists.txt b/CMakeLists.txt index 6c4f767..500565a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -109,7 +109,6 @@ find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs sensor_msgs messag node_get_velocity.srv node_remove.srv node_reset_functions.srv - node_reset_physics.srv node_move_viewpoint.srv node_set_visibility.srv node_set_velocity.srv diff --git a/src/complete_test.cpp b/src/complete_test.cpp index 8d601a8..774ba96 100644 --- a/src/complete_test.cpp +++ b/src/complete_test.cpp @@ -112,7 +112,6 @@ #include #include #include -#include #include #include #include @@ -3073,11 +3072,10 @@ int main(int argc, char **argv) { // test reset_physics // if the node isn't a top Solid webots will throw a warning but still return true to ros ros::ServiceClient supervisor_node_reset_physics_client = - n.serviceClient(model_name + "/supervisor/node/reset_physics"); - webots_ros::node_reset_physics supervisor_node_reset_physics_srv; + n.serviceClient(model_name + "/supervisor/node/reset_physics"); + webots_ros::node_reset_functions supervisor_node_reset_physics_srv; supervisor_node_reset_physics_srv.request.node = from_def_node; - supervisor_node_reset_physics_srv.request.recursive = true; if (supervisor_node_reset_physics_client.call(supervisor_node_reset_physics_srv) && supervisor_node_reset_physics_srv.response.success == 1) ROS_INFO("Node physics has been reset successfully."); diff --git a/srv/node_reset_physics.srv b/srv/node_reset_physics.srv deleted file mode 100644 index 2d11b80..0000000 --- a/srv/node_reset_physics.srv +++ /dev/null @@ -1,4 +0,0 @@ -uint64 node -bool recursive ---- -bool success From 5c9db2d060e3f9ba38850f373a943b154c203e41 Mon Sep 17 00:00:00 2001 From: Olivier Michel Date: Wed, 3 Mar 2021 16:17:03 +0100 Subject: [PATCH 14/72] Fixed URLs --- worlds/e-puck_line.wbt | 2 +- worlds/textures/floor.png | Bin 3573 -> 0 bytes 2 files changed, 1 insertion(+), 1 deletion(-) delete mode 100644 worlds/textures/floor.png diff --git a/worlds/e-puck_line.wbt b/worlds/e-puck_line.wbt index 97007e2..ea5a426 100644 --- a/worlds/e-puck_line.wbt +++ b/worlds/e-puck_line.wbt @@ -16,7 +16,7 @@ RectangleArena { floorAppearance PBRAppearance { baseColorMap ImageTexture { url [ - "textures/floor.png" + "https://raw.githubusercontent.com/cyberbotics/webots/R2021a/projects/robots/gctronic/e-puck/worlds/textures/floor.png" ] } roughness 1 diff --git a/worlds/textures/floor.png b/worlds/textures/floor.png deleted file mode 100644 index c80c394c5ea17048b676d2ea2dad6c46261c356d..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 3573 zcmdT{SzMD>7Qb0+5QHG0P^+LF5&BbLXjEJXiL$65Br1z#5rs;aAW${~2}{Ja3Min? z00Ba&BBY{-kg|kOL@2TuRw0B40a*k>2w@FLKO7&NhnbgosJ$=u{?57g-1B|sod5sa zABj#!)KxdB0sz#H9kn?L032Gvfy(Mc+hqM1l2;zHIpiG4m=rkPaEUNM^OA@e{dpYI{tNDoR&Pal8(p4GtG z60x@WOAd!4eb>yQuE8UN;fT-cdq2AXeBux}s1xzltgfyuNgqp=3RgbUh(hZ#($ey&8r!vi{T>WPs#q3_`^uz~Y+(=-^Va+~5>Y)X3%?u2w4}{V z^i;(xUqYY?^iS#vqot0W+BbH9yXQhfL;0nF6&ELG$movi;B|y|wB+SO#|#YvD^Ho` zTQfNhln2aD2?DwcWuxdl+d(+b_cr!oqY0Z~6;nRGtvYTS2yaX!SVakA=A(FV8==H< zA#6<7<~dI~V7C6c{)Cb{WNmZgp}>N=UF&EQp{A z5M&{n4g=%k*M))4&b_#HU7z}h?@KR5(rYT`dIJJmXXoc}(k2WRi#42p`56HnQN2N& z9109yze1^47AOH3I$oET9osv1JUCb!$QHQP=rOh7cylYITv|%%dhB7bZ{O*UvMMOd zrKG~jnO9o(!4@rW)7;9cf39ixpWbI`w3yb=sRL!p{AcGI?N7tQA(!iFYi;e{Zxi5-K|{EX7>j6msZff>V{Ed&^d)E0PXh zkQyiwJyUgPr8Q;v}i%ak>`||zS1-_^u4#Y_kJ3= zFm+=~8YYj;W?wO{g=&l^6e?e!aZRy)K0br-YNo~{LUY8-m3*FmQ&Us;%sW*%<_!EN zy^-v=SU%=R(&lf@HE<6TP^CeF-u8A;W?1f6ZG{!KJ?z0Y!W@C#CEL}6kFJ4%nB_TjG&;EV`9FkJF zFs8>;rL+YiTW16lf^O6D`F@%=b?ku#cSY4W4ZQZUfC$nkJ35qStLuVAE*yk0-@6ui zmc=O*CLX1c`&G!c!1zV{8nrW#16R>F`Si0!Qx;Wi8U+#U`>w})Zk&=P=1^X!TP)l$ zc5bmfnop%t3psPWt9GXcpxlyAzq+@RV~TWH4+@IH$J&yUlMyHl`D{C3mH4sqeHcs2 z=&7<1SB_fzuN#mXzS_LG?R+|U!!BHy80F>G-!{NS%?!=XhL`ZmN$n3ET?hci z^!oy}I#_35>noAS|Iv*q{-)>bJI5a=aW|Q7%;h6N0gqhbhnsFR Date: Fri, 5 Mar 2021 17:45:27 +0100 Subject: [PATCH 15/72] Implement ROS control example --- launch/pioneer3at.launch | 12 +- src/pioneer3at.cpp | 280 ++------------------------------------- worlds/pioneer3at.wbt | 1 + 3 files changed, 20 insertions(+), 273 deletions(-) diff --git a/launch/pioneer3at.launch b/launch/pioneer3at.launch index 903fa00..27001fa 100644 --- a/launch/pioneer3at.launch +++ b/launch/pioneer3at.launch @@ -1,7 +1,13 @@ + + joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 100 + + - + @@ -9,5 +15,7 @@ - + + + diff --git a/src/pioneer3at.cpp b/src/pioneer3at.cpp index caa4c35..b38eeb8 100644 --- a/src/pioneer3at.cpp +++ b/src/pioneer3at.cpp @@ -31,296 +31,34 @@ #include #define TIME_STEP 32 -#define NMOTORS 4 -#define MAX_SPEED 6.4 -#define OBSTACLE_THRESHOLD 0.1 -#define DECREASE_FACTOR 0.9 -#define BACK_SLOWDOWN 0.9 ros::NodeHandle *n; -static std::vector lidarValues; - -static int controllerCount; -static std::vector controllerList; - ros::ServiceClient timeStepClient; webots_ros::set_int timeStepSrv; -static const char *motorNames[NMOTORS] = {"front_left_wheel", "front_right_wheel", "back_left_wheel", "back_right_wheel"}; - -static double GPSValues[2] = {0, 0}; -static double inertialUnitValues[4] = {0, 0, 0, 0}; - -static int lms291Resolution = 0; -static int halfResolution = 0; -static double maxRange = 0.0; -static double rangeThreshold = 0.0; -static std::vector braitenbergCoefficients; -static bool areBraitenbergCoefficientsinitialized = false; - -// gaussian function -double gaussian(double x, double mu, double sigma) { - return (1.0 / (sigma * sqrt(2.0 * M_PI))) * exp(-((x - mu) * (x - mu)) / (2 * sigma * sigma)); -} - -void updateSpeed() { - // init dynamic variables - double leftObstacle = 0.0, rightObstacle = 0.0, obstacle = 0.0; - double speeds[NMOTORS]; - // apply the braitenberg coefficients on the resulted values of the lms291 - // near obstacle sensed on the left side - for (int i = 0; i < halfResolution; ++i) { - if (lidarValues[i] < rangeThreshold) // far obstacles are ignored - leftObstacle += braitenbergCoefficients[i] * (1.0 - lidarValues[i] / maxRange); - // near obstacle sensed on the right side - int j = lms291Resolution - i - 1; - if (lidarValues[j] < rangeThreshold) - rightObstacle += braitenbergCoefficients[i] * (1.0 - lidarValues[j] / maxRange); - } - // overall front obstacle - obstacle = leftObstacle + rightObstacle; - // compute the speed according to the information on - // obstacles - if (obstacle > OBSTACLE_THRESHOLD) { - const double speedFactor = (1.0 - DECREASE_FACTOR * obstacle) * MAX_SPEED / obstacle; - speeds[0] = speedFactor * leftObstacle; - speeds[1] = speedFactor * rightObstacle; - speeds[2] = BACK_SLOWDOWN * speeds[0]; - speeds[3] = BACK_SLOWDOWN * speeds[1]; - } else { - speeds[0] = MAX_SPEED; - speeds[1] = MAX_SPEED; - speeds[2] = MAX_SPEED; - speeds[3] = MAX_SPEED; - } - // set speeds - for (int i = 0; i < NMOTORS; ++i) { - ros::ServiceClient set_velocity_client; - webots_ros::set_float set_velocity_srv; - set_velocity_client = n->serviceClient(std::string("pioneer3at/") + std::string(motorNames[i]) + - std::string("/set_velocity")); - set_velocity_srv.request.value = speeds[i]; - set_velocity_client.call(set_velocity_srv); - } -} - -void broadcastTransform() { - static tf::TransformBroadcaster br; - tf::Transform transform; - - // Publish odometry transform - transform.setOrigin(tf::Vector3(GPSValues[0], GPSValues[1], 0)); - tf::Quaternion q(inertialUnitValues[0], inertialUnitValues[1], inertialUnitValues[2], inertialUnitValues[3]); - transform.setRotation(q); - br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "odom", "base_link")); - - // Publish Lidar transform (inverted to match ENU) - transform.setIdentity(); - transform.setRotation(tf::Quaternion(tf::Vector3(1, 0, 0), M_PI)); - br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "base_link", "pioneer3at/Sick_LMS_291")); -} +void nullFuncion(const std_msgs::String::ConstPtr &name){}; -void GPSCallback(const geometry_msgs::PointStamped::ConstPtr &values) { - GPSValues[0] = values->point.x; - GPSValues[1] = values->point.y; - broadcastTransform(); -} - -void inertialUnitCallback(const sensor_msgs::Imu::ConstPtr &values) { - inertialUnitValues[0] = values->orientation.x; - inertialUnitValues[1] = values->orientation.y; - inertialUnitValues[2] = values->orientation.z; - inertialUnitValues[3] = values->orientation.w; - broadcastTransform(); -} - -void lidarCallback(const sensor_msgs::LaserScan::ConstPtr &scan) { - int scanSize = scan->ranges.size(); - lidarValues.resize(scanSize); - for (int i = 0; i < scanSize; ++i) - lidarValues[i] = scan->ranges[i]; - - lms291Resolution = scanSize; - halfResolution = scanSize / 2; - maxRange = scan->range_max; - rangeThreshold = maxRange / 20.0; - if (!areBraitenbergCoefficientsinitialized) { - braitenbergCoefficients.resize(lms291Resolution); - for (int i = 0; i < lms291Resolution; ++i) - braitenbergCoefficients[i] = gaussian(i, halfResolution, lms291Resolution / 5); - areBraitenbergCoefficientsinitialized = true; - } - - updateSpeed(); -} - -// catch names of the controllers availables on ROS network -void controllerNameCallback(const std_msgs::String::ConstPtr &name) { - controllerCount++; - controllerList.push_back(name->data); - ROS_INFO("Controller #%d: %s.", controllerCount, controllerList.back().c_str()); -} - -void quit(int sig) { - ROS_INFO("User stopped the 'pioneer3at' node."); - timeStepSrv.request.value = 0; - timeStepClient.call(timeStepSrv); - ros::shutdown(); - exit(0); -} - -int main(int argc, char **argv) { +int main(int argc, char **argv) +{ std::string controllerName; // create a node named 'pioneer3at' on ROS network ros::init(argc, argv, "pioneer3at", ros::init_options::AnonymousName); n = new ros::NodeHandle; - - signal(SIGINT, quit); - - // subscribe to the topic model_name to get the list of availables controllers - ros::Subscriber nameSub = n->subscribe("model_name", 100, controllerNameCallback); - while (controllerCount == 0 || controllerCount < nameSub.getNumPublishers()) { + ros::Subscriber nameSub = n->subscribe("model_name", 100, nullFuncion); + while (nameSub.getNumPublishers() < 1) ros::spinOnce(); - ros::spinOnce(); - ros::spinOnce(); - } - ros::spinOnce(); - timeStepClient = n->serviceClient("pioneer3at/robot/time_step"); timeStepSrv.request.value = TIME_STEP; - - // if there is more than one controller available, it let the user choose - if (controllerCount == 1) - controllerName = controllerList[0]; - else { - int wantedController = 0; - std::cout << "Choose the # of the controller you want to use:\n"; - std::cin >> wantedController; - if (1 <= wantedController && wantedController <= controllerCount) - controllerName = controllerList[wantedController - 1]; - else { - ROS_ERROR("Invalid number for controller choice."); - return 1; - } - } - ROS_INFO("Using controller: '%s'", controllerName.c_str()); - // leave topic once it is not necessary anymore - nameSub.shutdown(); - - // init motors - for (int i = 0; i < NMOTORS; ++i) { - // position - ros::ServiceClient set_position_client; - webots_ros::set_float set_position_srv; - set_position_client = n->serviceClient(std::string("pioneer3at/") + std::string(motorNames[i]) + - std::string("/set_position")); - - set_position_srv.request.value = INFINITY; - if (set_position_client.call(set_position_srv) && set_position_srv.response.success) - ROS_INFO("Position set to INFINITY for motor %s.", motorNames[i]); - else - ROS_ERROR("Failed to call service set_position on motor %s.", motorNames[i]); - - // speed - ros::ServiceClient set_velocity_client; - webots_ros::set_float set_velocity_srv; - set_velocity_client = n->serviceClient(std::string("pioneer3at/") + std::string(motorNames[i]) + - std::string("/set_velocity")); - - set_velocity_srv.request.value = 0.0; - if (set_velocity_client.call(set_velocity_srv) && set_velocity_srv.response.success == 1) - ROS_INFO("Velocity set to 0.0 for motor %s.", motorNames[i]); - else - ROS_ERROR("Failed to call service set_velocity on motor %s.", motorNames[i]); - } - - // enable lidar - ros::ServiceClient set_lidar_client; - webots_ros::set_int lidar_srv; - ros::Subscriber sub_lidar_scan; - - set_lidar_client = n->serviceClient("pioneer3at/Sick_LMS_291/enable"); - lidar_srv.request.value = TIME_STEP; - if (set_lidar_client.call(lidar_srv) && lidar_srv.response.success) { - ROS_INFO("Lidar enabled."); - sub_lidar_scan = n->subscribe("pioneer3at/Sick_LMS_291/laser_scan/layer0", 10, lidarCallback); - ROS_INFO("Topic for lidar initialized."); - while (sub_lidar_scan.getNumPublishers() == 0) { - } - ROS_INFO("Topic for lidar scan connected."); - } else { - if (!lidar_srv.response.success) - ROS_ERROR("Sampling period is not valid."); - ROS_ERROR("Failed to enable lidar."); - return 1; - } - - // enable gps - ros::ServiceClient set_GPS_client; - webots_ros::set_int GPS_srv; - ros::Subscriber sub_GPS; - set_GPS_client = n->serviceClient("pioneer3at/gps/enable"); - GPS_srv.request.value = 32; - if (set_GPS_client.call(GPS_srv) && GPS_srv.response.success) { - sub_GPS = n->subscribe("pioneer3at/gps/values", 1, GPSCallback); - while (sub_GPS.getNumPublishers() == 0) { - } - ROS_INFO("GPS enabled."); - } else { - if (!GPS_srv.response.success) - ROS_ERROR("Sampling period is not valid."); - ROS_ERROR("Failed to enable GPS."); - return 1; - } - - // enable inertial unit - ros::ServiceClient set_inertial_unit_client; - webots_ros::set_int inertial_unit_srv; - ros::Subscriber sub_inertial_unit; - set_inertial_unit_client = n->serviceClient("pioneer3at/inertial_unit/enable"); - inertial_unit_srv.request.value = 32; - if (set_inertial_unit_client.call(inertial_unit_srv) && inertial_unit_srv.response.success) { - sub_inertial_unit = n->subscribe("pioneer3at/inertial_unit/quaternion", 1, inertialUnitCallback); - while (sub_inertial_unit.getNumPublishers() == 0) { - } - ROS_INFO("Inertial unit enabled."); - } else { - if (!inertial_unit_srv.response.success) - ROS_ERROR("Sampling period is not valid."); - ROS_ERROR("Failed to enable inertial unit."); - return 1; - } - - // enable accelerometer - ros::ServiceClient set_accelerometer_client; - webots_ros::set_int accelerometer_srv; - ros::Subscriber sub_accelerometer; - set_accelerometer_client = n->serviceClient("pioneer3at/accelerometer/enable"); - accelerometer_srv.request.value = 32; - set_accelerometer_client.call(accelerometer_srv); - // enable camera - ros::ServiceClient set_camera_client; - webots_ros::set_int camera_srv; - ros::Subscriber sub_camera; - set_camera_client = n->serviceClient("pioneer3at/camera/enable"); - camera_srv.request.value = 64; - set_camera_client.call(camera_srv); - // enable gyro - ros::ServiceClient set_gyro_client; - webots_ros::set_int gyro_srv; - ros::Subscriber sub_gyro; - set_gyro_client = n->serviceClient("pioneer3at/gyro/enable"); - gyro_srv.request.value = 32; - set_gyro_client.call(gyro_srv); - ROS_INFO("You can now start the creation of the map using 'rosrun gmapping slam_gmapping " "scan:=/pioneer3at/Sick_LMS_291/laser_scan/layer0 _xmax:=30 _xmin:=-30 _ymax:=30 _ymin:=-30 _delta:=0.2'."); ROS_INFO("You can now visualize the sensors output in rqt using 'rqt'."); // main loop - while (ros::ok()) { - if (!timeStepClient.call(timeStepSrv) || !timeStepSrv.response.success) { + while (ros::ok()) + { + if (!timeStepClient.call(timeStepSrv) || !timeStepSrv.response.success) + { ROS_ERROR("Failed to call service time_step for next step."); break; } diff --git a/worlds/pioneer3at.wbt b/worlds/pioneer3at.wbt index 8ca1183..85f4b12 100644 --- a/worlds/pioneer3at.wbt +++ b/worlds/pioneer3at.wbt @@ -717,6 +717,7 @@ DEF PIONEER_3AT Pioneer3at { controller "ros" controllerArgs [ "--name=pioneer3at" + "--use-ros-control" ] extensionSlot [ Camera { From d7e907341faeafb50d4344d8ebfcd9bdfde9b593 Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Fri, 5 Mar 2021 18:37:26 +0100 Subject: [PATCH 16/72] Add diff drive --- launch/pioneer3at.launch | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/launch/pioneer3at.launch b/launch/pioneer3at.launch index 27001fa..99b4f16 100644 --- a/launch/pioneer3at.launch +++ b/launch/pioneer3at.launch @@ -3,7 +3,13 @@ joint_state_controller: type: joint_state_controller/JointStateController - publish_rate: 100 + publish_rate: 100 + diff_drive_controller: + type: "diff_drive_controller/DiffDriveController" + left_wheel: 'front left wheel' + right_wheel: 'front right wheel' + pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] @@ -17,5 +23,5 @@ - + From 543df04d735aacc62d3355f815c7f68370d08c0b Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Fri, 5 Mar 2021 22:52:55 +0100 Subject: [PATCH 17/72] Add diff drive --- launch/pioneer3at.launch | 11 ++++------- package.xml | 2 ++ 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/launch/pioneer3at.launch b/launch/pioneer3at.launch index 99b4f16..2a544e6 100644 --- a/launch/pioneer3at.launch +++ b/launch/pioneer3at.launch @@ -1,13 +1,10 @@ - joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 100 - diff_drive_controller: + pioneer_diff_drive_controller: type: "diff_drive_controller/DiffDriveController" - left_wheel: 'front left wheel' - right_wheel: 'front right wheel' + left_wheel: ['front left wheel', 'back left wheel'] + right_wheel: ['front right wheel', 'back right wheel'] pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] @@ -23,5 +20,5 @@ - + diff --git a/package.xml b/package.xml index f77ed40..d67b59d 100644 --- a/package.xml +++ b/package.xml @@ -24,5 +24,7 @@ sensor_msgs message_runtime tf + ros_control + ros_controllers From d42daa22095f9f78935b2c850fa297a94f2cf74b Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Fri, 5 Mar 2021 23:08:21 +0100 Subject: [PATCH 18/72] Add robot state publisher --- launch/pioneer3at.launch | 2 ++ package.xml | 1 + 2 files changed, 3 insertions(+) diff --git a/launch/pioneer3at.launch b/launch/pioneer3at.launch index 2a544e6..56be2b1 100644 --- a/launch/pioneer3at.launch +++ b/launch/pioneer3at.launch @@ -17,6 +17,8 @@ + + diff --git a/package.xml b/package.xml index d67b59d..57c24d8 100644 --- a/package.xml +++ b/package.xml @@ -26,5 +26,6 @@ tf ros_control ros_controllers + robot_state_publisher From 60fcb523c2771fed2e7b400c4e7463b38dcf1a5d Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Sun, 7 Mar 2021 02:24:16 +0100 Subject: [PATCH 19/72] Pioneer NUE to NUE --- protos/Pioneer3at_ENU.proto | 690 ++++++++++++++++++++++++++++++++++++ worlds/pioneer3at.wbt | 5 +- 2 files changed, 693 insertions(+), 2 deletions(-) create mode 100644 protos/Pioneer3at_ENU.proto diff --git a/protos/Pioneer3at_ENU.proto b/protos/Pioneer3at_ENU.proto new file mode 100644 index 0000000..c4c020e --- /dev/null +++ b/protos/Pioneer3at_ENU.proto @@ -0,0 +1,690 @@ +#VRML_SIM R2021b utf8 +# license: Copyright Cyberbotics Ltd. Licensed for use only with Webots. +# license url: https://cyberbotics.com/webots_assets_license +# documentation url: https://www.cyberbotics.com/doc/guide/pioneer-3at +# Pioneer 3AT robot (Adept MobileRobots) is an all terrain four wheels drive robot. + +PROTO Pioneer3at_ENU [ + field SFVec3f translation 0 0.11 0 # Is `Transform.translation`. + field SFRotation rotation 0 1 0 0 # Is `Transform.rotation`. + field SFString name "Pioneer 3-AT" # Is `Solid.name`. + field SFString controller "void" # Is `Robot.controller`. + field MFString controllerArgs [] # Is `Robot.controllerArgs`. + field SFString customData "" # Is `Robot.customData`. + field SFBool supervisor FALSE # Is `Robot.supervisor`. + field SFBool synchronization TRUE # Is `Robot.synchronization`. + field MFNode extensionSlot [] # Extends the robot with new nodes in the extension slot. +] +{ + Robot { + translation IS translation + rotation IS rotation + children [ + Transform { + rotation -0.5774 0.5773 0.5774 -2.094 + children [ + DEF EXTENSION_SLOT Group { + children IS extensionSlot + } + DEF BODY_SHAPE Shape { + appearance PBRAppearance { + baseColor 1 0 0 + metalness 0 + baseColorMap ImageTexture { + url [ + "https://raw.githubusercontent.com/cyberbotics/webots/R2021a/projects/robots/adept/pioneer3/protos/textures/body_3at_base_color.jpg" + ] + } + roughnessMap ImageTexture { + url [ + "https://raw.githubusercontent.com/cyberbotics/webots/R2021a/projects/robots/adept/pioneer3/protos/textures/body_3at_roughness.jpg" + ] + } + normalMap ImageTexture { + url [ + "https://raw.githubusercontent.com/cyberbotics/webots/R2021a/projects/robots/adept/pioneer3/protos/textures/body_3at_normal.jpg" + ] + } + occlusionMap ImageTexture { + url [ + "https://raw.githubusercontent.com/cyberbotics/webots/R2021a/projects/robots/adept/pioneer3/protos/textures/body_3at_occlusion.jpg" + ] + } + } + geometry IndexedFaceSet { + coord Coordinate { + point [ + -0.132983 0.163081 0.156114, -0.0989178 0.163081 0.19185 0.0988502 0.163081 0.191849, 0.132915 0.163081 0.154961 0.132914 0.163081 -0.175904, -0.0989194 0.163081 -0.211218 -0.132984 0.163081 -0.175904, -0.132984 -0.0372167 -0.175904 -0.0989194 -0.0372167 -0.211218, 0.0988485 -0.0372167 -0.211218 0.132914 -0.0372167 -0.175904, 0.132915 -0.0372167 0.154961 0.0988502 -0.0372167 0.191849, -0.0989178 -0.0372166 0.19185 -0.132983 -0.0372166 0.156114, 0.0989815 0.162482 -0.211218 + ] + } + texCoord TextureCoordinate { + point [ + 0.489438 0.25385, 0.489438 0.259714, 0.488368 0.260724 0.489438 0.25385, 0.488368 0.260724, 0.488334 0.252839 0.477379 0.259714, 0.477379 0.25385, 0.478436 0.260724 0.477379 0.25385, 0.478436 0.252839, 0.478436 0.260724 0.478436 0.252839, 0.488334 0.252839, 0.488368 0.260724 0.478436 0.252839, 0.488368 0.260724, 0.478436 0.260724 0.474716 0.253418, 0.476081 0.253418, 0.476081 0.258979 0.474716 0.253418, 0.476081 0.258979, 0.474716 0.258979 0.467886 0.253418, 0.469274 0.253418, 0.469274 0.258979 0.467886 0.253418, 0.469274 0.258979, 0.467886 0.258979 0.28126 0.918852, 0.15407 0.918852, 0.154149 0.2931 0.28126 0.918852, 0.154149 0.2931, 0.28155 0.2931 0.8484 0.918852, 0.721 0.918852, 0.848385 0.2931 0.721 0.918852, 0.720985 0.2931, 0.848385 0.2931 0.721 0.918852, 0.28126 0.918852, 0.720985 0.2931 0.28126 0.918852, 0.28155 0.2931, 0.720985 0.2931 0.469274 0.253418, 0.474716 0.253418, 0.474716 0.258979 0.469274 0.253418, 0.474716 0.258979, 0.469274 0.258979 0.47782 0.261384, 0.487449 0.261384, 0.487449 0.267376 0.47782 0.261384, 0.487449 0.267376, 0.47782 0.267376 0.467362 0.262667, 0.477042 0.262667, 0.467362 0.26707 0.477042 0.262667, 0.477042 0.26707, 0.467362 0.26707 + ] + } + coordIndex [ + 12 13 14 -1 12 14 11 -1 8 9 7 -1 9 10 7 -1 10 11 14 -1 10 14 7 -1 1 0 14 -1 1 14 13 -1 3 2 12 -1 3 12 11 -1 15 4 10 -1 15 10 9 -1 6 5 7 -1 5 8 7 -1 5 15 8 -1 15 9 8 -1 2 1 13 -1 2 13 12 -1 4 3 11 -1 4 11 10 -1 0 6 14 -1 6 7 14 -1 + ] + texCoordIndex [ + 0 1 2 -1 3 4 5 -1 6 7 8 -1 9 10 11 -1 12 13 14 -1 15 16 17 -1 18 19 20 -1 21 22 23 -1 24 25 26 -1 27 28 29 -1 30 31 32 -1 33 34 35 -1 36 37 38 -1 39 40 41 -1 42 43 44 -1 45 46 47 -1 48 49 50 -1 51 52 53 -1 54 55 56 -1 57 58 59 -1 60 61 62 -1 63 64 65 -1 + ] + } + } + DEF AXLES_SHAPES Shape { + appearance PBRAppearance { + baseColor 0 0 0 + roughness 0.3 + metalness 0 + } + geometry IndexedFaceSet { + coord Coordinate { + point [ + 0.208747 0.0179386 -0.114638, 0.208747 0.00964805 -0.109099 0.208747 -0.000131354 -0.107153, 0.208747 -0.00991076 -0.109099 0.208747 -0.0182013 -0.114638, 0.208747 -0.0237409 -0.122929 0.208747 -0.0256862 -0.132708, 0.208747 -0.0237409 -0.142488 0.208747 -0.0182013 -0.150778, 0.208747 -0.00991076 -0.156318 0.208747 -0.000131353 -0.158263, 0.208747 0.00964806 -0.156318 0.208747 0.0179386 -0.150778, 0.208747 0.0234782 -0.142488 0.208747 0.0254235 -0.132708, 0.208747 0.0234782 -0.122929 -0.208177 0.0179386 -0.114638, -0.208177 0.00964802 -0.109099 -0.208177 -0.00013139 -0.107153, -0.208177 -0.0099108 -0.109099 -0.208177 -0.0182014 -0.114638, -0.208177 -0.0237409 -0.122929 -0.208177 -0.0256862 -0.132708, -0.208177 -0.0237409 -0.142488 -0.208177 -0.0182013 -0.150778, -0.208177 -0.00991071 -0.156318 -0.208177 -0.000131291 -0.158263, -0.208177 0.00964811 -0.156318 -0.208177 0.0179387 -0.150778, -0.208177 0.0234782 -0.142487 -0.208177 0.0254235 -0.132708, -0.208177 0.0234782 -0.122929 0.208747 -0.000131353 -0.132708, -0.208177 -0.000131357 -0.132708 -0.208177 -0.000131357 0.133169, 0.208747 -0.000131353 0.133169 -0.208177 0.0234782 0.142948, -0.208177 0.0254235 0.133169 -0.208177 0.0234782 0.123389, -0.208177 0.0179387 0.115099 -0.208177 0.00964811 0.109559, -0.208177 -0.000131291 0.107614 -0.208177 -0.00991071 0.109559, -0.208177 -0.0182013 0.115099 -0.208177 -0.0237409 0.123389, -0.208177 -0.0256862 0.133169 -0.208177 -0.0237409 0.142948, -0.208177 -0.0182014 0.151239 -0.208177 -0.0099108 0.156778, -0.208177 -0.00013139 0.158723 -0.208177 0.00964802 0.156778, -0.208177 0.0179386 0.151239 0.208747 0.0234782 0.142948, 0.208747 0.0254235 0.133169 0.208747 0.0234782 0.123389, 0.208747 0.0179386 0.115099 0.208747 0.00964806 0.109559, 0.208747 -0.000131353 0.107614 0.208747 -0.00991076 0.109559, 0.208747 -0.0182013 0.115099 0.208747 -0.0237409 0.123389, 0.208747 -0.0256862 0.133169 0.208747 -0.0237409 0.142948, 0.208747 -0.0182013 0.151239 0.208747 -0.00991076 0.156778, 0.208747 -0.000131354 0.158723 0.208747 0.00964805 0.156778, 0.208747 0.0179386 0.151239 + ] + } + coordIndex [ + 32 0 1 -1 33 17 16 -1 32 1 2 -1 33 18 17 -1 32 2 3 -1 33 19 18 -1 32 3 4 -1 33 20 19 -1 32 4 5 -1 33 21 20 -1 32 5 6 -1 33 22 21 -1 32 6 7 -1 33 23 22 -1 32 7 8 -1 33 24 23 -1 32 8 9 -1 33 25 24 -1 32 9 10 -1 33 26 25 -1 32 10 11 -1 33 27 26 -1 32 11 12 -1 33 28 27 -1 32 12 13 -1 33 29 28 -1 32 13 14 -1 33 30 29 -1 32 14 15 -1 33 31 30 -1 15 0 32 -1 33 16 31 -1 0 16 17 1 -1 1 17 18 2 -1 2 18 19 3 -1 3 19 20 4 -1 4 20 21 5 -1 5 21 22 6 -1 6 22 23 7 -1 7 23 24 8 -1 8 24 25 9 -1 9 25 26 10 -1 10 26 27 11 -1 11 27 28 12 -1 12 28 29 13 -1 13 29 30 14 -1 14 30 31 15 -1 16 0 15 31 -1 34 51 36 -1 52 67 35 -1 34 36 37 -1 35 53 52 -1 34 37 38 -1 35 54 53 -1 34 38 39 -1 35 55 54 -1 34 39 40 -1 35 56 55 -1 34 40 41 -1 35 57 56 -1 34 41 42 -1 35 58 57 -1 34 42 43 -1 35 59 58 -1 34 43 44 -1 35 60 59 -1 34 44 45 -1 35 61 60 -1 34 45 46 -1 35 62 61 -1 34 46 47 -1 35 63 62 -1 34 47 48 -1 35 64 63 -1 34 48 49 -1 35 65 64 -1 34 49 50 -1 35 66 65 -1 34 50 51 -1 35 67 66 -1 67 51 50 -1 67 50 66 -1 66 50 49 -1 66 49 65 -1 65 49 48 -1 65 48 64 -1 64 48 47 -1 64 47 63 -1 63 47 46 -1 63 46 62 -1 62 46 45 -1 62 45 61 -1 61 45 44 -1 61 44 60 -1 60 44 43 -1 60 43 59 -1 59 43 42 -1 59 42 58 -1 58 42 41 -1 58 41 57 -1 57 41 40 -1 57 40 56 -1 56 40 39 -1 56 39 55 -1 55 39 38 -1 55 38 54 -1 54 38 37 -1 54 37 53 -1 53 37 36 -1 53 36 52 -1 51 67 52 -1 51 52 36 -1 + ] + creaseAngle 1 + } + } + DEF BACK_LEFT_WHEEL HingeJoint { + device [ + RotationalMotor { + name "back left wheel" + maxVelocity 6.4 + maxTorque 20 + } + PositionSensor { + name "back left wheel sensor" + } + ] + jointParameters HingeJointParameters { + axis -1 0 0 + anchor -0.197 0 0.1331 + } + endPoint Solid { + translation -0.197 0 0.1331 + rotation -1 0 0 0 + children [ + DEF RIM_AND_TIRE_SHAPES Group { + children [ + DEF RIM_SHAPE Shape { + appearance PBRAppearance { + baseColor 1 0.7 0 + roughness 0.3 + metalness 0 + } + geometry IndexedFaceSet { + coord Coordinate { + point [ + 0.0106186 0 0, -0.0106186 0 0 -0.0482485 0.0116779 -0.0587088, -0.0106186 0.0021733 -0.0108281 -0.0134628 0.0021627 -0.0107747, -0.0138124 0.0086022 -0.0430499 -0.0170841 0.0089469 -0.044783, -0.0418327 0.0090817 -0.045657 -0.0438648 0.0097712 -0.0491232, -0.0445422 0.0104176 -0.0523728 -0.0448397 0.010924 -0.0549188, -0.0463033 0.0109184 -0.0548908 -0.0462741 0.0112769 -0.0566926, -0.0482485 0.0112769 -0.0566926 0.0482485 0.0116779 -0.0587088, 0.0106186 0.0021733 -0.0108281 0.0134627 0.0021627 -0.0107747, 0.0138124 0.0086022 -0.0430499 0.017084 0.0089469 -0.044783, 0.0418327 0.0090817 -0.045657 0.0438648 0.0097712 -0.0491232, 0.0445422 0.0104176 -0.0523728 0.0448397 0.010924 -0.0549188, 0.0463033 0.0109184 -0.0548908 0.0462741 0.0112769 -0.0566927, 0.0482485 0.0112769 -0.0566927 0.0482485 0.0221204 -0.0534033, 0.0462741 0.0221204 -0.0534033 0.0463033 0.0214173 -0.051706, 0.0448397 0.0214282 -0.0517324 0.0445422 0.0204348 -0.0493341, 0.0438648 0.0191669 -0.0462731 0.0418327 0.0178145 -0.043008, 0.017084 0.0175117 -0.0421771 0.0138124 0.0168355 -0.0405445, 0.0134627 0.0042232 -0.0101457 0.0106186 0.0042441 -0.0101961, 0.0482485 0.022907 -0.0553024 -0.0482485 0.0221204 -0.0534033, -0.0462741 0.0221204 -0.0534033 -0.0463033 0.0214173 -0.051706, -0.0448397 0.0214282 -0.0517324 -0.0445422 0.0204348 -0.0493341, -0.0438648 0.0191669 -0.0462731 -0.0418327 0.0178145 -0.043008, -0.0170841 0.0175117 -0.0421771 -0.0138124 0.0168355 -0.0405445, -0.0134628 0.0042232 -0.0101457 -0.0106186 0.0042441 -0.0101961, -0.0482485 0.022907 -0.0553024 -0.0482485 0.0332558 -0.0497709, -0.0106186 0.0061517 -0.0091722 -0.0134628 0.0061214 -0.0091269, -0.0138124 0.0244218 -0.036481 -0.0170841 0.0254036 -0.0379503, -0.0418327 0.0258626 -0.0387062 -0.0438648 0.0278261 -0.0416447, -0.0445422 0.0296668 -0.0443995 -0.0448397 0.031109 -0.0465579, -0.0463033 0.0310931 -0.0465341 -0.0462741 0.0321138 -0.0480617, -0.0482485 0.0321138 -0.0480617 0.0482485 0.0332558 -0.0497709, 0.0106186 0.0061517 -0.0091722 0.0134627 0.0061214 -0.0091269, 0.0138124 0.0244218 -0.036481 0.017084 0.0254036 -0.0379503, 0.0418327 0.0258626 -0.0387062 0.0438648 0.0278261 -0.0416447, 0.0445422 0.0296668 -0.0443995 0.0448397 0.031109 -0.0465579, 0.0463033 0.0310931 -0.0465342 0.0462741 0.0321138 -0.0480617, 0.0482485 0.0321138 -0.0480617 0.0482485 0.0408731 -0.0408731, 0.0462741 0.0408731 -0.0408731 0.0463033 0.039574 -0.039574, 0.0448397 0.0395942 -0.0395943 0.0445422 0.0377587 -0.0377587, 0.0438648 0.0354159 -0.0354159 0.0418327 0.0329169 -0.0329169, 0.017084 0.0323192 -0.0322651 0.0138124 0.0310697 -0.0310156, 0.0134628 0.0077843 -0.0077573 0.0106186 0.0078229 -0.0077958, 0.0482485 0.0423266 -0.0423267 -0.0482485 0.0408731 -0.0408731, -0.0462741 0.0408731 -0.0408731 -0.0463033 0.039574 -0.039574, -0.0448397 0.0395942 -0.0395942 -0.0445422 0.0377587 -0.0377587, -0.0438648 0.0354159 -0.0354159 -0.0418327 0.0329169 -0.0329169, -0.0170841 0.0323192 -0.0322651 -0.0138124 0.0310697 -0.0310156, -0.0134628 0.0077843 -0.0077573 -0.0106186 0.0078229 -0.0077958, -0.0482485 0.0423267 -0.0423266 -0.0482485 0.0497709 -0.0332558, -0.0106186 0.0091934 -0.0061198 -0.0134628 0.0091481 -0.0060896, -0.0138124 0.0365235 -0.0243582 -0.0170841 0.0379928 -0.02534, -0.0418327 0.0387062 -0.0258626 -0.0438648 0.0416447 -0.0278261, -0.0445422 0.0443995 -0.0296668 -0.0448397 0.0465579 -0.031109, -0.0463033 0.0465341 -0.0310931 -0.0462741 0.0480617 -0.0321138, -0.0482485 0.0480617 -0.0321138 0.0482485 0.0497709 -0.0332558, 0.0106186 0.0091934 -0.0061198 0.0134628 0.0091481 -0.0060896, 0.0138124 0.0365235 -0.0243582 0.017084 0.0379928 -0.02534, 0.0418327 0.0387062 -0.0258626 0.0438648 0.0416447 -0.0278261, 0.0445422 0.0443995 -0.0296668 0.0448397 0.0465579 -0.031109, 0.0463033 0.0465341 -0.0310931 0.0462741 0.0480617 -0.0321138, 0.0482485 0.0480617 -0.0321138 0.0482485 0.0534033 -0.0221204, 0.0462741 0.0534033 -0.0221204 0.0463033 0.051706 -0.0214173, 0.0448397 0.0517324 -0.0214283 0.0445422 0.0493341 -0.0204349, 0.0438648 0.0462731 -0.0191669 0.0418327 0.043008 -0.0178145, 0.017084 0.0422063 -0.017441 0.0138124 0.0405738 -0.0167648, 0.0134628 0.0101604 -0.0041879 0.0106186 0.0102107 -0.0042087, 0.0482485 0.0553024 -0.022907 -0.0482485 0.0534033 -0.0221204, -0.0462741 0.0534033 -0.0221204 -0.0463033 0.051706 -0.0214173, -0.0448397 0.0517324 -0.0214282 -0.0445422 0.0493341 -0.0204348, -0.0438648 0.0462731 -0.0191669 -0.0418327 0.043008 -0.0178145, -0.0170841 0.0422063 -0.017441 -0.0138124 0.0405738 -0.0167648, -0.0134628 0.0101604 -0.0041878 -0.0106186 0.0102107 -0.0042087, -0.0482485 0.0553024 -0.022907 -0.0482485 0.0587087 -0.0116779, -0.0106186 0.0108356 -0.0021358 -0.0134628 0.0107821 -0.0021252, -0.0138124 0.0430648 -0.0085271 -0.0170841 0.0447979 -0.0088719, -0.0418327 0.045657 -0.0090817 -0.0438648 0.0491232 -0.0097712, -0.0445422 0.0523728 -0.0104176 -0.0448397 0.0549188 -0.010924, -0.0463033 0.0548908 -0.0109184 -0.0462741 0.0566926 -0.0112769, -0.0482485 0.0566926 -0.0112769 0.0482485 0.0587087 -0.0116779, 0.0106186 0.0108356 -0.0021358 0.0134628 0.0107821 -0.0021252, 0.0138124 0.0430648 -0.0085271 0.017084 0.0447979 -0.0088719, 0.0418327 0.045657 -0.0090818 0.0438648 0.0491232 -0.0097712, 0.0445422 0.0523728 -0.0104176 0.0448397 0.0549188 -0.010924, 0.0463033 0.0548908 -0.0109185 0.0462741 0.0566926 -0.0112769, 0.0482485 0.0566926 -0.0112769 0.0482485 0.0578033 0, 0.0462741 0.0578033 0 0.0463033 0.0559661 0, 0.0448397 0.0559947 0 0.0445422 0.0533988 0, 0.0438648 0.0500856 0 0.0418327 0.0465515 0, 0.017084 0.045668 3.83e-05 0.0138124 0.0439009 3.82e-05, 0.0134628 0.0109896 1.91e-05 0.0106186 0.0110441 1.91e-05, 0.0482485 0.0598589 0 -0.0482485 0.0578033 0, -0.0462741 0.0578033 0 -0.0463033 0.0559661 0, -0.0448397 0.0559947 0 -0.0445422 0.0533988 0, -0.0438648 0.0500856 0 -0.0418327 0.0465515 0, -0.0170841 0.045668 3.83e-05 -0.0138124 0.0439009 3.83e-05, -0.0134628 0.0109896 1.91e-05 -0.0106186 0.0110441 1.91e-05, -0.0482485 0.0598589 0 -0.0482485 0.0587087 0.0116779, -0.0106186 0.0108281 0.0021734 -0.0134628 0.0107747 0.0021627, -0.0138124 0.0430499 0.0086022 -0.0170841 0.044783 0.0089469, -0.0418327 0.045657 0.0090818 -0.0438648 0.0491232 0.0097712, -0.0445422 0.0523728 0.0104176 -0.0448397 0.0549188 0.010924, -0.0463033 0.0548908 0.0109185 -0.0462741 0.0566926 0.0112769, -0.0482485 0.0566926 0.0112769 0.0482485 0.0587087 0.0116779, 0.0106186 0.0108281 0.0021734 0.0134628 0.0107747 0.0021627, 0.0138124 0.0430499 0.0086022 0.0170841 0.044783 0.0089469, 0.0418327 0.045657 0.0090817 0.0438648 0.0491232 0.0097712, 0.0445422 0.0523728 0.0104176 0.0448397 0.0549188 0.010924, 0.0463033 0.0548908 0.0109184 0.0462741 0.0566926 0.0112769, 0.0482485 0.0566926 0.0112769 0.0482485 0.0534033 0.0221204, 0.0462741 0.0534033 0.0221204 0.0463033 0.051706 0.0214173, 0.0448397 0.0517324 0.0214282 0.0445422 0.0493341 0.0204348, 0.0438648 0.046273 0.0191669 0.0418327 0.043008 0.0178145, 0.0170841 0.0421771 0.0175117 0.0138124 0.0405445 0.0168355, 0.0134628 0.0101457 0.0042232 0.0106186 0.0101961 0.0042441, 0.0482485 0.0553024 0.022907 -0.0482485 0.0534033 0.0221204, -0.0462741 0.0534033 0.0221204 -0.0463033 0.051706 0.0214173, -0.0448397 0.0517324 0.0214283 -0.0445422 0.0493341 0.0204349, -0.0438648 0.046273 0.0191669 -0.0418327 0.043008 0.0178145, -0.0170841 0.0421771 0.0175117 -0.0138124 0.0405445 0.0168355, -0.0134628 0.0101457 0.0042232 -0.0106186 0.0101961 0.0042441, -0.0482485 0.0553024 0.022907 -0.0482485 0.0497709 0.0332558, -0.0106186 0.0091722 0.0061517 -0.0134628 0.0091269 0.0061214, -0.0138124 0.036481 0.0244219 -0.0170841 0.0379503 0.0254036, -0.0418327 0.0387061 0.0258626 -0.0438648 0.0416446 0.0278261, -0.0445422 0.0443995 0.0296668 -0.0448397 0.0465579 0.031109, -0.0463033 0.0465341 0.0310931 -0.0462741 0.0480617 0.0321138, -0.0482485 0.0480617 0.0321138 0.0482486 0.0497709 0.0332558, 0.0106186 0.0091722 0.0061517 0.0134628 0.0091269 0.0061214, 0.0138124 0.036481 0.0244219 0.0170841 0.0379503 0.0254036, 0.0418327 0.0387061 0.0258626 0.0438648 0.0416446 0.0278261, 0.0445422 0.0443995 0.0296668 0.0448397 0.0465579 0.031109, 0.0463033 0.0465341 0.0310931 0.0462741 0.0480617 0.0321138, 0.0482486 0.0480617 0.0321138 0.0482486 0.0408731 0.0408731, 0.0462741 0.0408731 0.0408731 0.0463033 0.039574 0.039574, 0.0448397 0.0395942 0.0395942 0.0445422 0.0377587 0.0377587, 0.0438648 0.0354158 0.0354159 0.0418327 0.0329169 0.0329169, 0.0170841 0.0322651 0.0323192 0.0138124 0.0310156 0.0310697, 0.0134628 0.0077573 0.0077843 0.0106186 0.0077958 0.0078229, 0.0482486 0.0423266 0.0423266 -0.0482485 0.0408731 0.0408731, -0.0462741 0.0408731 0.0408731 -0.0463033 0.039574 0.039574, -0.0448397 0.0395942 0.0395943 -0.0445422 0.0377587 0.0377587, -0.0438648 0.0354158 0.0354159 -0.0418327 0.0329169 0.0329169, -0.0170841 0.0322651 0.0323192 -0.0138124 0.0310156 0.0310697, -0.0134628 0.0077573 0.0077843 -0.0106186 0.0077958 0.0078229, -0.0482485 0.0423266 0.0423267 -0.0482485 0.0332558 0.0497709, -0.0106186 0.0061198 0.0091934 -0.0134628 0.0060896 0.0091481, -0.0138124 0.0243582 0.0365235 -0.0170841 0.0253399 0.0379928, -0.0418327 0.0258626 0.0387062 -0.0438648 0.027826 0.0416447, -0.0445422 0.0296668 0.0443995 -0.0448397 0.031109 0.0465579, -0.0463033 0.0310931 0.0465341 -0.0462741 0.0321138 0.0480617, -0.0482485 0.0321138 0.0480617 0.0482486 0.0332558 0.0497709, 0.0106186 0.0061198 0.0091934 0.0134628 0.0060896 0.0091481, 0.0138124 0.0243582 0.0365235 0.0170841 0.0253399 0.0379928, 0.0418327 0.0258626 0.0387061 0.0438648 0.027826 0.0416446, 0.0445422 0.0296668 0.0443995 0.0448397 0.031109 0.0465579, 0.0463033 0.0310931 0.0465341 0.0462741 0.0321138 0.0480617, 0.0482486 0.0321138 0.0480617 0.0482486 0.0221204 0.0534033, 0.0462741 0.0221204 0.0534033 0.0463033 0.0214173 0.051706, 0.0448397 0.0214282 0.0517324 0.0445422 0.0204348 0.0493341, 0.0438648 0.0191669 0.046273 0.0418327 0.0178145 0.043008, 0.0170841 0.017441 0.0422063 0.0138124 0.0167648 0.0405738, 0.0134628 0.0041878 0.0101604 0.0106186 0.0042087 0.0102107, 0.0482486 0.022907 0.0553024 -0.0482485 0.0221204 0.0534033, -0.0462741 0.0221204 0.0534033 -0.0463033 0.0214173 0.051706, -0.0448397 0.0214282 0.0517324 -0.0445422 0.0204348 0.0493341, -0.0438648 0.0191669 0.0462731 -0.0418327 0.0178145 0.043008, -0.0170841 0.017441 0.0422063 -0.0138124 0.0167648 0.0405738, -0.0134628 0.0041878 0.0101604 -0.0106186 0.0042087 0.0102107, -0.0482485 0.022907 0.0553024 -0.0482485 0.0116779 0.0587087, -0.0106186 0.0021358 0.0108356 -0.0134628 0.0021252 0.0107821, -0.0138124 0.0085271 0.0430649 -0.0170841 0.0088719 0.0447979, -0.0418327 0.0090817 0.045657 -0.0438648 0.0097712 0.0491232, -0.0445422 0.0104176 0.0523728 -0.0448397 0.010924 0.0549188, -0.0463033 0.0109184 0.0548908 -0.0462741 0.0112769 0.0566927, -0.0482485 0.0112769 0.0566927 0.0482486 0.0116779 0.0587087, 0.0106186 0.0021358 0.0108356 0.0134628 0.0021252 0.0107821, 0.0138124 0.0085271 0.0430648 0.0170841 0.0088719 0.0447979, 0.0418327 0.0090817 0.045657 0.0438648 0.0097712 0.0491232, 0.0445422 0.0104176 0.0523728 0.0448397 0.010924 0.0549188, 0.0463034 0.0109184 0.0548907 0.0462741 0.0112769 0.0566926, 0.0482486 0.0112769 0.0566926 0.0482486 0 0.0578033, 0.0462741 0 0.0578033 0.0463034 0 0.0559661, 0.0448397 0 0.0559947 0.0445422 0 0.0533988, 0.0438648 0 0.0500856 0.0418327 0 0.0465515, 0.0170841 -3.83e-05 0.045668 0.0138124 -3.83e-05 0.0439009, 0.0134628 -1.91e-05 0.0109896 0.0106186 -1.91e-05 0.0110441, 0.0482486 0 0.0598589 -0.0482485 0 0.0578033, -0.0462741 0 0.0578033 -0.0463033 0 0.0559661, -0.0448397 0 0.0559947 -0.0445422 0 0.0533988, -0.0438648 0 0.0500856 -0.0418327 0 0.0465515, -0.0170841 -3.83e-05 0.045668 -0.0138124 -3.83e-05 0.0439009, -0.0134628 -1.91e-05 0.0109896 -0.0106186 -1.91e-05 0.0110441, -0.0482485 0 0.0598589 -0.0482485 -0.0116779 0.0587087, -0.0106186 -0.0021734 0.0108281 -0.0134628 -0.0021627 0.0107747, -0.0138124 -0.0086022 0.0430499 -0.0170841 -0.0089469 0.044783, -0.0418327 -0.0090818 0.045657 -0.0438648 -0.0097712 0.0491232, -0.0445422 -0.0104176 0.0523728 -0.0448397 -0.010924 0.0549188, -0.0463033 -0.0109185 0.0548908 -0.0462741 -0.0112769 0.0566926, -0.0482485 -0.0112769 0.0566926 0.0482486 -0.0116779 0.0587087, 0.0106186 -0.0021734 0.0108281 0.0134628 -0.0021627 0.0107747, 0.0138124 -0.0086022 0.0430499 0.0170841 -0.0089469 0.044783, 0.0418327 -0.0090818 0.045657 0.0438648 -0.0097712 0.0491232, 0.0445422 -0.0104176 0.0523728 0.0448397 -0.010924 0.0549188, 0.0463034 -0.0109185 0.0548907 0.0462741 -0.0112769 0.0566926, 0.0482486 -0.0112769 0.0566926 0.0482486 -0.0221204 0.0534033, 0.0462741 -0.0221204 0.0534033 0.0463033 -0.0214173 0.0517059, 0.0448397 -0.0214283 0.0517324 0.0445422 -0.0204349 0.0493341, 0.0438648 -0.0191669 0.046273 0.0418327 -0.0178145 0.043008, 0.0170841 -0.0175117 0.0421771 0.0138124 -0.0168355 0.0405445, 0.0134628 -0.0042232 0.0101457 0.0106186 -0.0042441 0.0101961, 0.0482486 -0.022907 0.0553024 -0.0482485 -0.0221204 0.0534033, -0.0462741 -0.0221204 0.0534033 -0.0463033 -0.0214173 0.051706, -0.0448397 -0.0214283 0.0517324 -0.0445422 -0.0204349 0.0493341, -0.0438648 -0.0191669 0.046273 -0.0418327 -0.0178145 0.043008, -0.0170841 -0.0175117 0.0421771 -0.0138124 -0.0168355 0.0405445, -0.0134628 -0.0042232 0.0101457 -0.0106186 -0.0042441 0.0101961, -0.0482485 -0.022907 0.0553024 -0.0482485 -0.0332558 0.0497709, -0.0106186 -0.0061517 0.0091722 -0.0134628 -0.0061214 0.0091269, -0.0138124 -0.0244219 0.036481 -0.0170841 -0.0254036 0.0379503, -0.0418327 -0.0258626 0.0387061 -0.0438648 -0.0278261 0.0416446, -0.0445422 -0.0296668 0.0443995 -0.0448397 -0.031109 0.0465579, -0.0463033 -0.0310931 0.0465341 -0.0462741 -0.0321138 0.0480617, -0.0482485 -0.0321138 0.0480617 0.0482486 -0.0332558 0.0497709, 0.0106186 -0.0061517 0.0091722 0.0134628 -0.0061214 0.0091269, 0.0138124 -0.0244219 0.036481 0.0170841 -0.0254036 0.0379503, 0.0418327 -0.0258626 0.0387061 0.0438648 -0.0278261 0.0416446, 0.0445422 -0.0296668 0.0443995 0.0448397 -0.031109 0.0465579, 0.0463033 -0.0310931 0.0465341 0.0462741 -0.0321138 0.0480617, 0.0482486 -0.0321138 0.0480617 0.0482486 -0.0408731 0.0408731, 0.0462741 -0.0408731 0.0408731 0.0463033 -0.039574 0.039574, 0.0448397 -0.0395942 0.0395942 0.0445422 -0.0377587 0.0377587, 0.0438648 -0.0354159 0.0354158 0.0418327 -0.0329169 0.0329169, 0.0170841 -0.0323192 0.0322651 0.0138124 -0.0310697 0.0310156, 0.0134628 -0.0077843 0.0077573 0.0106186 -0.0078229 0.0077958, 0.0482486 -0.0423267 0.0423266 -0.0482485 -0.0408731 0.0408731, -0.0462741 -0.0408731 0.0408731 -0.0463033 -0.039574 0.039574, -0.0448397 -0.0395942 0.0395942 -0.0445422 -0.0377587 0.0377587, -0.0438648 -0.0354159 0.0354158 -0.0418327 -0.0329169 0.0329169, -0.0170841 -0.0323192 0.0322651 -0.0138124 -0.0310697 0.0310156, -0.0134628 -0.0077843 0.0077573 -0.0106186 -0.0078229 0.0077958, -0.0482485 -0.0423266 0.0423266 -0.0482485 -0.0497709 0.0332558, -0.0106186 -0.0091934 0.0061198 -0.0134628 -0.0091481 0.0060896, -0.0138124 -0.0365235 0.0243582 -0.0170841 -0.0379928 0.02534, -0.0418327 -0.0387061 0.0258626 -0.0438648 -0.0416446 0.027826, -0.0445422 -0.0443995 0.0296668 -0.0448397 -0.0465579 0.031109, -0.0463033 -0.0465341 0.0310931 -0.0462741 -0.0480617 0.0321138, -0.0482485 -0.0480617 0.0321138 0.0482486 -0.0497709 0.0332558, 0.0106186 -0.0091934 0.0061198 0.0134628 -0.0091481 0.0060896, 0.0138124 -0.0365235 0.0243582 0.0170841 -0.0379928 0.0253399, 0.0418327 -0.0387061 0.0258626 0.0438648 -0.0416446 0.027826, 0.0445422 -0.0443995 0.0296668 0.0448397 -0.0465579 0.031109, 0.0463033 -0.0465341 0.0310931 0.0462741 -0.0480617 0.0321138, 0.0482486 -0.0480617 0.0321138 0.0482485 -0.0534033 0.0221204, 0.0462741 -0.0534033 0.0221204 0.0463033 -0.051706 0.0214173, 0.0448397 -0.0517324 0.0214282 0.0445422 -0.0493341 0.0204348, 0.0438648 -0.046273 0.0191669 0.0418327 -0.043008 0.0178145, 0.0170841 -0.0422063 0.017441 0.0138124 -0.0405738 0.0167648, 0.0134628 -0.0101604 0.0041878 0.0106186 -0.0102107 0.0042087, 0.0482485 -0.0553024 0.022907 -0.0482485 -0.0534033 0.0221204, -0.0462741 -0.0534033 0.0221204 -0.0463033 -0.051706 0.0214173, -0.0448397 -0.0517324 0.0214282 -0.0445422 -0.0493341 0.0204349, -0.0438648 -0.046273 0.0191669 -0.0418327 -0.043008 0.0178145, -0.0170841 -0.0422063 0.017441 -0.0138124 -0.0405738 0.0167648, -0.0134628 -0.0101604 0.0041878 -0.0106186 -0.0102107 0.0042087, -0.0482485 -0.0553024 0.022907 -0.0482485 -0.0587087 0.0116779, -0.0106186 -0.0108356 0.0021358 -0.0134628 -0.0107821 0.0021252, -0.0138124 -0.0430649 0.0085271 -0.0170841 -0.0447979 0.0088719, -0.0418327 -0.045657 0.0090817 -0.0438648 -0.0491232 0.0097712, -0.0445422 -0.0523728 0.0104176 -0.0448397 -0.0549188 0.010924, -0.0463033 -0.0548908 0.0109184 -0.0462741 -0.0566926 0.0112769, -0.0482485 -0.0566926 0.0112769 0.0482485 -0.0587087 0.0116779, 0.0106186 -0.0108356 0.0021358 0.0134628 -0.0107821 0.0021252, 0.0138124 -0.0430649 0.0085271 0.0170841 -0.0447979 0.0088719, 0.0418327 -0.045657 0.0090817 0.0438648 -0.0491232 0.0097712, 0.0445422 -0.0523728 0.0104176 0.0448397 -0.0549188 0.010924, 0.0463033 -0.0548908 0.0109184 0.0462741 -0.0566926 0.0112768, 0.0482485 -0.0566926 0.0112769 0.0482485 -0.0578033 0, 0.0462741 -0.0578033 0 0.0463033 -0.0559661 0, 0.0448397 -0.0559947 0 0.0445422 -0.0533988 0, 0.0438648 -0.0500856 0 0.0418327 -0.0465515 0, 0.017084 -0.045668 -3.83e-05 0.0138124 -0.0439009 -3.83e-05, 0.0134628 -0.0109896 -1.91e-05 0.0106186 -0.0110441 -1.91e-05, 0.0482485 -0.0598589 0 -0.0482485 -0.0578033 0, -0.0462741 -0.0578033 0 -0.0463033 -0.0559661 0, -0.0448397 -0.0559947 0 -0.0445422 -0.0533988 0, -0.0438648 -0.0500856 0 -0.0418327 -0.0465515 0, -0.0170841 -0.045668 -3.83e-05 -0.0138124 -0.0439009 -3.83e-05, -0.0134628 -0.0109896 -1.91e-05 -0.0106186 -0.0110441 -1.91e-05, -0.0482485 -0.0598589 0 -0.0482485 -0.0587087 -0.0116779, -0.0106186 -0.0108281 -0.0021734 -0.0134628 -0.0107747 -0.0021627, -0.0138124 -0.0430499 -0.0086022 -0.0170841 -0.044783 -0.0089469, -0.0418327 -0.045657 -0.0090817 -0.0438648 -0.0491232 -0.0097712, -0.0445422 -0.0523728 -0.0104176 -0.0448397 -0.0549188 -0.010924, -0.0463033 -0.0548907 -0.0109184 -0.0462741 -0.0566926 -0.0112769, -0.0482485 -0.0566926 -0.0112769 0.0482485 -0.0587087 -0.0116779, 0.0106186 -0.0108281 -0.0021734 0.0134628 -0.0107747 -0.0021627, 0.0138124 -0.0430499 -0.0086022 0.017084 -0.044783 -0.0089469, 0.0418327 -0.045657 -0.0090818 0.0438648 -0.0491232 -0.0097712, 0.0445422 -0.0523728 -0.0104176 0.0448397 -0.0549188 -0.010924, 0.0463033 -0.0548907 -0.0109185 0.0462741 -0.0566926 -0.0112769, 0.0482485 -0.0566926 -0.0112769 0.0482485 -0.0534033 -0.0221204, 0.0462741 -0.0534033 -0.0221204 0.0463033 -0.0517059 -0.0214173, 0.0448397 -0.0517324 -0.0214283 0.0445422 -0.0493341 -0.0204349, 0.0438648 -0.046273 -0.0191669 0.0418327 -0.043008 -0.0178145, 0.017084 -0.0421771 -0.0175117 0.0138124 -0.0405445 -0.0168355, 0.0134628 -0.0101457 -0.0042232 0.0106186 -0.0101961 -0.0042441, 0.0482485 -0.0553024 -0.022907 -0.0482485 -0.0534033 -0.0221204, -0.0462741 -0.0534033 -0.0221204 -0.0463033 -0.0517059 -0.0214173, -0.0448397 -0.0517324 -0.0214282 -0.0445422 -0.0493341 -0.0204348, -0.0438648 -0.046273 -0.0191669 -0.0418327 -0.043008 -0.0178145, -0.0170841 -0.0421771 -0.0175117 -0.0138124 -0.0405445 -0.0168355, -0.0134628 -0.0101457 -0.0042232 -0.0106186 -0.0101961 -0.0042441, -0.0482485 -0.0553024 -0.022907 -0.0482485 -0.0497709 -0.0332558, -0.0106186 -0.0091722 -0.0061517 -0.0134628 -0.0091269 -0.0061214, -0.0138124 -0.036481 -0.0244219 -0.0170841 -0.0379503 -0.0254036, -0.0418327 -0.0387061 -0.0258626 -0.0438648 -0.0416446 -0.0278261, -0.0445422 -0.0443995 -0.0296668 -0.0448397 -0.0465579 -0.031109, -0.0463033 -0.0465341 -0.0310931 -0.0462741 -0.0480617 -0.0321138, -0.0482485 -0.0480617 -0.0321138 0.0482485 -0.0497709 -0.0332558, 0.0106186 -0.0091722 -0.0061517 0.0134628 -0.0091269 -0.0061214, 0.0138124 -0.036481 -0.0244219 0.017084 -0.0379503 -0.0254036, 0.0418327 -0.0387061 -0.0258626 0.0438648 -0.0416446 -0.0278261, 0.0445422 -0.0443995 -0.0296668 0.0448397 -0.0465579 -0.031109, 0.0463033 -0.0465341 -0.0310931 0.0462741 -0.0480617 -0.0321138, 0.0482485 -0.0480617 -0.0321138 0.0482485 -0.0408731 -0.0408731, 0.0462741 -0.0408731 -0.0408731 0.0463033 -0.039574 -0.039574, 0.0448397 -0.0395942 -0.0395942 0.0445422 -0.0377587 -0.0377587, 0.0438648 -0.0354158 -0.0354159 0.0418327 -0.0329169 -0.0329169, 0.017084 -0.0322651 -0.0323192 0.0138124 -0.0310156 -0.0310697, 0.0134628 -0.0077573 -0.0077843 0.0106186 -0.0077958 -0.0078229, 0.0482485 -0.0423266 -0.0423267 -0.0482485 -0.0408731 -0.0408731, -0.0462741 -0.0408731 -0.0408731 -0.0463033 -0.039574 -0.039574, -0.0448397 -0.0395942 -0.0395942 -0.0445422 -0.0377587 -0.0377587, -0.0438648 -0.0354158 -0.0354158 -0.0418327 -0.0329169 -0.0329169, -0.0170841 -0.0322651 -0.0323192 -0.0138124 -0.0310156 -0.0310697, -0.0134628 -0.0077573 -0.0077843 -0.0106186 -0.0077958 -0.0078229, -0.0482485 -0.0423266 -0.0423266 -0.0482485 -0.0332558 -0.0497709, -0.0106186 -0.0061198 -0.0091934 -0.0134628 -0.0060896 -0.0091481, -0.0138124 -0.0243583 -0.0365235 -0.0170841 -0.0253399 -0.0379928, -0.0418327 -0.0258626 -0.0387061 -0.0438648 -0.027826 -0.0416446, -0.0445422 -0.0296668 -0.0443995 -0.0448397 -0.031109 -0.0465579, -0.0463033 -0.0310931 -0.0465341 -0.0462741 -0.0321138 -0.0480617, -0.0482485 -0.0321138 -0.0480617 0.0482485 -0.0332558 -0.0497709, 0.0106186 -0.0061198 -0.0091934 0.0134627 -0.0060896 -0.0091481, 0.0138124 -0.0243583 -0.0365235 0.017084 -0.0253399 -0.0379928, 0.0418327 -0.0258626 -0.0387061 0.0438648 -0.027826 -0.0416446, 0.0445422 -0.0296668 -0.0443995 0.0448397 -0.031109 -0.0465579, 0.0463033 -0.0310931 -0.0465341 0.0462741 -0.0321138 -0.0480617, 0.0482485 -0.0321138 -0.0480617 0.0482485 -0.0221204 -0.0534033, 0.0462741 -0.0221204 -0.0534033 0.0463033 -0.0214173 -0.0517059, 0.0448397 -0.0214282 -0.0517324 0.0445422 -0.0204348 -0.0493341, 0.0438648 -0.0191669 -0.046273 0.0418327 -0.0178145 -0.043008, 0.017084 -0.017441 -0.0422063 0.0138124 -0.0167648 -0.0405738, 0.0134627 -0.0041879 -0.0101604 0.0106186 -0.0042087 -0.0102107, 0.0482485 -0.022907 -0.0553024 -0.0482485 -0.0221204 -0.0534033, -0.0462741 -0.0221204 -0.0534033 -0.0463033 -0.0214173 -0.0517059, -0.0448397 -0.0214282 -0.0517323 -0.0445422 -0.0204348 -0.0493341, -0.0438648 -0.0191669 -0.046273 -0.0418327 -0.0178145 -0.0430079, -0.0170841 -0.017441 -0.0422063 -0.0138124 -0.0167648 -0.0405738, -0.0134628 -0.0041879 -0.0101604 -0.0106186 -0.0042087 -0.0102107, -0.0482485 -0.022907 -0.0553024 -0.0482485 -0.0116779 -0.0587087, -0.0106186 -0.0021358 -0.0108356 -0.0134628 -0.0021252 -0.0107821, -0.0138124 -0.0085271 -0.0430648 -0.0170841 -0.0088719 -0.0447979, -0.0418327 -0.0090817 -0.045657 -0.0438648 -0.0097712 -0.0491232, -0.0445422 -0.0104176 -0.0523728 -0.0448397 -0.010924 -0.0549188, -0.0463033 -0.0109184 -0.0548907 -0.0462741 -0.0112769 -0.0566926, -0.0482485 -0.0112769 -0.0566926 0.0482485 -0.0116779 -0.0587087, 0.0106186 -0.0021358 -0.0108356 0.0134627 -0.0021252 -0.0107821, 0.0138124 -0.0085271 -0.0430649 0.017084 -0.0088719 -0.0447979, 0.0418327 -0.0090817 -0.045657 0.0438648 -0.0097712 -0.0491232, 0.0445422 -0.0104176 -0.0523728 0.0448397 -0.010924 -0.0549188, 0.0463033 -0.0109184 -0.0548907 0.0462741 -0.0112769 -0.0566926, 0.0482485 -0.0112769 -0.0566926 0.0482485 0 -0.0578033, 0.0462741 0 -0.0578033 0.0463033 0 -0.0559661, 0.0448397 0 -0.0559947 0.0445422 0 -0.0533988, 0.0438648 0 -0.0500856 0.0418327 0 -0.0465515, 0.017084 3.83e-05 -0.0456679 0.0138124 3.82e-05 -0.0439009, 0.0134627 1.91e-05 -0.0109896 0.0106186 1.91e-05 -0.0110441, 0.0482485 0 -0.0598589 -0.0482485 0 -0.0578033, -0.0462741 0 -0.0578033 -0.0463033 0 -0.0559661, -0.0448397 0 -0.0559947 -0.0445422 0 -0.0533988, -0.0438648 0 -0.0500855 -0.0418327 0 -0.0465514, -0.0170841 3.83e-05 -0.0456679 -0.0138124 3.82e-05 -0.0439009, -0.0134628 1.91e-05 -0.0109896 -0.0106186 1.91e-05 -0.0110441, -0.0482485 0 -0.0598589 + ] + } + coordIndex [ + 722 757 734 -1 722 769 757 -1 722 733 758 -1 722 758 769 -1 723 767 724 -1 723 768 767 -1 724 766 725 -1 724 767 766 -1 725 765 726 -1 725 766 765 -1 726 764 727 -1 726 765 764 -1 727 763 728 -1 727 764 763 -1 728 762 729 -1 728 763 762 -1 729 761 730 -1 729 762 761 -1 730 760 731 -1 730 761 760 -1 731 759 732 -1 731 760 759 -1 732 758 733 -1 732 759 758 -1 734 746 745 -1 734 757 746 -1 735 736 755 -1 735 755 756 -1 736 737 754 -1 736 754 755 -1 737 738 753 -1 737 753 754 -1 738 739 752 -1 738 752 753 -1 739 740 751 -1 739 751 752 -1 740 741 750 -1 740 750 751 -1 741 742 749 -1 741 749 750 -1 742 743 748 -1 742 748 749 -1 743 744 747 -1 743 747 748 -1 744 745 746 -1 744 746 747 -1 699 698 745 -1 699 745 744 -1 700 699 744 -1 700 744 743 -1 701 700 743 -1 701 743 742 -1 702 701 742 -1 702 742 741 -1 703 702 741 -1 703 741 740 -1 704 703 740 -1 704 740 739 -1 705 704 739 -1 705 739 738 -1 706 705 738 -1 706 738 737 -1 707 706 737 -1 707 737 736 -1 708 707 736 -1 708 736 735 -1 709 745 698 -1 709 734 745 -1 711 733 710 -1 711 732 733 -1 712 732 711 -1 712 731 732 -1 713 731 712 -1 713 730 731 -1 714 730 713 -1 714 729 730 -1 715 729 714 -1 715 728 729 -1 716 728 715 -1 716 727 728 -1 717 727 716 -1 717 726 727 -1 718 726 717 -1 718 725 726 -1 719 725 718 -1 719 724 725 -1 720 724 719 -1 720 723 724 -1 721 710 733 -1 721 733 722 -1 721 734 709 -1 721 722 734 -1 674 709 686 -1 674 721 709 -1 674 685 710 -1 674 710 721 -1 675 719 676 -1 675 720 719 -1 676 718 677 -1 676 719 718 -1 677 717 678 -1 677 718 717 -1 678 716 679 -1 678 717 716 -1 679 715 680 -1 679 716 715 -1 680 714 681 -1 680 715 714 -1 681 713 682 -1 681 714 713 -1 682 712 683 -1 682 713 712 -1 683 711 684 -1 683 712 711 -1 684 710 685 -1 684 711 710 -1 686 698 697 -1 686 709 698 -1 687 688 707 -1 687 707 708 -1 688 689 706 -1 688 706 707 -1 689 690 705 -1 689 705 706 -1 690 691 704 -1 690 704 705 -1 691 692 703 -1 691 703 704 -1 692 693 702 -1 692 702 703 -1 693 694 701 -1 693 701 702 -1 694 695 700 -1 694 700 701 -1 695 696 699 -1 695 699 700 -1 696 697 698 -1 696 698 699 -1 651 650 697 -1 651 697 696 -1 652 651 696 -1 652 696 695 -1 653 652 695 -1 653 695 694 -1 654 653 694 -1 654 694 693 -1 655 654 693 -1 655 693 692 -1 656 655 692 -1 656 692 691 -1 657 656 691 -1 657 691 690 -1 658 657 690 -1 658 690 689 -1 659 658 689 -1 659 689 688 -1 660 659 688 -1 660 688 687 -1 661 697 650 -1 661 686 697 -1 663 685 662 -1 663 684 685 -1 664 684 663 -1 664 683 684 -1 665 683 664 -1 665 682 683 -1 666 682 665 -1 666 681 682 -1 667 681 666 -1 667 680 681 -1 668 680 667 -1 668 679 680 -1 669 679 668 -1 669 678 679 -1 670 678 669 -1 670 677 678 -1 671 677 670 -1 671 676 677 -1 672 676 671 -1 672 675 676 -1 673 662 685 -1 673 685 674 -1 673 686 661 -1 673 674 686 -1 626 661 638 -1 626 673 661 -1 626 637 662 -1 626 662 673 -1 627 671 628 -1 627 672 671 -1 628 670 629 -1 628 671 670 -1 629 669 630 -1 629 670 669 -1 630 668 631 -1 630 669 668 -1 631 667 632 -1 631 668 667 -1 632 666 633 -1 632 667 666 -1 633 665 634 -1 633 666 665 -1 634 664 635 -1 634 665 664 -1 635 663 636 -1 635 664 663 -1 636 662 637 -1 636 663 662 -1 638 650 649 -1 638 661 650 -1 639 640 659 -1 639 659 660 -1 640 641 658 -1 640 658 659 -1 641 642 657 -1 641 657 658 -1 642 643 656 -1 642 656 657 -1 643 644 655 -1 643 655 656 -1 644 645 654 -1 644 654 655 -1 645 646 653 -1 645 653 654 -1 646 647 652 -1 646 652 653 -1 647 648 651 -1 647 651 652 -1 648 649 650 -1 648 650 651 -1 603 602 649 -1 603 649 648 -1 604 603 648 -1 604 648 647 -1 605 604 647 -1 605 647 646 -1 606 605 646 -1 606 646 645 -1 607 606 645 -1 607 645 644 -1 608 607 644 -1 608 644 643 -1 609 608 643 -1 609 643 642 -1 610 609 642 -1 610 642 641 -1 611 610 641 -1 611 641 640 -1 612 611 640 -1 612 640 639 -1 613 649 602 -1 613 638 649 -1 615 637 614 -1 615 636 637 -1 616 636 615 -1 616 635 636 -1 617 635 616 -1 617 634 635 -1 618 634 617 -1 618 633 634 -1 619 633 618 -1 619 632 633 -1 620 632 619 -1 620 631 632 -1 621 631 620 -1 621 630 631 -1 622 630 621 -1 622 629 630 -1 623 629 622 -1 623 628 629 -1 624 628 623 -1 624 627 628 -1 625 614 637 -1 625 637 626 -1 625 638 613 -1 625 626 638 -1 578 613 590 -1 578 625 613 -1 578 589 614 -1 578 614 625 -1 579 623 580 -1 579 624 623 -1 580 622 581 -1 580 623 622 -1 581 621 582 -1 581 622 621 -1 582 620 583 -1 582 621 620 -1 583 619 584 -1 583 620 619 -1 584 618 585 -1 584 619 618 -1 585 617 586 -1 585 618 617 -1 586 616 587 -1 586 617 616 -1 587 615 588 -1 587 616 615 -1 588 614 589 -1 588 615 614 -1 590 602 601 -1 590 613 602 -1 591 592 611 -1 591 611 612 -1 592 593 610 -1 592 610 611 -1 593 594 609 -1 593 609 610 -1 594 595 608 -1 594 608 609 -1 595 596 607 -1 595 607 608 -1 596 597 606 -1 596 606 607 -1 597 598 605 -1 597 605 606 -1 598 599 604 -1 598 604 605 -1 599 600 603 -1 599 603 604 -1 600 601 602 -1 600 602 603 -1 555 554 601 -1 555 601 600 -1 556 555 600 -1 556 600 599 -1 557 556 599 -1 557 599 598 -1 558 557 598 -1 558 598 597 -1 559 558 597 -1 559 597 596 -1 560 559 596 -1 560 596 595 -1 561 560 595 -1 561 595 594 -1 562 561 594 -1 562 594 593 -1 563 562 593 -1 563 593 592 -1 564 563 592 -1 564 592 591 -1 565 601 554 -1 565 590 601 -1 567 589 566 -1 567 588 589 -1 568 588 567 -1 568 587 588 -1 569 587 568 -1 569 586 587 -1 570 586 569 -1 570 585 586 -1 571 585 570 -1 571 584 585 -1 572 584 571 -1 572 583 584 -1 573 583 572 -1 573 582 583 -1 574 582 573 -1 574 581 582 -1 575 581 574 -1 575 580 581 -1 576 580 575 -1 576 579 580 -1 577 566 589 -1 577 589 578 -1 577 590 565 -1 577 578 590 -1 530 565 542 -1 530 577 565 -1 530 541 566 -1 530 566 577 -1 531 575 532 -1 531 576 575 -1 532 574 533 -1 532 575 574 -1 533 573 534 -1 533 574 573 -1 534 572 535 -1 534 573 572 -1 535 571 536 -1 535 572 571 -1 536 570 537 -1 536 571 570 -1 537 569 538 -1 537 570 569 -1 538 568 539 -1 538 569 568 -1 539 567 540 -1 539 568 567 -1 540 566 541 -1 540 567 566 -1 542 554 553 -1 542 565 554 -1 543 544 563 -1 543 563 564 -1 544 545 562 -1 544 562 563 -1 545 546 561 -1 545 561 562 -1 546 547 560 -1 546 560 561 -1 547 548 559 -1 547 559 560 -1 548 549 558 -1 548 558 559 -1 549 550 557 -1 549 557 558 -1 550 551 556 -1 550 556 557 -1 551 552 555 -1 551 555 556 -1 552 553 554 -1 552 554 555 -1 507 506 553 -1 507 553 552 -1 508 507 552 -1 508 552 551 -1 509 508 551 -1 509 551 550 -1 510 509 550 -1 510 550 549 -1 511 510 549 -1 511 549 548 -1 512 511 548 -1 512 548 547 -1 513 512 547 -1 513 547 546 -1 514 513 546 -1 514 546 545 -1 515 514 545 -1 515 545 544 -1 516 515 544 -1 516 544 543 -1 517 553 506 -1 517 542 553 -1 519 541 518 -1 519 540 541 -1 520 540 519 -1 520 539 540 -1 521 539 520 -1 521 538 539 -1 522 538 521 -1 522 537 538 -1 523 537 522 -1 523 536 537 -1 524 536 523 -1 524 535 536 -1 525 535 524 -1 525 534 535 -1 526 534 525 -1 526 533 534 -1 527 533 526 -1 527 532 533 -1 528 532 527 -1 528 531 532 -1 529 518 541 -1 529 541 530 -1 529 542 517 -1 529 530 542 -1 482 517 494 -1 482 529 517 -1 482 493 518 -1 482 518 529 -1 483 527 484 -1 483 528 527 -1 484 526 485 -1 484 527 526 -1 485 525 486 -1 485 526 525 -1 486 524 487 -1 486 525 524 -1 487 523 488 -1 487 524 523 -1 488 522 489 -1 488 523 522 -1 489 521 490 -1 489 522 521 -1 490 520 491 -1 490 521 520 -1 491 519 492 -1 491 520 519 -1 492 518 493 -1 492 519 518 -1 494 506 505 -1 494 517 506 -1 495 496 515 -1 495 515 516 -1 496 497 514 -1 496 514 515 -1 497 498 513 -1 497 513 514 -1 498 499 512 -1 498 512 513 -1 499 500 511 -1 499 511 512 -1 500 501 510 -1 500 510 511 -1 501 502 509 -1 501 509 510 -1 502 503 508 -1 502 508 509 -1 503 504 507 -1 503 507 508 -1 504 505 506 -1 504 506 507 -1 459 458 505 -1 459 505 504 -1 460 459 504 -1 460 504 503 -1 461 460 503 -1 461 503 502 -1 462 461 502 -1 462 502 501 -1 463 462 501 -1 463 501 500 -1 464 463 500 -1 464 500 499 -1 465 464 499 -1 465 499 498 -1 466 465 498 -1 466 498 497 -1 467 466 497 -1 467 497 496 -1 468 467 496 -1 468 496 495 -1 469 505 458 -1 469 494 505 -1 471 493 470 -1 471 492 493 -1 472 492 471 -1 472 491 492 -1 473 491 472 -1 473 490 491 -1 474 490 473 -1 474 489 490 -1 475 489 474 -1 475 488 489 -1 476 488 475 -1 476 487 488 -1 477 487 476 -1 477 486 487 -1 478 486 477 -1 478 485 486 -1 479 485 478 -1 479 484 485 -1 480 484 479 -1 480 483 484 -1 481 470 493 -1 481 493 482 -1 481 494 469 -1 481 482 494 -1 434 469 446 -1 434 481 469 -1 434 445 470 -1 434 470 481 -1 435 479 436 -1 435 480 479 -1 436 478 437 -1 436 479 478 -1 437 477 438 -1 437 478 477 -1 438 476 439 -1 438 477 476 -1 439 475 440 -1 439 476 475 -1 440 474 441 -1 440 475 474 -1 441 473 442 -1 441 474 473 -1 442 472 443 -1 442 473 472 -1 443 471 444 -1 443 472 471 -1 444 470 445 -1 444 471 470 -1 446 458 457 -1 446 469 458 -1 447 448 467 -1 447 467 468 -1 448 449 466 -1 448 466 467 -1 449 450 465 -1 449 465 466 -1 450 451 464 -1 450 464 465 -1 451 452 463 -1 451 463 464 -1 452 453 462 -1 452 462 463 -1 453 454 461 -1 453 461 462 -1 454 455 460 -1 454 460 461 -1 455 456 459 -1 455 459 460 -1 456 457 458 -1 456 458 459 -1 411 410 457 -1 411 457 456 -1 412 411 456 -1 412 456 455 -1 413 412 455 -1 413 455 454 -1 414 413 454 -1 414 454 453 -1 415 414 453 -1 415 453 452 -1 416 415 452 -1 416 452 451 -1 417 416 451 -1 417 451 450 -1 418 417 450 -1 418 450 449 -1 419 418 449 -1 419 449 448 -1 420 419 448 -1 420 448 447 -1 421 457 410 -1 421 446 457 -1 423 445 422 -1 423 444 445 -1 424 444 423 -1 424 443 444 -1 425 443 424 -1 425 442 443 -1 426 442 425 -1 426 441 442 -1 427 441 426 -1 427 440 441 -1 428 440 427 -1 428 439 440 -1 429 439 428 -1 429 438 439 -1 430 438 429 -1 430 437 438 -1 431 437 430 -1 431 436 437 -1 432 436 431 -1 432 435 436 -1 433 422 445 -1 433 445 434 -1 433 446 421 -1 433 434 446 -1 386 421 398 -1 386 433 421 -1 386 397 422 -1 386 422 433 -1 387 431 388 -1 387 432 431 -1 388 430 389 -1 388 431 430 -1 389 429 390 -1 389 430 429 -1 390 428 391 -1 390 429 428 -1 391 427 392 -1 391 428 427 -1 392 426 393 -1 392 427 426 -1 393 425 394 -1 393 426 425 -1 394 424 395 -1 394 425 424 -1 395 423 396 -1 395 424 423 -1 396 422 397 -1 396 423 422 -1 398 410 409 -1 398 421 410 -1 399 400 419 -1 399 419 420 -1 400 401 418 -1 400 418 419 -1 401 402 417 -1 401 417 418 -1 402 403 416 -1 402 416 417 -1 403 404 415 -1 403 415 416 -1 404 405 414 -1 404 414 415 -1 405 406 413 -1 405 413 414 -1 406 407 412 -1 406 412 413 -1 407 408 411 -1 407 411 412 -1 408 409 410 -1 408 410 411 -1 363 362 409 -1 363 409 408 -1 364 363 408 -1 364 408 407 -1 365 364 407 -1 365 407 406 -1 366 365 406 -1 366 406 405 -1 367 366 405 -1 367 405 404 -1 368 367 404 -1 368 404 403 -1 369 368 403 -1 369 403 402 -1 370 369 402 -1 370 402 401 -1 371 370 401 -1 371 401 400 -1 372 371 400 -1 372 400 399 -1 373 409 362 -1 373 398 409 -1 375 397 374 -1 375 396 397 -1 376 396 375 -1 376 395 396 -1 377 395 376 -1 377 394 395 -1 378 394 377 -1 378 393 394 -1 379 393 378 -1 379 392 393 -1 380 392 379 -1 380 391 392 -1 381 391 380 -1 381 390 391 -1 382 390 381 -1 382 389 390 -1 383 389 382 -1 383 388 389 -1 384 388 383 -1 384 387 388 -1 385 374 397 -1 385 397 386 -1 385 398 373 -1 385 386 398 -1 338 373 350 -1 338 385 373 -1 338 349 374 -1 338 374 385 -1 339 383 340 -1 339 384 383 -1 340 382 341 -1 340 383 382 -1 341 381 342 -1 341 382 381 -1 342 380 343 -1 342 381 380 -1 343 379 344 -1 343 380 379 -1 344 378 345 -1 344 379 378 -1 345 377 346 -1 345 378 377 -1 346 376 347 -1 346 377 376 -1 347 375 348 -1 347 376 375 -1 348 374 349 -1 348 375 374 -1 350 362 361 -1 350 373 362 -1 351 352 371 -1 351 371 372 -1 352 353 370 -1 352 370 371 -1 353 354 369 -1 353 369 370 -1 354 355 368 -1 354 368 369 -1 355 356 367 -1 355 367 368 -1 356 357 366 -1 356 366 367 -1 357 358 365 -1 357 365 366 -1 358 359 364 -1 358 364 365 -1 359 360 363 -1 359 363 364 -1 360 361 362 -1 360 362 363 -1 315 314 361 -1 315 361 360 -1 316 315 360 -1 316 360 359 -1 317 316 359 -1 317 359 358 -1 318 317 358 -1 318 358 357 -1 319 318 357 -1 319 357 356 -1 320 319 356 -1 320 356 355 -1 321 320 355 -1 321 355 354 -1 322 321 354 -1 322 354 353 -1 323 322 353 -1 323 353 352 -1 324 323 352 -1 324 352 351 -1 325 361 314 -1 325 350 361 -1 327 349 326 -1 327 348 349 -1 328 348 327 -1 328 347 348 -1 329 347 328 -1 329 346 347 -1 330 346 329 -1 330 345 346 -1 331 345 330 -1 331 344 345 -1 332 344 331 -1 332 343 344 -1 333 343 332 -1 333 342 343 -1 334 342 333 -1 334 341 342 -1 335 341 334 -1 335 340 341 -1 336 340 335 -1 336 339 340 -1 337 326 349 -1 337 349 338 -1 337 350 325 -1 337 338 350 -1 290 325 302 -1 290 337 325 -1 290 301 326 -1 290 326 337 -1 291 335 292 -1 291 336 335 -1 292 334 293 -1 292 335 334 -1 293 333 294 -1 293 334 333 -1 294 332 295 -1 294 333 332 -1 295 331 296 -1 295 332 331 -1 296 330 297 -1 296 331 330 -1 297 329 298 -1 297 330 329 -1 298 328 299 -1 298 329 328 -1 299 327 300 -1 299 328 327 -1 300 326 301 -1 300 327 326 -1 302 314 313 -1 302 325 314 -1 303 304 323 -1 303 323 324 -1 304 305 322 -1 304 322 323 -1 305 306 321 -1 305 321 322 -1 306 307 320 -1 306 320 321 -1 307 308 319 -1 307 319 320 -1 308 309 318 -1 308 318 319 -1 309 310 317 -1 309 317 318 -1 310 311 316 -1 310 316 317 -1 311 312 315 -1 311 315 316 -1 312 313 314 -1 312 314 315 -1 267 266 313 -1 267 313 312 -1 268 267 312 -1 268 312 311 -1 269 268 311 -1 269 311 310 -1 270 269 310 -1 270 310 309 -1 271 270 309 -1 271 309 308 -1 272 271 308 -1 272 308 307 -1 273 272 307 -1 273 307 306 -1 274 273 306 -1 274 306 305 -1 275 274 305 -1 275 305 304 -1 276 275 304 -1 276 304 303 -1 277 313 266 -1 277 302 313 -1 279 301 278 -1 279 300 301 -1 280 300 279 -1 280 299 300 -1 281 299 280 -1 281 298 299 -1 282 298 281 -1 282 297 298 -1 283 297 282 -1 283 296 297 -1 284 296 283 -1 284 295 296 -1 285 295 284 -1 285 294 295 -1 286 294 285 -1 286 293 294 -1 287 293 286 -1 287 292 293 -1 288 292 287 -1 288 291 292 -1 289 278 301 -1 289 301 290 -1 289 302 277 -1 289 290 302 -1 242 277 254 -1 242 289 277 -1 242 253 278 -1 242 278 289 -1 243 287 244 -1 243 288 287 -1 244 286 245 -1 244 287 286 -1 245 285 246 -1 245 286 285 -1 246 284 247 -1 246 285 284 -1 247 283 248 -1 247 284 283 -1 248 282 249 -1 248 283 282 -1 249 281 250 -1 249 282 281 -1 250 280 251 -1 250 281 280 -1 251 279 252 -1 251 280 279 -1 252 278 253 -1 252 279 278 -1 254 266 265 -1 254 277 266 -1 255 256 275 -1 255 275 276 -1 256 257 274 -1 256 274 275 -1 257 258 273 -1 257 273 274 -1 258 259 272 -1 258 272 273 -1 259 260 271 -1 259 271 272 -1 260 261 270 -1 260 270 271 -1 261 262 269 -1 261 269 270 -1 262 263 268 -1 262 268 269 -1 263 264 267 -1 263 267 268 -1 264 265 266 -1 264 266 267 -1 219 218 265 -1 219 265 264 -1 220 219 264 -1 220 264 263 -1 221 220 263 -1 221 263 262 -1 222 221 262 -1 222 262 261 -1 223 222 261 -1 223 261 260 -1 224 223 260 -1 224 260 259 -1 225 224 259 -1 225 259 258 -1 226 225 258 -1 226 258 257 -1 227 226 257 -1 227 257 256 -1 228 227 256 -1 228 256 255 -1 229 265 218 -1 229 254 265 -1 231 253 230 -1 231 252 253 -1 232 252 231 -1 232 251 252 -1 233 251 232 -1 233 250 251 -1 234 250 233 -1 234 249 250 -1 235 249 234 -1 235 248 249 -1 236 248 235 -1 236 247 248 -1 237 247 236 -1 237 246 247 -1 238 246 237 -1 238 245 246 -1 239 245 238 -1 239 244 245 -1 240 244 239 -1 240 243 244 -1 241 230 253 -1 241 253 242 -1 241 254 229 -1 241 242 254 -1 194 229 206 -1 194 241 229 -1 194 205 230 -1 194 230 241 -1 195 239 196 -1 195 240 239 -1 196 238 197 -1 196 239 238 -1 197 237 198 -1 197 238 237 -1 198 236 199 -1 198 237 236 -1 199 235 200 -1 199 236 235 -1 200 234 201 -1 200 235 234 -1 201 233 202 -1 201 234 233 -1 202 232 203 -1 202 233 232 -1 203 231 204 -1 203 232 231 -1 204 230 205 -1 204 231 230 -1 206 218 217 -1 206 229 218 -1 207 208 227 -1 207 227 228 -1 208 209 226 -1 208 226 227 -1 209 210 225 -1 209 225 226 -1 210 211 224 -1 210 224 225 -1 211 212 223 -1 211 223 224 -1 212 213 222 -1 212 222 223 -1 213 214 221 -1 213 221 222 -1 214 215 220 -1 214 220 221 -1 215 216 219 -1 215 219 220 -1 216 217 218 -1 216 218 219 -1 171 170 217 -1 171 217 216 -1 172 171 216 -1 172 216 215 -1 173 172 215 -1 173 215 214 -1 174 173 214 -1 174 214 213 -1 175 174 213 -1 175 213 212 -1 176 175 212 -1 176 212 211 -1 177 176 211 -1 177 211 210 -1 178 177 210 -1 178 210 209 -1 179 178 209 -1 179 209 208 -1 180 179 208 -1 180 208 207 -1 181 217 170 -1 181 206 217 -1 183 205 182 -1 183 204 205 -1 184 204 183 -1 184 203 204 -1 185 203 184 -1 185 202 203 -1 186 202 185 -1 186 201 202 -1 187 201 186 -1 187 200 201 -1 188 200 187 -1 188 199 200 -1 189 199 188 -1 189 198 199 -1 190 198 189 -1 190 197 198 -1 191 197 190 -1 191 196 197 -1 192 196 191 -1 192 195 196 -1 193 182 205 -1 193 205 194 -1 193 206 181 -1 193 194 206 -1 146 181 158 -1 146 193 181 -1 146 157 182 -1 146 182 193 -1 147 191 148 -1 147 192 191 -1 148 190 149 -1 148 191 190 -1 149 189 150 -1 149 190 189 -1 150 188 151 -1 150 189 188 -1 151 187 152 -1 151 188 187 -1 152 186 153 -1 152 187 186 -1 153 185 154 -1 153 186 185 -1 154 184 155 -1 154 185 184 -1 155 183 156 -1 155 184 183 -1 156 182 157 -1 156 183 182 -1 158 170 169 -1 158 181 170 -1 159 160 179 -1 159 179 180 -1 160 161 178 -1 160 178 179 -1 161 162 177 -1 161 177 178 -1 162 163 176 -1 162 176 177 -1 163 164 175 -1 163 175 176 -1 164 165 174 -1 164 174 175 -1 165 166 173 -1 165 173 174 -1 166 167 172 -1 166 172 173 -1 167 168 171 -1 167 171 172 -1 168 169 170 -1 168 170 171 -1 123 122 169 -1 123 169 168 -1 124 123 168 -1 124 168 167 -1 125 124 167 -1 125 167 166 -1 126 125 166 -1 126 166 165 -1 127 126 165 -1 127 165 164 -1 128 127 164 -1 128 164 163 -1 129 128 163 -1 129 163 162 -1 130 129 162 -1 130 162 161 -1 131 130 161 -1 131 161 160 -1 132 131 160 -1 132 160 159 -1 133 169 122 -1 133 158 169 -1 135 157 134 -1 135 156 157 -1 136 156 135 -1 136 155 156 -1 137 155 136 -1 137 154 155 -1 138 154 137 -1 138 153 154 -1 139 153 138 -1 139 152 153 -1 140 152 139 -1 140 151 152 -1 141 151 140 -1 141 150 151 -1 142 150 141 -1 142 149 150 -1 143 149 142 -1 143 148 149 -1 144 148 143 -1 144 147 148 -1 145 134 157 -1 145 157 146 -1 145 158 133 -1 145 146 158 -1 98 133 110 -1 98 145 133 -1 98 109 134 -1 98 134 145 -1 99 143 100 -1 99 144 143 -1 100 142 101 -1 100 143 142 -1 101 141 102 -1 101 142 141 -1 102 140 103 -1 102 141 140 -1 103 139 104 -1 103 140 139 -1 104 138 105 -1 104 139 138 -1 105 137 106 -1 105 138 137 -1 106 136 107 -1 106 137 136 -1 107 135 108 -1 107 136 135 -1 108 134 109 -1 108 135 134 -1 110 122 121 -1 110 133 122 -1 111 112 131 -1 111 131 132 -1 112 113 130 -1 112 130 131 -1 113 114 129 -1 113 129 130 -1 114 115 128 -1 114 128 129 -1 115 116 127 -1 115 127 128 -1 116 117 126 -1 116 126 127 -1 117 118 125 -1 117 125 126 -1 118 119 124 -1 118 124 125 -1 119 120 123 -1 119 123 124 -1 120 121 122 -1 120 122 123 -1 75 74 121 -1 75 121 120 -1 76 75 120 -1 76 120 119 -1 77 76 119 -1 77 119 118 -1 78 77 118 -1 78 118 117 -1 79 78 117 -1 79 117 116 -1 80 79 116 -1 80 116 115 -1 81 80 115 -1 81 115 114 -1 82 81 114 -1 82 114 113 -1 83 82 113 -1 83 113 112 -1 84 83 112 -1 84 112 111 -1 85 121 74 -1 85 110 121 -1 87 109 86 -1 87 108 109 -1 88 108 87 -1 88 107 108 -1 89 107 88 -1 89 106 107 -1 90 106 89 -1 90 105 106 -1 91 105 90 -1 91 104 105 -1 92 104 91 -1 92 103 104 -1 93 103 92 -1 93 102 103 -1 94 102 93 -1 94 101 102 -1 95 101 94 -1 95 100 101 -1 96 100 95 -1 96 99 100 -1 97 86 109 -1 97 109 98 -1 97 110 85 -1 97 98 110 -1 50 85 62 -1 50 97 85 -1 50 61 86 -1 50 86 97 -1 51 95 52 -1 51 96 95 -1 52 94 53 -1 52 95 94 -1 53 93 54 -1 53 94 93 -1 54 92 55 -1 54 93 92 -1 55 91 56 -1 55 92 91 -1 56 90 57 -1 56 91 90 -1 57 89 58 -1 57 90 89 -1 58 88 59 -1 58 89 88 -1 59 87 60 -1 59 88 87 -1 60 86 61 -1 60 87 86 -1 62 74 73 -1 62 85 74 -1 63 64 83 -1 63 83 84 -1 64 65 82 -1 64 82 83 -1 65 66 81 -1 65 81 82 -1 66 67 80 -1 66 80 81 -1 67 68 79 -1 67 79 80 -1 68 69 78 -1 68 78 79 -1 69 70 77 -1 69 77 78 -1 70 71 76 -1 70 76 77 -1 71 72 75 -1 71 75 76 -1 72 73 74 -1 72 74 75 -1 27 26 73 -1 27 73 72 -1 28 27 72 -1 28 72 71 -1 29 28 71 -1 29 71 70 -1 30 29 70 -1 30 70 69 -1 31 30 69 -1 31 69 68 -1 32 31 68 -1 32 68 67 -1 33 32 67 -1 33 67 66 -1 34 33 66 -1 34 66 65 -1 35 34 65 -1 35 65 64 -1 36 35 64 -1 36 64 63 -1 37 73 26 -1 37 62 73 -1 39 61 38 -1 39 60 61 -1 40 60 39 -1 40 59 60 -1 41 59 40 -1 41 58 59 -1 42 58 41 -1 42 57 58 -1 43 57 42 -1 43 56 57 -1 44 56 43 -1 44 55 56 -1 45 55 44 -1 45 54 55 -1 46 54 45 -1 46 53 54 -1 47 53 46 -1 47 52 53 -1 48 52 47 -1 48 51 52 -1 49 38 61 -1 49 61 50 -1 49 62 37 -1 49 50 62 -1 2 37 14 -1 2 49 37 -1 2 13 38 -1 2 38 49 -1 3 47 4 -1 3 48 47 -1 4 46 5 -1 4 47 46 -1 5 45 6 -1 5 46 45 -1 6 44 7 -1 6 45 44 -1 7 43 8 -1 7 44 43 -1 8 42 9 -1 8 43 42 -1 9 41 10 -1 9 42 41 -1 10 40 11 -1 10 41 40 -1 11 39 12 -1 11 40 39 -1 12 38 13 -1 12 39 38 -1 14 26 25 -1 14 37 26 -1 15 16 35 -1 15 35 36 -1 16 17 34 -1 16 34 35 -1 17 18 33 -1 17 33 34 -1 18 19 32 -1 18 32 33 -1 19 20 31 -1 19 31 32 -1 20 21 30 -1 20 30 31 -1 21 22 29 -1 21 29 30 -1 22 23 28 -1 22 28 29 -1 23 24 27 -1 23 27 28 -1 24 25 26 -1 24 26 27 -1 747 746 25 -1 747 25 24 -1 748 747 24 -1 748 24 23 -1 749 748 23 -1 749 23 22 -1 750 749 22 -1 750 22 21 -1 751 750 21 -1 751 21 20 -1 752 751 20 -1 752 20 19 -1 753 752 19 -1 753 19 18 -1 754 753 18 -1 754 18 17 -1 755 754 17 -1 755 17 16 -1 756 755 16 -1 756 16 15 -1 757 25 746 -1 757 14 25 -1 759 13 758 -1 759 12 13 -1 760 12 759 -1 760 11 12 -1 761 11 760 -1 761 10 11 -1 762 10 761 -1 762 9 10 -1 763 9 762 -1 763 8 9 -1 764 8 763 -1 764 7 8 -1 765 7 764 -1 765 6 7 -1 766 6 765 -1 766 5 6 -1 767 5 766 -1 767 4 5 -1 768 4 767 -1 768 3 4 -1 769 758 13 -1 769 13 2 -1 769 14 757 -1 769 2 14 -1 + ] + creaseAngle 1 + } + } + DEF TIRE_TREAD Shape { + appearance PBRAppearance { + metalness 0 + roughness 0.9 + baseColorMap ImageTexture { + url [ + "https://raw.githubusercontent.com/cyberbotics/webots/R2021a/projects/robots/adept/pioneer3/protos/textures/cross_tire_base_color.jpg" + ] + } + normalMap ImageTexture { + url [ + "https://raw.githubusercontent.com/cyberbotics/webots/R2021a/projects/robots/adept/pioneer3/protos/textures/cross_tire_normal.jpg" + ] + } + occlusionMap ImageTexture { + url [ + "https://raw.githubusercontent.com/cyberbotics/webots/R2021a/projects/robots/adept/pioneer3/protos/textures/cross_tire_occlusion.jpg" + ] + } + } + geometry IndexedFaceSet { + coord Coordinate { + point [ -0.047241 -0.024940 0.093076 -0.042301 -0.027188 0.101466 -0.034163 -0.028413 0.106040 0.034163 -0.028413 0.106040 0.042301 -0.027188 0.101466 0.047241 -0.024940 0.093076 0.047241 -0.048180 0.083449 0.042301 -0.052522 0.090971 0.034163 -0.054890 0.095073 -0.034163 -0.054890 0.095073 -0.042301 -0.052522 0.090971 -0.047241 -0.048180 0.083449 -0.047241 -0.068136 0.068136 -0.042301 -0.074278 0.074278 -0.034163 -0.077627 0.077627 0.034163 -0.077627 0.077627 0.042301 -0.074278 0.074278 0.047241 -0.068136 0.068136 0.047241 -0.083449 0.048180 0.042301 -0.090971 0.052522 0.034163 -0.095073 0.054890 -0.034163 -0.095073 0.054890 -0.042301 -0.090971 0.052522 -0.047241 -0.083450 0.048180 -0.047241 -0.093076 0.024940 -0.042301 -0.101466 0.027188 -0.034163 -0.106040 0.028413 0.034163 -0.106040 0.028413 0.042301 -0.101466 0.027188 0.047241 -0.093076 0.024940 0.047241 -0.096359 0.000000 0.042301 -0.105045 0.000000 0.034163 -0.109781 0.000000 -0.034163 -0.109781 0.000000 -0.042301 -0.105045 0.000000 -0.047241 -0.096359 0.000000 -0.047241 -0.093076 -0.024940 -0.042301 -0.101466 -0.027188 -0.034163 -0.106040 -0.028413 0.034163 -0.106040 -0.028413 0.042301 -0.101466 -0.027188 0.047241 -0.093076 -0.024940 0.047241 -0.083449 -0.048180 0.042301 -0.090971 -0.052522 0.034163 -0.095073 -0.054890 -0.034163 -0.095073 -0.054890 -0.042301 -0.090971 -0.052522 -0.047241 -0.083449 -0.048180 -0.047241 -0.068136 -0.068136 -0.042301 -0.074278 -0.074278 -0.034163 -0.077627 -0.077627 0.034163 -0.077627 -0.077627 0.042301 -0.074278 -0.074278 0.047241 -0.068136 -0.068136 0.047241 -0.048180 -0.083450 0.042301 -0.052522 -0.090971 0.034163 -0.054890 -0.095073 -0.034163 -0.054890 -0.095073 -0.042301 -0.052522 -0.090971 -0.047241 -0.048180 -0.083450 -0.047241 -0.024940 -0.093076 -0.042301 -0.027188 -0.101466 -0.034163 -0.028413 -0.106040 0.034163 -0.028413 -0.106040 0.042301 -0.027188 -0.101466 0.047241 -0.024940 -0.093076 0.047241 0.000000 -0.096359 0.042301 0.000000 -0.105045 0.034163 0.000000 -0.109781 -0.034163 0.000000 -0.109781 -0.042301 0.000000 -0.105045 -0.047241 0.000000 -0.096359 -0.047241 0.024940 -0.093076 -0.042301 0.027188 -0.101466 -0.034163 0.028413 -0.106040 0.034163 0.028413 -0.106040 0.042301 0.027188 -0.101466 0.047241 0.024940 -0.093076 0.047241 0.048180 -0.083449 0.042301 0.052522 -0.090971 0.034163 0.054890 -0.095073 -0.034163 0.054890 -0.095073 -0.042301 0.052522 -0.090971 -0.047241 0.048180 -0.083449 -0.047241 0.068136 -0.068136 -0.042301 0.074278 -0.074278 -0.034163 0.077627 -0.077627 0.034163 0.077627 -0.077627 0.042301 0.074278 -0.074278 0.047241 0.068136 -0.068136 0.047241 0.083449 -0.048180 0.042301 0.090971 -0.052522 0.034163 0.095073 -0.054890 -0.034163 0.095073 -0.054890 -0.042301 0.090971 -0.052522 -0.047241 0.083449 -0.048180 -0.047241 0.093076 -0.024940 -0.042301 0.101466 -0.027188 -0.034163 0.106040 -0.028413 0.034163 0.106040 -0.028413 0.042301 0.101466 -0.027188 0.047241 0.093076 -0.024940 0.047241 0.096359 0.000000 0.042301 0.105045 0.000000 0.034163 0.109781 0.000000 -0.034163 0.109781 0.000000 -0.042301 0.105045 0.000000 -0.047241 0.096359 0.000000 -0.047241 0.093076 0.024940 -0.042301 0.101466 0.027188 -0.034163 0.106040 0.028413 0.034163 0.106040 0.028413 0.042301 0.101466 0.027188 0.047241 0.093076 0.024940 0.047241 0.083449 0.048180 0.042301 0.090971 0.052522 0.034163 0.095073 0.054890 -0.034163 0.095073 0.054890 -0.042301 0.090971 0.052522 -0.047241 0.083449 0.048180 -0.047241 0.068136 0.068136 -0.042301 0.074278 0.074278 -0.034163 0.077627 0.077627 0.034163 0.077627 0.077627 0.042301 0.074278 0.074278 0.047241 0.068136 0.068136 0.047241 0.048180 0.083450 0.042301 0.052522 0.090971 0.034163 0.054890 0.095073 -0.034163 0.054890 0.095073 -0.042301 0.052522 0.090971 -0.047241 0.048180 0.083450 -0.047241 0.024939 0.093076 -0.042301 0.027187 0.101466 -0.034163 0.028413 0.106040 0.034163 0.028413 0.106040 0.042301 0.027187 0.101466 0.047241 0.024939 0.093076 0.047241 -0.000000 0.096359 0.042301 -0.000000 0.105045 0.034163 -0.000000 0.109781 -0.034163 -0.000000 0.109781 -0.042301 -0.000000 0.105045 -0.047241 -0.000000 0.096359 ] + } + texCoord TextureCoordinate { + point [ 0.2070 1.2599 0.8079 1.3696 0.8079 1.2574 0.2070 1.2599 0.2064 1.3718 0.8079 1.3696 0.0169 1.2597 0.1131 1.3717 0.1131 1.2600 0.0169 1.2597 0.0169 1.3712 0.1131 1.3717 0.1131 1.2600 0.2064 1.3718 0.2070 1.2599 0.1131 1.2600 0.1131 1.3717 0.2064 1.3718 0.8079 1.2574 0.8892 1.3693 0.8892 1.2571 0.8079 1.2574 0.8079 1.3696 0.8892 1.3693 0.8892 1.2571 0.9902 1.3692 0.9902 1.2571 0.8892 1.2571 0.8892 1.3693 0.9902 1.3692 0.8892 1.1452 0.9902 1.2571 0.9902 1.1453 0.8892 1.1452 0.8892 1.2571 0.9902 1.2571 0.8079 1.1454 0.8892 1.2571 0.8892 1.1452 0.8079 1.1454 0.8079 1.2574 0.8892 1.2571 0.1131 1.1483 0.2070 1.2599 0.2068 1.1482 0.1131 1.1483 0.1131 1.2600 0.2070 1.2599 0.0169 1.1481 0.1131 1.2600 0.1131 1.1483 0.0169 1.1481 0.0169 1.2597 0.1131 1.2600 0.2068 1.1482 0.8079 1.2574 0.8079 1.1454 0.2068 1.1482 0.2070 1.2599 0.8079 1.2574 0.2063 1.0365 0.8079 1.1454 0.8079 1.0337 0.2063 1.0365 0.2068 1.1482 0.8079 1.1454 0.0169 1.0365 0.1131 1.1483 0.1131 1.0367 0.0169 1.0365 0.0169 1.1481 0.1131 1.1483 0.1131 1.0367 0.2068 1.1482 0.2063 1.0365 0.1131 1.0367 0.1131 1.1483 0.2068 1.1482 0.8079 1.0337 0.8892 1.1452 0.8892 1.0334 0.8079 1.0337 0.8079 1.1454 0.8892 1.1452 0.8892 1.0334 0.9902 1.1453 0.9902 1.0335 0.8892 1.0334 0.8892 1.1452 0.9902 1.1453 0.8892 0.9217 0.9902 1.0335 0.9902 0.9218 0.8892 0.9217 0.8892 1.0334 0.9902 1.0335 0.8079 0.9219 0.8892 1.0334 0.8892 0.9217 0.8079 0.9219 0.8079 1.0337 0.8892 1.0334 0.1131 0.9250 0.2063 1.0365 0.2056 0.9248 0.1131 0.9250 0.1131 1.0367 0.2063 1.0365 0.0169 0.9249 0.1131 1.0367 0.1131 0.9250 0.0169 0.9249 0.0169 1.0365 0.1131 1.0367 0.2056 0.9248 0.8079 1.0337 0.8079 0.9219 0.2056 0.9248 0.2063 1.0365 0.8079 1.0337 0.2048 0.8131 0.8079 0.9219 0.8071 0.8103 0.2048 0.8131 0.2056 0.9248 0.8079 0.9219 0.0169 0.8132 0.1131 0.9250 0.1131 0.8133 0.0169 0.8132 0.0169 0.9249 0.1131 0.9250 0.1131 0.8133 0.2056 0.9248 0.2048 0.8131 0.1131 0.8133 0.1131 0.9250 0.2056 0.9248 0.8071 0.8103 0.8892 0.9217 0.8892 0.8100 0.8071 0.8103 0.8079 0.9219 0.8892 0.9217 0.8892 0.8100 0.9902 0.9218 0.9902 0.8101 0.8892 0.8100 0.8892 0.9217 0.9902 0.9218 0.8892 0.6984 0.9902 0.8101 0.9902 0.6985 0.8892 0.6984 0.8892 0.8100 0.9902 0.8101 0.8062 0.6986 0.8892 0.8100 0.8892 0.6984 0.8062 0.6986 0.8071 0.8103 0.8892 0.8100 0.1131 0.7017 0.2048 0.8131 0.2041 0.7015 0.1131 0.7017 0.1131 0.8133 0.2048 0.8131 0.0169 0.7016 0.1131 0.8133 0.1131 0.7017 0.0169 0.7016 0.0169 0.8132 0.1131 0.8133 0.2041 0.7015 0.8071 0.8103 0.8062 0.6986 0.2041 0.7015 0.2048 0.8131 0.8071 0.8103 0.2033 0.5898 0.8062 0.6986 0.8054 0.5869 0.2033 0.5898 0.2041 0.7015 0.8062 0.6986 0.0169 0.5899 0.1131 0.7017 0.1131 0.5900 0.0169 0.5899 0.0169 0.7016 0.1131 0.7017 0.1131 0.5900 0.2041 0.7015 0.2033 0.5898 0.1131 0.5900 0.1131 0.7017 0.2041 0.7015 0.8054 0.5869 0.8892 0.6984 0.8892 0.5867 0.8054 0.5869 0.8062 0.6986 0.8892 0.6984 0.8892 0.5867 0.9902 0.6985 0.9902 0.5868 0.8892 0.5867 0.8892 0.6984 0.9902 0.6985 0.8892 0.4751 0.9902 0.5868 0.9902 0.4752 0.8892 0.4751 0.8892 0.5867 0.9902 0.5868 0.8046 0.4753 0.8892 0.5867 0.8892 0.4751 0.8046 0.4753 0.8054 0.5869 0.8892 0.5867 0.1131 0.4784 0.2033 0.5898 0.2025 0.4782 0.1131 0.4784 0.1131 0.5900 0.2033 0.5898 0.0169 0.4783 0.1131 0.5900 0.1131 0.4784 0.0169 0.4783 0.0169 0.5899 0.1131 0.5900 0.2025 0.4782 0.8054 0.5869 0.8046 0.4753 0.2025 0.4782 0.2033 0.5898 0.8054 0.5869 0.2017 0.3665 0.8046 0.4753 0.8038 0.3636 0.2017 0.3665 0.2025 0.4782 0.8046 0.4753 0.0169 0.3666 0.1131 0.4784 0.1131 0.3667 0.0169 0.3666 0.0169 0.4783 0.1131 0.4784 0.1131 0.3667 0.2025 0.4782 0.2017 0.3665 0.1131 0.3667 0.1131 0.4784 0.2025 0.4782 0.8038 0.3636 0.8892 0.4751 0.8892 0.3634 0.8038 0.3636 0.8046 0.4753 0.8892 0.4751 0.8892 0.3634 0.9902 0.4752 0.9902 0.3635 0.8892 0.3634 0.8892 0.4751 0.9902 0.4752 0.8892 0.2518 0.9902 0.3635 0.9902 0.2519 0.8892 0.2518 0.8892 0.3634 0.9902 0.3635 0.8030 0.2520 0.8892 0.3634 0.8892 0.2518 0.8030 0.2520 0.8038 0.3636 0.8892 0.3634 0.1131 0.2551 0.2017 0.3665 0.2009 0.2549 0.1131 0.2551 0.1131 0.3667 0.2017 0.3665 0.0169 0.2550 0.1131 0.3667 0.1131 0.2551 0.0169 0.2550 0.0169 0.3666 0.1131 0.3667 0.2009 0.2549 0.8038 0.3636 0.8030 0.2520 0.2009 0.2549 0.2017 0.3665 0.8038 0.3636 0.2001 0.1432 0.8030 0.2520 0.8022 0.1404 0.2001 0.1432 0.2009 0.2549 0.8030 0.2520 0.0169 0.1433 0.1131 0.2551 0.1131 0.1434 0.0169 0.1433 0.0169 0.2550 0.1131 0.2551 0.1131 0.1434 0.2009 0.2549 0.2001 0.1432 0.1131 0.1434 0.1131 0.2551 0.2009 0.2549 0.8022 0.1404 0.8892 0.2518 0.8892 0.1401 0.8022 0.1404 0.8030 0.2520 0.8892 0.2518 0.8892 0.1401 0.9902 0.2519 0.9902 0.1403 0.8892 0.1401 0.8892 0.2518 0.9902 0.2519 0.8892 0.0285 0.9902 0.1403 0.9902 0.0286 0.8892 0.0285 0.8892 0.1401 0.9902 0.1403 0.8015 0.0287 0.8892 0.1401 0.8892 0.0285 0.8015 0.0287 0.8022 0.1404 0.8892 0.1401 0.1131 0.0318 0.2001 0.1432 0.1993 0.0315 0.1131 0.0318 0.1131 0.1434 0.2001 0.1432 0.0169 0.0316 0.1131 0.1434 0.1131 0.0318 0.0169 0.0316 0.0169 0.1433 0.1131 0.1434 0.1993 0.0315 0.8022 0.1404 0.8015 0.0287 0.1993 0.0315 0.2001 0.1432 0.8022 0.1404 0.2048 -0.0802 0.8015 0.0287 0.8079 -0.0829 0.2048 -0.0802 0.1993 0.0315 0.8015 0.0287 0.0169 -0.0800 0.1131 0.0318 0.1131 -0.0799 0.0169 -0.0800 0.0169 0.0316 0.1131 0.0318 0.1131 -0.0799 0.1993 0.0315 0.2048 -0.0802 0.1131 -0.0799 0.1131 0.0318 0.1993 0.0315 0.8079 -0.0829 0.8892 0.0285 0.8892 -0.0831 0.8079 -0.0829 0.8015 0.0287 0.8892 0.0285 0.8892 -0.0831 0.9902 0.0286 0.9902 -0.0830 0.8892 -0.0831 0.8892 0.0285 0.9902 0.0286 0.8892 -0.1948 0.9902 -0.0830 0.9902 -0.1946 0.8892 -0.1948 0.8892 -0.0831 0.9902 -0.0830 0.8079 -0.1946 0.8892 -0.0831 0.8892 -0.1948 0.8079 -0.1946 0.8079 -0.0829 0.8892 -0.0831 0.1131 -0.1917 0.2048 -0.0802 0.2048 -0.1919 0.1131 -0.1917 0.1131 -0.0799 0.2048 -0.0802 0.0169 -0.1918 0.1131 -0.0799 0.1131 -0.1917 0.0169 -0.1918 0.0169 -0.0800 0.1131 -0.0799 0.2048 -0.1919 0.8079 -0.0829 0.8079 -0.1946 0.2048 -0.1919 0.2048 -0.0802 0.8079 -0.0829 0.2048 -0.3038 0.8079 -0.1946 0.8079 -0.3063 0.2048 -0.3038 0.2048 -0.1919 0.8079 -0.1946 0.0169 -0.3036 0.1131 -0.1917 0.1131 -0.3035 0.0169 -0.3036 0.0169 -0.1918 0.1131 -0.1917 0.1131 -0.3035 0.2048 -0.1919 0.2048 -0.3038 0.1131 -0.3035 0.1131 -0.1917 0.2048 -0.1919 0.8079 -0.3063 0.8892 -0.1948 0.8892 -0.3064 0.8079 -0.3063 0.8079 -0.1946 0.8892 -0.1948 0.8892 -0.3064 0.9902 -0.1946 0.9902 -0.3061 0.8892 -0.3064 0.8892 -0.1948 0.9902 -0.1946 0.8892 -0.4180 0.9902 -0.3061 0.9902 -0.4175 0.8892 -0.4180 0.8892 -0.3064 0.9902 -0.3061 0.8079 -0.4182 0.8892 -0.3064 0.8892 -0.4180 0.8079 -0.4182 0.8079 -0.3063 0.8892 -0.3064 0.1131 -0.4157 0.2048 -0.3038 0.2048 -0.4161 0.1131 -0.4157 0.1131 -0.3035 0.2048 -0.3038 0.0169 -0.4156 0.1131 -0.3035 0.1131 -0.4157 0.0169 -0.4156 0.0169 -0.3036 0.1131 -0.3035 0.2048 -0.4161 0.8079 -0.3063 0.8079 -0.4182 0.2048 -0.4161 0.2048 -0.3038 0.8079 -0.3063 0.2048 -0.5290 0.8079 -0.4182 0.8079 -0.5303 0.2048 -0.5290 0.2048 -0.4161 0.8079 -0.4182 0.0169 -0.5281 0.1131 -0.4157 0.1131 -0.5285 0.0169 -0.5281 0.0169 -0.4156 0.1131 -0.4157 0.1131 -0.5285 0.2048 -0.4161 0.2048 -0.5290 0.1131 -0.5285 0.1131 -0.4157 0.2048 -0.4161 0.8079 -0.5303 0.8892 -0.4180 0.8892 -0.5298 0.8079 -0.5303 0.8079 -0.4182 0.8892 -0.4180 0.8892 -0.5298 0.9902 -0.4175 0.9902 -0.5287 0.8892 -0.5298 0.8892 -0.4180 0.9902 -0.4175 0.8892 -0.6417 0.9902 -0.5287 0.9902 -0.6395 0.8892 -0.6417 0.8892 -0.5298 0.9902 -0.5287 0.8079 -0.6432 0.8892 -0.5298 0.8892 -0.6417 0.8079 -0.6432 0.8079 -0.5303 0.8892 -0.5298 0.1131 -0.6426 0.2048 -0.5290 0.2048 -0.6437 0.1131 -0.6426 0.1131 -0.5285 0.2048 -0.5290 0.0169 -0.6413 0.1131 -0.5285 0.1131 -0.6426 0.0169 -0.6413 0.0169 -0.5281 0.1131 -0.5285 0.2048 -0.6437 0.8079 -0.5303 0.8079 -0.6432 0.2048 -0.6437 0.2048 -0.5290 0.8079 -0.5303 0.2048 -0.7622 0.8079 -0.6432 0.8079 -0.7580 0.2048 -0.7622 0.2048 -0.6437 0.8079 -0.6432 0.0169 -0.7560 0.1131 -0.6426 0.1131 -0.7595 0.0169 -0.7560 0.0169 -0.6413 0.1131 -0.6426 0.1131 -0.7595 0.2048 -0.6437 0.2048 -0.7622 0.1131 -0.7595 0.1131 -0.6426 0.2048 -0.6437 0.8079 -0.7580 0.8892 -0.6417 0.8892 -0.7543 0.8079 -0.7580 0.8079 -0.6432 0.8892 -0.6417 0.8892 -0.7543 0.9902 -0.6395 0.9902 -0.7495 0.8892 -0.7543 0.8892 -0.6417 0.9902 -0.6395 0.8892 -0.8726 0.9902 -0.7495 0.9902 -0.8726 0.8892 -0.8726 0.8892 -0.7543 0.9902 -0.7495 0.8079 -0.8726 0.8892 -0.7543 0.8892 -0.8726 0.8079 -0.8726 0.8079 -0.7580 0.8892 -0.7543 0.1131 -0.8726 0.2048 -0.7622 0.2048 -0.8726 0.1131 -0.8726 0.1131 -0.7595 0.2048 -0.7622 0.0169 -0.8726 0.1131 -0.7595 0.1131 -0.8726 0.0169 -0.8726 0.0169 -0.7560 0.1131 -0.7595 0.2048 -0.8726 0.8079 -0.7580 0.8079 -0.8726 0.2048 -0.8726 0.2048 -0.7622 0.8079 -0.7580 0.2048 1.7118 0.8079 1.8263 0.8079 1.7158 0.2048 1.7118 0.2048 1.8263 0.8079 1.8263 0.0169 1.7032 0.1131 1.8263 0.1131 1.7080 0.0169 1.7032 0.0169 1.8263 0.1131 1.8263 0.1131 1.7080 0.2048 1.8263 0.2048 1.7118 0.1131 1.7080 0.1131 1.8263 0.2048 1.8263 0.8079 1.7158 0.8892 1.8263 0.8892 1.7131 0.8079 1.7158 0.8079 1.8263 0.8892 1.8263 0.8892 1.7131 0.9902 1.8263 0.9902 1.7097 0.8892 1.7131 0.8892 1.8263 0.9902 1.8263 0.8892 1.5962 0.9902 1.7097 0.9902 1.5949 0.8892 1.5962 0.8892 1.7131 0.9902 1.7097 0.8079 1.5973 0.8892 1.7131 0.8892 1.5962 0.8079 1.5973 0.8079 1.7158 0.8892 1.7131 0.1131 1.5954 0.2048 1.7118 0.2048 1.5969 0.1131 1.5954 0.1131 1.7080 0.2048 1.7118 0.0169 1.5932 0.1131 1.7080 0.1131 1.5954 0.0169 1.5932 0.0169 1.7032 0.1131 1.7080 0.2048 1.5969 0.8079 1.7158 0.8079 1.5973 0.2048 1.5969 0.2048 1.7118 0.8079 1.7158 0.2048 1.4840 0.8079 1.5973 0.8079 1.4827 0.2048 1.4840 0.2048 1.5969 0.8079 1.5973 0.0169 1.4824 0.1131 1.5954 0.1131 1.4834 0.0169 1.4824 0.0169 1.5932 0.1131 1.5954 0.1131 1.4834 0.2048 1.5969 0.2048 1.4840 0.1131 1.4834 0.1131 1.5954 0.2048 1.5969 0.8079 1.4827 0.8892 1.5962 0.8892 1.4821 0.8079 1.4827 0.8079 1.5973 0.8892 1.5962 0.8892 1.4821 0.9902 1.5949 0.9902 1.4816 0.8892 1.4821 0.8892 1.5962 0.9902 1.5949 0.8892 1.3693 0.9902 1.4816 0.9902 1.3692 0.8892 1.3693 0.8892 1.4821 0.9902 1.4816 0.8079 1.3696 0.8892 1.4821 0.8892 1.3693 0.8079 1.3696 0.8079 1.4827 0.8892 1.4821 0.1131 1.3717 0.2048 1.4840 0.2064 1.3718 0.1131 1.3717 0.1131 1.4834 0.2048 1.4840 0.0169 1.3712 0.1131 1.4834 0.1131 1.3717 0.0169 1.3712 0.0169 1.4824 0.1131 1.4834 0.2064 1.3718 0.8079 1.4827 0.8079 1.3696 0.2064 1.3718 0.2048 1.4840 0.8079 1.4827 ] + } + texCoordIndex [ 0 1 2 -1 3 4 5 -1 6 7 8 -1 9 10 11 -1 12 13 14 -1 15 16 17 -1 18 19 20 -1 21 22 23 -1 24 25 26 -1 27 28 29 -1 30 31 32 -1 33 34 35 -1 36 37 38 -1 39 40 41 -1 42 43 44 -1 45 46 47 -1 48 49 50 -1 51 52 53 -1 54 55 56 -1 57 58 59 -1 60 61 62 -1 63 64 65 -1 66 67 68 -1 69 70 71 -1 72 73 74 -1 75 76 77 -1 78 79 80 -1 81 82 83 -1 84 85 86 -1 87 88 89 -1 90 91 92 -1 93 94 95 -1 96 97 98 -1 99 100 101 -1 102 103 104 -1 105 106 107 -1 108 109 110 -1 111 112 113 -1 114 115 116 -1 117 118 119 -1 120 121 122 -1 123 124 125 -1 126 127 128 -1 129 130 131 -1 132 133 134 -1 135 136 137 -1 138 139 140 -1 141 142 143 -1 144 145 146 -1 147 148 149 -1 150 151 152 -1 153 154 155 -1 156 157 158 -1 159 160 161 -1 162 163 164 -1 165 166 167 -1 168 169 170 -1 171 172 173 -1 174 175 176 -1 177 178 179 -1 180 181 182 -1 183 184 185 -1 186 187 188 -1 189 190 191 -1 192 193 194 -1 195 196 197 -1 198 199 200 -1 201 202 203 -1 204 205 206 -1 207 208 209 -1 210 211 212 -1 213 214 215 -1 216 217 218 -1 219 220 221 -1 222 223 224 -1 225 226 227 -1 228 229 230 -1 231 232 233 -1 234 235 236 -1 237 238 239 -1 240 241 242 -1 243 244 245 -1 246 247 248 -1 249 250 251 -1 252 253 254 -1 255 256 257 -1 258 259 260 -1 261 262 263 -1 264 265 266 -1 267 268 269 -1 270 271 272 -1 273 274 275 -1 276 277 278 -1 279 280 281 -1 282 283 284 -1 285 286 287 -1 288 289 290 -1 291 292 293 -1 294 295 296 -1 297 298 299 -1 300 301 302 -1 303 304 305 -1 306 307 308 -1 309 310 311 -1 312 313 314 -1 315 316 317 -1 318 319 320 -1 321 322 323 -1 324 325 326 -1 327 328 329 -1 330 331 332 -1 333 334 335 -1 336 337 338 -1 339 340 341 -1 342 343 344 -1 345 346 347 -1 348 349 350 -1 351 352 353 -1 354 355 356 -1 357 358 359 -1 360 361 362 -1 363 364 365 -1 366 367 368 -1 369 370 371 -1 372 373 374 -1 375 376 377 -1 378 379 380 -1 381 382 383 -1 384 385 386 -1 387 388 389 -1 390 391 392 -1 393 394 395 -1 396 397 398 -1 399 400 401 -1 402 403 404 -1 405 406 407 -1 408 409 410 -1 411 412 413 -1 414 415 416 -1 417 418 419 -1 420 421 422 -1 423 424 425 -1 426 427 428 -1 429 430 431 -1 432 433 434 -1 435 436 437 -1 438 439 440 -1 441 442 443 -1 444 445 446 -1 447 448 449 -1 450 451 452 -1 453 454 455 -1 456 457 458 -1 459 460 461 -1 462 463 464 -1 465 466 467 -1 468 469 470 -1 471 472 473 -1 474 475 476 -1 477 478 479 -1 480 481 482 -1 483 484 485 -1 486 487 488 -1 489 490 491 -1 492 493 494 -1 495 496 497 -1 498 499 500 -1 501 502 503 -1 504 505 506 -1 507 508 509 -1 510 511 512 -1 513 514 515 -1 516 517 518 -1 519 520 521 -1 522 523 524 -1 525 526 527 -1 528 529 530 -1 531 532 533 -1 534 535 536 -1 537 538 539 -1 540 541 542 -1 543 544 545 -1 546 547 548 -1 549 550 551 -1 552 553 554 -1 555 556 557 -1 558 559 560 -1 561 562 563 -1 564 565 566 -1 567 568 569 -1 570 571 572 -1 573 574 575 -1 576 577 578 -1 579 580 581 -1 582 583 584 -1 585 586 587 -1 588 589 590 -1 591 592 593 -1 594 595 596 -1 597 598 599 -1 600 601 602 -1 603 604 605 -1 606 607 608 -1 609 610 611 -1 612 613 614 -1 615 616 617 -1 618 619 620 -1 621 622 623 -1 624 625 626 -1 627 628 629 -1 630 631 632 -1 633 634 635 -1 636 637 638 -1 639 640 641 -1 642 643 644 -1 645 646 647 -1 648 649 650 -1 651 652 653 -1 654 655 656 -1 657 658 659 -1 660 661 662 -1 663 664 665 -1 666 667 668 -1 669 670 671 -1 672 673 674 -1 675 676 677 -1 678 679 680 -1 681 682 683 -1 684 685 686 -1 687 688 689 -1 690 691 692 -1 693 694 695 -1 696 697 698 -1 699 700 701 -1 702 703 704 -1 705 706 707 -1 708 709 710 -1 711 712 713 -1 714 715 716 -1 717 718 719 -1 ] + coordIndex [ 134 140 135 -1 134 141 140 -1 132 142 133 -1 132 143 142 -1 133 141 134 -1 133 142 141 -1 135 139 136 -1 135 140 139 -1 136 138 137 -1 136 139 138 -1 127 137 126 -1 127 136 137 -1 128 136 127 -1 128 135 136 -1 130 134 129 -1 130 133 134 -1 131 133 130 -1 131 132 133 -1 129 135 128 -1 129 134 135 -1 122 128 123 -1 122 129 128 -1 120 130 121 -1 120 131 130 -1 121 129 122 -1 121 130 129 -1 123 127 124 -1 123 128 127 -1 124 126 125 -1 124 127 126 -1 115 125 114 -1 115 124 125 -1 116 124 115 -1 116 123 124 -1 118 122 117 -1 118 121 122 -1 119 121 118 -1 119 120 121 -1 117 123 116 -1 117 122 123 -1 110 116 111 -1 110 117 116 -1 108 118 109 -1 108 119 118 -1 109 117 110 -1 109 118 117 -1 111 115 112 -1 111 116 115 -1 112 114 113 -1 112 115 114 -1 103 113 102 -1 103 112 113 -1 104 112 103 -1 104 111 112 -1 106 110 105 -1 106 109 110 -1 107 109 106 -1 107 108 109 -1 105 111 104 -1 105 110 111 -1 98 104 99 -1 98 105 104 -1 96 106 97 -1 96 107 106 -1 97 105 98 -1 97 106 105 -1 99 103 100 -1 99 104 103 -1 100 102 101 -1 100 103 102 -1 91 101 90 -1 91 100 101 -1 92 100 91 -1 92 99 100 -1 94 98 93 -1 94 97 98 -1 95 97 94 -1 95 96 97 -1 93 99 92 -1 93 98 99 -1 86 92 87 -1 86 93 92 -1 84 94 85 -1 84 95 94 -1 85 93 86 -1 85 94 93 -1 87 91 88 -1 87 92 91 -1 88 90 89 -1 88 91 90 -1 79 89 78 -1 79 88 89 -1 80 88 79 -1 80 87 88 -1 82 86 81 -1 82 85 86 -1 83 85 82 -1 83 84 85 -1 81 87 80 -1 81 86 87 -1 74 80 75 -1 74 81 80 -1 72 82 73 -1 72 83 82 -1 73 81 74 -1 73 82 81 -1 75 79 76 -1 75 80 79 -1 76 78 77 -1 76 79 78 -1 67 77 66 -1 67 76 77 -1 68 76 67 -1 68 75 76 -1 70 74 69 -1 70 73 74 -1 71 73 70 -1 71 72 73 -1 69 75 68 -1 69 74 75 -1 62 68 63 -1 62 69 68 -1 60 70 61 -1 60 71 70 -1 61 69 62 -1 61 70 69 -1 63 67 64 -1 63 68 67 -1 64 66 65 -1 64 67 66 -1 55 65 54 -1 55 64 65 -1 56 64 55 -1 56 63 64 -1 58 62 57 -1 58 61 62 -1 59 61 58 -1 59 60 61 -1 57 63 56 -1 57 62 63 -1 50 56 51 -1 50 57 56 -1 48 58 49 -1 48 59 58 -1 49 57 50 -1 49 58 57 -1 51 55 52 -1 51 56 55 -1 52 54 53 -1 52 55 54 -1 43 53 42 -1 43 52 53 -1 44 52 43 -1 44 51 52 -1 46 50 45 -1 46 49 50 -1 47 49 46 -1 47 48 49 -1 45 51 44 -1 45 50 51 -1 38 44 39 -1 38 45 44 -1 36 46 37 -1 36 47 46 -1 37 45 38 -1 37 46 45 -1 39 43 40 -1 39 44 43 -1 40 42 41 -1 40 43 42 -1 31 41 30 -1 31 40 41 -1 32 40 31 -1 32 39 40 -1 34 38 33 -1 34 37 38 -1 35 37 34 -1 35 36 37 -1 33 39 32 -1 33 38 39 -1 26 32 27 -1 26 33 32 -1 24 34 25 -1 24 35 34 -1 25 33 26 -1 25 34 33 -1 27 31 28 -1 27 32 31 -1 28 30 29 -1 28 31 30 -1 19 29 18 -1 19 28 29 -1 20 28 19 -1 20 27 28 -1 22 26 21 -1 22 25 26 -1 23 25 22 -1 23 24 25 -1 21 27 20 -1 21 26 27 -1 14 20 15 -1 14 21 20 -1 12 22 13 -1 12 23 22 -1 13 21 14 -1 13 22 21 -1 15 19 16 -1 15 20 19 -1 16 18 17 -1 16 19 18 -1 7 17 6 -1 7 16 17 -1 8 16 7 -1 8 15 16 -1 10 14 9 -1 10 13 14 -1 11 13 10 -1 11 12 13 -1 9 15 8 -1 9 14 15 -1 2 8 3 -1 2 9 8 -1 0 10 1 -1 0 11 10 -1 1 9 2 -1 1 10 9 -1 3 7 4 -1 3 8 7 -1 4 6 5 -1 4 7 6 -1 139 5 138 -1 139 4 5 -1 140 4 139 -1 140 3 4 -1 142 2 141 -1 142 1 2 -1 143 1 142 -1 143 0 1 -1 141 3 140 -1 141 2 3 -1 ] + creaseAngle 1 + } + } + DEF TIRE_RIM Shape { + appearance PBRAppearance { + roughness 1 + metalness 0 + baseColor 0.1 0.1 0.1 + } + geometry IndexedFaceSet { + coord Coordinate { + point [ -0.044395 -0.015502 0.057854 -0.047241 -0.024940 0.093076 0.047241 -0.024940 0.093076 0.044394 -0.015502 0.057854 0.044394 -0.029947 0.051870 0.047241 -0.048180 0.083449 -0.047241 -0.048180 0.083449 -0.044395 -0.029947 0.051870 -0.044395 -0.042352 0.042352 -0.047241 -0.068136 0.068136 0.047241 -0.068136 0.068136 0.044394 -0.042352 0.042352 0.044394 -0.051870 0.029947 0.047241 -0.083449 0.048180 -0.047241 -0.083450 0.048180 -0.044395 -0.051870 0.029947 -0.044395 -0.057854 0.015502 -0.047241 -0.093076 0.024940 0.047241 -0.093076 0.024940 0.044394 -0.057854 0.015502 0.044394 -0.059895 0.000000 0.047241 -0.096359 0.000000 -0.047241 -0.096359 0.000000 -0.044395 -0.059895 0.000000 -0.044395 -0.057854 -0.015502 -0.047241 -0.093076 -0.024940 0.047241 -0.093076 -0.024940 0.044394 -0.057854 -0.015502 0.044394 -0.051870 -0.029947 0.047241 -0.083449 -0.048180 -0.047241 -0.083449 -0.048180 -0.044395 -0.051870 -0.029947 -0.044395 -0.042352 -0.042352 -0.047241 -0.068136 -0.068136 0.047241 -0.068136 -0.068136 0.044394 -0.042352 -0.042352 0.044394 -0.029947 -0.051870 0.047241 -0.048180 -0.083450 -0.047241 -0.048180 -0.083450 -0.044395 -0.029947 -0.051870 -0.044395 -0.015502 -0.057854 -0.047241 -0.024940 -0.093076 0.047241 -0.024940 -0.093076 0.044394 -0.015502 -0.057854 0.044394 0.000000 -0.059895 0.047241 0.000000 -0.096359 -0.047241 0.000000 -0.096359 -0.044395 0.000000 -0.059895 -0.044395 0.015502 -0.057854 -0.047241 0.024940 -0.093076 0.047241 0.024940 -0.093076 0.044394 0.015502 -0.057854 0.044394 0.029947 -0.051870 0.047241 0.048180 -0.083449 -0.047241 0.048180 -0.083449 -0.044395 0.029947 -0.051870 -0.044395 0.042352 -0.042352 -0.047241 0.068136 -0.068136 0.047241 0.068136 -0.068136 0.044394 0.042352 -0.042352 0.044394 0.051870 -0.029947 0.047241 0.083449 -0.048180 -0.047241 0.083449 -0.048180 -0.044395 0.051870 -0.029947 -0.044395 0.057854 -0.015502 -0.047241 0.093076 -0.024940 0.047241 0.093076 -0.024940 0.044394 0.057854 -0.015502 0.044394 0.059895 0.000000 0.047241 0.096359 0.000000 -0.047241 0.096359 0.000000 -0.044395 0.059895 0.000000 -0.044395 0.057854 0.015502 -0.047241 0.093076 0.024940 0.047241 0.093076 0.024940 0.044394 0.057854 0.015502 0.044394 0.051870 0.029947 0.047241 0.083449 0.048180 -0.047241 0.083449 0.048180 -0.044395 0.051870 0.029947 -0.044395 0.042352 0.042352 -0.047241 0.068136 0.068136 0.047241 0.068136 0.068136 0.044394 0.042352 0.042352 0.044394 0.029947 0.051870 0.047241 0.048180 0.083450 -0.047241 0.048180 0.083450 -0.044395 0.029947 0.051870 -0.044395 0.015502 0.057854 -0.047241 0.024939 0.093076 0.047241 0.024939 0.093076 0.044394 0.015502 0.057854 0.044394 0.000000 0.059895 0.047241 -0.000000 0.096359 -0.047241 -0.000000 0.096359 -0.044395 -0.000000 0.059895 ] + } + texCoord TextureCoordinate { + point [ 0.4054 0.5409 0.5000 0.4342 0.5000 0.5658 0.4054 0.5409 0.4054 0.4591 0.5000 0.4342 1.0000 0.4342 0.9054 0.5409 0.9054 0.4591 1.0000 0.4342 1.0000 0.5658 0.9054 0.5409 0.9830 0.3070 0.9054 0.4591 0.8948 0.3800 0.9830 0.3070 1.0000 0.4342 0.9054 0.4591 0.3948 0.6200 0.5000 0.5658 0.4830 0.6930 0.3948 0.6200 0.4054 0.5409 0.5000 0.5658 0.3743 0.6908 0.4830 0.6930 0.4500 0.8070 0.3743 0.6908 0.3948 0.6200 0.4830 0.6930 0.9500 0.1930 0.8948 0.3800 0.8743 0.3092 0.9500 0.1930 0.9830 0.3070 0.8948 0.3800 0.9035 0.0999 0.8743 0.3092 0.8454 0.2513 0.9035 0.0999 0.9500 0.1930 0.8743 0.3092 0.3454 0.7487 0.4500 0.8070 0.4035 0.9001 0.3454 0.7487 0.3743 0.6908 0.4500 0.8070 0.3100 0.7896 0.4035 0.9001 0.3465 0.9659 0.3100 0.7896 0.3454 0.7487 0.4035 0.9001 0.8465 0.0341 0.8454 0.2513 0.8100 0.2104 0.8465 0.0341 0.9035 0.0999 0.8454 0.2513 0.7829 0.0000 0.8100 0.2104 0.7705 0.1892 0.7829 0.0000 0.8465 0.0341 0.8100 0.2104 0.2705 0.8108 0.3465 0.9659 0.2829 1.0000 0.2705 0.8108 0.3100 0.7896 0.3465 0.9659 0.2295 0.8108 0.2829 1.0000 0.2171 1.0000 0.2295 0.8108 0.2705 0.8108 0.2829 1.0000 0.7171 0.0000 0.7705 0.1892 0.7295 0.1892 0.7171 0.0000 0.7829 0.0000 0.7705 0.1892 0.6535 0.0341 0.7295 0.1892 0.6900 0.2104 0.6535 0.0341 0.7171 0.0000 0.7295 0.1892 0.1900 0.7896 0.2171 1.0000 0.1535 0.9659 0.1900 0.7896 0.2295 0.8108 0.2171 1.0000 0.1546 0.7487 0.1535 0.9659 0.0965 0.9001 0.1546 0.7487 0.1900 0.7896 0.1535 0.9659 0.5965 0.0999 0.6900 0.2104 0.6546 0.2513 0.5965 0.0999 0.6535 0.0341 0.6900 0.2104 0.5500 0.1930 0.6546 0.2513 0.6257 0.3092 0.5500 0.1930 0.5965 0.0999 0.6546 0.2513 0.1257 0.6908 0.0965 0.9001 0.0499 0.8070 0.1257 0.6908 0.1546 0.7487 0.0965 0.9001 0.1052 0.6200 0.0499 0.8070 0.0170 0.6930 0.1052 0.6200 0.1257 0.6908 0.0499 0.8070 0.5170 0.3070 0.6257 0.3092 0.6052 0.3800 0.5170 0.3070 0.5500 0.1930 0.6257 0.3092 0.5000 0.4342 0.6052 0.3800 0.5946 0.4591 0.5000 0.4342 0.5170 0.3070 0.6052 0.3800 0.0946 0.5409 0.0170 0.6930 0.0000 0.5658 0.0946 0.5409 0.1052 0.6200 0.0170 0.6930 0.0946 0.4591 0.0000 0.5658 0.0000 0.4342 0.0946 0.4591 0.0946 0.5409 0.0000 0.5658 0.5000 0.5658 0.5946 0.4591 0.5946 0.5409 0.5000 0.5658 0.5000 0.4342 0.5946 0.4591 0.5170 0.6930 0.5946 0.5409 0.6052 0.6200 0.5170 0.6930 0.5000 0.5658 0.5946 0.5409 0.1052 0.3800 0.0000 0.4342 0.0170 0.3070 0.1052 0.3800 0.0946 0.4591 0.0000 0.4342 0.1257 0.3092 0.0170 0.3070 0.0500 0.1930 0.1257 0.3092 0.1052 0.3800 0.0170 0.3070 0.5500 0.8070 0.6052 0.6200 0.6257 0.6908 0.5500 0.8070 0.5170 0.6930 0.6052 0.6200 0.5965 0.9001 0.6257 0.6908 0.6546 0.7487 0.5965 0.9001 0.5500 0.8070 0.6257 0.6908 0.1546 0.2513 0.0500 0.1930 0.0965 0.0999 0.1546 0.2513 0.1257 0.3092 0.0500 0.1930 0.1900 0.2104 0.0965 0.0999 0.1535 0.0341 0.1900 0.2104 0.1546 0.2513 0.0965 0.0999 0.6535 0.9659 0.6546 0.7487 0.6900 0.7896 0.6535 0.9659 0.5965 0.9001 0.6546 0.7487 0.7171 1.0000 0.6900 0.7896 0.7295 0.8108 0.7171 1.0000 0.6535 0.9659 0.6900 0.7896 0.2295 0.1892 0.1535 0.0341 0.2171 0.0000 0.2295 0.1892 0.1900 0.2104 0.1535 0.0341 0.2705 0.1892 0.2171 0.0000 0.2829 0.0000 0.2705 0.1892 0.2295 0.1892 0.2171 0.0000 0.7829 1.0000 0.7295 0.8108 0.7705 0.8108 0.7829 1.0000 0.7171 1.0000 0.7295 0.8108 0.8465 0.9659 0.7705 0.8108 0.8100 0.7896 0.8465 0.9659 0.7829 1.0000 0.7705 0.8108 0.3100 0.2104 0.2829 0.0000 0.3465 0.0341 0.3100 0.2104 0.2705 0.1892 0.2829 0.0000 0.3454 0.2513 0.3465 0.0341 0.4035 0.0999 0.3454 0.2513 0.3100 0.2104 0.3465 0.0341 0.9035 0.9001 0.8100 0.7896 0.8454 0.7487 0.9035 0.9001 0.8465 0.9659 0.8100 0.7896 0.9501 0.8070 0.8454 0.7487 0.8743 0.6908 0.9501 0.8070 0.9035 0.9001 0.8454 0.7487 0.3743 0.3092 0.4035 0.0999 0.4501 0.1930 0.3743 0.3092 0.3454 0.2513 0.4035 0.0999 0.3948 0.3800 0.4501 0.1930 0.4830 0.3070 0.3948 0.3800 0.3743 0.3092 0.4501 0.1930 0.9830 0.6930 0.8743 0.6908 0.8948 0.6200 0.9830 0.6930 0.9501 0.8070 0.8743 0.6908 1.0000 0.5658 0.8948 0.6200 0.9054 0.5409 1.0000 0.5658 0.9830 0.6930 0.8948 0.6200 0.4054 0.4591 0.4830 0.3070 0.5000 0.4342 0.4054 0.4591 0.3948 0.3800 0.4830 0.3070 ] + } + texCoordIndex [ 0 1 2 -1 3 4 5 -1 6 7 8 -1 9 10 11 -1 12 13 14 -1 15 16 17 -1 18 19 20 -1 21 22 23 -1 24 25 26 -1 27 28 29 -1 30 31 32 -1 33 34 35 -1 36 37 38 -1 39 40 41 -1 42 43 44 -1 45 46 47 -1 48 49 50 -1 51 52 53 -1 54 55 56 -1 57 58 59 -1 60 61 62 -1 63 64 65 -1 66 67 68 -1 69 70 71 -1 72 73 74 -1 75 76 77 -1 78 79 80 -1 81 82 83 -1 84 85 86 -1 87 88 89 -1 90 91 92 -1 93 94 95 -1 96 97 98 -1 99 100 101 -1 102 103 104 -1 105 106 107 -1 108 109 110 -1 111 112 113 -1 114 115 116 -1 117 118 119 -1 120 121 122 -1 123 124 125 -1 126 127 128 -1 129 130 131 -1 132 133 134 -1 135 136 137 -1 138 139 140 -1 141 142 143 -1 144 145 146 -1 147 148 149 -1 150 151 152 -1 153 154 155 -1 156 157 158 -1 159 160 161 -1 162 163 164 -1 165 166 167 -1 168 169 170 -1 171 172 173 -1 174 175 176 -1 177 178 179 -1 180 181 182 -1 183 184 185 -1 186 187 188 -1 189 190 191 -1 192 193 194 -1 195 196 197 -1 198 199 200 -1 201 202 203 -1 204 205 206 -1 207 208 209 -1 210 211 212 -1 213 214 215 -1 216 217 218 -1 219 220 221 -1 222 223 224 -1 225 226 227 -1 228 229 230 -1 231 232 233 -1 234 235 236 -1 237 238 239 -1 240 241 242 -1 243 244 245 -1 246 247 248 -1 249 250 251 -1 252 253 254 -1 255 256 257 -1 258 259 260 -1 261 262 263 -1 264 265 266 -1 267 268 269 -1 270 271 272 -1 273 274 275 -1 276 277 278 -1 279 280 281 -1 282 283 284 -1 285 286 287 -1 ] + coordIndex [ 88 94 89 -1 88 95 94 -1 90 92 91 -1 90 93 92 -1 85 91 84 -1 85 90 91 -1 87 89 86 -1 87 88 89 -1 80 86 81 -1 80 87 86 -1 82 84 83 -1 82 85 84 -1 77 83 76 -1 77 82 83 -1 79 81 78 -1 79 80 81 -1 72 78 73 -1 72 79 78 -1 74 76 75 -1 74 77 76 -1 69 75 68 -1 69 74 75 -1 71 73 70 -1 71 72 73 -1 64 70 65 -1 64 71 70 -1 66 68 67 -1 66 69 68 -1 61 67 60 -1 61 66 67 -1 63 65 62 -1 63 64 65 -1 56 62 57 -1 56 63 62 -1 58 60 59 -1 58 61 60 -1 53 59 52 -1 53 58 59 -1 55 57 54 -1 55 56 57 -1 48 54 49 -1 48 55 54 -1 50 52 51 -1 50 53 52 -1 45 51 44 -1 45 50 51 -1 47 49 46 -1 47 48 49 -1 40 46 41 -1 40 47 46 -1 42 44 43 -1 42 45 44 -1 37 43 36 -1 37 42 43 -1 39 41 38 -1 39 40 41 -1 32 38 33 -1 32 39 38 -1 34 36 35 -1 34 37 36 -1 29 35 28 -1 29 34 35 -1 31 33 30 -1 31 32 33 -1 24 30 25 -1 24 31 30 -1 26 28 27 -1 26 29 28 -1 21 27 20 -1 21 26 27 -1 23 25 22 -1 23 24 25 -1 16 22 17 -1 16 23 22 -1 18 20 19 -1 18 21 20 -1 13 19 12 -1 13 18 19 -1 15 17 14 -1 15 16 17 -1 8 14 9 -1 8 15 14 -1 10 12 11 -1 10 13 12 -1 5 11 4 -1 5 10 11 -1 7 9 6 -1 7 8 9 -1 0 6 1 -1 0 7 6 -1 2 4 3 -1 2 5 4 -1 93 3 92 -1 93 2 3 -1 95 1 94 -1 95 0 1 -1 ] + } + } + ] + } + DEF LEFT_CAP_AND_VALVE_SHAPES Group { + children [ + DEF LEFT_CAP_SHAPE Shape { + appearance PBRAppearance { + baseColor 0 0 0 + } + geometry IndexedFaceSet { + coord Coordinate { + point [ + -0.042613 0.0264202 0.0226794, -0.042613 0.0256074 0.0238958 -0.042613 0.025322 0.0253307, -0.042613 0.0256074 0.0267656 -0.042613 0.0264202 0.0279821, -0.042613 0.0276366 0.0287949 -0.042613 0.0290715 0.0290803, -0.042613 0.0305064 0.0287949 -0.042613 0.0317229 0.0279821, -0.042613 0.0325357 0.0267656 -0.042613 0.0328211 0.0253307, -0.042613 0.0325357 0.0238958 -0.042613 0.0317229 0.0226794, -0.042613 0.0305064 0.0218666 -0.042613 0.0290715 0.0215812, -0.042613 0.0276366 0.0218666 -0.038451 0.0264202 0.0226794, -0.038451 0.0256074 0.0238959 -0.038451 0.025322 0.0253308, -0.038451 0.0256074 0.0267657 -0.038451 0.0264202 0.0279821, -0.038451 0.0276366 0.0287949 -0.038451 0.0290715 0.0290803, -0.038451 0.0305064 0.0287949 -0.038451 0.0317229 0.0279821, -0.038451 0.0325357 0.0267656 -0.038451 0.0328211 0.0253307, -0.038451 0.0325357 0.0238958 -0.038451 0.0317229 0.0226794, -0.038451 0.0305064 0.0218666 -0.038451 0.0290715 0.0215812, -0.038451 0.0276366 0.0218666 -0.038451 0.0290715 0.0253307, -0.0454428 0.0290715 0.0253307 -0.0454428 0.0290715 0.0235262, -0.0454428 0.0303476 0.0240547 -0.0454428 0.0308761 0.0253307, -0.0454428 0.0303476 0.0266068 -0.0454428 0.0290715 0.0271353, -0.0454428 0.0277955 0.0266068 -0.0454428 0.027267 0.0253307, -0.0454428 0.0277955 0.0240547 + ] + } + coordIndex [ + 32 17 16 -1 32 18 17 -1 32 19 18 -1 32 20 19 -1 32 21 20 -1 32 22 21 -1 32 23 22 -1 32 24 23 -1 32 25 24 -1 32 26 25 -1 32 27 26 -1 32 28 27 -1 32 29 28 -1 32 30 29 -1 32 31 30 -1 32 16 31 -1 0 16 17 1 -1 1 17 18 2 -1 2 18 19 3 -1 3 19 20 4 -1 4 20 21 5 -1 5 21 22 6 -1 6 22 23 7 -1 7 23 24 8 -1 8 24 25 9 -1 9 25 26 10 -1 10 26 27 11 -1 11 27 28 12 -1 12 28 29 13 -1 13 29 30 14 -1 14 30 31 15 -1 16 0 15 31 -1 15 0 41 -1 14 15 41 34 -1 13 14 34 -1 12 13 34 35 -1 11 12 35 36 -1 10 11 36 -1 9 10 36 37 -1 8 9 37 -1 7 8 37 38 -1 6 7 38 -1 5 6 38 39 -1 4 5 39 -1 3 4 39 -1 2 3 39 40 -1 1 2 40 -1 0 1 40 41 -1 33 41 40 -1 33 40 39 -1 33 39 38 -1 33 38 37 -1 33 37 36 -1 33 36 35 -1 33 35 34 -1 33 34 41 -1 + ] + creaseAngle 1 + } + } + DEF LEFT_VALVE_SHAPE Shape { + appearance PBRAppearance { + baseColor 0.95894 0.8 0 + roughness 0.3 + metalness 0 + } + geometry IndexedFaceSet { + coord Coordinate { + point [ + -0.0153188 0.0271786 0.0234378, -0.0153188 0.0265983 0.0243063 -0.0153188 0.0263945 0.0253308, -0.0153188 0.0271786 0.0272237 -0.0153188 0.0290715 0.0280078, -0.0153188 0.030096 0.027804 -0.0153188 0.0309645 0.0272237, -0.0153188 0.0315448 0.0263552 -0.0153188 0.0317485 0.0253308, -0.0153188 0.0315448 0.0243063 -0.0153188 0.030096 0.0228575, -0.0153188 0.0290715 0.0226537 -0.0153188 0.0280471 0.0228575, -0.0401998 0.0271786 0.0234378 -0.0401998 0.0265983 0.0243063, -0.0401998 0.0263945 0.0253308 -0.0401998 0.0271786 0.0272237, -0.0401998 0.0290715 0.0280078 -0.0401998 0.030096 0.027804, -0.0401998 0.0309645 0.0272237 -0.0401998 0.0315448 0.0263552, -0.0401998 0.0317485 0.0253307 -0.0401998 0.0315448 0.0243063, -0.0401998 0.030096 0.0228575 -0.0401998 0.0290715 0.0226537, -0.0401998 0.0280471 0.0228575 + ] + } + coordIndex [ + 0 1 14 13 -1 1 2 15 14 -1 2 3 16 15 -1 3 4 17 16 -1 4 5 18 17 -1 5 6 19 18 -1 6 7 20 19 -1 7 8 21 20 -1 8 9 22 21 -1 9 10 23 22 -1 10 11 24 23 -1 11 12 25 24 -1 12 0 13 25 -1 + ] + creaseAngle 1 + } + } + ] + } + ] + name "back left wheel" + boundingObject DEF BOUNDING_WHEEL Transform { + rotation 0 0 1 1.5708 + children [ + Cylinder { + height 0.095 + radius 0.11 + subdivision 24 + } + ] + } + physics DEF WHEEL_PHYSICS Physics { + density -1 + mass 0.8 + } + } + } + DEF FRONT_BLACK_PANEL Shape { + appearance PBRAppearance { + baseColor 0.06 0.06 0.06 + metalness 0 + roughness 0.3 + } + geometry IndexedFaceSet { + coord Coordinate { + point [ + -0.0068561 -0.0299255 -0.214614, 0.0743534 -0.0299255 -0.214614 0.0749096 0.00276717 -0.214614, -0.00685611 0.00276717 -0.214614 -0.00685611 0.00276717 -0.209428, 0.0749096 0.00276717 -0.209428 0.0743534 -0.0299255 -0.209428, -0.0068561 -0.0299255 -0.209428 + ] + } + coordIndex [ + 5 4 6 -1 4 7 6 -1 7 0 1 -1 7 1 6 -1 2 5 6 -1 2 6 1 -1 3 4 5 -1 3 5 2 -1 0 7 4 -1 0 4 3 -1 0 3 1 -1 3 2 1 -1 + ] + } + } + DEF BACK_RIGHT_WHEEL HingeJoint { + device [ + RotationalMotor { + name "back right wheel" + maxVelocity 6.4 + maxTorque 20 + } + PositionSensor { + name "back right wheel sensor" + } + ] + jointParameters HingeJointParameters { + axis -1 0 0 + anchor 0.197 0 0.1331 + } + endPoint Solid { + translation 0.197 0 0.1331 + rotation -1 0 0 0 + children [ + USE RIM_AND_TIRE_SHAPES + DEF RIGHT_CAP_AND_VALVE_SHAPES Group { + children [ + DEF RIGHT_CAP_SHAPE Shape { + appearance PBRAppearance { + baseColor 0 0 0 + metalness 0 + roughness 0.3 + } + geometry IndexedFaceSet { + coord Coordinate { + point [ + 0.0405424 0.0264202 0.0226794, 0.0405424 0.0256074 0.0238959 0.0405424 0.025322 0.0253308, 0.0405424 0.0256074 0.0267657 0.0405424 0.0264202 0.0279821, 0.0405424 0.0276366 0.0287949 0.0405424 0.0290715 0.0290803, 0.0405424 0.0305064 0.0287949 0.0405424 0.0317229 0.0279821, 0.0405424 0.0325357 0.0267657 0.0405424 0.0328211 0.0253308, 0.0405424 0.0325357 0.0238959 0.0405424 0.0317229 0.0226794, 0.0405424 0.0305064 0.0218666 0.0405424 0.0290715 0.0215812, 0.0405424 0.0276366 0.0218666 0.0363803 0.0264202 0.0226794, 0.0363803 0.0256074 0.0238959 0.0363803 0.025322 0.0253308, 0.0363803 0.0256074 0.0267657 0.0363803 0.0264202 0.0279821, 0.0363803 0.0276366 0.0287949 0.0363803 0.0290715 0.0290803, 0.0363803 0.0305064 0.0287949 0.0363803 0.0317229 0.0279821, 0.0363803 0.0325357 0.0267657 0.0363803 0.0328211 0.0253308, 0.0363803 0.0325357 0.0238959 0.0363803 0.0317229 0.0226794, 0.0363803 0.0305064 0.0218666 0.0363803 0.0290715 0.0215812, 0.0363803 0.0276366 0.0218666 0.0363803 0.0290715 0.0253308, 0.0433722 0.0290715 0.0253308 0.0433722 0.0290715 0.0235262, 0.0433722 0.0303476 0.0240547 0.0433722 0.0308761 0.0253308, 0.0433722 0.0303476 0.0266068 0.0433722 0.0290715 0.0271353, 0.0433722 0.0277955 0.0266068 0.0433722 0.027267 0.0253308, 0.0433722 0.0277955 0.0240547 + ] + } + coordIndex [ + 32 16 17 -1 32 17 18 -1 32 18 19 -1 32 19 20 -1 32 20 21 -1 32 21 22 -1 32 22 23 -1 32 23 24 -1 32 24 25 -1 32 25 26 -1 32 26 27 -1 32 27 28 -1 32 28 29 -1 32 29 30 -1 32 30 31 -1 32 31 16 -1 0 1 17 16 -1 1 2 18 17 -1 2 3 19 18 -1 3 4 20 19 -1 4 5 21 20 -1 5 6 22 21 -1 6 7 23 22 -1 7 8 24 23 -1 8 9 25 24 -1 9 10 26 25 -1 10 11 27 26 -1 11 12 28 27 -1 12 13 29 28 -1 13 14 30 29 -1 14 15 31 30 -1 15 0 16 31 -1 41 0 15 -1 14 34 41 15 -1 13 34 14 -1 12 35 34 13 -1 11 36 35 12 -1 10 36 11 -1 9 37 36 10 -1 8 37 9 -1 7 38 37 8 -1 6 38 7 -1 5 39 38 6 -1 4 39 5 -1 3 39 4 -1 2 40 39 3 -1 1 40 2 -1 0 41 40 1 -1 33 40 41 -1 33 39 40 -1 33 38 39 -1 33 37 38 -1 33 36 37 -1 33 35 36 -1 33 34 35 -1 33 41 34 -1 + ] + creaseAngle 1 + } + } + DEF RIGHT_VALVE_SHAPE Shape { + appearance PBRAppearance { + baseColor 0.95894 0.8 0 + metalness 0 + roughness 0.3 + } + geometry IndexedFaceSet { + coord Coordinate { + point [ + 0.0132481 0.0271786 0.0234378, 0.0132481 0.0265983 0.0243063 0.0132481 0.0263945 0.0253308, 0.0132481 0.0271786 0.0272237 0.0132481 0.0290715 0.0280078, 0.0132481 0.030096 0.027804 0.0132481 0.0309645 0.0272237, 0.0132481 0.0315448 0.0263552 0.0132481 0.0317485 0.0253308, 0.0132481 0.0315448 0.0243063 0.0132481 0.030096 0.0228575, 0.0132481 0.0290715 0.0226538 0.0132481 0.0280471 0.0228575, 0.0381291 0.0271786 0.0234378 0.0381291 0.0265983 0.0243063, 0.0381291 0.0263945 0.0253308 0.0381291 0.0271786 0.0272237, 0.0381291 0.0290715 0.0280078 0.0381291 0.030096 0.027804, 0.0381291 0.0309645 0.0272237 0.0381291 0.0315448 0.0263552, 0.0381291 0.0317485 0.0253308 0.0381291 0.0315448 0.0243063, 0.0381291 0.030096 0.0228575 0.0381291 0.0290715 0.0226538, 0.0381291 0.0280471 0.0228575 + ] + } + coordIndex [ + 0 13 14 1 -1 1 14 15 2 -1 2 15 16 3 -1 3 16 17 4 -1 4 17 18 5 -1 5 18 19 6 -1 6 19 20 7 -1 7 20 21 8 -1 8 21 22 9 -1 9 22 23 -1 9 23 10 -1 10 23 24 11 -1 11 24 25 12 -1 13 0 12 25 -1 + ] + creaseAngle 1 + } + } + ] + } + ] + name "back right wheel" + boundingObject USE BOUNDING_WHEEL + physics USE WHEEL_PHYSICS + } + } + DEF FRONT_LEFT_WHEEL HingeJoint { + device [ + RotationalMotor { + name "front left wheel" + maxVelocity 6.4 + maxTorque 20 + } + PositionSensor { + name "front left wheel sensor" + } + ] + jointParameters HingeJointParameters { + axis -1 0 0 + anchor -0.197 0 -0.1331 + } + endPoint Solid { + translation -0.197 0 -0.1331 + rotation -1 0 0 0 + children [ + USE RIM_AND_TIRE_SHAPES + USE LEFT_CAP_AND_VALVE_SHAPES + ] + name "front left wheel" + boundingObject USE BOUNDING_WHEEL + physics USE WHEEL_PHYSICS + } + } + DEF FRONT_RIGHT_WHEEL HingeJoint { + device [ + RotationalMotor { + name "front right wheel" + maxVelocity 6.4 + maxTorque 20 + } + PositionSensor { + name "front right wheel sensor" + } + ] + jointParameters HingeJointParameters { + axis -1 0 0 + anchor 0.197 0 -0.1331 + } + endPoint Solid { + translation 0.197 0 -0.1331 + rotation -1 0 0 0 + children [ + USE RIM_AND_TIRE_SHAPES + USE RIGHT_CAP_AND_VALVE_SHAPES + ] + name "front right wheel" + boundingObject USE BOUNDING_WHEEL + physics USE WHEEL_PHYSICS + } + } + DEF SO_0 Pioneer3DistanceSensor { + translation -0.135387 0.137 -0.146604 + rotation 0 -1 0 3.14159 + name "so0" + } + DEF SO_1 Pioneer3DistanceSensor { + translation -0.117971 0.137 -0.194455 + rotation 0 1 0 2.44346 + name "so1" + } + DEF SO_10 Pioneer3DistanceSensor { + translation 0.0769827 0.137 0.226776 + rotation 0 -1 0 1.0472 + name "so10" + } + DEF SO_11 Pioneer3DistanceSensor { + translation 0.0266877 0.137 0.244814 + rotation 0 -1 0 1.39626 + name "so11" + } + DEF SO_12 Pioneer3DistanceSensor { + translation -0.0267158 0.137 0.244668 + rotation 0 -1 0 1.74533 + name "so12" + } + DEF SO_13 Pioneer3DistanceSensor { + translation -0.0769749 0.137 0.226483 + rotation 0 -1 0 2.0944 + name "so13" + } + DEF SO_14 Pioneer3DistanceSensor { + translation -0.117967 0.137 0.192247 + rotation 0 -1 0 2.44346 + name "so14" + } + DEF SO_15 Pioneer3DistanceSensor { + translation -0.135383 0.137 0.144395 + rotation 0 -1 0 3.14159 + name "so15" + } + DEF SO_2 Pioneer3DistanceSensor { + translation -0.0769789 0.137 -0.228691 + rotation 0 1 0 2.0944 + name "so2" + } + DEF SO_3 Pioneer3DistanceSensor { + translation -0.0267197 0.137 -0.246876 + rotation 0 1 0 1.74533 + name "so3" + } + DEF SO_4 Pioneer3DistanceSensor { + translation 0.0266839 0.137 -0.247023 + rotation 0 1 0 1.39626 + name "so4" + } + DEF SO_5 Pioneer3DistanceSensor { + translation 0.0769787 0.137 -0.228985 + rotation 0 1 0 1.0472 + name "so5" + } + DEF SO_6 Pioneer3DistanceSensor { + translation 0.117971 0.137 -0.194749 + rotation 0 1 0 0.698132 + name "so6" + } + DEF SO_7 Pioneer3DistanceSensor { + translation 0.135387 0.137 -0.146897 + name "so7" + } + DEF SO_8 Pioneer3DistanceSensor { + translation 0.135391 0.137 0.144689 + name "so8" + } + DEF SO_9 Pioneer3DistanceSensor { + translation 0.117975 0.137 0.19254 + rotation 0 -1 0 0.698131 + name "so9" + } + DEF SONAR_RING_SHAPE Shape { + appearance PBRAppearance { + baseColor 1 0 0 + metalness 0 + roughness 0.3 + } + geometry IndexedFaceSet { + coord Coordinate { + point [ + 0.135387 0.112153 -0.119802, 0.135387 0.112153 -0.173993 0.100554 0.112153 -0.215505, 0.0534035 0.112153 -0.242465 -3.57e-05 0.112153 -0.251581, -0.0534037 0.112153 -0.242171 -0.100554 0.112153 -0.215212, -0.135387 0.112153 -0.173699 -0.135387 0.112153 -0.119508, -0.135387 0.162005 -0.119508 -0.135387 0.162005 -0.173699, -0.100554 0.162005 -0.215212 -0.0534037 0.162005 -0.242171, -3.57e-05 0.162005 -0.251581 0.0534035 0.162005 -0.242465, 0.100554 0.162005 -0.215505 0.135387 0.162005 -0.173993, 0.135387 0.162005 -0.119802 0.135391 0.162005 0.117593, 0.135391 0.162005 0.171784 0.100558 0.162005 0.213297, 0.0534075 0.162005 0.240256 -3.2e-05 0.162005 0.249373, -0.0533997 0.162005 0.239962 -0.10055 0.162005 0.213003, -0.135383 0.162005 0.17149 -0.135383 0.162005 0.1173, -0.135383 0.112153 0.1173 -0.135383 0.112153 0.17149, -0.10055 0.112153 0.213003 -0.0533997 0.112153 0.239962, -3.2e-05 0.112153 0.249373 0.0534075 0.112153 0.240256, 0.100558 0.112153 0.213297 0.135391 0.112153 0.171784, 0.135391 0.112153 0.117593 + ] + } + coordIndex [ + 3 5 4 -1 2 5 3 -1 2 6 5 -1 1 6 2 -1 1 7 6 -1 0 7 1 -1 0 8 7 -1 0 17 9 8 -1 4 13 14 3 -1 7 10 11 6 -1 5 12 13 4 -1 6 11 12 5 -1 8 9 10 7 -1 17 0 1 16 -1 3 14 15 2 -1 2 15 16 1 -1 33 34 19 20 -1 32 33 20 21 -1 34 35 18 19 -1 27 28 25 26 -1 29 30 23 24 -1 30 31 22 23 -1 28 29 24 25 -1 31 32 21 22 -1 35 27 26 18 -1 35 28 27 -1 35 34 28 -1 34 29 28 -1 34 33 29 -1 33 30 29 -1 33 32 30 -1 32 31 30 -1 + ] + } + } + DEF TOP_SHAPE Shape { + appearance PBRAppearance { + roughness 0.3 + metalness 0 + baseColorMap ImageTexture { + url [ + "https://raw.githubusercontent.com/cyberbotics/webots/R2021a/projects/robots/adept/pioneer3/protos/textures/top_3at_base_color.jpg" + ] + } + } + geometry IndexedFaceSet { + coord Coordinate { + point [ + -0.161502 0.165434 0.0273416, -0.187667 0.165434 -0.0087482 -0.187667 0.165434 -0.139574, -0.181351 0.165434 -0.157619 -0.130826 0.165434 -0.209949, -0.0757887 0.165434 -0.239723 0.0013534 0.165434 -0.255061, -0.161502 0.165434 0.168092 -0.151577 0.165434 0.187942, -0.0775932 0.165434 0.237565 0.0013534 0.165434 0.254244, 0.0802999 0.165434 0.237565 0.154284 0.165434 0.187941, 0.164209 0.165434 0.168092 0.0784954 0.165434 -0.239723, 0.133532 0.165434 -0.209949 0.184058 0.165434 -0.157619, 0.190374 0.165434 -0.139574 0.190374 0.165434 -0.0087482, 0.164209 0.165434 0.0273416 0.164209 0.161275 0.0273416, 0.190374 0.161275 -0.0087482 0.190374 0.161275 -0.139574, 0.184058 0.161275 -0.157619 0.133532 0.161275 -0.209949, 0.0784954 0.161275 -0.239723 0.164209 0.161275 0.168092, 0.154284 0.161275 0.187941 0.0802999 0.161275 0.237565, 0.0013534 0.161275 0.254244 -0.0775932 0.161275 0.237565, -0.151577 0.161275 0.187942 -0.161502 0.161275 0.168092, 0.0013534 0.161275 -0.255061 -0.0757887 0.161275 -0.239723, -0.130826 0.161275 -0.209949 -0.181351 0.161275 -0.157619, -0.187667 0.161275 -0.139574 -0.187667 0.161275 -0.0087482, -0.161502 0.161275 0.0273416 + ] + } + texCoord TextureCoordinate { + point [ + 0.661139 0.955673, 0.50258 0.98603, 0.344021 0.955673 0.661139 0.955673, 0.344021 0.955673, 0.230898 0.896745 0.774262 0.896745, 0.661139 0.955673, 0.230898 0.896745 0.774262 0.896745, 0.230898 0.896745, 0.127046 0.793173 0.878114 0.793173, 0.774262 0.896745, 0.127046 0.793173 0.878114 0.793173, 0.127046 0.793173, 0.114065 0.757459 0.891095 0.757459, 0.878114 0.793173, 0.114065 0.757459 0.891095 0.757459, 0.114065 0.757459, 0.114065 0.49853 0.891095 0.49853, 0.891095 0.757459, 0.114065 0.49853 0.114065 0.49853, 0.167845 0.427102, 0.891095 0.49853 0.891095 0.49853, 0.167845 0.427102, 0.837315 0.427102 0.837315 0.427102, 0.167845 0.427102, 0.167845 0.14853 0.837315 0.14853, 0.837315 0.427102, 0.167845 0.14853 0.837315 0.14853, 0.167845 0.14853, 0.188244 0.109244 0.816916 0.109244, 0.837315 0.14853, 0.188244 0.109244 0.664848 0.0110302, 0.816916 0.109244, 0.188244 0.109244 0.664848 0.0110302, 0.188244 0.109244, 0.340312 0.0110302 0.664848 0.0110302, 0.340312 0.0110302, 0.50258 0.0023283 0.1032 0.92567, 0.13135 0.92567, 0.13135 0.961898 0.1032 0.92567, 0.13135 0.92567, 0.13135 0.961898 0.1032 0.92567, 0.13135 0.92567, 0.13135 0.961898 0.1032 0.92567, 0.13135 0.92567, 0.13135 0.961898 0.1032 0.92567, 0.13135 0.92567, 0.13135 0.961898 0.1032 0.92567, 0.13135 0.92567, 0.13135 0.961898 0.1032 0.92567, 0.13135 0.92567, 0.13135 0.961898 0.1032 0.92567, 0.13135 0.92567, 0.13135 0.961898 0.1032 0.92567, 0.13135 0.92567, 0.13135 0.961898 0.1032 0.92567, 0.13135 0.92567, 0.13135 0.961898 0.1032 0.92567, 0.13135 0.92567, 0.13135 0.961898 0.1032 0.92567, 0.13135 0.92567, 0.13135 0.961898 0.1032 0.92567, 0.13135 0.92567, 0.13135 0.961898 0.1032 0.92567, 0.13135 0.92567, 0.13135 0.961898 0.1032 0.92567, 0.13135 0.92567, 0.13135 0.961898 0.1032 0.92567, 0.13135 0.92567, 0.13135 0.961898 0.1032 0.92567, 0.13135 0.92567, 0.13135 0.961898 0.1032 0.92567, 0.13135 0.92567, 0.13135 0.961898 0.167845 0.427102, 0.114065 0.49853, 0.114065 0.49853 0.167845 0.427102, 0.114065 0.49853, 0.167845 0.427102 0.114065 0.49853, 0.114065 0.757459, 0.114065 0.757459 0.114065 0.49853, 0.114065 0.757459, 0.114065 0.49853 0.114065 0.757459, 0.127046 0.793173, 0.127046 0.793173 0.114065 0.757459, 0.127046 0.793173, 0.114065 0.757459 0.127046 0.793173, 0.230898 0.896745, 0.230898 0.896745 0.127046 0.793173, 0.230898 0.896745, 0.127046 0.793173 0.230898 0.896745, 0.344021 0.955673, 0.344021 0.955673 0.230898 0.896745, 0.344021 0.955673, 0.230898 0.896745 0.344021 0.955673, 0.50258 0.98603, 0.50258 0.98603 0.344021 0.955673, 0.50258 0.98603, 0.344021 0.955673 0.188244 0.109244, 0.167845 0.14853, 0.167845 0.14853 0.188244 0.109244, 0.167845 0.14853, 0.188244 0.109244 0.340312 0.0110302, 0.188244 0.109244, 0.188244 0.109244 0.340312 0.0110302, 0.188244 0.109244, 0.340312 0.0110302 0.50258 0.0023283, 0.340312 0.0110302, 0.340312 0.0110302 0.50258 0.0023283, 0.340312 0.0110302, 0.50258 0.0023283 0.167845 0.14853, 0.167845 0.427102, 0.167845 0.427102 0.167845 0.14853, 0.167845 0.427102, 0.167845 0.14853 0.837315 0.427102, 0.837315 0.14853, 0.837315 0.14853 0.837315 0.427102, 0.837315 0.14853, 0.837315 0.427102 0.816916 0.109244, 0.664848 0.0110302, 0.664848 0.0110302 0.816916 0.109244, 0.664848 0.0110302, 0.816916 0.109244 0.837315 0.14853, 0.816916 0.109244, 0.816916 0.109244 0.837315 0.14853, 0.816916 0.109244, 0.837315 0.14853 0.661139 0.955673, 0.774262 0.896745, 0.774262 0.896745 0.661139 0.955673, 0.774262 0.896745, 0.661139 0.955673 0.774262 0.896745, 0.878114 0.793173, 0.878114 0.793173 0.774262 0.896745, 0.878114 0.793173, 0.774262 0.896745 0.878114 0.793173, 0.891095 0.757459, 0.891095 0.757459 0.878114 0.793173, 0.891095 0.757459, 0.878114 0.793173 0.891095 0.757459, 0.891095 0.49853, 0.891095 0.49853 0.891095 0.757459, 0.891095 0.49853, 0.891095 0.757459 0.891095 0.49853, 0.837315 0.427102, 0.837315 0.427102 0.891095 0.49853, 0.837315 0.427102, 0.891095 0.49853 0.50258 0.98603, 0.661139 0.955673, 0.661139 0.955673 0.50258 0.98603, 0.661139 0.955673, 0.50258 0.98603 0.664848 0.0110302, 0.50258 0.0023283, 0.50258 0.0023283 0.664848 0.0110302, 0.50258 0.0023283, 0.664848 0.0110302 + ] + } + coordIndex [ + 14 6 5 -1 14 5 4 -1 15 14 4 -1 15 4 3 -1 16 15 3 -1 16 3 2 -1 17 16 2 -1 17 2 1 -1 18 17 1 -1 1 0 18 -1 18 0 19 -1 19 0 7 -1 13 19 7 -1 13 7 8 -1 12 13 8 -1 11 12 8 -1 11 8 9 -1 11 9 10 -1 34 33 25 -1 35 34 25 -1 35 25 24 -1 36 35 24 -1 36 24 23 -1 37 36 23 -1 37 23 22 -1 38 37 22 -1 38 22 21 -1 39 38 21 -1 39 21 20 -1 32 39 20 -1 32 20 26 -1 31 32 26 -1 31 26 27 -1 31 27 28 -1 30 31 28 -1 29 30 28 -1 0 1 38 -1 0 38 39 -1 1 2 37 -1 1 37 38 -1 2 3 36 -1 2 36 37 -1 3 4 35 -1 3 35 36 -1 4 5 34 -1 4 34 35 -1 5 6 33 -1 5 33 34 -1 8 7 32 -1 8 32 31 -1 9 8 31 -1 9 31 30 -1 10 9 30 -1 10 30 29 -1 7 0 39 -1 7 39 32 -1 19 13 26 -1 19 26 20 -1 12 11 28 -1 12 28 27 -1 13 12 27 -1 13 27 26 -1 14 15 24 -1 14 24 25 -1 15 16 23 -1 15 23 24 -1 16 17 22 -1 16 22 23 -1 17 18 21 -1 17 21 22 -1 18 19 20 -1 18 20 21 -1 6 14 25 -1 6 25 33 -1 11 10 29 -1 11 29 28 -1 + ] + texCoordIndex [ + 0 1 2 -1 3 4 5 -1 6 7 8 -1 9 10 11 -1 12 13 14 -1 15 16 17 -1 18 19 20 -1 21 22 23 -1 24 25 26 -1 27 28 29 -1 30 31 32 -1 33 34 35 -1 36 37 38 -1 39 40 41 -1 42 43 44 -1 45 46 47 -1 48 49 50 -1 51 52 53 -1 54 55 56 -1 57 58 59 -1 60 61 62 -1 63 64 65 -1 66 67 68 -1 69 70 71 -1 72 73 74 -1 75 76 77 -1 78 79 80 -1 81 82 83 -1 84 85 86 -1 87 88 89 -1 90 91 92 -1 93 94 95 -1 96 97 98 -1 99 100 101 -1 102 103 104 -1 105 106 107 -1 108 109 110 -1 111 112 113 -1 114 115 116 -1 117 118 119 -1 120 121 122 -1 123 124 125 -1 126 127 128 -1 129 130 131 -1 132 133 134 -1 135 136 137 -1 138 139 140 -1 141 142 143 -1 144 145 146 -1 147 148 149 -1 150 151 152 -1 153 154 155 -1 156 157 158 -1 159 160 161 -1 162 163 164 -1 165 166 167 -1 168 169 170 -1 171 172 173 -1 174 175 176 -1 177 178 179 -1 180 181 182 -1 183 184 185 -1 186 187 188 -1 189 190 191 -1 192 193 194 -1 195 196 197 -1 198 199 200 -1 201 202 203 -1 204 205 206 -1 207 208 209 -1 210 211 212 -1 213 214 215 -1 216 217 218 -1 219 220 221 -1 222 223 224 -1 225 226 227 -1 + ] + creaseAngle 0.6 + } + } + DEF ANTENNAS_SHAPES Shape { + appearance PBRAppearance { + baseColor 0.9 0.9 0.9 + roughness 0.3 + } + geometry IndexedFaceSet { + coord Coordinate { + point [ + -0.0783471 0.165559 0.0121679, -0.0769598 0.165559 0.0142442 -0.0764726 0.165559 0.0166934, -0.0769598 0.165559 0.0191425 -0.0783471 0.165559 0.0212188, -0.0804234 0.165559 0.0226062 -0.0828726 0.165559 0.0230934, -0.0853217 0.165559 0.0226062 -0.0873981 0.165559 0.0212188, -0.0887854 0.165559 0.0191425 -0.0892726 0.165559 0.0166934, -0.0887854 0.165559 0.0142442 -0.0873981 0.165559 0.0121679, -0.0853217 0.165559 0.0107805 -0.0828726 0.165559 0.0102934, -0.0804234 0.165559 0.0107805 -0.0804234 0.170194 0.0107805, -0.0828726 0.170194 0.0102934 -0.0853217 0.170194 0.0107805, -0.0873981 0.170194 0.0121679 -0.0887854 0.170194 0.0142442, -0.0892726 0.170194 0.0166934 -0.0887854 0.170194 0.0191425, -0.0873981 0.170194 0.0212188 -0.0853217 0.170194 0.0226062, -0.0828726 0.170194 0.0230934 -0.0804234 0.170194 0.0226062, -0.0783471 0.170194 0.0212188 -0.0769598 0.170194 0.0191425, -0.0764726 0.170194 0.0166934 -0.0769598 0.170194 0.0142442, -0.0783471 0.170194 0.0121679 -0.0798949 0.170194 0.0137156, -0.078982 0.170194 0.0150818 -0.0786614 0.170194 0.0166934, -0.078982 0.170194 0.0183049 -0.0798949 0.170194 0.0196711, -0.081261 0.170194 0.0205839 -0.0828726 0.170194 0.0209045, -0.0844841 0.170194 0.0205839 -0.0858503 0.170194 0.0196711, -0.0867632 0.170194 0.0183049 -0.0870837 0.170194 0.0166934, -0.0867632 0.170194 0.0150818 -0.0858503 0.170194 0.0137156, -0.0844841 0.170194 0.0128028 -0.0828726 0.170194 0.0124822, -0.081261 0.170194 0.0128028 -0.081261 0.172863 0.0128028, -0.0828726 0.172863 0.0124822 -0.0844841 0.172863 0.0128028, -0.0858503 0.172863 0.0137156 -0.0867632 0.172863 0.0150818, -0.0870837 0.172863 0.0166934 -0.0867632 0.172863 0.0183049, -0.0858503 0.172863 0.0196711 -0.0844841 0.172863 0.0205839, -0.0828726 0.172863 0.0209045 -0.081261 0.172863 0.0205839, -0.0798949 0.172863 0.0196711 -0.078982 0.172863 0.0183049, -0.0786614 0.172863 0.0166934 -0.078982 0.172863 0.0150818, -0.0798949 0.172863 0.0137156 -0.0817977 0.172863 0.0140985, -0.0828726 0.172863 0.0138847 -0.0839474 0.172863 0.0140985, -0.0848586 0.172863 0.0147073 -0.0854675 0.172863 0.0156185, -0.0856813 0.172863 0.0166934 -0.0854675 0.172863 0.0177682, -0.0848586 0.172863 0.0186794 -0.0839474 0.172863 0.0192882, -0.0828726 0.172863 0.019502 -0.0817977 0.172863 0.0192882, -0.0808865 0.172863 0.0186794 -0.0802777 0.172863 0.0177682, -0.0800639 0.172863 0.0166934 -0.0802777 0.172863 0.0156185, -0.0808865 0.172863 0.0147073 -0.0808865 0.178136 0.0147073, -0.0802777 0.178136 0.0156185 -0.0800639 0.178136 0.0166934, -0.0802777 0.178136 0.0177682 -0.0808865 0.178136 0.0186794, -0.0817977 0.178136 0.0192882 -0.0828726 0.178136 0.019502, -0.0839474 0.178136 0.0192882 -0.0848586 0.178136 0.0186794, -0.0854675 0.178136 0.0177682 -0.0856813 0.178136 0.0166934, -0.0854675 0.178136 0.0156185 -0.0848586 0.178136 0.0147073, -0.0839474 0.178136 0.0140985 -0.0828726 0.178136 0.0138847, -0.0817977 0.178136 0.0140985 -0.0799784 0.178136 0.0137992, -0.0790911 0.178136 0.015127 -0.0787795 0.178136 0.0166934, -0.0790911 0.178136 0.0182597 -0.0799784 0.178136 0.0195876, -0.0813062 0.178136 0.0204748 -0.0828726 0.178136 0.0207864, -0.0844389 0.178136 0.0204748 -0.0857668 0.178136 0.0195876, -0.086654 0.178136 0.0182597 -0.0869656 0.178136 0.0166934, -0.086654 0.178136 0.015127 -0.0857668 0.178136 0.0137992, -0.0844389 0.178136 0.0129119 -0.0828726 0.178136 0.0126003, -0.0813062 0.178136 0.0129119 -0.0813062 0.180074 0.0129119, -0.0828726 0.180074 0.0126003 -0.0844389 0.180074 0.0129119, -0.0857668 0.180074 0.0137992 -0.086654 0.180074 0.015127, -0.0869656 0.180074 0.0166934 -0.086654 0.180074 0.0182597, -0.0857668 0.180074 0.0195876 -0.0844389 0.180074 0.0204748, -0.0828726 0.180074 0.0207864 -0.0813062 0.180074 0.0204748, -0.0799784 0.180074 0.0195876 -0.0790911 0.180074 0.0182597, -0.0787795 0.180074 0.0166934 -0.0790911 0.180074 0.015127, -0.0799784 0.180074 0.0137992 -0.0783919 0.180074 0.0122126, -0.0770182 0.180074 0.0142684 -0.0765359 0.180074 0.0166934, -0.0770182 0.180074 0.0191183 -0.0783919 0.180074 0.0211741, -0.0804476 0.180074 0.0225477 -0.0828726 0.180074 0.0230301, -0.0852975 0.180074 0.0225477 -0.0873533 0.180074 0.0211741, -0.0887269 0.180074 0.0191183 -0.0892093 0.180074 0.0166934, -0.0887269 0.180074 0.0142684 -0.0873533 0.180074 0.0122126, -0.0852975 0.180074 0.010839 -0.0828726 0.180074 0.0103567, -0.0804476 0.180074 0.010839 -0.0804476 0.181528 0.010839, -0.0828726 0.181528 0.0103567 -0.0852975 0.181528 0.010839, -0.0873533 0.181528 0.0122126 -0.0887269 0.181528 0.0142684, -0.0892093 0.181528 0.0166934 -0.0887269 0.181528 0.0191183, -0.0873533 0.181528 0.0211741 -0.0852975 0.181528 0.0225477, -0.0828726 0.181528 0.0230301 -0.0804476 0.181528 0.0225477, -0.0783919 0.181528 0.0211741 -0.0770182 0.181528 0.0191183, -0.0765359 0.181528 0.0166934 -0.0770182 0.181528 0.0142684, -0.0783919 0.181528 0.0122126 0.0900723 0.165559 0.0125202, 0.0914596 0.165559 0.0145965 0.0919468 0.165559 0.0170457, 0.0914596 0.165559 0.0194949 0.0900723 0.165559 0.0215712, 0.087996 0.165559 0.0229585 0.0855468 0.165559 0.0234457, 0.0830976 0.165559 0.0229585 0.0810213 0.165559 0.0215712, 0.0796339 0.165559 0.0194949 0.0791468 0.165559 0.0170457, 0.0796339 0.165559 0.0145965 0.0810213 0.165559 0.0125202, 0.0830976 0.165559 0.0111329 0.0855468 0.165559 0.0106457, 0.087996 0.165559 0.0111329 0.087996 0.170194 0.0111329, 0.0855468 0.170194 0.0106457 0.0830976 0.170194 0.0111329, 0.0810213 0.170194 0.0125202 0.0796339 0.170194 0.0145965, 0.0791468 0.170194 0.0170457 0.0796339 0.170194 0.0194949, 0.0810213 0.170194 0.0215712 0.0830976 0.170194 0.0229585, 0.0855468 0.170194 0.0234457 0.087996 0.170194 0.0229585, 0.0900723 0.170194 0.0215712 0.0914596 0.170194 0.0194949, 0.0919468 0.170194 0.0170457 0.0914596 0.170194 0.0145965, 0.0900723 0.170194 0.0125202 0.0885245 0.170194 0.014068, 0.0894374 0.170194 0.0154342 0.0897579 0.170194 0.0170457, 0.0894374 0.170194 0.0186572 0.0885245 0.170194 0.0200234, 0.0871583 0.170194 0.0209363 0.0855468 0.170194 0.0212568, 0.0839353 0.170194 0.0209363 0.0825691 0.170194 0.0200234, 0.0816562 0.170194 0.0186572 0.0813356 0.170194 0.0170457, 0.0816562 0.170194 0.0154342 0.0825691 0.170194 0.014068, 0.0839353 0.170194 0.0131551 0.0855468 0.170194 0.0128346, 0.0871583 0.170194 0.0131551 0.0871583 0.172863 0.0131551, 0.0855468 0.172863 0.0128346 0.0839353 0.172863 0.0131551, 0.0825691 0.172863 0.014068 0.0816562 0.172863 0.0154342, 0.0813356 0.172863 0.0170457 0.0816562 0.172863 0.0186572, 0.0825691 0.172863 0.0200234 0.0839353 0.172863 0.0209363, 0.0855468 0.172863 0.0212568 0.0871583 0.172863 0.0209363, 0.0885245 0.172863 0.0200234 0.0894374 0.172863 0.0186572, 0.0897579 0.172863 0.0170457 0.0894374 0.172863 0.0154342, 0.0885245 0.172863 0.014068 0.0866216 0.172863 0.0144508, 0.0855468 0.172863 0.014237 0.0844719 0.172863 0.0144508, 0.0835607 0.172863 0.0150597 0.0829519 0.172863 0.0159709, 0.0827381 0.172863 0.0170457 0.0829519 0.172863 0.0181205, 0.0835607 0.172863 0.0190317 0.0844719 0.172863 0.0196406, 0.0855468 0.172863 0.0198544 0.0866216 0.172863 0.0196406, 0.0875328 0.172863 0.0190317 0.0881416 0.172863 0.0181205, 0.0883554 0.172863 0.0170457 0.0881416 0.172863 0.0159709, 0.0875328 0.172863 0.0150597 0.0875328 0.178136 0.0150597, 0.0881416 0.178136 0.0159709 0.0883554 0.178136 0.0170457, 0.0881416 0.178136 0.0181205 0.0875328 0.178136 0.0190317, 0.0866216 0.178136 0.0196406 0.0855468 0.178136 0.0198544, 0.0844719 0.178136 0.0196406 0.0835607 0.178136 0.0190317, 0.0829519 0.178136 0.0181205 0.0827381 0.178136 0.0170457, 0.0829519 0.178136 0.0159709 0.0835607 0.178136 0.0150597, 0.0844719 0.178136 0.0144508 0.0855468 0.178136 0.014237, 0.0866216 0.178136 0.0144508 0.088441 0.178136 0.0141515, 0.0893282 0.178136 0.0154794 0.0896398 0.178136 0.0170457, 0.0893282 0.178136 0.018612 0.088441 0.178136 0.0199399, 0.0871131 0.178136 0.0208272 0.0855468 0.178136 0.0211387, 0.0839804 0.178136 0.0208272 0.0826526 0.178136 0.0199399, 0.0817653 0.178136 0.018612 0.0814537 0.178136 0.0170457, 0.0817653 0.178136 0.0154794 0.0826526 0.178136 0.0141515, 0.0839804 0.178136 0.0132642 0.0855468 0.178136 0.0129527, 0.0871131 0.178136 0.0132642 0.0871131 0.180074 0.0132642, 0.0855468 0.180074 0.0129527 0.0839804 0.180074 0.0132642, 0.0826526 0.180074 0.0141515 0.0817653 0.180074 0.0154794, 0.0814537 0.180074 0.0170457 0.0817653 0.180074 0.018612, 0.0826526 0.180074 0.0199399 0.0839804 0.180074 0.0208272, 0.0855468 0.180074 0.0211387 0.0871131 0.180074 0.0208272, 0.088441 0.180074 0.0199399 0.0893282 0.180074 0.018612, 0.0896398 0.180074 0.0170457 0.0893282 0.180074 0.0154794, 0.088441 0.180074 0.0141515 0.0900275 0.180074 0.012565, 0.0914011 0.180074 0.0146208 0.0918835 0.180074 0.0170457, 0.0914011 0.180074 0.0194706 0.0900275 0.180074 0.0215264, 0.0879717 0.180074 0.0229 0.0855468 0.180074 0.0233824, 0.0831218 0.180074 0.0229001 0.0810661 0.180074 0.0215264, 0.0796924 0.180074 0.0194707 0.0792101 0.180074 0.0170457, 0.0796924 0.180074 0.0146207 0.0810661 0.180074 0.012565, 0.0831218 0.180074 0.0111914 0.0855468 0.180074 0.010709, 0.0879717 0.180074 0.0111914 0.0879717 0.181528 0.0111914, 0.0855468 0.181528 0.010709 0.0831218 0.181528 0.0111914, 0.0810661 0.181528 0.012565 0.0796924 0.181528 0.0146207, 0.0792101 0.181528 0.0170457 0.0796924 0.181528 0.0194707, 0.0810661 0.181528 0.0215264 0.0831218 0.181528 0.0229001, 0.0855468 0.181528 0.0233824 0.0879717 0.181528 0.0229, 0.0900275 0.181528 0.0215264 0.0914011 0.181528 0.0194706, 0.0918835 0.181528 0.0170457 0.0914011 0.181528 0.0146208, 0.0900275 0.181528 0.012565 + ] + } + coordIndex [ + 15 13 14 -1 15 12 13 -1 0 12 15 -1 0 11 12 -1 11 0 1 -1 1 10 11 -1 2 10 1 -1 2 9 10 -1 3 9 2 -1 4 9 3 -1 4 8 9 -1 5 8 4 -1 5 7 8 -1 6 7 5 -1 146 144 145 -1 147 144 146 -1 147 159 144 -1 148 159 147 -1 148 158 159 -1 149 158 148 -1 149 157 158 -1 150 157 149 -1 150 156 157 -1 150 155 156 -1 151 155 150 -1 151 154 155 -1 152 154 151 -1 152 153 154 -1 175 173 174 -1 175 172 173 -1 160 172 175 -1 160 171 172 -1 161 171 160 -1 161 170 171 -1 162 170 161 -1 162 169 170 -1 163 169 162 -1 164 169 163 -1 164 168 169 -1 165 168 164 -1 165 167 168 -1 166 167 165 -1 306 304 305 -1 307 304 306 -1 307 319 304 -1 308 319 307 -1 308 318 319 -1 309 318 308 -1 309 317 318 -1 310 317 309 -1 310 316 317 -1 310 315 316 -1 311 315 310 -1 311 314 315 -1 312 314 311 -1 312 313 314 -1 288 318 289 -1 288 319 318 -1 289 317 290 -1 289 318 317 -1 290 316 291 -1 290 317 316 -1 291 315 292 -1 291 316 315 -1 292 314 293 -1 292 315 314 -1 293 313 294 -1 293 314 313 -1 294 312 295 -1 294 313 312 -1 295 311 296 -1 295 312 311 -1 296 310 297 -1 296 311 310 -1 297 309 298 -1 297 310 309 -1 298 308 299 -1 298 309 308 -1 299 307 300 -1 299 308 307 -1 300 306 301 -1 300 307 306 -1 301 305 302 -1 301 306 305 -1 302 304 303 -1 302 305 304 -1 303 319 288 -1 303 304 319 -1 272 288 287 -1 272 303 288 -1 273 303 272 -1 273 302 303 -1 274 302 273 -1 274 301 302 -1 275 301 274 -1 275 300 301 -1 276 300 275 -1 276 299 300 -1 277 299 276 -1 277 298 299 -1 278 298 277 -1 278 297 298 -1 279 297 278 -1 279 296 297 -1 280 296 279 -1 280 295 296 -1 281 295 280 -1 281 294 295 -1 282 294 281 -1 282 293 294 -1 283 293 282 -1 283 292 293 -1 284 292 283 -1 284 291 292 -1 285 291 284 -1 285 290 291 -1 286 290 285 -1 286 289 290 -1 287 289 286 -1 287 288 289 -1 256 286 257 -1 256 287 286 -1 257 285 258 -1 257 286 285 -1 258 284 259 -1 258 285 284 -1 259 283 260 -1 259 284 283 -1 260 282 261 -1 260 283 282 -1 261 281 262 -1 261 282 281 -1 262 280 263 -1 262 281 280 -1 263 279 264 -1 263 280 279 -1 264 278 265 -1 264 279 278 -1 265 277 266 -1 265 278 277 -1 266 276 267 -1 266 277 276 -1 267 275 268 -1 267 276 275 -1 268 274 269 -1 268 275 274 -1 269 273 270 -1 269 274 273 -1 270 272 271 -1 270 273 272 -1 271 287 256 -1 271 272 287 -1 255 256 240 -1 255 271 256 -1 254 271 255 -1 254 270 271 -1 253 270 254 -1 253 269 270 -1 252 269 253 -1 252 268 269 -1 251 267 252 -1 252 267 268 -1 250 267 251 -1 250 266 267 -1 249 266 250 -1 249 265 266 -1 248 265 249 -1 248 264 265 -1 247 264 248 -1 247 263 264 -1 246 263 247 -1 246 262 263 -1 245 262 246 -1 245 261 262 -1 244 261 245 -1 244 260 261 -1 243 259 244 -1 244 259 260 -1 242 259 243 -1 242 258 259 -1 241 258 242 -1 241 257 258 -1 240 257 241 -1 240 256 257 -1 224 240 239 -1 224 255 240 -1 225 255 224 -1 225 254 255 -1 226 254 225 -1 226 253 254 -1 227 253 226 -1 227 252 253 -1 228 252 227 -1 228 251 252 -1 229 251 228 -1 229 250 251 -1 230 250 229 -1 230 249 250 -1 231 249 230 -1 231 248 249 -1 232 248 231 -1 232 247 248 -1 233 247 232 -1 233 246 247 -1 234 246 233 -1 234 245 246 -1 235 245 234 -1 235 244 245 -1 236 244 235 -1 236 243 244 -1 237 243 236 -1 237 242 243 -1 238 242 237 -1 238 241 242 -1 239 241 238 -1 239 240 241 -1 223 239 222 -1 222 239 238 -1 222 237 221 -1 222 238 237 -1 221 236 220 -1 221 237 236 -1 220 235 219 -1 220 236 235 -1 219 234 218 -1 219 235 234 -1 218 233 217 -1 218 234 233 -1 217 232 216 -1 217 233 232 -1 216 232 215 -1 215 232 231 -1 215 230 214 -1 215 231 230 -1 214 229 213 -1 214 230 229 -1 213 228 212 -1 213 229 228 -1 212 227 211 -1 212 228 227 -1 211 226 210 -1 211 227 226 -1 210 225 209 -1 210 226 225 -1 209 224 208 -1 209 225 224 -1 208 239 223 -1 208 224 239 -1 192 222 193 -1 192 223 222 -1 193 221 194 -1 193 222 221 -1 194 220 195 -1 194 221 220 -1 195 219 196 -1 195 220 219 -1 196 218 197 -1 196 219 218 -1 197 217 198 -1 197 218 217 -1 198 216 199 -1 198 217 216 -1 199 215 200 -1 199 216 215 -1 200 214 201 -1 200 215 214 -1 201 213 202 -1 201 214 213 -1 202 212 203 -1 202 213 212 -1 203 211 204 -1 203 212 211 -1 204 210 205 -1 204 211 210 -1 205 209 206 -1 205 210 209 -1 206 208 207 -1 206 209 208 -1 207 223 192 -1 207 208 223 -1 176 192 191 -1 176 207 192 -1 177 207 176 -1 177 206 207 -1 178 206 177 -1 178 205 206 -1 179 205 178 -1 179 204 205 -1 180 204 179 -1 180 203 204 -1 181 203 180 -1 181 202 203 -1 182 202 181 -1 182 201 202 -1 183 201 182 -1 183 200 201 -1 184 200 183 -1 184 199 200 -1 185 199 184 -1 185 198 199 -1 186 198 185 -1 186 197 198 -1 187 197 186 -1 187 196 197 -1 188 196 187 -1 188 195 196 -1 189 195 188 -1 189 194 195 -1 190 194 189 -1 190 193 194 -1 191 193 190 -1 191 192 193 -1 160 190 161 -1 160 191 190 -1 161 189 162 -1 161 190 189 -1 162 188 163 -1 162 189 188 -1 163 187 164 -1 163 188 187 -1 164 186 165 -1 164 187 186 -1 165 185 166 -1 165 186 185 -1 166 184 167 -1 166 185 184 -1 167 183 168 -1 167 184 183 -1 168 182 169 -1 168 183 182 -1 169 181 170 -1 169 182 181 -1 170 180 171 -1 170 181 180 -1 171 179 172 -1 171 180 179 -1 172 178 173 -1 172 179 178 -1 173 177 174 -1 173 178 177 -1 174 176 175 -1 174 177 176 -1 175 191 160 -1 175 176 191 -1 31 0 15 -1 15 16 31 -1 14 16 15 -1 14 17 16 -1 13 17 14 -1 13 18 17 -1 12 18 13 -1 12 19 18 -1 11 19 12 -1 11 20 19 -1 10 20 11 -1 10 21 20 -1 9 21 10 -1 9 22 21 -1 8 22 9 -1 8 23 22 -1 7 23 8 -1 7 24 23 -1 6 24 7 -1 6 25 24 -1 5 25 6 -1 5 26 25 -1 4 26 5 -1 4 27 26 -1 3 27 4 -1 3 28 27 -1 2 28 3 -1 2 29 28 -1 1 29 2 -1 1 30 29 -1 0 30 1 -1 0 31 30 -1 31 33 30 -1 31 32 33 -1 30 34 29 -1 30 33 34 -1 29 35 28 -1 29 34 35 -1 28 36 27 -1 28 35 36 -1 27 37 26 -1 27 36 37 -1 26 38 25 -1 26 37 38 -1 25 39 24 -1 25 38 39 -1 24 40 23 -1 24 39 40 -1 23 41 22 -1 23 40 41 -1 22 42 21 -1 22 41 42 -1 21 43 20 -1 21 42 43 -1 20 44 19 -1 20 43 44 -1 19 45 18 -1 19 44 45 -1 18 46 17 -1 18 45 46 -1 17 47 16 -1 17 46 47 -1 16 32 31 -1 16 47 32 -1 47 63 32 -1 47 48 63 -1 46 48 47 -1 46 49 48 -1 45 49 46 -1 45 50 49 -1 44 50 45 -1 44 51 50 -1 43 51 44 -1 43 52 51 -1 42 52 43 -1 42 53 52 -1 41 53 42 -1 41 54 53 -1 40 54 41 -1 40 55 54 -1 39 55 40 -1 39 56 55 -1 38 56 39 -1 38 57 56 -1 37 57 38 -1 37 58 57 -1 36 58 37 -1 36 59 58 -1 35 59 36 -1 35 60 59 -1 34 60 35 -1 34 61 60 -1 33 61 34 -1 33 62 61 -1 32 62 33 -1 32 63 62 -1 48 79 63 -1 48 64 79 -1 49 64 48 -1 49 65 64 -1 50 65 49 -1 50 66 65 -1 51 66 50 -1 51 67 66 -1 52 67 51 -1 52 68 67 -1 53 68 52 -1 53 69 68 -1 54 69 53 -1 54 70 69 -1 55 70 54 -1 55 71 70 -1 56 71 55 -1 56 72 71 -1 57 72 56 -1 57 73 72 -1 58 73 57 -1 58 74 73 -1 59 74 58 -1 59 75 74 -1 60 75 59 -1 60 76 75 -1 61 76 60 -1 61 77 76 -1 62 77 61 -1 62 78 77 -1 63 78 62 -1 63 79 78 -1 79 81 78 -1 79 80 81 -1 78 82 77 -1 78 81 82 -1 77 83 76 -1 77 82 83 -1 76 84 75 -1 76 83 84 -1 75 85 74 -1 75 84 85 -1 74 86 73 -1 74 85 86 -1 73 87 72 -1 73 86 87 -1 72 88 71 -1 72 87 88 -1 71 89 70 -1 71 88 89 -1 70 90 69 -1 70 89 90 -1 69 91 68 -1 69 90 91 -1 68 92 67 -1 68 91 92 -1 67 93 66 -1 67 92 93 -1 66 94 65 -1 66 93 94 -1 65 95 64 -1 65 94 95 -1 64 80 79 -1 64 95 80 -1 80 97 81 -1 80 96 97 -1 81 98 82 -1 81 97 98 -1 82 99 83 -1 82 98 99 -1 83 99 84 -1 84 99 100 -1 84 101 85 -1 84 100 101 -1 85 102 86 -1 85 101 102 -1 86 103 87 -1 86 102 103 -1 87 104 88 -1 87 103 104 -1 88 105 89 -1 88 104 105 -1 89 106 90 -1 89 105 106 -1 90 107 91 -1 90 106 107 -1 91 107 92 -1 92 107 108 -1 92 109 93 -1 92 108 109 -1 93 110 94 -1 93 109 110 -1 94 111 95 -1 94 110 111 -1 95 96 80 -1 95 111 96 -1 111 127 96 -1 111 112 127 -1 110 112 111 -1 110 113 112 -1 109 113 110 -1 109 114 113 -1 108 114 109 -1 108 115 114 -1 107 115 108 -1 107 116 115 -1 106 116 107 -1 106 117 116 -1 105 117 106 -1 105 118 117 -1 104 118 105 -1 104 119 118 -1 103 119 104 -1 103 120 119 -1 102 120 103 -1 102 121 120 -1 101 121 102 -1 101 122 121 -1 100 122 101 -1 100 123 122 -1 99 123 100 -1 99 124 123 -1 98 124 99 -1 98 125 124 -1 97 125 98 -1 97 126 125 -1 96 126 97 -1 96 127 126 -1 127 129 126 -1 127 128 129 -1 126 130 125 -1 126 129 130 -1 125 131 124 -1 125 130 131 -1 124 132 123 -1 124 131 132 -1 123 133 122 -1 123 132 133 -1 122 134 121 -1 122 133 134 -1 121 135 120 -1 121 134 135 -1 120 136 119 -1 120 135 136 -1 119 137 118 -1 119 136 137 -1 118 138 117 -1 118 137 138 -1 117 139 116 -1 117 138 139 -1 116 140 115 -1 116 139 140 -1 115 141 114 -1 115 140 141 -1 114 142 113 -1 114 141 142 -1 113 143 112 -1 113 142 143 -1 112 128 127 -1 112 143 128 -1 143 159 128 -1 143 144 159 -1 142 144 143 -1 142 145 144 -1 141 145 142 -1 141 146 145 -1 140 146 141 -1 140 147 146 -1 139 147 140 -1 139 148 147 -1 138 148 139 -1 138 149 148 -1 137 149 138 -1 137 150 149 -1 136 150 137 -1 136 151 150 -1 135 151 136 -1 135 152 151 -1 134 152 135 -1 134 153 152 -1 133 153 134 -1 133 154 153 -1 132 154 133 -1 132 155 154 -1 131 155 132 -1 131 156 155 -1 130 156 131 -1 130 157 156 -1 129 157 130 -1 129 158 157 -1 128 158 129 -1 128 159 158 -1 + ] + creaseAngle 1 + } + } + DEF UPPER_BLACK_PANEL_SHAPE Shape { + appearance PBRAppearance { + baseColor 0.2 0.2 0.2 + roughness 0.3 + metalness 0 + } + geometry IndexedFaceSet { + coord Coordinate { + point [ + -0.0294513 0.168028 -0.0961899, 0.0338456 0.168028 -0.0961899 0.0338456 0.168028 -0.0645414, -0.0294513 0.168028 -0.0645414 -0.0294513 0.165223 -0.0645414, 0.0338456 0.165223 -0.0645414 0.0338456 0.165223 -0.0961899, -0.0294513 0.165223 -0.0961899 + ] + } + coordIndex [ + 5 4 6 -1 4 7 6 -1 7 0 1 -1 7 1 6 -1 2 5 6 -1 2 6 1 -1 3 4 5 -1 3 5 2 -1 0 7 4 -1 0 4 3 -1 0 3 1 -1 3 2 1 -1 + ] + } + } + ] + } + ] + name IS name + model "Adept Pioneer 3-AT" + boundingObject Group { + children [ + DEF MAIN_BODY_BOX Transform { + rotation -0.5774 0.5773 0.5774 -2.094 + translation 0.009 -0.0 0.063 + children [ + Box { + size 0.197 0.2 0.402 + } + ] + } + DEF BODY_BOX Transform { + rotation -0.5774 0.5773 0.5774 -2.094 + translation 0.01 -0.0 0.063 + children [ + Box { + size 0.264 0.2 0.332 + } + ] + } + DEF FRONT_TOP_CYLINDER Transform { + rotation -0.5774 0.5773 0.5774 -2.094 + translation 0.064 -0.0 0.163 + children [ + Cylinder { + height 0.004 + radius 0.19 + subdivision 24 + } + ] + } + DEF BACK_TOP_CYLINDER Transform { + rotation -0.5774 0.5773 0.5774 -2.094 + translation -0.084 -0.0 0.163 + children [ + Cylinder { + height 0.004 + radius 0.17 + subdivision 24 + } + ] + } + DEF FRONT_RING_CYLINDER Transform { + rotation -0.5774 0.5773 0.5774 -2.094 + translation 0.106 -0.0 0.137 + children [ + Cylinder { + height 0.05 + radius 0.145 + subdivision 24 + } + ] + } + DEF BACK_RING_CYLINDER Transform { + rotation -0.5774 0.5773 0.5774 -2.094 + translation -0.104 -0.0 0.137 + children [ + Cylinder { + height 0.05 + radius 0.145 + subdivision 24 + } + ] + } + DEF LEFT_ANTENNA_CYLINDER Transform { + rotation -0.5774 0.5773 0.5774 -2.094 + translation -0.017 0.082 0.174 + children [ + Cylinder { + height 0.016 + radius 0.0058 + subdivision 24 + } + ] + } + DEF RIGHT_ANTENNA_CYLINDER Transform { + rotation -0.5774 0.5773 0.5774 -2.094 + translation -0.017 -0.087 0.174 + children [ + Cylinder { + height 0.016 + radius 0.0058 + subdivision 24 + } + ] + } + DEF FRONT_LEFT_CORNER_BOX Transform { + translation 0.176 0.097 0.038 + rotation 0.8735 -0.3442 -0.3442 1.706 + children [ + Box { + size 0.049 0.15 0.05 + } + ] + } + DEF FRONT_RIGHT_CORNER_BOX Transform { + translation 0.176 -0.097 0.038 + rotation 0.2684 -0.6812 -0.6812 2.617 + children [ + Box { + size 0.049 0.15 0.05 + } + ] + } + DEF BACK_RIGHT_CORNER_BOX Transform { + translation -0.156 -0.097 0.038 + rotation 0.8735 -0.3442 -0.3442 1.706 + children [ + Box { + size 0.049 0.15 0.05 + } + ] + } + DEF BACK_LEFT_CORNER_BOX Transform { + translation -0.156 0.098 0.038 + rotation 0.2684 -0.6812 -0.6812 2.617 + children [ + Box { + size 0.049 0.15 0.05 + } + ] + } + DEF FRONT_AXLE Transform { + translation 0.134 -0.0 0.001 + rotation -0.0 1.0 0.0 -1.571 + children [ + DEF AXLE_CYLINDER Cylinder { + height 0.4 + radius 0.025 + subdivision 24 + } + ] + } + DEF BACK_AXLE Transform { + translation -0.132 0.0 0.001 + rotation -0.0 1.0 0.0 -1.571 + children [ + USE AXLE_CYLINDER + ] + } + ] + } + physics Physics { + density -1 + mass 8.8 + } + controller IS controller + controllerArgs IS controllerArgs + customData IS customData + supervisor IS supervisor + synchronization IS synchronization + } +} diff --git a/worlds/pioneer3at.wbt b/worlds/pioneer3at.wbt index 85f4b12..f175828 100644 --- a/worlds/pioneer3at.wbt +++ b/worlds/pioneer3at.wbt @@ -711,13 +711,14 @@ DEF KITCHEN Transform { } ] } -DEF PIONEER_3AT Pioneer3at { +DEF PIONEER_3AT Pioneer3at_ENU { translation 1.69482 -4.10279 0.11 - rotation 1 0 0 1.5707963267948966 + rotation 1 0 0 4.692820414042842e-06 controller "ros" controllerArgs [ "--name=pioneer3at" "--use-ros-control" + "--auto-publish" ] extensionSlot [ Camera { From 7a5c3c2a60983a9e7a09277a6938b2f0f49c6704 Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Sun, 7 Mar 2021 03:24:58 +0100 Subject: [PATCH 20/72] Useless controller --- CMakeLists.txt | 3 ++ launch/pioneer3at.launch | 11 +++--- src/pioneer3at.cpp | 72 ---------------------------------------- 3 files changed, 9 insertions(+), 77 deletions(-) delete mode 100644 src/pioneer3at.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 500565a..ceccaf0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -287,3 +287,6 @@ install(DIRECTORY plugins install(DIRECTORY worlds DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +install(DIRECTORY protos + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/launch/pioneer3at.launch b/launch/pioneer3at.launch index 56be2b1..7fbf4c0 100644 --- a/launch/pioneer3at.launch +++ b/launch/pioneer3at.launch @@ -1,5 +1,6 @@ + pioneer_diff_drive_controller: type: "diff_drive_controller/DiffDriveController" @@ -8,8 +9,9 @@ pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + - + @@ -17,10 +19,9 @@ + - - - - + + diff --git a/src/pioneer3at.cpp b/src/pioneer3at.cpp deleted file mode 100644 index b38eeb8..0000000 --- a/src/pioneer3at.cpp +++ /dev/null @@ -1,72 +0,0 @@ -// Copyright 1996-2020 Cyberbotics Ltd. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/* - * To run gmapping you should start gmapping: - * rosrun gmapping slam_gmapping scan:=/pioneer3at/Sick_LMS_291/laser_scan/layer0 _xmax:=30 _xmin:=-30 _ymax:=30 _ymin:=-30 - * _delta:=0.2 - */ - -#include -#include -#include -#include -#include -#include -#include -#include "ros/ros.h" - -#include -#include - -#define TIME_STEP 32 - -ros::NodeHandle *n; - -ros::ServiceClient timeStepClient; -webots_ros::set_int timeStepSrv; - -void nullFuncion(const std_msgs::String::ConstPtr &name){}; - -int main(int argc, char **argv) -{ - std::string controllerName; - // create a node named 'pioneer3at' on ROS network - ros::init(argc, argv, "pioneer3at", ros::init_options::AnonymousName); - n = new ros::NodeHandle; - ros::Subscriber nameSub = n->subscribe("model_name", 100, nullFuncion); - while (nameSub.getNumPublishers() < 1) - ros::spinOnce(); - timeStepClient = n->serviceClient("pioneer3at/robot/time_step"); - timeStepSrv.request.value = TIME_STEP; - ROS_INFO("You can now start the creation of the map using 'rosrun gmapping slam_gmapping " - "scan:=/pioneer3at/Sick_LMS_291/laser_scan/layer0 _xmax:=30 _xmin:=-30 _ymax:=30 _ymin:=-30 _delta:=0.2'."); - ROS_INFO("You can now visualize the sensors output in rqt using 'rqt'."); - - // main loop - while (ros::ok()) - { - if (!timeStepClient.call(timeStepSrv) || !timeStepSrv.response.success) - { - ROS_ERROR("Failed to call service time_step for next step."); - break; - } - ros::spinOnce(); - } - timeStepSrv.request.value = 0; - timeStepClient.call(timeStepSrv); - - ros::shutdown(); - return 0; -} From 55aa572180912732060436a0886061fd4c903cc9 Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Mon, 8 Mar 2021 11:06:05 +0100 Subject: [PATCH 21/72] Remove controller --- CMakeLists.txt | 13 ------------- launch/pioneer3at.launch | 5 +++++ worlds/pioneer3at.wbt | 1 + 3 files changed, 6 insertions(+), 13 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ceccaf0..e0face7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -234,16 +234,6 @@ target_link_libraries(panoramic_view_recorder ${catkin_LIBRARIES} ) -#instructions for pioneer3at node - -add_executable(pioneer3at src/pioneer3at.cpp) - -add_dependencies(pioneer3at webots_ros_generate_messages_cpp) - -target_link_libraries(pioneer3at - ${catkin_LIBRARIES} -) - ############# ## Install ## @@ -267,9 +257,6 @@ install(TARGETS catch_the_bird install(TARGETS panoramic_view_recorder RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) -install(TARGETS pioneer3at - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) - catkin_install_python(PROGRAMS scripts/ros_controller.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) diff --git a/launch/pioneer3at.launch b/launch/pioneer3at.launch index 7fbf4c0..0b0f9cd 100644 --- a/launch/pioneer3at.launch +++ b/launch/pioneer3at.launch @@ -1,4 +1,9 @@ + + diff --git a/worlds/pioneer3at.wbt b/worlds/pioneer3at.wbt index f175828..c91f423 100644 --- a/worlds/pioneer3at.wbt +++ b/worlds/pioneer3at.wbt @@ -719,6 +719,7 @@ DEF PIONEER_3AT Pioneer3at_ENU { "--name=pioneer3at" "--use-ros-control" "--auto-publish" + "--robot-description" ] extensionSlot [ Camera { From 1c70b12708888733c576bb1e7856514f05e2ee95 Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Mon, 8 Mar 2021 12:59:09 +0100 Subject: [PATCH 22/72] Add dependencies --- package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/package.xml b/package.xml index 57c24d8..30f3f21 100644 --- a/package.xml +++ b/package.xml @@ -27,5 +27,7 @@ ros_control ros_controllers robot_state_publisher + class_loader + roslib From 2ee7aa53e437f096f03bfa3d10ec6a7dc57ebe8a Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Mon, 8 Mar 2021 18:15:47 +0100 Subject: [PATCH 23/72] Add tiago world --- launch/tiago.launch | 12 ++ worlds/tiago.wbt | 449 ++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 461 insertions(+) create mode 100644 launch/tiago.launch create mode 100644 worlds/tiago.wbt diff --git a/launch/tiago.launch b/launch/tiago.launch new file mode 100644 index 0000000..0452441 --- /dev/null +++ b/launch/tiago.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/worlds/tiago.wbt b/worlds/tiago.wbt new file mode 100644 index 0000000..52d63d1 --- /dev/null +++ b/worlds/tiago.wbt @@ -0,0 +1,449 @@ +#VRML_SIM R2021b utf8 +WorldInfo { + info [ + "This simulation shows a Tiago Steel making hello in an industrial environment. It can also been controlled using the keyboard arrows." + ] + title "Simulation of PAL Robotics' TIAGo Steel robot" + coordinateSystem "NUE" +} +Viewpoint { + orientation -0.13870670326979867 -0.9256806146864625 -0.3519600120208505 2.431279426949263 + position -9.395105901154711 9.204146785481651 -9.007040615249405 + follow "TIAGo Steel" +} +TexturedBackground { + texture "factory" + skybox FALSE +} +TexturedBackgroundLight { + texture "factory" +} +RectangleArena { + floorSize 15 15 + floorTileSize 1 1 + floorAppearance ThreadMetalPlate { + } + wallHeight 2.5 + wallAppearance Roughcast { + colorOverride 0.533333 0.541176 0.521569 + textureTransform TextureTransform { + scale 5 1.75 + } + } +} +DEF ROOF RectangleArena { + translation 0 2.5 0 + rotation 1 0 0 3.141592653589793 + name "rectangle arena(1)" + floorSize 15 15 + floorTileSize 1 1 + floorAppearance Roughcast { + colorOverride 0.533333 0.541176 0.521569 + textureTransform TextureTransform { + scale 5 1.75 + } + } + wallHeight 2.5 + wallAppearance Roughcast { + colorOverride 0.533333 0.541176 0.521569 + textureTransform TextureTransform { + scale 5 1.75 + } + } +} +TiagoSteel { + translation -4.416 0.095 -1.063 + controller "ros" +} +WoodenBox { + translation -6.2 0.405 -7 +} +WoodenBox { + translation -4.6 0.405 -7 + name "wooden box(1)" +} +WoodenBox { + translation -0.189 0.3 -0.174 + name "wooden box(2)" +} +WoodenBox { + translation -0.098 0.3 -0.994 + rotation 0 1 0 -2.618 + name "wooden box(3)" +} +WoodenBox { + translation 0.248 0.901 -0.48 + rotation 0 1 0 -0.524 + name "wooden box(4)" +} +WoodenBox { + translation 0.631 0.3 -0.397 + rotation 0 1 0 0.262 + name "wooden box(5)" +} +WoodenBox { + translation -2.99 0.44 4 + name "wooden box(6)" +} +WoodenBox { + translation 0 1.1 4 + name "wooden box(7)" +} +WoodenBox { + translation 3.6 1.1 4 + name "wooden box(8)" +} +WoodenBox { + translation 7.18 1.1 4 + name "wooden box(9)" +} +WoodenBox { + translation 0 1.1 6 + name "wooden box(10)" +} +WoodenBox { + translation 3.6 1.1 6 + name "wooden box(11)" +} +WoodenBox { + translation 7.18 1.1 6 + name "wooden box(12)" +} +SolidBox { + translation 7.49 0.85 4 + size 0.03 1.7 1 + appearance PBRAppearance { + baseColor 0 0 0 + baseColorMap ImageTexture { + url [ + "textures/tagged_wall.jpg" + ] + } + roughness 0.5 + metalness 0 + } +} +SolidBox { + translation 7.49 0.85 6 + name "box(1)" + size 0.03 1.7 1 + appearance PBRAppearance { + baseColor 0 0 0 + baseColorMap ImageTexture { + url [ + "textures/tagged_wall.jpg" + ] + } + roughness 0.5 + metalness 0 + } +} +ConveyorBelt { + translation 2.47 0 4 + rotation 1 0 0 0 + size 10 0.8 0.8 + speed 0 +} +ConveyorBelt { + translation 2.47 0 6 + rotation 1 0 0 0 + name "conveyor belt(1)" + size 10 0.8 0.8 + speed 0 +} +FireExtinguisher { + translation -1.18 0 -7.38 +} +WoodenPalletStack { + translation -7 0 -6.85 +} +WoodenPalletStack { + translation -5.4 0 -6.85 + name "wooden pallet stack(1)" +} +WoodenPalletStack { + translation -3.8 0 -6.85 + name "wooden pallet stack(2)" +} +WoodenPalletStack { + translation -2.2 0 -6.85 + name "wooden pallet stack(3)" +} +WoodenPallet { + translation -6.24 0 5.27 + rotation 0 1 0 1.8326 +} +WoodenPallet { + translation -3.11 0 4 + rotation 0 1 0 1.5708 + name "wooden pallet(1)" +} +WoodenPallet { + translation -3.112 0 6 + rotation 0 1 0 1.5708 + name "wooden pallet(2)" +} +WoodenPallet { + translation -6.274 0.141 5.328 + rotation 0 1 0 -0.524 + name "wooden pallet(3)" +} +WoodenPallet { + translation -6.318 0.282 5.083 + rotation 0 1 0 0.524 + name "wooden pallet(4)" +} +WoodenPallet { + translation -6.326 0.423 5.112 + name "wooden pallet(5)" +} +PlatformCart { + translation 0.472 0 0.498 + rotation 0 -1 0 -0.524 +} +PlatformCart { + translation -4.6 0 -7 + rotation 0 1 0 1.5708 + name "platform cart(1)" +} +PlatformCart { + translation -6.2 0 -7 + rotation 0 -1 0 -1.571 + name "platform cart(2)" +} +Sofa { + translation 5.586 0 -7.001 + rotation 0 1 0 -1.571 +} +Sofa { + translation 6.99 0 -5.543 + rotation 0 1 0 3.1416 + name "sofa(1)" +} +Table { + translation 5.507 0 -5.65 + size 1 0.4 1 +} +FloorLight { + translation 7.073 0 -7.105 +} +PanelWithTubes { + translation 7.1 0 -3.65 + rotation 0 1 0 3.14159 +} +PanelWithTubes { + translation 6.32 0 -3.65 + rotation 0 1 0 3.14159 + name "panel with tubes(1)" +} +PanelWithTubes { + translation 5.54 0 -3.65 + rotation 0 1 0 3.14159 + name "panel with tubes(2)" +} +PanelWithTubes { + translation 4.76 0 -3.65 + rotation 0 1 0 3.14159 + name "panel with tubes(3)" +} +PanelWithTubes { + translation 3.98 0 -3.65 + rotation 0 1 0 3.14159 + name "panel with tubes(4)" +} +PanelWithTubes { + translation 3.2 0 -4.47 + rotation 0 1 0 1.57059 + name "panel with tubes(5)" +} +PanelWithTubes { + translation 3.2 0 -5.25 + rotation 0 1 0 1.57059 + name "panel with tubes(6)" +} +PanelWithTubes { + translation 3.2 0 -6.03 + rotation 0 1 0 1.57059 + name "panel with tubes(7)" +} +PanelWithTubes { + translation 2.77 0 -6.45 + rotation 0 1 0 -3.1456 + name "panel with tubes(8)" +} +PottedTree { + translation 3.339 0 -3.785 +} +DoubleFluorescentLamp { + translation -5 2.5 5 + rotation -0.7071067811865475 0 0.7071067811865475 3.1416 +} +DoubleFluorescentLamp { + translation -5 2.5 0 + rotation -0.7071067811865475 0 0.7071067811865475 3.1416 + name "double fuorescent lamp(1)" +} +DoubleFluorescentLamp { + translation -5 2.5 -5 + rotation -0.7071067811865475 0 0.7071067811865475 3.1416 + name "double fuorescent lamp(2)" +} +DoubleFluorescentLamp { + translation 0 2.5 5 + rotation -0.7071067811865475 0 0.7071067811865475 3.1416 + name "double fuorescent lamp(3)" +} +DoubleFluorescentLamp { + translation 0 2.5 0 + rotation -0.7071067811865475 0 0.7071067811865475 3.1416 + name "double fuorescent lamp(4)" +} +DoubleFluorescentLamp { + translation 0 2.5 -5 + rotation -0.7071067811865475 0 0.7071067811865475 3.1416 + name "double fuorescent lamp(5)" +} +DoubleFluorescentLamp { + translation 5 2.5 5 + rotation -0.7071067811865475 0 0.7071067811865475 3.1416 + name "double fuorescent lamp(6)" +} +DoubleFluorescentLamp { + translation 5 2.5 0 + rotation -0.7071067811865475 0 0.7071067811865475 3.1416 + name "double fuorescent lamp(7)" +} +DoubleFluorescentLamp { + translation 5 2.5 -5 + rotation -0.7071067811865475 0 0.7071067811865475 3.1416 + name "double fuorescent lamp(8)" +} +StraightStairs { + translation -6.92 0 0.6 + rotation 0 1 0 1.5708 + stepSize 0.2 0.03 1.1 + stepRise 0.13 + nSteps 20 + stringerWidth 0.03 + stepAppearance ThreadMetalPlate { + textureTransform TextureTransform { + scale 0.4 2.2 + } + } + stringerAppearance BrushedAluminium { + colorOverride 0.533333 0.541176 0.521569 + textureTransform TextureTransform { + scale 20 20 + } + } + leftRail [] + rightRail [ + StraightStairsRail { + translation -0.02 -0.13 0 + run 3.84 + rise 2.5 + newelHeight 0.8 + balusterHeight 0.62 + appearance Rubber { + } + } + ] +} +OilBarrel { + translation -7.18 0.44 -1.203 +} +OilBarrel { + translation -7.18 0.45 -1.9 + name "oil barrel(1)" +} +OilBarrel { + translation -7.18 0.44 -2.57 + name "oil barrel(2)" +} +OilBarrel { + translation -7.18 0.44 -3.25 + name "oil barrel(3)" +} +OilBarrel { + translation -7.18 0.44 -3.95 + name "oil barrel(4)" +} +OilBarrel { + translation -6.55 0.44 -3.64 + name "oil barrel(5)" +} +OilBarrel { + translation -6.55 0.44 -2.95 + name "oil barrel(6)" +} +OilBarrel { + translation -6.55 0.44 -2.26 + name "oil barrel(7)" +} +OilBarrel { + translation -6.55 0.44 -1.58 + name "oil barrel(8)" +} +OilBarrel { + translation 5.826 0.305 0.356 + rotation -0.6778359835355757 0.5198742056615036 -0.5198742056615036 -1.95 + name "oil barrel(9)" +} +OilBarrel { + translation 5.892 0.305 -1.534 + rotation -0.9353625232302415 -0.25009693134498434 0.25009693134498434 -1.638 + name "oil barrel(10)" +} +OilBarrel { + translation 1.31 0.44 -5.783 + name "oil barrel(11)" +} +OilBarrel { + translation 1.017 0.44 -6.932 + name "oil barrel(12)" +} +OilBarrel { + translation -0.08 0.44 -6.53 + name "oil barrel(13)" +} +TrafficCone { + translation -5.892 0 1.397 +} +TrafficCone { + translation -5.045 0 2.377 + rotation 0 1 0 -0.785 + name "traffic cone(1)" +} +TrafficCone { + translation -3.996 0 3.072 + rotation 0 1 0 0.262 + name "traffic cone(2)" +} +TrafficCone { + translation 6.425 0 2.282 + rotation 0 1 0 0.262 + name "traffic cone(3)" +} +TrafficCone { + translation 5.034 0 2.12 + name "traffic cone(4)" +} +TrafficCone { + translation 4.128 0 1.239 + rotation 0 1 0 -0.262 + name "traffic cone(5)" +} +TrafficCone { + translation 4.289 0 -0.082 + rotation 0 -1 0 0.524 + name "traffic cone(6)" +} +TrafficCone { + translation 4.158 0 -1.273 + rotation 0 1 0 0.524 + name "traffic cone(7)" +} +TrafficCone { + translation 4.317 0 -2.52 + name "traffic cone(8)" +} From d524c40983467898802428d660bce001dde0bbbd Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Tue, 9 Mar 2021 15:38:35 +0100 Subject: [PATCH 24/72] Add joint state publisher --- CMakeLists.txt | 18 +- config/tiago_kinematics.yaml | 4 + config/tiago_semantic.srdf | 326 +++++++++++++++++++++++++++++++++++ launch/tiago.launch | 31 +++- package.xml | 2 + src/tiago.cpp | 69 ++++++++ worlds/tiago.wbt | 6 + 7 files changed, 454 insertions(+), 2 deletions(-) create mode 100644 config/tiago_kinematics.yaml create mode 100644 config/tiago_semantic.srdf create mode 100644 src/tiago.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 500565a..38cb9a4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,7 +4,7 @@ project(webots_ros) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs sensor_msgs message_generation tf) +find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs sensor_msgs message_generation tf moveit_ros_planning_interface) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) @@ -224,6 +224,19 @@ target_link_libraries(catch_the_bird ${catkin_LIBRARIES} ) +add_executable(tiago src/tiago.cpp) + +include_directories( + tiago + ${catkin_INCLUDE_DIRS} +) + +add_dependencies(tiago ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +target_link_libraries(tiago + ${catkin_LIBRARIES} +) + #instructions for panoramic_view_recorder node add_executable(panoramic_view_recorder src/panoramic_view_recorder.cpp) @@ -287,3 +300,6 @@ install(DIRECTORY plugins install(DIRECTORY worlds DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +install(DIRECTORY config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) \ No newline at end of file diff --git a/config/tiago_kinematics.yaml b/config/tiago_kinematics.yaml new file mode 100644 index 0000000..05573e5 --- /dev/null +++ b/config/tiago_kinematics.yaml @@ -0,0 +1,4 @@ +arm: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 \ No newline at end of file diff --git a/config/tiago_semantic.srdf b/config/tiago_semantic.srdf new file mode 100644 index 0000000..121fc80 --- /dev/null +++ b/config/tiago_semantic.srdf @@ -0,0 +1,326 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tiago.launch b/launch/tiago.launch index 0452441..3102819 100644 --- a/launch/tiago.launch +++ b/launch/tiago.launch @@ -1,5 +1,24 @@ + + + + tiago_arm_controller: + type: "position_controllers/JointTrajectoryController" + joints: + - arm_1_joint + - arm_2_joint + - arm_3_joint + - arm_4_joint + - arm_5_joint + - arm_6_joint + - arm_7_joint + tiago_joint_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 + + + @@ -7,6 +26,16 @@ - + + + + + + + + + diff --git a/package.xml b/package.xml index f77ed40..338493e 100644 --- a/package.xml +++ b/package.xml @@ -11,6 +11,7 @@ Apache 2.0 + moveit_ros_planning_interface catkin rospy roscpp @@ -24,5 +25,6 @@ sensor_msgs message_runtime tf + moveit_ros_planning_interface diff --git a/src/tiago.cpp b/src/tiago.cpp new file mode 100644 index 0000000..f55d41f --- /dev/null +++ b/src/tiago.cpp @@ -0,0 +1,69 @@ +#include +#include +#include +#include +#include +#include + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "plan_arm_torso_ik"); + + if (argc < 7) + { + ROS_INFO(" "); + ROS_INFO("\tUsage:"); + ROS_INFO(" "); + ROS_INFO("\trosrun tiago_moveit_tutorial plan_arm_torso_ik x y z r p y"); + ROS_INFO(" "); + ROS_INFO("\twhere the list of arguments specify the target pose of /arm_tool_link expressed in /base_footprint"); + ROS_INFO(" "); + return EXIT_FAILURE; + } + + geometry_msgs::PoseStamped goal_pose; + goal_pose.header.frame_id = "base_link"; + goal_pose.pose.position.x = atof(argv[1]); + goal_pose.pose.position.y = atof(argv[2]); + goal_pose.pose.position.z = atof(argv[3]); + goal_pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(atof(argv[4]), atof(argv[5]), atof(argv[6])); + + ros::NodeHandle nh; + ros::AsyncSpinner spinner(1); + spinner.start(); + + std::vector torso_arm_joint_names; + //select group of joints + moveit::planning_interface::MoveGroupInterface group_arm_torso("arm"); + //choose your preferred planner + group_arm_torso.setPlannerId("SBLkConfigDefault"); + group_arm_torso.setPoseReferenceFrame("base_link"); + group_arm_torso.setPoseTarget(goal_pose); + + ROS_INFO_STREAM("Planning to move " << group_arm_torso.getEndEffectorLink() << " to a target pose expressed in " << group_arm_torso.getPlanningFrame()); + + group_arm_torso.setStartStateToCurrentState(); + group_arm_torso.setMaxVelocityScalingFactor(1.0); + + moveit::planning_interface::MoveGroupInterface::Plan my_plan; + //set maximum time to find a plan + group_arm_torso.setPlanningTime(5.0); + + moveit::planning_interface::MoveItErrorCode success = group_arm_torso.plan(my_plan); + + if (!success) + throw std::runtime_error("No plan found"); + + ROS_INFO_STREAM("Plan found in " << my_plan.planning_time_ << " seconds"); + + // Execute the plan + ros::Time start = ros::Time::now(); + + group_arm_torso.move(); + + ROS_INFO_STREAM("Motion duration: " << (ros::Time::now() - start).toSec()); + + spinner.stop(); + + return EXIT_SUCCESS; +} diff --git a/worlds/tiago.wbt b/worlds/tiago.wbt index 52d63d1..f2785e9 100644 --- a/worlds/tiago.wbt +++ b/worlds/tiago.wbt @@ -54,6 +54,12 @@ DEF ROOF RectangleArena { TiagoSteel { translation -4.416 0.095 -1.063 controller "ros" + controllerArgs [ + "--name=tiago" + "--use-ros-control" + "--auto-publish" + "--robot-description" + ] } WoodenBox { translation -6.2 0.405 -7 From bf371c9cdc4055e39dd6bc5d8cda776199999455 Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Tue, 9 Mar 2021 23:51:55 +0100 Subject: [PATCH 25/72] Update --- CMakeLists.txt | 3 - config/tiago_kinematics.yaml | 4 - config/tiago_semantic.srdf | 326 ----------------------------------- launch/tiago.launch | 8 +- 4 files changed, 2 insertions(+), 339 deletions(-) delete mode 100644 config/tiago_kinematics.yaml delete mode 100644 config/tiago_semantic.srdf diff --git a/CMakeLists.txt b/CMakeLists.txt index 38cb9a4..5d50bd4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -300,6 +300,3 @@ install(DIRECTORY plugins install(DIRECTORY worlds DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) - -install(DIRECTORY config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) \ No newline at end of file diff --git a/config/tiago_kinematics.yaml b/config/tiago_kinematics.yaml deleted file mode 100644 index 05573e5..0000000 --- a/config/tiago_kinematics.yaml +++ /dev/null @@ -1,4 +0,0 @@ -arm: - kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin - kinematics_solver_search_resolution: 0.005 - kinematics_solver_timeout: 0.005 \ No newline at end of file diff --git a/config/tiago_semantic.srdf b/config/tiago_semantic.srdf deleted file mode 100644 index 121fc80..0000000 --- a/config/tiago_semantic.srdf +++ /dev/null @@ -1,326 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/tiago.launch b/launch/tiago.launch index 3102819..e5723c4 100644 --- a/launch/tiago.launch +++ b/launch/tiago.launch @@ -3,7 +3,7 @@ - tiago_arm_controller: + arm_controller: type: "position_controllers/JointTrajectoryController" joints: - arm_1_joint @@ -17,7 +17,7 @@ type: joint_state_controller/JointStateController publish_rate: 50 - + @@ -31,10 +31,6 @@ - - - - From 821f51d0705abb427fb4e25333720e3385787ed0 Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Wed, 10 Mar 2021 09:55:19 +0100 Subject: [PATCH 26/72] Add diff_drive controller --- launch/tiago.launch | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/launch/tiago.launch b/launch/tiago.launch index e5723c4..e7d98bd 100644 --- a/launch/tiago.launch +++ b/launch/tiago.launch @@ -13,11 +13,17 @@ - arm_5_joint - arm_6_joint - arm_7_joint - tiago_joint_controller: + joint_controller: type: joint_state_controller/JointStateController publish_rate: 50 + diff_drive_controller: + type: diff_drive_controller/DiffDriveController + left_wheel: wheel_left_joint + right_wheel: wheel_right_joint + pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] - + From 10f20ceecbedbfb7a727dd2d0fa642632aa779f2 Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Mon, 15 Mar 2021 12:33:07 +0100 Subject: [PATCH 27/72] Delete the custom proto --- CMakeLists.txt | 3 - protos/Pioneer3at_ENU.proto | 690 ------------------------------------ worlds/pioneer3at.wbt | 15 +- 3 files changed, 4 insertions(+), 704 deletions(-) delete mode 100644 protos/Pioneer3at_ENU.proto diff --git a/CMakeLists.txt b/CMakeLists.txt index e0face7..4b28734 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -274,6 +274,3 @@ install(DIRECTORY plugins install(DIRECTORY worlds DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) - -install(DIRECTORY protos - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/protos/Pioneer3at_ENU.proto b/protos/Pioneer3at_ENU.proto deleted file mode 100644 index c4c020e..0000000 --- a/protos/Pioneer3at_ENU.proto +++ /dev/null @@ -1,690 +0,0 @@ -#VRML_SIM R2021b utf8 -# license: Copyright Cyberbotics Ltd. Licensed for use only with Webots. -# license url: https://cyberbotics.com/webots_assets_license -# documentation url: https://www.cyberbotics.com/doc/guide/pioneer-3at -# Pioneer 3AT robot (Adept MobileRobots) is an all terrain four wheels drive robot. - -PROTO Pioneer3at_ENU [ - field SFVec3f translation 0 0.11 0 # Is `Transform.translation`. - field SFRotation rotation 0 1 0 0 # Is `Transform.rotation`. - field SFString name "Pioneer 3-AT" # Is `Solid.name`. - field SFString controller "void" # Is `Robot.controller`. - field MFString controllerArgs [] # Is `Robot.controllerArgs`. - field SFString customData "" # Is `Robot.customData`. - field SFBool supervisor FALSE # Is `Robot.supervisor`. - field SFBool synchronization TRUE # Is `Robot.synchronization`. - field MFNode extensionSlot [] # Extends the robot with new nodes in the extension slot. -] -{ - Robot { - translation IS translation - rotation IS rotation - children [ - Transform { - rotation -0.5774 0.5773 0.5774 -2.094 - children [ - DEF EXTENSION_SLOT Group { - children IS extensionSlot - } - DEF BODY_SHAPE Shape { - appearance PBRAppearance { - baseColor 1 0 0 - metalness 0 - baseColorMap ImageTexture { - url [ - "https://raw.githubusercontent.com/cyberbotics/webots/R2021a/projects/robots/adept/pioneer3/protos/textures/body_3at_base_color.jpg" - ] - } - roughnessMap ImageTexture { - url [ - "https://raw.githubusercontent.com/cyberbotics/webots/R2021a/projects/robots/adept/pioneer3/protos/textures/body_3at_roughness.jpg" - ] - } - normalMap ImageTexture { - url [ - "https://raw.githubusercontent.com/cyberbotics/webots/R2021a/projects/robots/adept/pioneer3/protos/textures/body_3at_normal.jpg" - ] - } - occlusionMap ImageTexture { - url [ - "https://raw.githubusercontent.com/cyberbotics/webots/R2021a/projects/robots/adept/pioneer3/protos/textures/body_3at_occlusion.jpg" - ] - } - } - geometry IndexedFaceSet { - coord Coordinate { - point [ - -0.132983 0.163081 0.156114, -0.0989178 0.163081 0.19185 0.0988502 0.163081 0.191849, 0.132915 0.163081 0.154961 0.132914 0.163081 -0.175904, -0.0989194 0.163081 -0.211218 -0.132984 0.163081 -0.175904, -0.132984 -0.0372167 -0.175904 -0.0989194 -0.0372167 -0.211218, 0.0988485 -0.0372167 -0.211218 0.132914 -0.0372167 -0.175904, 0.132915 -0.0372167 0.154961 0.0988502 -0.0372167 0.191849, -0.0989178 -0.0372166 0.19185 -0.132983 -0.0372166 0.156114, 0.0989815 0.162482 -0.211218 - ] - } - texCoord TextureCoordinate { - point [ - 0.489438 0.25385, 0.489438 0.259714, 0.488368 0.260724 0.489438 0.25385, 0.488368 0.260724, 0.488334 0.252839 0.477379 0.259714, 0.477379 0.25385, 0.478436 0.260724 0.477379 0.25385, 0.478436 0.252839, 0.478436 0.260724 0.478436 0.252839, 0.488334 0.252839, 0.488368 0.260724 0.478436 0.252839, 0.488368 0.260724, 0.478436 0.260724 0.474716 0.253418, 0.476081 0.253418, 0.476081 0.258979 0.474716 0.253418, 0.476081 0.258979, 0.474716 0.258979 0.467886 0.253418, 0.469274 0.253418, 0.469274 0.258979 0.467886 0.253418, 0.469274 0.258979, 0.467886 0.258979 0.28126 0.918852, 0.15407 0.918852, 0.154149 0.2931 0.28126 0.918852, 0.154149 0.2931, 0.28155 0.2931 0.8484 0.918852, 0.721 0.918852, 0.848385 0.2931 0.721 0.918852, 0.720985 0.2931, 0.848385 0.2931 0.721 0.918852, 0.28126 0.918852, 0.720985 0.2931 0.28126 0.918852, 0.28155 0.2931, 0.720985 0.2931 0.469274 0.253418, 0.474716 0.253418, 0.474716 0.258979 0.469274 0.253418, 0.474716 0.258979, 0.469274 0.258979 0.47782 0.261384, 0.487449 0.261384, 0.487449 0.267376 0.47782 0.261384, 0.487449 0.267376, 0.47782 0.267376 0.467362 0.262667, 0.477042 0.262667, 0.467362 0.26707 0.477042 0.262667, 0.477042 0.26707, 0.467362 0.26707 - ] - } - coordIndex [ - 12 13 14 -1 12 14 11 -1 8 9 7 -1 9 10 7 -1 10 11 14 -1 10 14 7 -1 1 0 14 -1 1 14 13 -1 3 2 12 -1 3 12 11 -1 15 4 10 -1 15 10 9 -1 6 5 7 -1 5 8 7 -1 5 15 8 -1 15 9 8 -1 2 1 13 -1 2 13 12 -1 4 3 11 -1 4 11 10 -1 0 6 14 -1 6 7 14 -1 - ] - texCoordIndex [ - 0 1 2 -1 3 4 5 -1 6 7 8 -1 9 10 11 -1 12 13 14 -1 15 16 17 -1 18 19 20 -1 21 22 23 -1 24 25 26 -1 27 28 29 -1 30 31 32 -1 33 34 35 -1 36 37 38 -1 39 40 41 -1 42 43 44 -1 45 46 47 -1 48 49 50 -1 51 52 53 -1 54 55 56 -1 57 58 59 -1 60 61 62 -1 63 64 65 -1 - ] - } - } - DEF AXLES_SHAPES Shape { - appearance PBRAppearance { - baseColor 0 0 0 - roughness 0.3 - metalness 0 - } - geometry IndexedFaceSet { - coord Coordinate { - point [ - 0.208747 0.0179386 -0.114638, 0.208747 0.00964805 -0.109099 0.208747 -0.000131354 -0.107153, 0.208747 -0.00991076 -0.109099 0.208747 -0.0182013 -0.114638, 0.208747 -0.0237409 -0.122929 0.208747 -0.0256862 -0.132708, 0.208747 -0.0237409 -0.142488 0.208747 -0.0182013 -0.150778, 0.208747 -0.00991076 -0.156318 0.208747 -0.000131353 -0.158263, 0.208747 0.00964806 -0.156318 0.208747 0.0179386 -0.150778, 0.208747 0.0234782 -0.142488 0.208747 0.0254235 -0.132708, 0.208747 0.0234782 -0.122929 -0.208177 0.0179386 -0.114638, -0.208177 0.00964802 -0.109099 -0.208177 -0.00013139 -0.107153, -0.208177 -0.0099108 -0.109099 -0.208177 -0.0182014 -0.114638, -0.208177 -0.0237409 -0.122929 -0.208177 -0.0256862 -0.132708, -0.208177 -0.0237409 -0.142488 -0.208177 -0.0182013 -0.150778, -0.208177 -0.00991071 -0.156318 -0.208177 -0.000131291 -0.158263, -0.208177 0.00964811 -0.156318 -0.208177 0.0179387 -0.150778, -0.208177 0.0234782 -0.142487 -0.208177 0.0254235 -0.132708, -0.208177 0.0234782 -0.122929 0.208747 -0.000131353 -0.132708, -0.208177 -0.000131357 -0.132708 -0.208177 -0.000131357 0.133169, 0.208747 -0.000131353 0.133169 -0.208177 0.0234782 0.142948, -0.208177 0.0254235 0.133169 -0.208177 0.0234782 0.123389, -0.208177 0.0179387 0.115099 -0.208177 0.00964811 0.109559, -0.208177 -0.000131291 0.107614 -0.208177 -0.00991071 0.109559, -0.208177 -0.0182013 0.115099 -0.208177 -0.0237409 0.123389, -0.208177 -0.0256862 0.133169 -0.208177 -0.0237409 0.142948, -0.208177 -0.0182014 0.151239 -0.208177 -0.0099108 0.156778, -0.208177 -0.00013139 0.158723 -0.208177 0.00964802 0.156778, -0.208177 0.0179386 0.151239 0.208747 0.0234782 0.142948, 0.208747 0.0254235 0.133169 0.208747 0.0234782 0.123389, 0.208747 0.0179386 0.115099 0.208747 0.00964806 0.109559, 0.208747 -0.000131353 0.107614 0.208747 -0.00991076 0.109559, 0.208747 -0.0182013 0.115099 0.208747 -0.0237409 0.123389, 0.208747 -0.0256862 0.133169 0.208747 -0.0237409 0.142948, 0.208747 -0.0182013 0.151239 0.208747 -0.00991076 0.156778, 0.208747 -0.000131354 0.158723 0.208747 0.00964805 0.156778, 0.208747 0.0179386 0.151239 - ] - } - coordIndex [ - 32 0 1 -1 33 17 16 -1 32 1 2 -1 33 18 17 -1 32 2 3 -1 33 19 18 -1 32 3 4 -1 33 20 19 -1 32 4 5 -1 33 21 20 -1 32 5 6 -1 33 22 21 -1 32 6 7 -1 33 23 22 -1 32 7 8 -1 33 24 23 -1 32 8 9 -1 33 25 24 -1 32 9 10 -1 33 26 25 -1 32 10 11 -1 33 27 26 -1 32 11 12 -1 33 28 27 -1 32 12 13 -1 33 29 28 -1 32 13 14 -1 33 30 29 -1 32 14 15 -1 33 31 30 -1 15 0 32 -1 33 16 31 -1 0 16 17 1 -1 1 17 18 2 -1 2 18 19 3 -1 3 19 20 4 -1 4 20 21 5 -1 5 21 22 6 -1 6 22 23 7 -1 7 23 24 8 -1 8 24 25 9 -1 9 25 26 10 -1 10 26 27 11 -1 11 27 28 12 -1 12 28 29 13 -1 13 29 30 14 -1 14 30 31 15 -1 16 0 15 31 -1 34 51 36 -1 52 67 35 -1 34 36 37 -1 35 53 52 -1 34 37 38 -1 35 54 53 -1 34 38 39 -1 35 55 54 -1 34 39 40 -1 35 56 55 -1 34 40 41 -1 35 57 56 -1 34 41 42 -1 35 58 57 -1 34 42 43 -1 35 59 58 -1 34 43 44 -1 35 60 59 -1 34 44 45 -1 35 61 60 -1 34 45 46 -1 35 62 61 -1 34 46 47 -1 35 63 62 -1 34 47 48 -1 35 64 63 -1 34 48 49 -1 35 65 64 -1 34 49 50 -1 35 66 65 -1 34 50 51 -1 35 67 66 -1 67 51 50 -1 67 50 66 -1 66 50 49 -1 66 49 65 -1 65 49 48 -1 65 48 64 -1 64 48 47 -1 64 47 63 -1 63 47 46 -1 63 46 62 -1 62 46 45 -1 62 45 61 -1 61 45 44 -1 61 44 60 -1 60 44 43 -1 60 43 59 -1 59 43 42 -1 59 42 58 -1 58 42 41 -1 58 41 57 -1 57 41 40 -1 57 40 56 -1 56 40 39 -1 56 39 55 -1 55 39 38 -1 55 38 54 -1 54 38 37 -1 54 37 53 -1 53 37 36 -1 53 36 52 -1 51 67 52 -1 51 52 36 -1 - ] - creaseAngle 1 - } - } - DEF BACK_LEFT_WHEEL HingeJoint { - device [ - RotationalMotor { - name "back left wheel" - maxVelocity 6.4 - maxTorque 20 - } - PositionSensor { - name "back left wheel sensor" - } - ] - jointParameters HingeJointParameters { - axis -1 0 0 - anchor -0.197 0 0.1331 - } - endPoint Solid { - translation -0.197 0 0.1331 - rotation -1 0 0 0 - children [ - DEF RIM_AND_TIRE_SHAPES Group { - children [ - DEF RIM_SHAPE Shape { - appearance PBRAppearance { - baseColor 1 0.7 0 - roughness 0.3 - metalness 0 - } - geometry IndexedFaceSet { - coord Coordinate { - point [ - 0.0106186 0 0, -0.0106186 0 0 -0.0482485 0.0116779 -0.0587088, -0.0106186 0.0021733 -0.0108281 -0.0134628 0.0021627 -0.0107747, -0.0138124 0.0086022 -0.0430499 -0.0170841 0.0089469 -0.044783, -0.0418327 0.0090817 -0.045657 -0.0438648 0.0097712 -0.0491232, -0.0445422 0.0104176 -0.0523728 -0.0448397 0.010924 -0.0549188, -0.0463033 0.0109184 -0.0548908 -0.0462741 0.0112769 -0.0566926, -0.0482485 0.0112769 -0.0566926 0.0482485 0.0116779 -0.0587088, 0.0106186 0.0021733 -0.0108281 0.0134627 0.0021627 -0.0107747, 0.0138124 0.0086022 -0.0430499 0.017084 0.0089469 -0.044783, 0.0418327 0.0090817 -0.045657 0.0438648 0.0097712 -0.0491232, 0.0445422 0.0104176 -0.0523728 0.0448397 0.010924 -0.0549188, 0.0463033 0.0109184 -0.0548908 0.0462741 0.0112769 -0.0566927, 0.0482485 0.0112769 -0.0566927 0.0482485 0.0221204 -0.0534033, 0.0462741 0.0221204 -0.0534033 0.0463033 0.0214173 -0.051706, 0.0448397 0.0214282 -0.0517324 0.0445422 0.0204348 -0.0493341, 0.0438648 0.0191669 -0.0462731 0.0418327 0.0178145 -0.043008, 0.017084 0.0175117 -0.0421771 0.0138124 0.0168355 -0.0405445, 0.0134627 0.0042232 -0.0101457 0.0106186 0.0042441 -0.0101961, 0.0482485 0.022907 -0.0553024 -0.0482485 0.0221204 -0.0534033, -0.0462741 0.0221204 -0.0534033 -0.0463033 0.0214173 -0.051706, -0.0448397 0.0214282 -0.0517324 -0.0445422 0.0204348 -0.0493341, -0.0438648 0.0191669 -0.0462731 -0.0418327 0.0178145 -0.043008, -0.0170841 0.0175117 -0.0421771 -0.0138124 0.0168355 -0.0405445, -0.0134628 0.0042232 -0.0101457 -0.0106186 0.0042441 -0.0101961, -0.0482485 0.022907 -0.0553024 -0.0482485 0.0332558 -0.0497709, -0.0106186 0.0061517 -0.0091722 -0.0134628 0.0061214 -0.0091269, -0.0138124 0.0244218 -0.036481 -0.0170841 0.0254036 -0.0379503, -0.0418327 0.0258626 -0.0387062 -0.0438648 0.0278261 -0.0416447, -0.0445422 0.0296668 -0.0443995 -0.0448397 0.031109 -0.0465579, -0.0463033 0.0310931 -0.0465341 -0.0462741 0.0321138 -0.0480617, -0.0482485 0.0321138 -0.0480617 0.0482485 0.0332558 -0.0497709, 0.0106186 0.0061517 -0.0091722 0.0134627 0.0061214 -0.0091269, 0.0138124 0.0244218 -0.036481 0.017084 0.0254036 -0.0379503, 0.0418327 0.0258626 -0.0387062 0.0438648 0.0278261 -0.0416447, 0.0445422 0.0296668 -0.0443995 0.0448397 0.031109 -0.0465579, 0.0463033 0.0310931 -0.0465342 0.0462741 0.0321138 -0.0480617, 0.0482485 0.0321138 -0.0480617 0.0482485 0.0408731 -0.0408731, 0.0462741 0.0408731 -0.0408731 0.0463033 0.039574 -0.039574, 0.0448397 0.0395942 -0.0395943 0.0445422 0.0377587 -0.0377587, 0.0438648 0.0354159 -0.0354159 0.0418327 0.0329169 -0.0329169, 0.017084 0.0323192 -0.0322651 0.0138124 0.0310697 -0.0310156, 0.0134628 0.0077843 -0.0077573 0.0106186 0.0078229 -0.0077958, 0.0482485 0.0423266 -0.0423267 -0.0482485 0.0408731 -0.0408731, -0.0462741 0.0408731 -0.0408731 -0.0463033 0.039574 -0.039574, -0.0448397 0.0395942 -0.0395942 -0.0445422 0.0377587 -0.0377587, -0.0438648 0.0354159 -0.0354159 -0.0418327 0.0329169 -0.0329169, -0.0170841 0.0323192 -0.0322651 -0.0138124 0.0310697 -0.0310156, -0.0134628 0.0077843 -0.0077573 -0.0106186 0.0078229 -0.0077958, -0.0482485 0.0423267 -0.0423266 -0.0482485 0.0497709 -0.0332558, -0.0106186 0.0091934 -0.0061198 -0.0134628 0.0091481 -0.0060896, -0.0138124 0.0365235 -0.0243582 -0.0170841 0.0379928 -0.02534, -0.0418327 0.0387062 -0.0258626 -0.0438648 0.0416447 -0.0278261, -0.0445422 0.0443995 -0.0296668 -0.0448397 0.0465579 -0.031109, -0.0463033 0.0465341 -0.0310931 -0.0462741 0.0480617 -0.0321138, -0.0482485 0.0480617 -0.0321138 0.0482485 0.0497709 -0.0332558, 0.0106186 0.0091934 -0.0061198 0.0134628 0.0091481 -0.0060896, 0.0138124 0.0365235 -0.0243582 0.017084 0.0379928 -0.02534, 0.0418327 0.0387062 -0.0258626 0.0438648 0.0416447 -0.0278261, 0.0445422 0.0443995 -0.0296668 0.0448397 0.0465579 -0.031109, 0.0463033 0.0465341 -0.0310931 0.0462741 0.0480617 -0.0321138, 0.0482485 0.0480617 -0.0321138 0.0482485 0.0534033 -0.0221204, 0.0462741 0.0534033 -0.0221204 0.0463033 0.051706 -0.0214173, 0.0448397 0.0517324 -0.0214283 0.0445422 0.0493341 -0.0204349, 0.0438648 0.0462731 -0.0191669 0.0418327 0.043008 -0.0178145, 0.017084 0.0422063 -0.017441 0.0138124 0.0405738 -0.0167648, 0.0134628 0.0101604 -0.0041879 0.0106186 0.0102107 -0.0042087, 0.0482485 0.0553024 -0.022907 -0.0482485 0.0534033 -0.0221204, -0.0462741 0.0534033 -0.0221204 -0.0463033 0.051706 -0.0214173, -0.0448397 0.0517324 -0.0214282 -0.0445422 0.0493341 -0.0204348, -0.0438648 0.0462731 -0.0191669 -0.0418327 0.043008 -0.0178145, -0.0170841 0.0422063 -0.017441 -0.0138124 0.0405738 -0.0167648, -0.0134628 0.0101604 -0.0041878 -0.0106186 0.0102107 -0.0042087, -0.0482485 0.0553024 -0.022907 -0.0482485 0.0587087 -0.0116779, -0.0106186 0.0108356 -0.0021358 -0.0134628 0.0107821 -0.0021252, -0.0138124 0.0430648 -0.0085271 -0.0170841 0.0447979 -0.0088719, -0.0418327 0.045657 -0.0090817 -0.0438648 0.0491232 -0.0097712, -0.0445422 0.0523728 -0.0104176 -0.0448397 0.0549188 -0.010924, -0.0463033 0.0548908 -0.0109184 -0.0462741 0.0566926 -0.0112769, -0.0482485 0.0566926 -0.0112769 0.0482485 0.0587087 -0.0116779, 0.0106186 0.0108356 -0.0021358 0.0134628 0.0107821 -0.0021252, 0.0138124 0.0430648 -0.0085271 0.017084 0.0447979 -0.0088719, 0.0418327 0.045657 -0.0090818 0.0438648 0.0491232 -0.0097712, 0.0445422 0.0523728 -0.0104176 0.0448397 0.0549188 -0.010924, 0.0463033 0.0548908 -0.0109185 0.0462741 0.0566926 -0.0112769, 0.0482485 0.0566926 -0.0112769 0.0482485 0.0578033 0, 0.0462741 0.0578033 0 0.0463033 0.0559661 0, 0.0448397 0.0559947 0 0.0445422 0.0533988 0, 0.0438648 0.0500856 0 0.0418327 0.0465515 0, 0.017084 0.045668 3.83e-05 0.0138124 0.0439009 3.82e-05, 0.0134628 0.0109896 1.91e-05 0.0106186 0.0110441 1.91e-05, 0.0482485 0.0598589 0 -0.0482485 0.0578033 0, -0.0462741 0.0578033 0 -0.0463033 0.0559661 0, -0.0448397 0.0559947 0 -0.0445422 0.0533988 0, -0.0438648 0.0500856 0 -0.0418327 0.0465515 0, -0.0170841 0.045668 3.83e-05 -0.0138124 0.0439009 3.83e-05, -0.0134628 0.0109896 1.91e-05 -0.0106186 0.0110441 1.91e-05, -0.0482485 0.0598589 0 -0.0482485 0.0587087 0.0116779, -0.0106186 0.0108281 0.0021734 -0.0134628 0.0107747 0.0021627, -0.0138124 0.0430499 0.0086022 -0.0170841 0.044783 0.0089469, -0.0418327 0.045657 0.0090818 -0.0438648 0.0491232 0.0097712, -0.0445422 0.0523728 0.0104176 -0.0448397 0.0549188 0.010924, -0.0463033 0.0548908 0.0109185 -0.0462741 0.0566926 0.0112769, -0.0482485 0.0566926 0.0112769 0.0482485 0.0587087 0.0116779, 0.0106186 0.0108281 0.0021734 0.0134628 0.0107747 0.0021627, 0.0138124 0.0430499 0.0086022 0.0170841 0.044783 0.0089469, 0.0418327 0.045657 0.0090817 0.0438648 0.0491232 0.0097712, 0.0445422 0.0523728 0.0104176 0.0448397 0.0549188 0.010924, 0.0463033 0.0548908 0.0109184 0.0462741 0.0566926 0.0112769, 0.0482485 0.0566926 0.0112769 0.0482485 0.0534033 0.0221204, 0.0462741 0.0534033 0.0221204 0.0463033 0.051706 0.0214173, 0.0448397 0.0517324 0.0214282 0.0445422 0.0493341 0.0204348, 0.0438648 0.046273 0.0191669 0.0418327 0.043008 0.0178145, 0.0170841 0.0421771 0.0175117 0.0138124 0.0405445 0.0168355, 0.0134628 0.0101457 0.0042232 0.0106186 0.0101961 0.0042441, 0.0482485 0.0553024 0.022907 -0.0482485 0.0534033 0.0221204, -0.0462741 0.0534033 0.0221204 -0.0463033 0.051706 0.0214173, -0.0448397 0.0517324 0.0214283 -0.0445422 0.0493341 0.0204349, -0.0438648 0.046273 0.0191669 -0.0418327 0.043008 0.0178145, -0.0170841 0.0421771 0.0175117 -0.0138124 0.0405445 0.0168355, -0.0134628 0.0101457 0.0042232 -0.0106186 0.0101961 0.0042441, -0.0482485 0.0553024 0.022907 -0.0482485 0.0497709 0.0332558, -0.0106186 0.0091722 0.0061517 -0.0134628 0.0091269 0.0061214, -0.0138124 0.036481 0.0244219 -0.0170841 0.0379503 0.0254036, -0.0418327 0.0387061 0.0258626 -0.0438648 0.0416446 0.0278261, -0.0445422 0.0443995 0.0296668 -0.0448397 0.0465579 0.031109, -0.0463033 0.0465341 0.0310931 -0.0462741 0.0480617 0.0321138, -0.0482485 0.0480617 0.0321138 0.0482486 0.0497709 0.0332558, 0.0106186 0.0091722 0.0061517 0.0134628 0.0091269 0.0061214, 0.0138124 0.036481 0.0244219 0.0170841 0.0379503 0.0254036, 0.0418327 0.0387061 0.0258626 0.0438648 0.0416446 0.0278261, 0.0445422 0.0443995 0.0296668 0.0448397 0.0465579 0.031109, 0.0463033 0.0465341 0.0310931 0.0462741 0.0480617 0.0321138, 0.0482486 0.0480617 0.0321138 0.0482486 0.0408731 0.0408731, 0.0462741 0.0408731 0.0408731 0.0463033 0.039574 0.039574, 0.0448397 0.0395942 0.0395942 0.0445422 0.0377587 0.0377587, 0.0438648 0.0354158 0.0354159 0.0418327 0.0329169 0.0329169, 0.0170841 0.0322651 0.0323192 0.0138124 0.0310156 0.0310697, 0.0134628 0.0077573 0.0077843 0.0106186 0.0077958 0.0078229, 0.0482486 0.0423266 0.0423266 -0.0482485 0.0408731 0.0408731, -0.0462741 0.0408731 0.0408731 -0.0463033 0.039574 0.039574, -0.0448397 0.0395942 0.0395943 -0.0445422 0.0377587 0.0377587, -0.0438648 0.0354158 0.0354159 -0.0418327 0.0329169 0.0329169, -0.0170841 0.0322651 0.0323192 -0.0138124 0.0310156 0.0310697, -0.0134628 0.0077573 0.0077843 -0.0106186 0.0077958 0.0078229, -0.0482485 0.0423266 0.0423267 -0.0482485 0.0332558 0.0497709, -0.0106186 0.0061198 0.0091934 -0.0134628 0.0060896 0.0091481, -0.0138124 0.0243582 0.0365235 -0.0170841 0.0253399 0.0379928, -0.0418327 0.0258626 0.0387062 -0.0438648 0.027826 0.0416447, -0.0445422 0.0296668 0.0443995 -0.0448397 0.031109 0.0465579, -0.0463033 0.0310931 0.0465341 -0.0462741 0.0321138 0.0480617, -0.0482485 0.0321138 0.0480617 0.0482486 0.0332558 0.0497709, 0.0106186 0.0061198 0.0091934 0.0134628 0.0060896 0.0091481, 0.0138124 0.0243582 0.0365235 0.0170841 0.0253399 0.0379928, 0.0418327 0.0258626 0.0387061 0.0438648 0.027826 0.0416446, 0.0445422 0.0296668 0.0443995 0.0448397 0.031109 0.0465579, 0.0463033 0.0310931 0.0465341 0.0462741 0.0321138 0.0480617, 0.0482486 0.0321138 0.0480617 0.0482486 0.0221204 0.0534033, 0.0462741 0.0221204 0.0534033 0.0463033 0.0214173 0.051706, 0.0448397 0.0214282 0.0517324 0.0445422 0.0204348 0.0493341, 0.0438648 0.0191669 0.046273 0.0418327 0.0178145 0.043008, 0.0170841 0.017441 0.0422063 0.0138124 0.0167648 0.0405738, 0.0134628 0.0041878 0.0101604 0.0106186 0.0042087 0.0102107, 0.0482486 0.022907 0.0553024 -0.0482485 0.0221204 0.0534033, -0.0462741 0.0221204 0.0534033 -0.0463033 0.0214173 0.051706, -0.0448397 0.0214282 0.0517324 -0.0445422 0.0204348 0.0493341, -0.0438648 0.0191669 0.0462731 -0.0418327 0.0178145 0.043008, -0.0170841 0.017441 0.0422063 -0.0138124 0.0167648 0.0405738, -0.0134628 0.0041878 0.0101604 -0.0106186 0.0042087 0.0102107, -0.0482485 0.022907 0.0553024 -0.0482485 0.0116779 0.0587087, -0.0106186 0.0021358 0.0108356 -0.0134628 0.0021252 0.0107821, -0.0138124 0.0085271 0.0430649 -0.0170841 0.0088719 0.0447979, -0.0418327 0.0090817 0.045657 -0.0438648 0.0097712 0.0491232, -0.0445422 0.0104176 0.0523728 -0.0448397 0.010924 0.0549188, -0.0463033 0.0109184 0.0548908 -0.0462741 0.0112769 0.0566927, -0.0482485 0.0112769 0.0566927 0.0482486 0.0116779 0.0587087, 0.0106186 0.0021358 0.0108356 0.0134628 0.0021252 0.0107821, 0.0138124 0.0085271 0.0430648 0.0170841 0.0088719 0.0447979, 0.0418327 0.0090817 0.045657 0.0438648 0.0097712 0.0491232, 0.0445422 0.0104176 0.0523728 0.0448397 0.010924 0.0549188, 0.0463034 0.0109184 0.0548907 0.0462741 0.0112769 0.0566926, 0.0482486 0.0112769 0.0566926 0.0482486 0 0.0578033, 0.0462741 0 0.0578033 0.0463034 0 0.0559661, 0.0448397 0 0.0559947 0.0445422 0 0.0533988, 0.0438648 0 0.0500856 0.0418327 0 0.0465515, 0.0170841 -3.83e-05 0.045668 0.0138124 -3.83e-05 0.0439009, 0.0134628 -1.91e-05 0.0109896 0.0106186 -1.91e-05 0.0110441, 0.0482486 0 0.0598589 -0.0482485 0 0.0578033, -0.0462741 0 0.0578033 -0.0463033 0 0.0559661, -0.0448397 0 0.0559947 -0.0445422 0 0.0533988, -0.0438648 0 0.0500856 -0.0418327 0 0.0465515, -0.0170841 -3.83e-05 0.045668 -0.0138124 -3.83e-05 0.0439009, -0.0134628 -1.91e-05 0.0109896 -0.0106186 -1.91e-05 0.0110441, -0.0482485 0 0.0598589 -0.0482485 -0.0116779 0.0587087, -0.0106186 -0.0021734 0.0108281 -0.0134628 -0.0021627 0.0107747, -0.0138124 -0.0086022 0.0430499 -0.0170841 -0.0089469 0.044783, -0.0418327 -0.0090818 0.045657 -0.0438648 -0.0097712 0.0491232, -0.0445422 -0.0104176 0.0523728 -0.0448397 -0.010924 0.0549188, -0.0463033 -0.0109185 0.0548908 -0.0462741 -0.0112769 0.0566926, -0.0482485 -0.0112769 0.0566926 0.0482486 -0.0116779 0.0587087, 0.0106186 -0.0021734 0.0108281 0.0134628 -0.0021627 0.0107747, 0.0138124 -0.0086022 0.0430499 0.0170841 -0.0089469 0.044783, 0.0418327 -0.0090818 0.045657 0.0438648 -0.0097712 0.0491232, 0.0445422 -0.0104176 0.0523728 0.0448397 -0.010924 0.0549188, 0.0463034 -0.0109185 0.0548907 0.0462741 -0.0112769 0.0566926, 0.0482486 -0.0112769 0.0566926 0.0482486 -0.0221204 0.0534033, 0.0462741 -0.0221204 0.0534033 0.0463033 -0.0214173 0.0517059, 0.0448397 -0.0214283 0.0517324 0.0445422 -0.0204349 0.0493341, 0.0438648 -0.0191669 0.046273 0.0418327 -0.0178145 0.043008, 0.0170841 -0.0175117 0.0421771 0.0138124 -0.0168355 0.0405445, 0.0134628 -0.0042232 0.0101457 0.0106186 -0.0042441 0.0101961, 0.0482486 -0.022907 0.0553024 -0.0482485 -0.0221204 0.0534033, -0.0462741 -0.0221204 0.0534033 -0.0463033 -0.0214173 0.051706, -0.0448397 -0.0214283 0.0517324 -0.0445422 -0.0204349 0.0493341, -0.0438648 -0.0191669 0.046273 -0.0418327 -0.0178145 0.043008, -0.0170841 -0.0175117 0.0421771 -0.0138124 -0.0168355 0.0405445, -0.0134628 -0.0042232 0.0101457 -0.0106186 -0.0042441 0.0101961, -0.0482485 -0.022907 0.0553024 -0.0482485 -0.0332558 0.0497709, -0.0106186 -0.0061517 0.0091722 -0.0134628 -0.0061214 0.0091269, -0.0138124 -0.0244219 0.036481 -0.0170841 -0.0254036 0.0379503, -0.0418327 -0.0258626 0.0387061 -0.0438648 -0.0278261 0.0416446, -0.0445422 -0.0296668 0.0443995 -0.0448397 -0.031109 0.0465579, -0.0463033 -0.0310931 0.0465341 -0.0462741 -0.0321138 0.0480617, -0.0482485 -0.0321138 0.0480617 0.0482486 -0.0332558 0.0497709, 0.0106186 -0.0061517 0.0091722 0.0134628 -0.0061214 0.0091269, 0.0138124 -0.0244219 0.036481 0.0170841 -0.0254036 0.0379503, 0.0418327 -0.0258626 0.0387061 0.0438648 -0.0278261 0.0416446, 0.0445422 -0.0296668 0.0443995 0.0448397 -0.031109 0.0465579, 0.0463033 -0.0310931 0.0465341 0.0462741 -0.0321138 0.0480617, 0.0482486 -0.0321138 0.0480617 0.0482486 -0.0408731 0.0408731, 0.0462741 -0.0408731 0.0408731 0.0463033 -0.039574 0.039574, 0.0448397 -0.0395942 0.0395942 0.0445422 -0.0377587 0.0377587, 0.0438648 -0.0354159 0.0354158 0.0418327 -0.0329169 0.0329169, 0.0170841 -0.0323192 0.0322651 0.0138124 -0.0310697 0.0310156, 0.0134628 -0.0077843 0.0077573 0.0106186 -0.0078229 0.0077958, 0.0482486 -0.0423267 0.0423266 -0.0482485 -0.0408731 0.0408731, -0.0462741 -0.0408731 0.0408731 -0.0463033 -0.039574 0.039574, -0.0448397 -0.0395942 0.0395942 -0.0445422 -0.0377587 0.0377587, -0.0438648 -0.0354159 0.0354158 -0.0418327 -0.0329169 0.0329169, -0.0170841 -0.0323192 0.0322651 -0.0138124 -0.0310697 0.0310156, -0.0134628 -0.0077843 0.0077573 -0.0106186 -0.0078229 0.0077958, -0.0482485 -0.0423266 0.0423266 -0.0482485 -0.0497709 0.0332558, -0.0106186 -0.0091934 0.0061198 -0.0134628 -0.0091481 0.0060896, -0.0138124 -0.0365235 0.0243582 -0.0170841 -0.0379928 0.02534, -0.0418327 -0.0387061 0.0258626 -0.0438648 -0.0416446 0.027826, -0.0445422 -0.0443995 0.0296668 -0.0448397 -0.0465579 0.031109, -0.0463033 -0.0465341 0.0310931 -0.0462741 -0.0480617 0.0321138, -0.0482485 -0.0480617 0.0321138 0.0482486 -0.0497709 0.0332558, 0.0106186 -0.0091934 0.0061198 0.0134628 -0.0091481 0.0060896, 0.0138124 -0.0365235 0.0243582 0.0170841 -0.0379928 0.0253399, 0.0418327 -0.0387061 0.0258626 0.0438648 -0.0416446 0.027826, 0.0445422 -0.0443995 0.0296668 0.0448397 -0.0465579 0.031109, 0.0463033 -0.0465341 0.0310931 0.0462741 -0.0480617 0.0321138, 0.0482486 -0.0480617 0.0321138 0.0482485 -0.0534033 0.0221204, 0.0462741 -0.0534033 0.0221204 0.0463033 -0.051706 0.0214173, 0.0448397 -0.0517324 0.0214282 0.0445422 -0.0493341 0.0204348, 0.0438648 -0.046273 0.0191669 0.0418327 -0.043008 0.0178145, 0.0170841 -0.0422063 0.017441 0.0138124 -0.0405738 0.0167648, 0.0134628 -0.0101604 0.0041878 0.0106186 -0.0102107 0.0042087, 0.0482485 -0.0553024 0.022907 -0.0482485 -0.0534033 0.0221204, -0.0462741 -0.0534033 0.0221204 -0.0463033 -0.051706 0.0214173, -0.0448397 -0.0517324 0.0214282 -0.0445422 -0.0493341 0.0204349, -0.0438648 -0.046273 0.0191669 -0.0418327 -0.043008 0.0178145, -0.0170841 -0.0422063 0.017441 -0.0138124 -0.0405738 0.0167648, -0.0134628 -0.0101604 0.0041878 -0.0106186 -0.0102107 0.0042087, -0.0482485 -0.0553024 0.022907 -0.0482485 -0.0587087 0.0116779, -0.0106186 -0.0108356 0.0021358 -0.0134628 -0.0107821 0.0021252, -0.0138124 -0.0430649 0.0085271 -0.0170841 -0.0447979 0.0088719, -0.0418327 -0.045657 0.0090817 -0.0438648 -0.0491232 0.0097712, -0.0445422 -0.0523728 0.0104176 -0.0448397 -0.0549188 0.010924, -0.0463033 -0.0548908 0.0109184 -0.0462741 -0.0566926 0.0112769, -0.0482485 -0.0566926 0.0112769 0.0482485 -0.0587087 0.0116779, 0.0106186 -0.0108356 0.0021358 0.0134628 -0.0107821 0.0021252, 0.0138124 -0.0430649 0.0085271 0.0170841 -0.0447979 0.0088719, 0.0418327 -0.045657 0.0090817 0.0438648 -0.0491232 0.0097712, 0.0445422 -0.0523728 0.0104176 0.0448397 -0.0549188 0.010924, 0.0463033 -0.0548908 0.0109184 0.0462741 -0.0566926 0.0112768, 0.0482485 -0.0566926 0.0112769 0.0482485 -0.0578033 0, 0.0462741 -0.0578033 0 0.0463033 -0.0559661 0, 0.0448397 -0.0559947 0 0.0445422 -0.0533988 0, 0.0438648 -0.0500856 0 0.0418327 -0.0465515 0, 0.017084 -0.045668 -3.83e-05 0.0138124 -0.0439009 -3.83e-05, 0.0134628 -0.0109896 -1.91e-05 0.0106186 -0.0110441 -1.91e-05, 0.0482485 -0.0598589 0 -0.0482485 -0.0578033 0, -0.0462741 -0.0578033 0 -0.0463033 -0.0559661 0, -0.0448397 -0.0559947 0 -0.0445422 -0.0533988 0, -0.0438648 -0.0500856 0 -0.0418327 -0.0465515 0, -0.0170841 -0.045668 -3.83e-05 -0.0138124 -0.0439009 -3.83e-05, -0.0134628 -0.0109896 -1.91e-05 -0.0106186 -0.0110441 -1.91e-05, -0.0482485 -0.0598589 0 -0.0482485 -0.0587087 -0.0116779, -0.0106186 -0.0108281 -0.0021734 -0.0134628 -0.0107747 -0.0021627, -0.0138124 -0.0430499 -0.0086022 -0.0170841 -0.044783 -0.0089469, -0.0418327 -0.045657 -0.0090817 -0.0438648 -0.0491232 -0.0097712, -0.0445422 -0.0523728 -0.0104176 -0.0448397 -0.0549188 -0.010924, -0.0463033 -0.0548907 -0.0109184 -0.0462741 -0.0566926 -0.0112769, -0.0482485 -0.0566926 -0.0112769 0.0482485 -0.0587087 -0.0116779, 0.0106186 -0.0108281 -0.0021734 0.0134628 -0.0107747 -0.0021627, 0.0138124 -0.0430499 -0.0086022 0.017084 -0.044783 -0.0089469, 0.0418327 -0.045657 -0.0090818 0.0438648 -0.0491232 -0.0097712, 0.0445422 -0.0523728 -0.0104176 0.0448397 -0.0549188 -0.010924, 0.0463033 -0.0548907 -0.0109185 0.0462741 -0.0566926 -0.0112769, 0.0482485 -0.0566926 -0.0112769 0.0482485 -0.0534033 -0.0221204, 0.0462741 -0.0534033 -0.0221204 0.0463033 -0.0517059 -0.0214173, 0.0448397 -0.0517324 -0.0214283 0.0445422 -0.0493341 -0.0204349, 0.0438648 -0.046273 -0.0191669 0.0418327 -0.043008 -0.0178145, 0.017084 -0.0421771 -0.0175117 0.0138124 -0.0405445 -0.0168355, 0.0134628 -0.0101457 -0.0042232 0.0106186 -0.0101961 -0.0042441, 0.0482485 -0.0553024 -0.022907 -0.0482485 -0.0534033 -0.0221204, -0.0462741 -0.0534033 -0.0221204 -0.0463033 -0.0517059 -0.0214173, -0.0448397 -0.0517324 -0.0214282 -0.0445422 -0.0493341 -0.0204348, -0.0438648 -0.046273 -0.0191669 -0.0418327 -0.043008 -0.0178145, -0.0170841 -0.0421771 -0.0175117 -0.0138124 -0.0405445 -0.0168355, -0.0134628 -0.0101457 -0.0042232 -0.0106186 -0.0101961 -0.0042441, -0.0482485 -0.0553024 -0.022907 -0.0482485 -0.0497709 -0.0332558, -0.0106186 -0.0091722 -0.0061517 -0.0134628 -0.0091269 -0.0061214, -0.0138124 -0.036481 -0.0244219 -0.0170841 -0.0379503 -0.0254036, -0.0418327 -0.0387061 -0.0258626 -0.0438648 -0.0416446 -0.0278261, -0.0445422 -0.0443995 -0.0296668 -0.0448397 -0.0465579 -0.031109, -0.0463033 -0.0465341 -0.0310931 -0.0462741 -0.0480617 -0.0321138, -0.0482485 -0.0480617 -0.0321138 0.0482485 -0.0497709 -0.0332558, 0.0106186 -0.0091722 -0.0061517 0.0134628 -0.0091269 -0.0061214, 0.0138124 -0.036481 -0.0244219 0.017084 -0.0379503 -0.0254036, 0.0418327 -0.0387061 -0.0258626 0.0438648 -0.0416446 -0.0278261, 0.0445422 -0.0443995 -0.0296668 0.0448397 -0.0465579 -0.031109, 0.0463033 -0.0465341 -0.0310931 0.0462741 -0.0480617 -0.0321138, 0.0482485 -0.0480617 -0.0321138 0.0482485 -0.0408731 -0.0408731, 0.0462741 -0.0408731 -0.0408731 0.0463033 -0.039574 -0.039574, 0.0448397 -0.0395942 -0.0395942 0.0445422 -0.0377587 -0.0377587, 0.0438648 -0.0354158 -0.0354159 0.0418327 -0.0329169 -0.0329169, 0.017084 -0.0322651 -0.0323192 0.0138124 -0.0310156 -0.0310697, 0.0134628 -0.0077573 -0.0077843 0.0106186 -0.0077958 -0.0078229, 0.0482485 -0.0423266 -0.0423267 -0.0482485 -0.0408731 -0.0408731, -0.0462741 -0.0408731 -0.0408731 -0.0463033 -0.039574 -0.039574, -0.0448397 -0.0395942 -0.0395942 -0.0445422 -0.0377587 -0.0377587, -0.0438648 -0.0354158 -0.0354158 -0.0418327 -0.0329169 -0.0329169, -0.0170841 -0.0322651 -0.0323192 -0.0138124 -0.0310156 -0.0310697, -0.0134628 -0.0077573 -0.0077843 -0.0106186 -0.0077958 -0.0078229, -0.0482485 -0.0423266 -0.0423266 -0.0482485 -0.0332558 -0.0497709, -0.0106186 -0.0061198 -0.0091934 -0.0134628 -0.0060896 -0.0091481, -0.0138124 -0.0243583 -0.0365235 -0.0170841 -0.0253399 -0.0379928, -0.0418327 -0.0258626 -0.0387061 -0.0438648 -0.027826 -0.0416446, -0.0445422 -0.0296668 -0.0443995 -0.0448397 -0.031109 -0.0465579, -0.0463033 -0.0310931 -0.0465341 -0.0462741 -0.0321138 -0.0480617, -0.0482485 -0.0321138 -0.0480617 0.0482485 -0.0332558 -0.0497709, 0.0106186 -0.0061198 -0.0091934 0.0134627 -0.0060896 -0.0091481, 0.0138124 -0.0243583 -0.0365235 0.017084 -0.0253399 -0.0379928, 0.0418327 -0.0258626 -0.0387061 0.0438648 -0.027826 -0.0416446, 0.0445422 -0.0296668 -0.0443995 0.0448397 -0.031109 -0.0465579, 0.0463033 -0.0310931 -0.0465341 0.0462741 -0.0321138 -0.0480617, 0.0482485 -0.0321138 -0.0480617 0.0482485 -0.0221204 -0.0534033, 0.0462741 -0.0221204 -0.0534033 0.0463033 -0.0214173 -0.0517059, 0.0448397 -0.0214282 -0.0517324 0.0445422 -0.0204348 -0.0493341, 0.0438648 -0.0191669 -0.046273 0.0418327 -0.0178145 -0.043008, 0.017084 -0.017441 -0.0422063 0.0138124 -0.0167648 -0.0405738, 0.0134627 -0.0041879 -0.0101604 0.0106186 -0.0042087 -0.0102107, 0.0482485 -0.022907 -0.0553024 -0.0482485 -0.0221204 -0.0534033, -0.0462741 -0.0221204 -0.0534033 -0.0463033 -0.0214173 -0.0517059, -0.0448397 -0.0214282 -0.0517323 -0.0445422 -0.0204348 -0.0493341, -0.0438648 -0.0191669 -0.046273 -0.0418327 -0.0178145 -0.0430079, -0.0170841 -0.017441 -0.0422063 -0.0138124 -0.0167648 -0.0405738, -0.0134628 -0.0041879 -0.0101604 -0.0106186 -0.0042087 -0.0102107, -0.0482485 -0.022907 -0.0553024 -0.0482485 -0.0116779 -0.0587087, -0.0106186 -0.0021358 -0.0108356 -0.0134628 -0.0021252 -0.0107821, -0.0138124 -0.0085271 -0.0430648 -0.0170841 -0.0088719 -0.0447979, -0.0418327 -0.0090817 -0.045657 -0.0438648 -0.0097712 -0.0491232, -0.0445422 -0.0104176 -0.0523728 -0.0448397 -0.010924 -0.0549188, -0.0463033 -0.0109184 -0.0548907 -0.0462741 -0.0112769 -0.0566926, -0.0482485 -0.0112769 -0.0566926 0.0482485 -0.0116779 -0.0587087, 0.0106186 -0.0021358 -0.0108356 0.0134627 -0.0021252 -0.0107821, 0.0138124 -0.0085271 -0.0430649 0.017084 -0.0088719 -0.0447979, 0.0418327 -0.0090817 -0.045657 0.0438648 -0.0097712 -0.0491232, 0.0445422 -0.0104176 -0.0523728 0.0448397 -0.010924 -0.0549188, 0.0463033 -0.0109184 -0.0548907 0.0462741 -0.0112769 -0.0566926, 0.0482485 -0.0112769 -0.0566926 0.0482485 0 -0.0578033, 0.0462741 0 -0.0578033 0.0463033 0 -0.0559661, 0.0448397 0 -0.0559947 0.0445422 0 -0.0533988, 0.0438648 0 -0.0500856 0.0418327 0 -0.0465515, 0.017084 3.83e-05 -0.0456679 0.0138124 3.82e-05 -0.0439009, 0.0134627 1.91e-05 -0.0109896 0.0106186 1.91e-05 -0.0110441, 0.0482485 0 -0.0598589 -0.0482485 0 -0.0578033, -0.0462741 0 -0.0578033 -0.0463033 0 -0.0559661, -0.0448397 0 -0.0559947 -0.0445422 0 -0.0533988, -0.0438648 0 -0.0500855 -0.0418327 0 -0.0465514, -0.0170841 3.83e-05 -0.0456679 -0.0138124 3.82e-05 -0.0439009, -0.0134628 1.91e-05 -0.0109896 -0.0106186 1.91e-05 -0.0110441, -0.0482485 0 -0.0598589 - ] - } - coordIndex [ - 722 757 734 -1 722 769 757 -1 722 733 758 -1 722 758 769 -1 723 767 724 -1 723 768 767 -1 724 766 725 -1 724 767 766 -1 725 765 726 -1 725 766 765 -1 726 764 727 -1 726 765 764 -1 727 763 728 -1 727 764 763 -1 728 762 729 -1 728 763 762 -1 729 761 730 -1 729 762 761 -1 730 760 731 -1 730 761 760 -1 731 759 732 -1 731 760 759 -1 732 758 733 -1 732 759 758 -1 734 746 745 -1 734 757 746 -1 735 736 755 -1 735 755 756 -1 736 737 754 -1 736 754 755 -1 737 738 753 -1 737 753 754 -1 738 739 752 -1 738 752 753 -1 739 740 751 -1 739 751 752 -1 740 741 750 -1 740 750 751 -1 741 742 749 -1 741 749 750 -1 742 743 748 -1 742 748 749 -1 743 744 747 -1 743 747 748 -1 744 745 746 -1 744 746 747 -1 699 698 745 -1 699 745 744 -1 700 699 744 -1 700 744 743 -1 701 700 743 -1 701 743 742 -1 702 701 742 -1 702 742 741 -1 703 702 741 -1 703 741 740 -1 704 703 740 -1 704 740 739 -1 705 704 739 -1 705 739 738 -1 706 705 738 -1 706 738 737 -1 707 706 737 -1 707 737 736 -1 708 707 736 -1 708 736 735 -1 709 745 698 -1 709 734 745 -1 711 733 710 -1 711 732 733 -1 712 732 711 -1 712 731 732 -1 713 731 712 -1 713 730 731 -1 714 730 713 -1 714 729 730 -1 715 729 714 -1 715 728 729 -1 716 728 715 -1 716 727 728 -1 717 727 716 -1 717 726 727 -1 718 726 717 -1 718 725 726 -1 719 725 718 -1 719 724 725 -1 720 724 719 -1 720 723 724 -1 721 710 733 -1 721 733 722 -1 721 734 709 -1 721 722 734 -1 674 709 686 -1 674 721 709 -1 674 685 710 -1 674 710 721 -1 675 719 676 -1 675 720 719 -1 676 718 677 -1 676 719 718 -1 677 717 678 -1 677 718 717 -1 678 716 679 -1 678 717 716 -1 679 715 680 -1 679 716 715 -1 680 714 681 -1 680 715 714 -1 681 713 682 -1 681 714 713 -1 682 712 683 -1 682 713 712 -1 683 711 684 -1 683 712 711 -1 684 710 685 -1 684 711 710 -1 686 698 697 -1 686 709 698 -1 687 688 707 -1 687 707 708 -1 688 689 706 -1 688 706 707 -1 689 690 705 -1 689 705 706 -1 690 691 704 -1 690 704 705 -1 691 692 703 -1 691 703 704 -1 692 693 702 -1 692 702 703 -1 693 694 701 -1 693 701 702 -1 694 695 700 -1 694 700 701 -1 695 696 699 -1 695 699 700 -1 696 697 698 -1 696 698 699 -1 651 650 697 -1 651 697 696 -1 652 651 696 -1 652 696 695 -1 653 652 695 -1 653 695 694 -1 654 653 694 -1 654 694 693 -1 655 654 693 -1 655 693 692 -1 656 655 692 -1 656 692 691 -1 657 656 691 -1 657 691 690 -1 658 657 690 -1 658 690 689 -1 659 658 689 -1 659 689 688 -1 660 659 688 -1 660 688 687 -1 661 697 650 -1 661 686 697 -1 663 685 662 -1 663 684 685 -1 664 684 663 -1 664 683 684 -1 665 683 664 -1 665 682 683 -1 666 682 665 -1 666 681 682 -1 667 681 666 -1 667 680 681 -1 668 680 667 -1 668 679 680 -1 669 679 668 -1 669 678 679 -1 670 678 669 -1 670 677 678 -1 671 677 670 -1 671 676 677 -1 672 676 671 -1 672 675 676 -1 673 662 685 -1 673 685 674 -1 673 686 661 -1 673 674 686 -1 626 661 638 -1 626 673 661 -1 626 637 662 -1 626 662 673 -1 627 671 628 -1 627 672 671 -1 628 670 629 -1 628 671 670 -1 629 669 630 -1 629 670 669 -1 630 668 631 -1 630 669 668 -1 631 667 632 -1 631 668 667 -1 632 666 633 -1 632 667 666 -1 633 665 634 -1 633 666 665 -1 634 664 635 -1 634 665 664 -1 635 663 636 -1 635 664 663 -1 636 662 637 -1 636 663 662 -1 638 650 649 -1 638 661 650 -1 639 640 659 -1 639 659 660 -1 640 641 658 -1 640 658 659 -1 641 642 657 -1 641 657 658 -1 642 643 656 -1 642 656 657 -1 643 644 655 -1 643 655 656 -1 644 645 654 -1 644 654 655 -1 645 646 653 -1 645 653 654 -1 646 647 652 -1 646 652 653 -1 647 648 651 -1 647 651 652 -1 648 649 650 -1 648 650 651 -1 603 602 649 -1 603 649 648 -1 604 603 648 -1 604 648 647 -1 605 604 647 -1 605 647 646 -1 606 605 646 -1 606 646 645 -1 607 606 645 -1 607 645 644 -1 608 607 644 -1 608 644 643 -1 609 608 643 -1 609 643 642 -1 610 609 642 -1 610 642 641 -1 611 610 641 -1 611 641 640 -1 612 611 640 -1 612 640 639 -1 613 649 602 -1 613 638 649 -1 615 637 614 -1 615 636 637 -1 616 636 615 -1 616 635 636 -1 617 635 616 -1 617 634 635 -1 618 634 617 -1 618 633 634 -1 619 633 618 -1 619 632 633 -1 620 632 619 -1 620 631 632 -1 621 631 620 -1 621 630 631 -1 622 630 621 -1 622 629 630 -1 623 629 622 -1 623 628 629 -1 624 628 623 -1 624 627 628 -1 625 614 637 -1 625 637 626 -1 625 638 613 -1 625 626 638 -1 578 613 590 -1 578 625 613 -1 578 589 614 -1 578 614 625 -1 579 623 580 -1 579 624 623 -1 580 622 581 -1 580 623 622 -1 581 621 582 -1 581 622 621 -1 582 620 583 -1 582 621 620 -1 583 619 584 -1 583 620 619 -1 584 618 585 -1 584 619 618 -1 585 617 586 -1 585 618 617 -1 586 616 587 -1 586 617 616 -1 587 615 588 -1 587 616 615 -1 588 614 589 -1 588 615 614 -1 590 602 601 -1 590 613 602 -1 591 592 611 -1 591 611 612 -1 592 593 610 -1 592 610 611 -1 593 594 609 -1 593 609 610 -1 594 595 608 -1 594 608 609 -1 595 596 607 -1 595 607 608 -1 596 597 606 -1 596 606 607 -1 597 598 605 -1 597 605 606 -1 598 599 604 -1 598 604 605 -1 599 600 603 -1 599 603 604 -1 600 601 602 -1 600 602 603 -1 555 554 601 -1 555 601 600 -1 556 555 600 -1 556 600 599 -1 557 556 599 -1 557 599 598 -1 558 557 598 -1 558 598 597 -1 559 558 597 -1 559 597 596 -1 560 559 596 -1 560 596 595 -1 561 560 595 -1 561 595 594 -1 562 561 594 -1 562 594 593 -1 563 562 593 -1 563 593 592 -1 564 563 592 -1 564 592 591 -1 565 601 554 -1 565 590 601 -1 567 589 566 -1 567 588 589 -1 568 588 567 -1 568 587 588 -1 569 587 568 -1 569 586 587 -1 570 586 569 -1 570 585 586 -1 571 585 570 -1 571 584 585 -1 572 584 571 -1 572 583 584 -1 573 583 572 -1 573 582 583 -1 574 582 573 -1 574 581 582 -1 575 581 574 -1 575 580 581 -1 576 580 575 -1 576 579 580 -1 577 566 589 -1 577 589 578 -1 577 590 565 -1 577 578 590 -1 530 565 542 -1 530 577 565 -1 530 541 566 -1 530 566 577 -1 531 575 532 -1 531 576 575 -1 532 574 533 -1 532 575 574 -1 533 573 534 -1 533 574 573 -1 534 572 535 -1 534 573 572 -1 535 571 536 -1 535 572 571 -1 536 570 537 -1 536 571 570 -1 537 569 538 -1 537 570 569 -1 538 568 539 -1 538 569 568 -1 539 567 540 -1 539 568 567 -1 540 566 541 -1 540 567 566 -1 542 554 553 -1 542 565 554 -1 543 544 563 -1 543 563 564 -1 544 545 562 -1 544 562 563 -1 545 546 561 -1 545 561 562 -1 546 547 560 -1 546 560 561 -1 547 548 559 -1 547 559 560 -1 548 549 558 -1 548 558 559 -1 549 550 557 -1 549 557 558 -1 550 551 556 -1 550 556 557 -1 551 552 555 -1 551 555 556 -1 552 553 554 -1 552 554 555 -1 507 506 553 -1 507 553 552 -1 508 507 552 -1 508 552 551 -1 509 508 551 -1 509 551 550 -1 510 509 550 -1 510 550 549 -1 511 510 549 -1 511 549 548 -1 512 511 548 -1 512 548 547 -1 513 512 547 -1 513 547 546 -1 514 513 546 -1 514 546 545 -1 515 514 545 -1 515 545 544 -1 516 515 544 -1 516 544 543 -1 517 553 506 -1 517 542 553 -1 519 541 518 -1 519 540 541 -1 520 540 519 -1 520 539 540 -1 521 539 520 -1 521 538 539 -1 522 538 521 -1 522 537 538 -1 523 537 522 -1 523 536 537 -1 524 536 523 -1 524 535 536 -1 525 535 524 -1 525 534 535 -1 526 534 525 -1 526 533 534 -1 527 533 526 -1 527 532 533 -1 528 532 527 -1 528 531 532 -1 529 518 541 -1 529 541 530 -1 529 542 517 -1 529 530 542 -1 482 517 494 -1 482 529 517 -1 482 493 518 -1 482 518 529 -1 483 527 484 -1 483 528 527 -1 484 526 485 -1 484 527 526 -1 485 525 486 -1 485 526 525 -1 486 524 487 -1 486 525 524 -1 487 523 488 -1 487 524 523 -1 488 522 489 -1 488 523 522 -1 489 521 490 -1 489 522 521 -1 490 520 491 -1 490 521 520 -1 491 519 492 -1 491 520 519 -1 492 518 493 -1 492 519 518 -1 494 506 505 -1 494 517 506 -1 495 496 515 -1 495 515 516 -1 496 497 514 -1 496 514 515 -1 497 498 513 -1 497 513 514 -1 498 499 512 -1 498 512 513 -1 499 500 511 -1 499 511 512 -1 500 501 510 -1 500 510 511 -1 501 502 509 -1 501 509 510 -1 502 503 508 -1 502 508 509 -1 503 504 507 -1 503 507 508 -1 504 505 506 -1 504 506 507 -1 459 458 505 -1 459 505 504 -1 460 459 504 -1 460 504 503 -1 461 460 503 -1 461 503 502 -1 462 461 502 -1 462 502 501 -1 463 462 501 -1 463 501 500 -1 464 463 500 -1 464 500 499 -1 465 464 499 -1 465 499 498 -1 466 465 498 -1 466 498 497 -1 467 466 497 -1 467 497 496 -1 468 467 496 -1 468 496 495 -1 469 505 458 -1 469 494 505 -1 471 493 470 -1 471 492 493 -1 472 492 471 -1 472 491 492 -1 473 491 472 -1 473 490 491 -1 474 490 473 -1 474 489 490 -1 475 489 474 -1 475 488 489 -1 476 488 475 -1 476 487 488 -1 477 487 476 -1 477 486 487 -1 478 486 477 -1 478 485 486 -1 479 485 478 -1 479 484 485 -1 480 484 479 -1 480 483 484 -1 481 470 493 -1 481 493 482 -1 481 494 469 -1 481 482 494 -1 434 469 446 -1 434 481 469 -1 434 445 470 -1 434 470 481 -1 435 479 436 -1 435 480 479 -1 436 478 437 -1 436 479 478 -1 437 477 438 -1 437 478 477 -1 438 476 439 -1 438 477 476 -1 439 475 440 -1 439 476 475 -1 440 474 441 -1 440 475 474 -1 441 473 442 -1 441 474 473 -1 442 472 443 -1 442 473 472 -1 443 471 444 -1 443 472 471 -1 444 470 445 -1 444 471 470 -1 446 458 457 -1 446 469 458 -1 447 448 467 -1 447 467 468 -1 448 449 466 -1 448 466 467 -1 449 450 465 -1 449 465 466 -1 450 451 464 -1 450 464 465 -1 451 452 463 -1 451 463 464 -1 452 453 462 -1 452 462 463 -1 453 454 461 -1 453 461 462 -1 454 455 460 -1 454 460 461 -1 455 456 459 -1 455 459 460 -1 456 457 458 -1 456 458 459 -1 411 410 457 -1 411 457 456 -1 412 411 456 -1 412 456 455 -1 413 412 455 -1 413 455 454 -1 414 413 454 -1 414 454 453 -1 415 414 453 -1 415 453 452 -1 416 415 452 -1 416 452 451 -1 417 416 451 -1 417 451 450 -1 418 417 450 -1 418 450 449 -1 419 418 449 -1 419 449 448 -1 420 419 448 -1 420 448 447 -1 421 457 410 -1 421 446 457 -1 423 445 422 -1 423 444 445 -1 424 444 423 -1 424 443 444 -1 425 443 424 -1 425 442 443 -1 426 442 425 -1 426 441 442 -1 427 441 426 -1 427 440 441 -1 428 440 427 -1 428 439 440 -1 429 439 428 -1 429 438 439 -1 430 438 429 -1 430 437 438 -1 431 437 430 -1 431 436 437 -1 432 436 431 -1 432 435 436 -1 433 422 445 -1 433 445 434 -1 433 446 421 -1 433 434 446 -1 386 421 398 -1 386 433 421 -1 386 397 422 -1 386 422 433 -1 387 431 388 -1 387 432 431 -1 388 430 389 -1 388 431 430 -1 389 429 390 -1 389 430 429 -1 390 428 391 -1 390 429 428 -1 391 427 392 -1 391 428 427 -1 392 426 393 -1 392 427 426 -1 393 425 394 -1 393 426 425 -1 394 424 395 -1 394 425 424 -1 395 423 396 -1 395 424 423 -1 396 422 397 -1 396 423 422 -1 398 410 409 -1 398 421 410 -1 399 400 419 -1 399 419 420 -1 400 401 418 -1 400 418 419 -1 401 402 417 -1 401 417 418 -1 402 403 416 -1 402 416 417 -1 403 404 415 -1 403 415 416 -1 404 405 414 -1 404 414 415 -1 405 406 413 -1 405 413 414 -1 406 407 412 -1 406 412 413 -1 407 408 411 -1 407 411 412 -1 408 409 410 -1 408 410 411 -1 363 362 409 -1 363 409 408 -1 364 363 408 -1 364 408 407 -1 365 364 407 -1 365 407 406 -1 366 365 406 -1 366 406 405 -1 367 366 405 -1 367 405 404 -1 368 367 404 -1 368 404 403 -1 369 368 403 -1 369 403 402 -1 370 369 402 -1 370 402 401 -1 371 370 401 -1 371 401 400 -1 372 371 400 -1 372 400 399 -1 373 409 362 -1 373 398 409 -1 375 397 374 -1 375 396 397 -1 376 396 375 -1 376 395 396 -1 377 395 376 -1 377 394 395 -1 378 394 377 -1 378 393 394 -1 379 393 378 -1 379 392 393 -1 380 392 379 -1 380 391 392 -1 381 391 380 -1 381 390 391 -1 382 390 381 -1 382 389 390 -1 383 389 382 -1 383 388 389 -1 384 388 383 -1 384 387 388 -1 385 374 397 -1 385 397 386 -1 385 398 373 -1 385 386 398 -1 338 373 350 -1 338 385 373 -1 338 349 374 -1 338 374 385 -1 339 383 340 -1 339 384 383 -1 340 382 341 -1 340 383 382 -1 341 381 342 -1 341 382 381 -1 342 380 343 -1 342 381 380 -1 343 379 344 -1 343 380 379 -1 344 378 345 -1 344 379 378 -1 345 377 346 -1 345 378 377 -1 346 376 347 -1 346 377 376 -1 347 375 348 -1 347 376 375 -1 348 374 349 -1 348 375 374 -1 350 362 361 -1 350 373 362 -1 351 352 371 -1 351 371 372 -1 352 353 370 -1 352 370 371 -1 353 354 369 -1 353 369 370 -1 354 355 368 -1 354 368 369 -1 355 356 367 -1 355 367 368 -1 356 357 366 -1 356 366 367 -1 357 358 365 -1 357 365 366 -1 358 359 364 -1 358 364 365 -1 359 360 363 -1 359 363 364 -1 360 361 362 -1 360 362 363 -1 315 314 361 -1 315 361 360 -1 316 315 360 -1 316 360 359 -1 317 316 359 -1 317 359 358 -1 318 317 358 -1 318 358 357 -1 319 318 357 -1 319 357 356 -1 320 319 356 -1 320 356 355 -1 321 320 355 -1 321 355 354 -1 322 321 354 -1 322 354 353 -1 323 322 353 -1 323 353 352 -1 324 323 352 -1 324 352 351 -1 325 361 314 -1 325 350 361 -1 327 349 326 -1 327 348 349 -1 328 348 327 -1 328 347 348 -1 329 347 328 -1 329 346 347 -1 330 346 329 -1 330 345 346 -1 331 345 330 -1 331 344 345 -1 332 344 331 -1 332 343 344 -1 333 343 332 -1 333 342 343 -1 334 342 333 -1 334 341 342 -1 335 341 334 -1 335 340 341 -1 336 340 335 -1 336 339 340 -1 337 326 349 -1 337 349 338 -1 337 350 325 -1 337 338 350 -1 290 325 302 -1 290 337 325 -1 290 301 326 -1 290 326 337 -1 291 335 292 -1 291 336 335 -1 292 334 293 -1 292 335 334 -1 293 333 294 -1 293 334 333 -1 294 332 295 -1 294 333 332 -1 295 331 296 -1 295 332 331 -1 296 330 297 -1 296 331 330 -1 297 329 298 -1 297 330 329 -1 298 328 299 -1 298 329 328 -1 299 327 300 -1 299 328 327 -1 300 326 301 -1 300 327 326 -1 302 314 313 -1 302 325 314 -1 303 304 323 -1 303 323 324 -1 304 305 322 -1 304 322 323 -1 305 306 321 -1 305 321 322 -1 306 307 320 -1 306 320 321 -1 307 308 319 -1 307 319 320 -1 308 309 318 -1 308 318 319 -1 309 310 317 -1 309 317 318 -1 310 311 316 -1 310 316 317 -1 311 312 315 -1 311 315 316 -1 312 313 314 -1 312 314 315 -1 267 266 313 -1 267 313 312 -1 268 267 312 -1 268 312 311 -1 269 268 311 -1 269 311 310 -1 270 269 310 -1 270 310 309 -1 271 270 309 -1 271 309 308 -1 272 271 308 -1 272 308 307 -1 273 272 307 -1 273 307 306 -1 274 273 306 -1 274 306 305 -1 275 274 305 -1 275 305 304 -1 276 275 304 -1 276 304 303 -1 277 313 266 -1 277 302 313 -1 279 301 278 -1 279 300 301 -1 280 300 279 -1 280 299 300 -1 281 299 280 -1 281 298 299 -1 282 298 281 -1 282 297 298 -1 283 297 282 -1 283 296 297 -1 284 296 283 -1 284 295 296 -1 285 295 284 -1 285 294 295 -1 286 294 285 -1 286 293 294 -1 287 293 286 -1 287 292 293 -1 288 292 287 -1 288 291 292 -1 289 278 301 -1 289 301 290 -1 289 302 277 -1 289 290 302 -1 242 277 254 -1 242 289 277 -1 242 253 278 -1 242 278 289 -1 243 287 244 -1 243 288 287 -1 244 286 245 -1 244 287 286 -1 245 285 246 -1 245 286 285 -1 246 284 247 -1 246 285 284 -1 247 283 248 -1 247 284 283 -1 248 282 249 -1 248 283 282 -1 249 281 250 -1 249 282 281 -1 250 280 251 -1 250 281 280 -1 251 279 252 -1 251 280 279 -1 252 278 253 -1 252 279 278 -1 254 266 265 -1 254 277 266 -1 255 256 275 -1 255 275 276 -1 256 257 274 -1 256 274 275 -1 257 258 273 -1 257 273 274 -1 258 259 272 -1 258 272 273 -1 259 260 271 -1 259 271 272 -1 260 261 270 -1 260 270 271 -1 261 262 269 -1 261 269 270 -1 262 263 268 -1 262 268 269 -1 263 264 267 -1 263 267 268 -1 264 265 266 -1 264 266 267 -1 219 218 265 -1 219 265 264 -1 220 219 264 -1 220 264 263 -1 221 220 263 -1 221 263 262 -1 222 221 262 -1 222 262 261 -1 223 222 261 -1 223 261 260 -1 224 223 260 -1 224 260 259 -1 225 224 259 -1 225 259 258 -1 226 225 258 -1 226 258 257 -1 227 226 257 -1 227 257 256 -1 228 227 256 -1 228 256 255 -1 229 265 218 -1 229 254 265 -1 231 253 230 -1 231 252 253 -1 232 252 231 -1 232 251 252 -1 233 251 232 -1 233 250 251 -1 234 250 233 -1 234 249 250 -1 235 249 234 -1 235 248 249 -1 236 248 235 -1 236 247 248 -1 237 247 236 -1 237 246 247 -1 238 246 237 -1 238 245 246 -1 239 245 238 -1 239 244 245 -1 240 244 239 -1 240 243 244 -1 241 230 253 -1 241 253 242 -1 241 254 229 -1 241 242 254 -1 194 229 206 -1 194 241 229 -1 194 205 230 -1 194 230 241 -1 195 239 196 -1 195 240 239 -1 196 238 197 -1 196 239 238 -1 197 237 198 -1 197 238 237 -1 198 236 199 -1 198 237 236 -1 199 235 200 -1 199 236 235 -1 200 234 201 -1 200 235 234 -1 201 233 202 -1 201 234 233 -1 202 232 203 -1 202 233 232 -1 203 231 204 -1 203 232 231 -1 204 230 205 -1 204 231 230 -1 206 218 217 -1 206 229 218 -1 207 208 227 -1 207 227 228 -1 208 209 226 -1 208 226 227 -1 209 210 225 -1 209 225 226 -1 210 211 224 -1 210 224 225 -1 211 212 223 -1 211 223 224 -1 212 213 222 -1 212 222 223 -1 213 214 221 -1 213 221 222 -1 214 215 220 -1 214 220 221 -1 215 216 219 -1 215 219 220 -1 216 217 218 -1 216 218 219 -1 171 170 217 -1 171 217 216 -1 172 171 216 -1 172 216 215 -1 173 172 215 -1 173 215 214 -1 174 173 214 -1 174 214 213 -1 175 174 213 -1 175 213 212 -1 176 175 212 -1 176 212 211 -1 177 176 211 -1 177 211 210 -1 178 177 210 -1 178 210 209 -1 179 178 209 -1 179 209 208 -1 180 179 208 -1 180 208 207 -1 181 217 170 -1 181 206 217 -1 183 205 182 -1 183 204 205 -1 184 204 183 -1 184 203 204 -1 185 203 184 -1 185 202 203 -1 186 202 185 -1 186 201 202 -1 187 201 186 -1 187 200 201 -1 188 200 187 -1 188 199 200 -1 189 199 188 -1 189 198 199 -1 190 198 189 -1 190 197 198 -1 191 197 190 -1 191 196 197 -1 192 196 191 -1 192 195 196 -1 193 182 205 -1 193 205 194 -1 193 206 181 -1 193 194 206 -1 146 181 158 -1 146 193 181 -1 146 157 182 -1 146 182 193 -1 147 191 148 -1 147 192 191 -1 148 190 149 -1 148 191 190 -1 149 189 150 -1 149 190 189 -1 150 188 151 -1 150 189 188 -1 151 187 152 -1 151 188 187 -1 152 186 153 -1 152 187 186 -1 153 185 154 -1 153 186 185 -1 154 184 155 -1 154 185 184 -1 155 183 156 -1 155 184 183 -1 156 182 157 -1 156 183 182 -1 158 170 169 -1 158 181 170 -1 159 160 179 -1 159 179 180 -1 160 161 178 -1 160 178 179 -1 161 162 177 -1 161 177 178 -1 162 163 176 -1 162 176 177 -1 163 164 175 -1 163 175 176 -1 164 165 174 -1 164 174 175 -1 165 166 173 -1 165 173 174 -1 166 167 172 -1 166 172 173 -1 167 168 171 -1 167 171 172 -1 168 169 170 -1 168 170 171 -1 123 122 169 -1 123 169 168 -1 124 123 168 -1 124 168 167 -1 125 124 167 -1 125 167 166 -1 126 125 166 -1 126 166 165 -1 127 126 165 -1 127 165 164 -1 128 127 164 -1 128 164 163 -1 129 128 163 -1 129 163 162 -1 130 129 162 -1 130 162 161 -1 131 130 161 -1 131 161 160 -1 132 131 160 -1 132 160 159 -1 133 169 122 -1 133 158 169 -1 135 157 134 -1 135 156 157 -1 136 156 135 -1 136 155 156 -1 137 155 136 -1 137 154 155 -1 138 154 137 -1 138 153 154 -1 139 153 138 -1 139 152 153 -1 140 152 139 -1 140 151 152 -1 141 151 140 -1 141 150 151 -1 142 150 141 -1 142 149 150 -1 143 149 142 -1 143 148 149 -1 144 148 143 -1 144 147 148 -1 145 134 157 -1 145 157 146 -1 145 158 133 -1 145 146 158 -1 98 133 110 -1 98 145 133 -1 98 109 134 -1 98 134 145 -1 99 143 100 -1 99 144 143 -1 100 142 101 -1 100 143 142 -1 101 141 102 -1 101 142 141 -1 102 140 103 -1 102 141 140 -1 103 139 104 -1 103 140 139 -1 104 138 105 -1 104 139 138 -1 105 137 106 -1 105 138 137 -1 106 136 107 -1 106 137 136 -1 107 135 108 -1 107 136 135 -1 108 134 109 -1 108 135 134 -1 110 122 121 -1 110 133 122 -1 111 112 131 -1 111 131 132 -1 112 113 130 -1 112 130 131 -1 113 114 129 -1 113 129 130 -1 114 115 128 -1 114 128 129 -1 115 116 127 -1 115 127 128 -1 116 117 126 -1 116 126 127 -1 117 118 125 -1 117 125 126 -1 118 119 124 -1 118 124 125 -1 119 120 123 -1 119 123 124 -1 120 121 122 -1 120 122 123 -1 75 74 121 -1 75 121 120 -1 76 75 120 -1 76 120 119 -1 77 76 119 -1 77 119 118 -1 78 77 118 -1 78 118 117 -1 79 78 117 -1 79 117 116 -1 80 79 116 -1 80 116 115 -1 81 80 115 -1 81 115 114 -1 82 81 114 -1 82 114 113 -1 83 82 113 -1 83 113 112 -1 84 83 112 -1 84 112 111 -1 85 121 74 -1 85 110 121 -1 87 109 86 -1 87 108 109 -1 88 108 87 -1 88 107 108 -1 89 107 88 -1 89 106 107 -1 90 106 89 -1 90 105 106 -1 91 105 90 -1 91 104 105 -1 92 104 91 -1 92 103 104 -1 93 103 92 -1 93 102 103 -1 94 102 93 -1 94 101 102 -1 95 101 94 -1 95 100 101 -1 96 100 95 -1 96 99 100 -1 97 86 109 -1 97 109 98 -1 97 110 85 -1 97 98 110 -1 50 85 62 -1 50 97 85 -1 50 61 86 -1 50 86 97 -1 51 95 52 -1 51 96 95 -1 52 94 53 -1 52 95 94 -1 53 93 54 -1 53 94 93 -1 54 92 55 -1 54 93 92 -1 55 91 56 -1 55 92 91 -1 56 90 57 -1 56 91 90 -1 57 89 58 -1 57 90 89 -1 58 88 59 -1 58 89 88 -1 59 87 60 -1 59 88 87 -1 60 86 61 -1 60 87 86 -1 62 74 73 -1 62 85 74 -1 63 64 83 -1 63 83 84 -1 64 65 82 -1 64 82 83 -1 65 66 81 -1 65 81 82 -1 66 67 80 -1 66 80 81 -1 67 68 79 -1 67 79 80 -1 68 69 78 -1 68 78 79 -1 69 70 77 -1 69 77 78 -1 70 71 76 -1 70 76 77 -1 71 72 75 -1 71 75 76 -1 72 73 74 -1 72 74 75 -1 27 26 73 -1 27 73 72 -1 28 27 72 -1 28 72 71 -1 29 28 71 -1 29 71 70 -1 30 29 70 -1 30 70 69 -1 31 30 69 -1 31 69 68 -1 32 31 68 -1 32 68 67 -1 33 32 67 -1 33 67 66 -1 34 33 66 -1 34 66 65 -1 35 34 65 -1 35 65 64 -1 36 35 64 -1 36 64 63 -1 37 73 26 -1 37 62 73 -1 39 61 38 -1 39 60 61 -1 40 60 39 -1 40 59 60 -1 41 59 40 -1 41 58 59 -1 42 58 41 -1 42 57 58 -1 43 57 42 -1 43 56 57 -1 44 56 43 -1 44 55 56 -1 45 55 44 -1 45 54 55 -1 46 54 45 -1 46 53 54 -1 47 53 46 -1 47 52 53 -1 48 52 47 -1 48 51 52 -1 49 38 61 -1 49 61 50 -1 49 62 37 -1 49 50 62 -1 2 37 14 -1 2 49 37 -1 2 13 38 -1 2 38 49 -1 3 47 4 -1 3 48 47 -1 4 46 5 -1 4 47 46 -1 5 45 6 -1 5 46 45 -1 6 44 7 -1 6 45 44 -1 7 43 8 -1 7 44 43 -1 8 42 9 -1 8 43 42 -1 9 41 10 -1 9 42 41 -1 10 40 11 -1 10 41 40 -1 11 39 12 -1 11 40 39 -1 12 38 13 -1 12 39 38 -1 14 26 25 -1 14 37 26 -1 15 16 35 -1 15 35 36 -1 16 17 34 -1 16 34 35 -1 17 18 33 -1 17 33 34 -1 18 19 32 -1 18 32 33 -1 19 20 31 -1 19 31 32 -1 20 21 30 -1 20 30 31 -1 21 22 29 -1 21 29 30 -1 22 23 28 -1 22 28 29 -1 23 24 27 -1 23 27 28 -1 24 25 26 -1 24 26 27 -1 747 746 25 -1 747 25 24 -1 748 747 24 -1 748 24 23 -1 749 748 23 -1 749 23 22 -1 750 749 22 -1 750 22 21 -1 751 750 21 -1 751 21 20 -1 752 751 20 -1 752 20 19 -1 753 752 19 -1 753 19 18 -1 754 753 18 -1 754 18 17 -1 755 754 17 -1 755 17 16 -1 756 755 16 -1 756 16 15 -1 757 25 746 -1 757 14 25 -1 759 13 758 -1 759 12 13 -1 760 12 759 -1 760 11 12 -1 761 11 760 -1 761 10 11 -1 762 10 761 -1 762 9 10 -1 763 9 762 -1 763 8 9 -1 764 8 763 -1 764 7 8 -1 765 7 764 -1 765 6 7 -1 766 6 765 -1 766 5 6 -1 767 5 766 -1 767 4 5 -1 768 4 767 -1 768 3 4 -1 769 758 13 -1 769 13 2 -1 769 14 757 -1 769 2 14 -1 - ] - creaseAngle 1 - } - } - DEF TIRE_TREAD Shape { - appearance PBRAppearance { - metalness 0 - roughness 0.9 - baseColorMap ImageTexture { - url [ - "https://raw.githubusercontent.com/cyberbotics/webots/R2021a/projects/robots/adept/pioneer3/protos/textures/cross_tire_base_color.jpg" - ] - } - normalMap ImageTexture { - url [ - "https://raw.githubusercontent.com/cyberbotics/webots/R2021a/projects/robots/adept/pioneer3/protos/textures/cross_tire_normal.jpg" - ] - } - occlusionMap ImageTexture { - url [ - "https://raw.githubusercontent.com/cyberbotics/webots/R2021a/projects/robots/adept/pioneer3/protos/textures/cross_tire_occlusion.jpg" - ] - } - } - geometry IndexedFaceSet { - coord Coordinate { - point [ -0.047241 -0.024940 0.093076 -0.042301 -0.027188 0.101466 -0.034163 -0.028413 0.106040 0.034163 -0.028413 0.106040 0.042301 -0.027188 0.101466 0.047241 -0.024940 0.093076 0.047241 -0.048180 0.083449 0.042301 -0.052522 0.090971 0.034163 -0.054890 0.095073 -0.034163 -0.054890 0.095073 -0.042301 -0.052522 0.090971 -0.047241 -0.048180 0.083449 -0.047241 -0.068136 0.068136 -0.042301 -0.074278 0.074278 -0.034163 -0.077627 0.077627 0.034163 -0.077627 0.077627 0.042301 -0.074278 0.074278 0.047241 -0.068136 0.068136 0.047241 -0.083449 0.048180 0.042301 -0.090971 0.052522 0.034163 -0.095073 0.054890 -0.034163 -0.095073 0.054890 -0.042301 -0.090971 0.052522 -0.047241 -0.083450 0.048180 -0.047241 -0.093076 0.024940 -0.042301 -0.101466 0.027188 -0.034163 -0.106040 0.028413 0.034163 -0.106040 0.028413 0.042301 -0.101466 0.027188 0.047241 -0.093076 0.024940 0.047241 -0.096359 0.000000 0.042301 -0.105045 0.000000 0.034163 -0.109781 0.000000 -0.034163 -0.109781 0.000000 -0.042301 -0.105045 0.000000 -0.047241 -0.096359 0.000000 -0.047241 -0.093076 -0.024940 -0.042301 -0.101466 -0.027188 -0.034163 -0.106040 -0.028413 0.034163 -0.106040 -0.028413 0.042301 -0.101466 -0.027188 0.047241 -0.093076 -0.024940 0.047241 -0.083449 -0.048180 0.042301 -0.090971 -0.052522 0.034163 -0.095073 -0.054890 -0.034163 -0.095073 -0.054890 -0.042301 -0.090971 -0.052522 -0.047241 -0.083449 -0.048180 -0.047241 -0.068136 -0.068136 -0.042301 -0.074278 -0.074278 -0.034163 -0.077627 -0.077627 0.034163 -0.077627 -0.077627 0.042301 -0.074278 -0.074278 0.047241 -0.068136 -0.068136 0.047241 -0.048180 -0.083450 0.042301 -0.052522 -0.090971 0.034163 -0.054890 -0.095073 -0.034163 -0.054890 -0.095073 -0.042301 -0.052522 -0.090971 -0.047241 -0.048180 -0.083450 -0.047241 -0.024940 -0.093076 -0.042301 -0.027188 -0.101466 -0.034163 -0.028413 -0.106040 0.034163 -0.028413 -0.106040 0.042301 -0.027188 -0.101466 0.047241 -0.024940 -0.093076 0.047241 0.000000 -0.096359 0.042301 0.000000 -0.105045 0.034163 0.000000 -0.109781 -0.034163 0.000000 -0.109781 -0.042301 0.000000 -0.105045 -0.047241 0.000000 -0.096359 -0.047241 0.024940 -0.093076 -0.042301 0.027188 -0.101466 -0.034163 0.028413 -0.106040 0.034163 0.028413 -0.106040 0.042301 0.027188 -0.101466 0.047241 0.024940 -0.093076 0.047241 0.048180 -0.083449 0.042301 0.052522 -0.090971 0.034163 0.054890 -0.095073 -0.034163 0.054890 -0.095073 -0.042301 0.052522 -0.090971 -0.047241 0.048180 -0.083449 -0.047241 0.068136 -0.068136 -0.042301 0.074278 -0.074278 -0.034163 0.077627 -0.077627 0.034163 0.077627 -0.077627 0.042301 0.074278 -0.074278 0.047241 0.068136 -0.068136 0.047241 0.083449 -0.048180 0.042301 0.090971 -0.052522 0.034163 0.095073 -0.054890 -0.034163 0.095073 -0.054890 -0.042301 0.090971 -0.052522 -0.047241 0.083449 -0.048180 -0.047241 0.093076 -0.024940 -0.042301 0.101466 -0.027188 -0.034163 0.106040 -0.028413 0.034163 0.106040 -0.028413 0.042301 0.101466 -0.027188 0.047241 0.093076 -0.024940 0.047241 0.096359 0.000000 0.042301 0.105045 0.000000 0.034163 0.109781 0.000000 -0.034163 0.109781 0.000000 -0.042301 0.105045 0.000000 -0.047241 0.096359 0.000000 -0.047241 0.093076 0.024940 -0.042301 0.101466 0.027188 -0.034163 0.106040 0.028413 0.034163 0.106040 0.028413 0.042301 0.101466 0.027188 0.047241 0.093076 0.024940 0.047241 0.083449 0.048180 0.042301 0.090971 0.052522 0.034163 0.095073 0.054890 -0.034163 0.095073 0.054890 -0.042301 0.090971 0.052522 -0.047241 0.083449 0.048180 -0.047241 0.068136 0.068136 -0.042301 0.074278 0.074278 -0.034163 0.077627 0.077627 0.034163 0.077627 0.077627 0.042301 0.074278 0.074278 0.047241 0.068136 0.068136 0.047241 0.048180 0.083450 0.042301 0.052522 0.090971 0.034163 0.054890 0.095073 -0.034163 0.054890 0.095073 -0.042301 0.052522 0.090971 -0.047241 0.048180 0.083450 -0.047241 0.024939 0.093076 -0.042301 0.027187 0.101466 -0.034163 0.028413 0.106040 0.034163 0.028413 0.106040 0.042301 0.027187 0.101466 0.047241 0.024939 0.093076 0.047241 -0.000000 0.096359 0.042301 -0.000000 0.105045 0.034163 -0.000000 0.109781 -0.034163 -0.000000 0.109781 -0.042301 -0.000000 0.105045 -0.047241 -0.000000 0.096359 ] - } - texCoord TextureCoordinate { - point [ 0.2070 1.2599 0.8079 1.3696 0.8079 1.2574 0.2070 1.2599 0.2064 1.3718 0.8079 1.3696 0.0169 1.2597 0.1131 1.3717 0.1131 1.2600 0.0169 1.2597 0.0169 1.3712 0.1131 1.3717 0.1131 1.2600 0.2064 1.3718 0.2070 1.2599 0.1131 1.2600 0.1131 1.3717 0.2064 1.3718 0.8079 1.2574 0.8892 1.3693 0.8892 1.2571 0.8079 1.2574 0.8079 1.3696 0.8892 1.3693 0.8892 1.2571 0.9902 1.3692 0.9902 1.2571 0.8892 1.2571 0.8892 1.3693 0.9902 1.3692 0.8892 1.1452 0.9902 1.2571 0.9902 1.1453 0.8892 1.1452 0.8892 1.2571 0.9902 1.2571 0.8079 1.1454 0.8892 1.2571 0.8892 1.1452 0.8079 1.1454 0.8079 1.2574 0.8892 1.2571 0.1131 1.1483 0.2070 1.2599 0.2068 1.1482 0.1131 1.1483 0.1131 1.2600 0.2070 1.2599 0.0169 1.1481 0.1131 1.2600 0.1131 1.1483 0.0169 1.1481 0.0169 1.2597 0.1131 1.2600 0.2068 1.1482 0.8079 1.2574 0.8079 1.1454 0.2068 1.1482 0.2070 1.2599 0.8079 1.2574 0.2063 1.0365 0.8079 1.1454 0.8079 1.0337 0.2063 1.0365 0.2068 1.1482 0.8079 1.1454 0.0169 1.0365 0.1131 1.1483 0.1131 1.0367 0.0169 1.0365 0.0169 1.1481 0.1131 1.1483 0.1131 1.0367 0.2068 1.1482 0.2063 1.0365 0.1131 1.0367 0.1131 1.1483 0.2068 1.1482 0.8079 1.0337 0.8892 1.1452 0.8892 1.0334 0.8079 1.0337 0.8079 1.1454 0.8892 1.1452 0.8892 1.0334 0.9902 1.1453 0.9902 1.0335 0.8892 1.0334 0.8892 1.1452 0.9902 1.1453 0.8892 0.9217 0.9902 1.0335 0.9902 0.9218 0.8892 0.9217 0.8892 1.0334 0.9902 1.0335 0.8079 0.9219 0.8892 1.0334 0.8892 0.9217 0.8079 0.9219 0.8079 1.0337 0.8892 1.0334 0.1131 0.9250 0.2063 1.0365 0.2056 0.9248 0.1131 0.9250 0.1131 1.0367 0.2063 1.0365 0.0169 0.9249 0.1131 1.0367 0.1131 0.9250 0.0169 0.9249 0.0169 1.0365 0.1131 1.0367 0.2056 0.9248 0.8079 1.0337 0.8079 0.9219 0.2056 0.9248 0.2063 1.0365 0.8079 1.0337 0.2048 0.8131 0.8079 0.9219 0.8071 0.8103 0.2048 0.8131 0.2056 0.9248 0.8079 0.9219 0.0169 0.8132 0.1131 0.9250 0.1131 0.8133 0.0169 0.8132 0.0169 0.9249 0.1131 0.9250 0.1131 0.8133 0.2056 0.9248 0.2048 0.8131 0.1131 0.8133 0.1131 0.9250 0.2056 0.9248 0.8071 0.8103 0.8892 0.9217 0.8892 0.8100 0.8071 0.8103 0.8079 0.9219 0.8892 0.9217 0.8892 0.8100 0.9902 0.9218 0.9902 0.8101 0.8892 0.8100 0.8892 0.9217 0.9902 0.9218 0.8892 0.6984 0.9902 0.8101 0.9902 0.6985 0.8892 0.6984 0.8892 0.8100 0.9902 0.8101 0.8062 0.6986 0.8892 0.8100 0.8892 0.6984 0.8062 0.6986 0.8071 0.8103 0.8892 0.8100 0.1131 0.7017 0.2048 0.8131 0.2041 0.7015 0.1131 0.7017 0.1131 0.8133 0.2048 0.8131 0.0169 0.7016 0.1131 0.8133 0.1131 0.7017 0.0169 0.7016 0.0169 0.8132 0.1131 0.8133 0.2041 0.7015 0.8071 0.8103 0.8062 0.6986 0.2041 0.7015 0.2048 0.8131 0.8071 0.8103 0.2033 0.5898 0.8062 0.6986 0.8054 0.5869 0.2033 0.5898 0.2041 0.7015 0.8062 0.6986 0.0169 0.5899 0.1131 0.7017 0.1131 0.5900 0.0169 0.5899 0.0169 0.7016 0.1131 0.7017 0.1131 0.5900 0.2041 0.7015 0.2033 0.5898 0.1131 0.5900 0.1131 0.7017 0.2041 0.7015 0.8054 0.5869 0.8892 0.6984 0.8892 0.5867 0.8054 0.5869 0.8062 0.6986 0.8892 0.6984 0.8892 0.5867 0.9902 0.6985 0.9902 0.5868 0.8892 0.5867 0.8892 0.6984 0.9902 0.6985 0.8892 0.4751 0.9902 0.5868 0.9902 0.4752 0.8892 0.4751 0.8892 0.5867 0.9902 0.5868 0.8046 0.4753 0.8892 0.5867 0.8892 0.4751 0.8046 0.4753 0.8054 0.5869 0.8892 0.5867 0.1131 0.4784 0.2033 0.5898 0.2025 0.4782 0.1131 0.4784 0.1131 0.5900 0.2033 0.5898 0.0169 0.4783 0.1131 0.5900 0.1131 0.4784 0.0169 0.4783 0.0169 0.5899 0.1131 0.5900 0.2025 0.4782 0.8054 0.5869 0.8046 0.4753 0.2025 0.4782 0.2033 0.5898 0.8054 0.5869 0.2017 0.3665 0.8046 0.4753 0.8038 0.3636 0.2017 0.3665 0.2025 0.4782 0.8046 0.4753 0.0169 0.3666 0.1131 0.4784 0.1131 0.3667 0.0169 0.3666 0.0169 0.4783 0.1131 0.4784 0.1131 0.3667 0.2025 0.4782 0.2017 0.3665 0.1131 0.3667 0.1131 0.4784 0.2025 0.4782 0.8038 0.3636 0.8892 0.4751 0.8892 0.3634 0.8038 0.3636 0.8046 0.4753 0.8892 0.4751 0.8892 0.3634 0.9902 0.4752 0.9902 0.3635 0.8892 0.3634 0.8892 0.4751 0.9902 0.4752 0.8892 0.2518 0.9902 0.3635 0.9902 0.2519 0.8892 0.2518 0.8892 0.3634 0.9902 0.3635 0.8030 0.2520 0.8892 0.3634 0.8892 0.2518 0.8030 0.2520 0.8038 0.3636 0.8892 0.3634 0.1131 0.2551 0.2017 0.3665 0.2009 0.2549 0.1131 0.2551 0.1131 0.3667 0.2017 0.3665 0.0169 0.2550 0.1131 0.3667 0.1131 0.2551 0.0169 0.2550 0.0169 0.3666 0.1131 0.3667 0.2009 0.2549 0.8038 0.3636 0.8030 0.2520 0.2009 0.2549 0.2017 0.3665 0.8038 0.3636 0.2001 0.1432 0.8030 0.2520 0.8022 0.1404 0.2001 0.1432 0.2009 0.2549 0.8030 0.2520 0.0169 0.1433 0.1131 0.2551 0.1131 0.1434 0.0169 0.1433 0.0169 0.2550 0.1131 0.2551 0.1131 0.1434 0.2009 0.2549 0.2001 0.1432 0.1131 0.1434 0.1131 0.2551 0.2009 0.2549 0.8022 0.1404 0.8892 0.2518 0.8892 0.1401 0.8022 0.1404 0.8030 0.2520 0.8892 0.2518 0.8892 0.1401 0.9902 0.2519 0.9902 0.1403 0.8892 0.1401 0.8892 0.2518 0.9902 0.2519 0.8892 0.0285 0.9902 0.1403 0.9902 0.0286 0.8892 0.0285 0.8892 0.1401 0.9902 0.1403 0.8015 0.0287 0.8892 0.1401 0.8892 0.0285 0.8015 0.0287 0.8022 0.1404 0.8892 0.1401 0.1131 0.0318 0.2001 0.1432 0.1993 0.0315 0.1131 0.0318 0.1131 0.1434 0.2001 0.1432 0.0169 0.0316 0.1131 0.1434 0.1131 0.0318 0.0169 0.0316 0.0169 0.1433 0.1131 0.1434 0.1993 0.0315 0.8022 0.1404 0.8015 0.0287 0.1993 0.0315 0.2001 0.1432 0.8022 0.1404 0.2048 -0.0802 0.8015 0.0287 0.8079 -0.0829 0.2048 -0.0802 0.1993 0.0315 0.8015 0.0287 0.0169 -0.0800 0.1131 0.0318 0.1131 -0.0799 0.0169 -0.0800 0.0169 0.0316 0.1131 0.0318 0.1131 -0.0799 0.1993 0.0315 0.2048 -0.0802 0.1131 -0.0799 0.1131 0.0318 0.1993 0.0315 0.8079 -0.0829 0.8892 0.0285 0.8892 -0.0831 0.8079 -0.0829 0.8015 0.0287 0.8892 0.0285 0.8892 -0.0831 0.9902 0.0286 0.9902 -0.0830 0.8892 -0.0831 0.8892 0.0285 0.9902 0.0286 0.8892 -0.1948 0.9902 -0.0830 0.9902 -0.1946 0.8892 -0.1948 0.8892 -0.0831 0.9902 -0.0830 0.8079 -0.1946 0.8892 -0.0831 0.8892 -0.1948 0.8079 -0.1946 0.8079 -0.0829 0.8892 -0.0831 0.1131 -0.1917 0.2048 -0.0802 0.2048 -0.1919 0.1131 -0.1917 0.1131 -0.0799 0.2048 -0.0802 0.0169 -0.1918 0.1131 -0.0799 0.1131 -0.1917 0.0169 -0.1918 0.0169 -0.0800 0.1131 -0.0799 0.2048 -0.1919 0.8079 -0.0829 0.8079 -0.1946 0.2048 -0.1919 0.2048 -0.0802 0.8079 -0.0829 0.2048 -0.3038 0.8079 -0.1946 0.8079 -0.3063 0.2048 -0.3038 0.2048 -0.1919 0.8079 -0.1946 0.0169 -0.3036 0.1131 -0.1917 0.1131 -0.3035 0.0169 -0.3036 0.0169 -0.1918 0.1131 -0.1917 0.1131 -0.3035 0.2048 -0.1919 0.2048 -0.3038 0.1131 -0.3035 0.1131 -0.1917 0.2048 -0.1919 0.8079 -0.3063 0.8892 -0.1948 0.8892 -0.3064 0.8079 -0.3063 0.8079 -0.1946 0.8892 -0.1948 0.8892 -0.3064 0.9902 -0.1946 0.9902 -0.3061 0.8892 -0.3064 0.8892 -0.1948 0.9902 -0.1946 0.8892 -0.4180 0.9902 -0.3061 0.9902 -0.4175 0.8892 -0.4180 0.8892 -0.3064 0.9902 -0.3061 0.8079 -0.4182 0.8892 -0.3064 0.8892 -0.4180 0.8079 -0.4182 0.8079 -0.3063 0.8892 -0.3064 0.1131 -0.4157 0.2048 -0.3038 0.2048 -0.4161 0.1131 -0.4157 0.1131 -0.3035 0.2048 -0.3038 0.0169 -0.4156 0.1131 -0.3035 0.1131 -0.4157 0.0169 -0.4156 0.0169 -0.3036 0.1131 -0.3035 0.2048 -0.4161 0.8079 -0.3063 0.8079 -0.4182 0.2048 -0.4161 0.2048 -0.3038 0.8079 -0.3063 0.2048 -0.5290 0.8079 -0.4182 0.8079 -0.5303 0.2048 -0.5290 0.2048 -0.4161 0.8079 -0.4182 0.0169 -0.5281 0.1131 -0.4157 0.1131 -0.5285 0.0169 -0.5281 0.0169 -0.4156 0.1131 -0.4157 0.1131 -0.5285 0.2048 -0.4161 0.2048 -0.5290 0.1131 -0.5285 0.1131 -0.4157 0.2048 -0.4161 0.8079 -0.5303 0.8892 -0.4180 0.8892 -0.5298 0.8079 -0.5303 0.8079 -0.4182 0.8892 -0.4180 0.8892 -0.5298 0.9902 -0.4175 0.9902 -0.5287 0.8892 -0.5298 0.8892 -0.4180 0.9902 -0.4175 0.8892 -0.6417 0.9902 -0.5287 0.9902 -0.6395 0.8892 -0.6417 0.8892 -0.5298 0.9902 -0.5287 0.8079 -0.6432 0.8892 -0.5298 0.8892 -0.6417 0.8079 -0.6432 0.8079 -0.5303 0.8892 -0.5298 0.1131 -0.6426 0.2048 -0.5290 0.2048 -0.6437 0.1131 -0.6426 0.1131 -0.5285 0.2048 -0.5290 0.0169 -0.6413 0.1131 -0.5285 0.1131 -0.6426 0.0169 -0.6413 0.0169 -0.5281 0.1131 -0.5285 0.2048 -0.6437 0.8079 -0.5303 0.8079 -0.6432 0.2048 -0.6437 0.2048 -0.5290 0.8079 -0.5303 0.2048 -0.7622 0.8079 -0.6432 0.8079 -0.7580 0.2048 -0.7622 0.2048 -0.6437 0.8079 -0.6432 0.0169 -0.7560 0.1131 -0.6426 0.1131 -0.7595 0.0169 -0.7560 0.0169 -0.6413 0.1131 -0.6426 0.1131 -0.7595 0.2048 -0.6437 0.2048 -0.7622 0.1131 -0.7595 0.1131 -0.6426 0.2048 -0.6437 0.8079 -0.7580 0.8892 -0.6417 0.8892 -0.7543 0.8079 -0.7580 0.8079 -0.6432 0.8892 -0.6417 0.8892 -0.7543 0.9902 -0.6395 0.9902 -0.7495 0.8892 -0.7543 0.8892 -0.6417 0.9902 -0.6395 0.8892 -0.8726 0.9902 -0.7495 0.9902 -0.8726 0.8892 -0.8726 0.8892 -0.7543 0.9902 -0.7495 0.8079 -0.8726 0.8892 -0.7543 0.8892 -0.8726 0.8079 -0.8726 0.8079 -0.7580 0.8892 -0.7543 0.1131 -0.8726 0.2048 -0.7622 0.2048 -0.8726 0.1131 -0.8726 0.1131 -0.7595 0.2048 -0.7622 0.0169 -0.8726 0.1131 -0.7595 0.1131 -0.8726 0.0169 -0.8726 0.0169 -0.7560 0.1131 -0.7595 0.2048 -0.8726 0.8079 -0.7580 0.8079 -0.8726 0.2048 -0.8726 0.2048 -0.7622 0.8079 -0.7580 0.2048 1.7118 0.8079 1.8263 0.8079 1.7158 0.2048 1.7118 0.2048 1.8263 0.8079 1.8263 0.0169 1.7032 0.1131 1.8263 0.1131 1.7080 0.0169 1.7032 0.0169 1.8263 0.1131 1.8263 0.1131 1.7080 0.2048 1.8263 0.2048 1.7118 0.1131 1.7080 0.1131 1.8263 0.2048 1.8263 0.8079 1.7158 0.8892 1.8263 0.8892 1.7131 0.8079 1.7158 0.8079 1.8263 0.8892 1.8263 0.8892 1.7131 0.9902 1.8263 0.9902 1.7097 0.8892 1.7131 0.8892 1.8263 0.9902 1.8263 0.8892 1.5962 0.9902 1.7097 0.9902 1.5949 0.8892 1.5962 0.8892 1.7131 0.9902 1.7097 0.8079 1.5973 0.8892 1.7131 0.8892 1.5962 0.8079 1.5973 0.8079 1.7158 0.8892 1.7131 0.1131 1.5954 0.2048 1.7118 0.2048 1.5969 0.1131 1.5954 0.1131 1.7080 0.2048 1.7118 0.0169 1.5932 0.1131 1.7080 0.1131 1.5954 0.0169 1.5932 0.0169 1.7032 0.1131 1.7080 0.2048 1.5969 0.8079 1.7158 0.8079 1.5973 0.2048 1.5969 0.2048 1.7118 0.8079 1.7158 0.2048 1.4840 0.8079 1.5973 0.8079 1.4827 0.2048 1.4840 0.2048 1.5969 0.8079 1.5973 0.0169 1.4824 0.1131 1.5954 0.1131 1.4834 0.0169 1.4824 0.0169 1.5932 0.1131 1.5954 0.1131 1.4834 0.2048 1.5969 0.2048 1.4840 0.1131 1.4834 0.1131 1.5954 0.2048 1.5969 0.8079 1.4827 0.8892 1.5962 0.8892 1.4821 0.8079 1.4827 0.8079 1.5973 0.8892 1.5962 0.8892 1.4821 0.9902 1.5949 0.9902 1.4816 0.8892 1.4821 0.8892 1.5962 0.9902 1.5949 0.8892 1.3693 0.9902 1.4816 0.9902 1.3692 0.8892 1.3693 0.8892 1.4821 0.9902 1.4816 0.8079 1.3696 0.8892 1.4821 0.8892 1.3693 0.8079 1.3696 0.8079 1.4827 0.8892 1.4821 0.1131 1.3717 0.2048 1.4840 0.2064 1.3718 0.1131 1.3717 0.1131 1.4834 0.2048 1.4840 0.0169 1.3712 0.1131 1.4834 0.1131 1.3717 0.0169 1.3712 0.0169 1.4824 0.1131 1.4834 0.2064 1.3718 0.8079 1.4827 0.8079 1.3696 0.2064 1.3718 0.2048 1.4840 0.8079 1.4827 ] - } - texCoordIndex [ 0 1 2 -1 3 4 5 -1 6 7 8 -1 9 10 11 -1 12 13 14 -1 15 16 17 -1 18 19 20 -1 21 22 23 -1 24 25 26 -1 27 28 29 -1 30 31 32 -1 33 34 35 -1 36 37 38 -1 39 40 41 -1 42 43 44 -1 45 46 47 -1 48 49 50 -1 51 52 53 -1 54 55 56 -1 57 58 59 -1 60 61 62 -1 63 64 65 -1 66 67 68 -1 69 70 71 -1 72 73 74 -1 75 76 77 -1 78 79 80 -1 81 82 83 -1 84 85 86 -1 87 88 89 -1 90 91 92 -1 93 94 95 -1 96 97 98 -1 99 100 101 -1 102 103 104 -1 105 106 107 -1 108 109 110 -1 111 112 113 -1 114 115 116 -1 117 118 119 -1 120 121 122 -1 123 124 125 -1 126 127 128 -1 129 130 131 -1 132 133 134 -1 135 136 137 -1 138 139 140 -1 141 142 143 -1 144 145 146 -1 147 148 149 -1 150 151 152 -1 153 154 155 -1 156 157 158 -1 159 160 161 -1 162 163 164 -1 165 166 167 -1 168 169 170 -1 171 172 173 -1 174 175 176 -1 177 178 179 -1 180 181 182 -1 183 184 185 -1 186 187 188 -1 189 190 191 -1 192 193 194 -1 195 196 197 -1 198 199 200 -1 201 202 203 -1 204 205 206 -1 207 208 209 -1 210 211 212 -1 213 214 215 -1 216 217 218 -1 219 220 221 -1 222 223 224 -1 225 226 227 -1 228 229 230 -1 231 232 233 -1 234 235 236 -1 237 238 239 -1 240 241 242 -1 243 244 245 -1 246 247 248 -1 249 250 251 -1 252 253 254 -1 255 256 257 -1 258 259 260 -1 261 262 263 -1 264 265 266 -1 267 268 269 -1 270 271 272 -1 273 274 275 -1 276 277 278 -1 279 280 281 -1 282 283 284 -1 285 286 287 -1 288 289 290 -1 291 292 293 -1 294 295 296 -1 297 298 299 -1 300 301 302 -1 303 304 305 -1 306 307 308 -1 309 310 311 -1 312 313 314 -1 315 316 317 -1 318 319 320 -1 321 322 323 -1 324 325 326 -1 327 328 329 -1 330 331 332 -1 333 334 335 -1 336 337 338 -1 339 340 341 -1 342 343 344 -1 345 346 347 -1 348 349 350 -1 351 352 353 -1 354 355 356 -1 357 358 359 -1 360 361 362 -1 363 364 365 -1 366 367 368 -1 369 370 371 -1 372 373 374 -1 375 376 377 -1 378 379 380 -1 381 382 383 -1 384 385 386 -1 387 388 389 -1 390 391 392 -1 393 394 395 -1 396 397 398 -1 399 400 401 -1 402 403 404 -1 405 406 407 -1 408 409 410 -1 411 412 413 -1 414 415 416 -1 417 418 419 -1 420 421 422 -1 423 424 425 -1 426 427 428 -1 429 430 431 -1 432 433 434 -1 435 436 437 -1 438 439 440 -1 441 442 443 -1 444 445 446 -1 447 448 449 -1 450 451 452 -1 453 454 455 -1 456 457 458 -1 459 460 461 -1 462 463 464 -1 465 466 467 -1 468 469 470 -1 471 472 473 -1 474 475 476 -1 477 478 479 -1 480 481 482 -1 483 484 485 -1 486 487 488 -1 489 490 491 -1 492 493 494 -1 495 496 497 -1 498 499 500 -1 501 502 503 -1 504 505 506 -1 507 508 509 -1 510 511 512 -1 513 514 515 -1 516 517 518 -1 519 520 521 -1 522 523 524 -1 525 526 527 -1 528 529 530 -1 531 532 533 -1 534 535 536 -1 537 538 539 -1 540 541 542 -1 543 544 545 -1 546 547 548 -1 549 550 551 -1 552 553 554 -1 555 556 557 -1 558 559 560 -1 561 562 563 -1 564 565 566 -1 567 568 569 -1 570 571 572 -1 573 574 575 -1 576 577 578 -1 579 580 581 -1 582 583 584 -1 585 586 587 -1 588 589 590 -1 591 592 593 -1 594 595 596 -1 597 598 599 -1 600 601 602 -1 603 604 605 -1 606 607 608 -1 609 610 611 -1 612 613 614 -1 615 616 617 -1 618 619 620 -1 621 622 623 -1 624 625 626 -1 627 628 629 -1 630 631 632 -1 633 634 635 -1 636 637 638 -1 639 640 641 -1 642 643 644 -1 645 646 647 -1 648 649 650 -1 651 652 653 -1 654 655 656 -1 657 658 659 -1 660 661 662 -1 663 664 665 -1 666 667 668 -1 669 670 671 -1 672 673 674 -1 675 676 677 -1 678 679 680 -1 681 682 683 -1 684 685 686 -1 687 688 689 -1 690 691 692 -1 693 694 695 -1 696 697 698 -1 699 700 701 -1 702 703 704 -1 705 706 707 -1 708 709 710 -1 711 712 713 -1 714 715 716 -1 717 718 719 -1 ] - coordIndex [ 134 140 135 -1 134 141 140 -1 132 142 133 -1 132 143 142 -1 133 141 134 -1 133 142 141 -1 135 139 136 -1 135 140 139 -1 136 138 137 -1 136 139 138 -1 127 137 126 -1 127 136 137 -1 128 136 127 -1 128 135 136 -1 130 134 129 -1 130 133 134 -1 131 133 130 -1 131 132 133 -1 129 135 128 -1 129 134 135 -1 122 128 123 -1 122 129 128 -1 120 130 121 -1 120 131 130 -1 121 129 122 -1 121 130 129 -1 123 127 124 -1 123 128 127 -1 124 126 125 -1 124 127 126 -1 115 125 114 -1 115 124 125 -1 116 124 115 -1 116 123 124 -1 118 122 117 -1 118 121 122 -1 119 121 118 -1 119 120 121 -1 117 123 116 -1 117 122 123 -1 110 116 111 -1 110 117 116 -1 108 118 109 -1 108 119 118 -1 109 117 110 -1 109 118 117 -1 111 115 112 -1 111 116 115 -1 112 114 113 -1 112 115 114 -1 103 113 102 -1 103 112 113 -1 104 112 103 -1 104 111 112 -1 106 110 105 -1 106 109 110 -1 107 109 106 -1 107 108 109 -1 105 111 104 -1 105 110 111 -1 98 104 99 -1 98 105 104 -1 96 106 97 -1 96 107 106 -1 97 105 98 -1 97 106 105 -1 99 103 100 -1 99 104 103 -1 100 102 101 -1 100 103 102 -1 91 101 90 -1 91 100 101 -1 92 100 91 -1 92 99 100 -1 94 98 93 -1 94 97 98 -1 95 97 94 -1 95 96 97 -1 93 99 92 -1 93 98 99 -1 86 92 87 -1 86 93 92 -1 84 94 85 -1 84 95 94 -1 85 93 86 -1 85 94 93 -1 87 91 88 -1 87 92 91 -1 88 90 89 -1 88 91 90 -1 79 89 78 -1 79 88 89 -1 80 88 79 -1 80 87 88 -1 82 86 81 -1 82 85 86 -1 83 85 82 -1 83 84 85 -1 81 87 80 -1 81 86 87 -1 74 80 75 -1 74 81 80 -1 72 82 73 -1 72 83 82 -1 73 81 74 -1 73 82 81 -1 75 79 76 -1 75 80 79 -1 76 78 77 -1 76 79 78 -1 67 77 66 -1 67 76 77 -1 68 76 67 -1 68 75 76 -1 70 74 69 -1 70 73 74 -1 71 73 70 -1 71 72 73 -1 69 75 68 -1 69 74 75 -1 62 68 63 -1 62 69 68 -1 60 70 61 -1 60 71 70 -1 61 69 62 -1 61 70 69 -1 63 67 64 -1 63 68 67 -1 64 66 65 -1 64 67 66 -1 55 65 54 -1 55 64 65 -1 56 64 55 -1 56 63 64 -1 58 62 57 -1 58 61 62 -1 59 61 58 -1 59 60 61 -1 57 63 56 -1 57 62 63 -1 50 56 51 -1 50 57 56 -1 48 58 49 -1 48 59 58 -1 49 57 50 -1 49 58 57 -1 51 55 52 -1 51 56 55 -1 52 54 53 -1 52 55 54 -1 43 53 42 -1 43 52 53 -1 44 52 43 -1 44 51 52 -1 46 50 45 -1 46 49 50 -1 47 49 46 -1 47 48 49 -1 45 51 44 -1 45 50 51 -1 38 44 39 -1 38 45 44 -1 36 46 37 -1 36 47 46 -1 37 45 38 -1 37 46 45 -1 39 43 40 -1 39 44 43 -1 40 42 41 -1 40 43 42 -1 31 41 30 -1 31 40 41 -1 32 40 31 -1 32 39 40 -1 34 38 33 -1 34 37 38 -1 35 37 34 -1 35 36 37 -1 33 39 32 -1 33 38 39 -1 26 32 27 -1 26 33 32 -1 24 34 25 -1 24 35 34 -1 25 33 26 -1 25 34 33 -1 27 31 28 -1 27 32 31 -1 28 30 29 -1 28 31 30 -1 19 29 18 -1 19 28 29 -1 20 28 19 -1 20 27 28 -1 22 26 21 -1 22 25 26 -1 23 25 22 -1 23 24 25 -1 21 27 20 -1 21 26 27 -1 14 20 15 -1 14 21 20 -1 12 22 13 -1 12 23 22 -1 13 21 14 -1 13 22 21 -1 15 19 16 -1 15 20 19 -1 16 18 17 -1 16 19 18 -1 7 17 6 -1 7 16 17 -1 8 16 7 -1 8 15 16 -1 10 14 9 -1 10 13 14 -1 11 13 10 -1 11 12 13 -1 9 15 8 -1 9 14 15 -1 2 8 3 -1 2 9 8 -1 0 10 1 -1 0 11 10 -1 1 9 2 -1 1 10 9 -1 3 7 4 -1 3 8 7 -1 4 6 5 -1 4 7 6 -1 139 5 138 -1 139 4 5 -1 140 4 139 -1 140 3 4 -1 142 2 141 -1 142 1 2 -1 143 1 142 -1 143 0 1 -1 141 3 140 -1 141 2 3 -1 ] - creaseAngle 1 - } - } - DEF TIRE_RIM Shape { - appearance PBRAppearance { - roughness 1 - metalness 0 - baseColor 0.1 0.1 0.1 - } - geometry IndexedFaceSet { - coord Coordinate { - point [ -0.044395 -0.015502 0.057854 -0.047241 -0.024940 0.093076 0.047241 -0.024940 0.093076 0.044394 -0.015502 0.057854 0.044394 -0.029947 0.051870 0.047241 -0.048180 0.083449 -0.047241 -0.048180 0.083449 -0.044395 -0.029947 0.051870 -0.044395 -0.042352 0.042352 -0.047241 -0.068136 0.068136 0.047241 -0.068136 0.068136 0.044394 -0.042352 0.042352 0.044394 -0.051870 0.029947 0.047241 -0.083449 0.048180 -0.047241 -0.083450 0.048180 -0.044395 -0.051870 0.029947 -0.044395 -0.057854 0.015502 -0.047241 -0.093076 0.024940 0.047241 -0.093076 0.024940 0.044394 -0.057854 0.015502 0.044394 -0.059895 0.000000 0.047241 -0.096359 0.000000 -0.047241 -0.096359 0.000000 -0.044395 -0.059895 0.000000 -0.044395 -0.057854 -0.015502 -0.047241 -0.093076 -0.024940 0.047241 -0.093076 -0.024940 0.044394 -0.057854 -0.015502 0.044394 -0.051870 -0.029947 0.047241 -0.083449 -0.048180 -0.047241 -0.083449 -0.048180 -0.044395 -0.051870 -0.029947 -0.044395 -0.042352 -0.042352 -0.047241 -0.068136 -0.068136 0.047241 -0.068136 -0.068136 0.044394 -0.042352 -0.042352 0.044394 -0.029947 -0.051870 0.047241 -0.048180 -0.083450 -0.047241 -0.048180 -0.083450 -0.044395 -0.029947 -0.051870 -0.044395 -0.015502 -0.057854 -0.047241 -0.024940 -0.093076 0.047241 -0.024940 -0.093076 0.044394 -0.015502 -0.057854 0.044394 0.000000 -0.059895 0.047241 0.000000 -0.096359 -0.047241 0.000000 -0.096359 -0.044395 0.000000 -0.059895 -0.044395 0.015502 -0.057854 -0.047241 0.024940 -0.093076 0.047241 0.024940 -0.093076 0.044394 0.015502 -0.057854 0.044394 0.029947 -0.051870 0.047241 0.048180 -0.083449 -0.047241 0.048180 -0.083449 -0.044395 0.029947 -0.051870 -0.044395 0.042352 -0.042352 -0.047241 0.068136 -0.068136 0.047241 0.068136 -0.068136 0.044394 0.042352 -0.042352 0.044394 0.051870 -0.029947 0.047241 0.083449 -0.048180 -0.047241 0.083449 -0.048180 -0.044395 0.051870 -0.029947 -0.044395 0.057854 -0.015502 -0.047241 0.093076 -0.024940 0.047241 0.093076 -0.024940 0.044394 0.057854 -0.015502 0.044394 0.059895 0.000000 0.047241 0.096359 0.000000 -0.047241 0.096359 0.000000 -0.044395 0.059895 0.000000 -0.044395 0.057854 0.015502 -0.047241 0.093076 0.024940 0.047241 0.093076 0.024940 0.044394 0.057854 0.015502 0.044394 0.051870 0.029947 0.047241 0.083449 0.048180 -0.047241 0.083449 0.048180 -0.044395 0.051870 0.029947 -0.044395 0.042352 0.042352 -0.047241 0.068136 0.068136 0.047241 0.068136 0.068136 0.044394 0.042352 0.042352 0.044394 0.029947 0.051870 0.047241 0.048180 0.083450 -0.047241 0.048180 0.083450 -0.044395 0.029947 0.051870 -0.044395 0.015502 0.057854 -0.047241 0.024939 0.093076 0.047241 0.024939 0.093076 0.044394 0.015502 0.057854 0.044394 0.000000 0.059895 0.047241 -0.000000 0.096359 -0.047241 -0.000000 0.096359 -0.044395 -0.000000 0.059895 ] - } - texCoord TextureCoordinate { - point [ 0.4054 0.5409 0.5000 0.4342 0.5000 0.5658 0.4054 0.5409 0.4054 0.4591 0.5000 0.4342 1.0000 0.4342 0.9054 0.5409 0.9054 0.4591 1.0000 0.4342 1.0000 0.5658 0.9054 0.5409 0.9830 0.3070 0.9054 0.4591 0.8948 0.3800 0.9830 0.3070 1.0000 0.4342 0.9054 0.4591 0.3948 0.6200 0.5000 0.5658 0.4830 0.6930 0.3948 0.6200 0.4054 0.5409 0.5000 0.5658 0.3743 0.6908 0.4830 0.6930 0.4500 0.8070 0.3743 0.6908 0.3948 0.6200 0.4830 0.6930 0.9500 0.1930 0.8948 0.3800 0.8743 0.3092 0.9500 0.1930 0.9830 0.3070 0.8948 0.3800 0.9035 0.0999 0.8743 0.3092 0.8454 0.2513 0.9035 0.0999 0.9500 0.1930 0.8743 0.3092 0.3454 0.7487 0.4500 0.8070 0.4035 0.9001 0.3454 0.7487 0.3743 0.6908 0.4500 0.8070 0.3100 0.7896 0.4035 0.9001 0.3465 0.9659 0.3100 0.7896 0.3454 0.7487 0.4035 0.9001 0.8465 0.0341 0.8454 0.2513 0.8100 0.2104 0.8465 0.0341 0.9035 0.0999 0.8454 0.2513 0.7829 0.0000 0.8100 0.2104 0.7705 0.1892 0.7829 0.0000 0.8465 0.0341 0.8100 0.2104 0.2705 0.8108 0.3465 0.9659 0.2829 1.0000 0.2705 0.8108 0.3100 0.7896 0.3465 0.9659 0.2295 0.8108 0.2829 1.0000 0.2171 1.0000 0.2295 0.8108 0.2705 0.8108 0.2829 1.0000 0.7171 0.0000 0.7705 0.1892 0.7295 0.1892 0.7171 0.0000 0.7829 0.0000 0.7705 0.1892 0.6535 0.0341 0.7295 0.1892 0.6900 0.2104 0.6535 0.0341 0.7171 0.0000 0.7295 0.1892 0.1900 0.7896 0.2171 1.0000 0.1535 0.9659 0.1900 0.7896 0.2295 0.8108 0.2171 1.0000 0.1546 0.7487 0.1535 0.9659 0.0965 0.9001 0.1546 0.7487 0.1900 0.7896 0.1535 0.9659 0.5965 0.0999 0.6900 0.2104 0.6546 0.2513 0.5965 0.0999 0.6535 0.0341 0.6900 0.2104 0.5500 0.1930 0.6546 0.2513 0.6257 0.3092 0.5500 0.1930 0.5965 0.0999 0.6546 0.2513 0.1257 0.6908 0.0965 0.9001 0.0499 0.8070 0.1257 0.6908 0.1546 0.7487 0.0965 0.9001 0.1052 0.6200 0.0499 0.8070 0.0170 0.6930 0.1052 0.6200 0.1257 0.6908 0.0499 0.8070 0.5170 0.3070 0.6257 0.3092 0.6052 0.3800 0.5170 0.3070 0.5500 0.1930 0.6257 0.3092 0.5000 0.4342 0.6052 0.3800 0.5946 0.4591 0.5000 0.4342 0.5170 0.3070 0.6052 0.3800 0.0946 0.5409 0.0170 0.6930 0.0000 0.5658 0.0946 0.5409 0.1052 0.6200 0.0170 0.6930 0.0946 0.4591 0.0000 0.5658 0.0000 0.4342 0.0946 0.4591 0.0946 0.5409 0.0000 0.5658 0.5000 0.5658 0.5946 0.4591 0.5946 0.5409 0.5000 0.5658 0.5000 0.4342 0.5946 0.4591 0.5170 0.6930 0.5946 0.5409 0.6052 0.6200 0.5170 0.6930 0.5000 0.5658 0.5946 0.5409 0.1052 0.3800 0.0000 0.4342 0.0170 0.3070 0.1052 0.3800 0.0946 0.4591 0.0000 0.4342 0.1257 0.3092 0.0170 0.3070 0.0500 0.1930 0.1257 0.3092 0.1052 0.3800 0.0170 0.3070 0.5500 0.8070 0.6052 0.6200 0.6257 0.6908 0.5500 0.8070 0.5170 0.6930 0.6052 0.6200 0.5965 0.9001 0.6257 0.6908 0.6546 0.7487 0.5965 0.9001 0.5500 0.8070 0.6257 0.6908 0.1546 0.2513 0.0500 0.1930 0.0965 0.0999 0.1546 0.2513 0.1257 0.3092 0.0500 0.1930 0.1900 0.2104 0.0965 0.0999 0.1535 0.0341 0.1900 0.2104 0.1546 0.2513 0.0965 0.0999 0.6535 0.9659 0.6546 0.7487 0.6900 0.7896 0.6535 0.9659 0.5965 0.9001 0.6546 0.7487 0.7171 1.0000 0.6900 0.7896 0.7295 0.8108 0.7171 1.0000 0.6535 0.9659 0.6900 0.7896 0.2295 0.1892 0.1535 0.0341 0.2171 0.0000 0.2295 0.1892 0.1900 0.2104 0.1535 0.0341 0.2705 0.1892 0.2171 0.0000 0.2829 0.0000 0.2705 0.1892 0.2295 0.1892 0.2171 0.0000 0.7829 1.0000 0.7295 0.8108 0.7705 0.8108 0.7829 1.0000 0.7171 1.0000 0.7295 0.8108 0.8465 0.9659 0.7705 0.8108 0.8100 0.7896 0.8465 0.9659 0.7829 1.0000 0.7705 0.8108 0.3100 0.2104 0.2829 0.0000 0.3465 0.0341 0.3100 0.2104 0.2705 0.1892 0.2829 0.0000 0.3454 0.2513 0.3465 0.0341 0.4035 0.0999 0.3454 0.2513 0.3100 0.2104 0.3465 0.0341 0.9035 0.9001 0.8100 0.7896 0.8454 0.7487 0.9035 0.9001 0.8465 0.9659 0.8100 0.7896 0.9501 0.8070 0.8454 0.7487 0.8743 0.6908 0.9501 0.8070 0.9035 0.9001 0.8454 0.7487 0.3743 0.3092 0.4035 0.0999 0.4501 0.1930 0.3743 0.3092 0.3454 0.2513 0.4035 0.0999 0.3948 0.3800 0.4501 0.1930 0.4830 0.3070 0.3948 0.3800 0.3743 0.3092 0.4501 0.1930 0.9830 0.6930 0.8743 0.6908 0.8948 0.6200 0.9830 0.6930 0.9501 0.8070 0.8743 0.6908 1.0000 0.5658 0.8948 0.6200 0.9054 0.5409 1.0000 0.5658 0.9830 0.6930 0.8948 0.6200 0.4054 0.4591 0.4830 0.3070 0.5000 0.4342 0.4054 0.4591 0.3948 0.3800 0.4830 0.3070 ] - } - texCoordIndex [ 0 1 2 -1 3 4 5 -1 6 7 8 -1 9 10 11 -1 12 13 14 -1 15 16 17 -1 18 19 20 -1 21 22 23 -1 24 25 26 -1 27 28 29 -1 30 31 32 -1 33 34 35 -1 36 37 38 -1 39 40 41 -1 42 43 44 -1 45 46 47 -1 48 49 50 -1 51 52 53 -1 54 55 56 -1 57 58 59 -1 60 61 62 -1 63 64 65 -1 66 67 68 -1 69 70 71 -1 72 73 74 -1 75 76 77 -1 78 79 80 -1 81 82 83 -1 84 85 86 -1 87 88 89 -1 90 91 92 -1 93 94 95 -1 96 97 98 -1 99 100 101 -1 102 103 104 -1 105 106 107 -1 108 109 110 -1 111 112 113 -1 114 115 116 -1 117 118 119 -1 120 121 122 -1 123 124 125 -1 126 127 128 -1 129 130 131 -1 132 133 134 -1 135 136 137 -1 138 139 140 -1 141 142 143 -1 144 145 146 -1 147 148 149 -1 150 151 152 -1 153 154 155 -1 156 157 158 -1 159 160 161 -1 162 163 164 -1 165 166 167 -1 168 169 170 -1 171 172 173 -1 174 175 176 -1 177 178 179 -1 180 181 182 -1 183 184 185 -1 186 187 188 -1 189 190 191 -1 192 193 194 -1 195 196 197 -1 198 199 200 -1 201 202 203 -1 204 205 206 -1 207 208 209 -1 210 211 212 -1 213 214 215 -1 216 217 218 -1 219 220 221 -1 222 223 224 -1 225 226 227 -1 228 229 230 -1 231 232 233 -1 234 235 236 -1 237 238 239 -1 240 241 242 -1 243 244 245 -1 246 247 248 -1 249 250 251 -1 252 253 254 -1 255 256 257 -1 258 259 260 -1 261 262 263 -1 264 265 266 -1 267 268 269 -1 270 271 272 -1 273 274 275 -1 276 277 278 -1 279 280 281 -1 282 283 284 -1 285 286 287 -1 ] - coordIndex [ 88 94 89 -1 88 95 94 -1 90 92 91 -1 90 93 92 -1 85 91 84 -1 85 90 91 -1 87 89 86 -1 87 88 89 -1 80 86 81 -1 80 87 86 -1 82 84 83 -1 82 85 84 -1 77 83 76 -1 77 82 83 -1 79 81 78 -1 79 80 81 -1 72 78 73 -1 72 79 78 -1 74 76 75 -1 74 77 76 -1 69 75 68 -1 69 74 75 -1 71 73 70 -1 71 72 73 -1 64 70 65 -1 64 71 70 -1 66 68 67 -1 66 69 68 -1 61 67 60 -1 61 66 67 -1 63 65 62 -1 63 64 65 -1 56 62 57 -1 56 63 62 -1 58 60 59 -1 58 61 60 -1 53 59 52 -1 53 58 59 -1 55 57 54 -1 55 56 57 -1 48 54 49 -1 48 55 54 -1 50 52 51 -1 50 53 52 -1 45 51 44 -1 45 50 51 -1 47 49 46 -1 47 48 49 -1 40 46 41 -1 40 47 46 -1 42 44 43 -1 42 45 44 -1 37 43 36 -1 37 42 43 -1 39 41 38 -1 39 40 41 -1 32 38 33 -1 32 39 38 -1 34 36 35 -1 34 37 36 -1 29 35 28 -1 29 34 35 -1 31 33 30 -1 31 32 33 -1 24 30 25 -1 24 31 30 -1 26 28 27 -1 26 29 28 -1 21 27 20 -1 21 26 27 -1 23 25 22 -1 23 24 25 -1 16 22 17 -1 16 23 22 -1 18 20 19 -1 18 21 20 -1 13 19 12 -1 13 18 19 -1 15 17 14 -1 15 16 17 -1 8 14 9 -1 8 15 14 -1 10 12 11 -1 10 13 12 -1 5 11 4 -1 5 10 11 -1 7 9 6 -1 7 8 9 -1 0 6 1 -1 0 7 6 -1 2 4 3 -1 2 5 4 -1 93 3 92 -1 93 2 3 -1 95 1 94 -1 95 0 1 -1 ] - } - } - ] - } - DEF LEFT_CAP_AND_VALVE_SHAPES Group { - children [ - DEF LEFT_CAP_SHAPE Shape { - appearance PBRAppearance { - baseColor 0 0 0 - } - geometry IndexedFaceSet { - coord Coordinate { - point [ - -0.042613 0.0264202 0.0226794, -0.042613 0.0256074 0.0238958 -0.042613 0.025322 0.0253307, -0.042613 0.0256074 0.0267656 -0.042613 0.0264202 0.0279821, -0.042613 0.0276366 0.0287949 -0.042613 0.0290715 0.0290803, -0.042613 0.0305064 0.0287949 -0.042613 0.0317229 0.0279821, -0.042613 0.0325357 0.0267656 -0.042613 0.0328211 0.0253307, -0.042613 0.0325357 0.0238958 -0.042613 0.0317229 0.0226794, -0.042613 0.0305064 0.0218666 -0.042613 0.0290715 0.0215812, -0.042613 0.0276366 0.0218666 -0.038451 0.0264202 0.0226794, -0.038451 0.0256074 0.0238959 -0.038451 0.025322 0.0253308, -0.038451 0.0256074 0.0267657 -0.038451 0.0264202 0.0279821, -0.038451 0.0276366 0.0287949 -0.038451 0.0290715 0.0290803, -0.038451 0.0305064 0.0287949 -0.038451 0.0317229 0.0279821, -0.038451 0.0325357 0.0267656 -0.038451 0.0328211 0.0253307, -0.038451 0.0325357 0.0238958 -0.038451 0.0317229 0.0226794, -0.038451 0.0305064 0.0218666 -0.038451 0.0290715 0.0215812, -0.038451 0.0276366 0.0218666 -0.038451 0.0290715 0.0253307, -0.0454428 0.0290715 0.0253307 -0.0454428 0.0290715 0.0235262, -0.0454428 0.0303476 0.0240547 -0.0454428 0.0308761 0.0253307, -0.0454428 0.0303476 0.0266068 -0.0454428 0.0290715 0.0271353, -0.0454428 0.0277955 0.0266068 -0.0454428 0.027267 0.0253307, -0.0454428 0.0277955 0.0240547 - ] - } - coordIndex [ - 32 17 16 -1 32 18 17 -1 32 19 18 -1 32 20 19 -1 32 21 20 -1 32 22 21 -1 32 23 22 -1 32 24 23 -1 32 25 24 -1 32 26 25 -1 32 27 26 -1 32 28 27 -1 32 29 28 -1 32 30 29 -1 32 31 30 -1 32 16 31 -1 0 16 17 1 -1 1 17 18 2 -1 2 18 19 3 -1 3 19 20 4 -1 4 20 21 5 -1 5 21 22 6 -1 6 22 23 7 -1 7 23 24 8 -1 8 24 25 9 -1 9 25 26 10 -1 10 26 27 11 -1 11 27 28 12 -1 12 28 29 13 -1 13 29 30 14 -1 14 30 31 15 -1 16 0 15 31 -1 15 0 41 -1 14 15 41 34 -1 13 14 34 -1 12 13 34 35 -1 11 12 35 36 -1 10 11 36 -1 9 10 36 37 -1 8 9 37 -1 7 8 37 38 -1 6 7 38 -1 5 6 38 39 -1 4 5 39 -1 3 4 39 -1 2 3 39 40 -1 1 2 40 -1 0 1 40 41 -1 33 41 40 -1 33 40 39 -1 33 39 38 -1 33 38 37 -1 33 37 36 -1 33 36 35 -1 33 35 34 -1 33 34 41 -1 - ] - creaseAngle 1 - } - } - DEF LEFT_VALVE_SHAPE Shape { - appearance PBRAppearance { - baseColor 0.95894 0.8 0 - roughness 0.3 - metalness 0 - } - geometry IndexedFaceSet { - coord Coordinate { - point [ - -0.0153188 0.0271786 0.0234378, -0.0153188 0.0265983 0.0243063 -0.0153188 0.0263945 0.0253308, -0.0153188 0.0271786 0.0272237 -0.0153188 0.0290715 0.0280078, -0.0153188 0.030096 0.027804 -0.0153188 0.0309645 0.0272237, -0.0153188 0.0315448 0.0263552 -0.0153188 0.0317485 0.0253308, -0.0153188 0.0315448 0.0243063 -0.0153188 0.030096 0.0228575, -0.0153188 0.0290715 0.0226537 -0.0153188 0.0280471 0.0228575, -0.0401998 0.0271786 0.0234378 -0.0401998 0.0265983 0.0243063, -0.0401998 0.0263945 0.0253308 -0.0401998 0.0271786 0.0272237, -0.0401998 0.0290715 0.0280078 -0.0401998 0.030096 0.027804, -0.0401998 0.0309645 0.0272237 -0.0401998 0.0315448 0.0263552, -0.0401998 0.0317485 0.0253307 -0.0401998 0.0315448 0.0243063, -0.0401998 0.030096 0.0228575 -0.0401998 0.0290715 0.0226537, -0.0401998 0.0280471 0.0228575 - ] - } - coordIndex [ - 0 1 14 13 -1 1 2 15 14 -1 2 3 16 15 -1 3 4 17 16 -1 4 5 18 17 -1 5 6 19 18 -1 6 7 20 19 -1 7 8 21 20 -1 8 9 22 21 -1 9 10 23 22 -1 10 11 24 23 -1 11 12 25 24 -1 12 0 13 25 -1 - ] - creaseAngle 1 - } - } - ] - } - ] - name "back left wheel" - boundingObject DEF BOUNDING_WHEEL Transform { - rotation 0 0 1 1.5708 - children [ - Cylinder { - height 0.095 - radius 0.11 - subdivision 24 - } - ] - } - physics DEF WHEEL_PHYSICS Physics { - density -1 - mass 0.8 - } - } - } - DEF FRONT_BLACK_PANEL Shape { - appearance PBRAppearance { - baseColor 0.06 0.06 0.06 - metalness 0 - roughness 0.3 - } - geometry IndexedFaceSet { - coord Coordinate { - point [ - -0.0068561 -0.0299255 -0.214614, 0.0743534 -0.0299255 -0.214614 0.0749096 0.00276717 -0.214614, -0.00685611 0.00276717 -0.214614 -0.00685611 0.00276717 -0.209428, 0.0749096 0.00276717 -0.209428 0.0743534 -0.0299255 -0.209428, -0.0068561 -0.0299255 -0.209428 - ] - } - coordIndex [ - 5 4 6 -1 4 7 6 -1 7 0 1 -1 7 1 6 -1 2 5 6 -1 2 6 1 -1 3 4 5 -1 3 5 2 -1 0 7 4 -1 0 4 3 -1 0 3 1 -1 3 2 1 -1 - ] - } - } - DEF BACK_RIGHT_WHEEL HingeJoint { - device [ - RotationalMotor { - name "back right wheel" - maxVelocity 6.4 - maxTorque 20 - } - PositionSensor { - name "back right wheel sensor" - } - ] - jointParameters HingeJointParameters { - axis -1 0 0 - anchor 0.197 0 0.1331 - } - endPoint Solid { - translation 0.197 0 0.1331 - rotation -1 0 0 0 - children [ - USE RIM_AND_TIRE_SHAPES - DEF RIGHT_CAP_AND_VALVE_SHAPES Group { - children [ - DEF RIGHT_CAP_SHAPE Shape { - appearance PBRAppearance { - baseColor 0 0 0 - metalness 0 - roughness 0.3 - } - geometry IndexedFaceSet { - coord Coordinate { - point [ - 0.0405424 0.0264202 0.0226794, 0.0405424 0.0256074 0.0238959 0.0405424 0.025322 0.0253308, 0.0405424 0.0256074 0.0267657 0.0405424 0.0264202 0.0279821, 0.0405424 0.0276366 0.0287949 0.0405424 0.0290715 0.0290803, 0.0405424 0.0305064 0.0287949 0.0405424 0.0317229 0.0279821, 0.0405424 0.0325357 0.0267657 0.0405424 0.0328211 0.0253308, 0.0405424 0.0325357 0.0238959 0.0405424 0.0317229 0.0226794, 0.0405424 0.0305064 0.0218666 0.0405424 0.0290715 0.0215812, 0.0405424 0.0276366 0.0218666 0.0363803 0.0264202 0.0226794, 0.0363803 0.0256074 0.0238959 0.0363803 0.025322 0.0253308, 0.0363803 0.0256074 0.0267657 0.0363803 0.0264202 0.0279821, 0.0363803 0.0276366 0.0287949 0.0363803 0.0290715 0.0290803, 0.0363803 0.0305064 0.0287949 0.0363803 0.0317229 0.0279821, 0.0363803 0.0325357 0.0267657 0.0363803 0.0328211 0.0253308, 0.0363803 0.0325357 0.0238959 0.0363803 0.0317229 0.0226794, 0.0363803 0.0305064 0.0218666 0.0363803 0.0290715 0.0215812, 0.0363803 0.0276366 0.0218666 0.0363803 0.0290715 0.0253308, 0.0433722 0.0290715 0.0253308 0.0433722 0.0290715 0.0235262, 0.0433722 0.0303476 0.0240547 0.0433722 0.0308761 0.0253308, 0.0433722 0.0303476 0.0266068 0.0433722 0.0290715 0.0271353, 0.0433722 0.0277955 0.0266068 0.0433722 0.027267 0.0253308, 0.0433722 0.0277955 0.0240547 - ] - } - coordIndex [ - 32 16 17 -1 32 17 18 -1 32 18 19 -1 32 19 20 -1 32 20 21 -1 32 21 22 -1 32 22 23 -1 32 23 24 -1 32 24 25 -1 32 25 26 -1 32 26 27 -1 32 27 28 -1 32 28 29 -1 32 29 30 -1 32 30 31 -1 32 31 16 -1 0 1 17 16 -1 1 2 18 17 -1 2 3 19 18 -1 3 4 20 19 -1 4 5 21 20 -1 5 6 22 21 -1 6 7 23 22 -1 7 8 24 23 -1 8 9 25 24 -1 9 10 26 25 -1 10 11 27 26 -1 11 12 28 27 -1 12 13 29 28 -1 13 14 30 29 -1 14 15 31 30 -1 15 0 16 31 -1 41 0 15 -1 14 34 41 15 -1 13 34 14 -1 12 35 34 13 -1 11 36 35 12 -1 10 36 11 -1 9 37 36 10 -1 8 37 9 -1 7 38 37 8 -1 6 38 7 -1 5 39 38 6 -1 4 39 5 -1 3 39 4 -1 2 40 39 3 -1 1 40 2 -1 0 41 40 1 -1 33 40 41 -1 33 39 40 -1 33 38 39 -1 33 37 38 -1 33 36 37 -1 33 35 36 -1 33 34 35 -1 33 41 34 -1 - ] - creaseAngle 1 - } - } - DEF RIGHT_VALVE_SHAPE Shape { - appearance PBRAppearance { - baseColor 0.95894 0.8 0 - metalness 0 - roughness 0.3 - } - geometry IndexedFaceSet { - coord Coordinate { - point [ - 0.0132481 0.0271786 0.0234378, 0.0132481 0.0265983 0.0243063 0.0132481 0.0263945 0.0253308, 0.0132481 0.0271786 0.0272237 0.0132481 0.0290715 0.0280078, 0.0132481 0.030096 0.027804 0.0132481 0.0309645 0.0272237, 0.0132481 0.0315448 0.0263552 0.0132481 0.0317485 0.0253308, 0.0132481 0.0315448 0.0243063 0.0132481 0.030096 0.0228575, 0.0132481 0.0290715 0.0226538 0.0132481 0.0280471 0.0228575, 0.0381291 0.0271786 0.0234378 0.0381291 0.0265983 0.0243063, 0.0381291 0.0263945 0.0253308 0.0381291 0.0271786 0.0272237, 0.0381291 0.0290715 0.0280078 0.0381291 0.030096 0.027804, 0.0381291 0.0309645 0.0272237 0.0381291 0.0315448 0.0263552, 0.0381291 0.0317485 0.0253308 0.0381291 0.0315448 0.0243063, 0.0381291 0.030096 0.0228575 0.0381291 0.0290715 0.0226538, 0.0381291 0.0280471 0.0228575 - ] - } - coordIndex [ - 0 13 14 1 -1 1 14 15 2 -1 2 15 16 3 -1 3 16 17 4 -1 4 17 18 5 -1 5 18 19 6 -1 6 19 20 7 -1 7 20 21 8 -1 8 21 22 9 -1 9 22 23 -1 9 23 10 -1 10 23 24 11 -1 11 24 25 12 -1 13 0 12 25 -1 - ] - creaseAngle 1 - } - } - ] - } - ] - name "back right wheel" - boundingObject USE BOUNDING_WHEEL - physics USE WHEEL_PHYSICS - } - } - DEF FRONT_LEFT_WHEEL HingeJoint { - device [ - RotationalMotor { - name "front left wheel" - maxVelocity 6.4 - maxTorque 20 - } - PositionSensor { - name "front left wheel sensor" - } - ] - jointParameters HingeJointParameters { - axis -1 0 0 - anchor -0.197 0 -0.1331 - } - endPoint Solid { - translation -0.197 0 -0.1331 - rotation -1 0 0 0 - children [ - USE RIM_AND_TIRE_SHAPES - USE LEFT_CAP_AND_VALVE_SHAPES - ] - name "front left wheel" - boundingObject USE BOUNDING_WHEEL - physics USE WHEEL_PHYSICS - } - } - DEF FRONT_RIGHT_WHEEL HingeJoint { - device [ - RotationalMotor { - name "front right wheel" - maxVelocity 6.4 - maxTorque 20 - } - PositionSensor { - name "front right wheel sensor" - } - ] - jointParameters HingeJointParameters { - axis -1 0 0 - anchor 0.197 0 -0.1331 - } - endPoint Solid { - translation 0.197 0 -0.1331 - rotation -1 0 0 0 - children [ - USE RIM_AND_TIRE_SHAPES - USE RIGHT_CAP_AND_VALVE_SHAPES - ] - name "front right wheel" - boundingObject USE BOUNDING_WHEEL - physics USE WHEEL_PHYSICS - } - } - DEF SO_0 Pioneer3DistanceSensor { - translation -0.135387 0.137 -0.146604 - rotation 0 -1 0 3.14159 - name "so0" - } - DEF SO_1 Pioneer3DistanceSensor { - translation -0.117971 0.137 -0.194455 - rotation 0 1 0 2.44346 - name "so1" - } - DEF SO_10 Pioneer3DistanceSensor { - translation 0.0769827 0.137 0.226776 - rotation 0 -1 0 1.0472 - name "so10" - } - DEF SO_11 Pioneer3DistanceSensor { - translation 0.0266877 0.137 0.244814 - rotation 0 -1 0 1.39626 - name "so11" - } - DEF SO_12 Pioneer3DistanceSensor { - translation -0.0267158 0.137 0.244668 - rotation 0 -1 0 1.74533 - name "so12" - } - DEF SO_13 Pioneer3DistanceSensor { - translation -0.0769749 0.137 0.226483 - rotation 0 -1 0 2.0944 - name "so13" - } - DEF SO_14 Pioneer3DistanceSensor { - translation -0.117967 0.137 0.192247 - rotation 0 -1 0 2.44346 - name "so14" - } - DEF SO_15 Pioneer3DistanceSensor { - translation -0.135383 0.137 0.144395 - rotation 0 -1 0 3.14159 - name "so15" - } - DEF SO_2 Pioneer3DistanceSensor { - translation -0.0769789 0.137 -0.228691 - rotation 0 1 0 2.0944 - name "so2" - } - DEF SO_3 Pioneer3DistanceSensor { - translation -0.0267197 0.137 -0.246876 - rotation 0 1 0 1.74533 - name "so3" - } - DEF SO_4 Pioneer3DistanceSensor { - translation 0.0266839 0.137 -0.247023 - rotation 0 1 0 1.39626 - name "so4" - } - DEF SO_5 Pioneer3DistanceSensor { - translation 0.0769787 0.137 -0.228985 - rotation 0 1 0 1.0472 - name "so5" - } - DEF SO_6 Pioneer3DistanceSensor { - translation 0.117971 0.137 -0.194749 - rotation 0 1 0 0.698132 - name "so6" - } - DEF SO_7 Pioneer3DistanceSensor { - translation 0.135387 0.137 -0.146897 - name "so7" - } - DEF SO_8 Pioneer3DistanceSensor { - translation 0.135391 0.137 0.144689 - name "so8" - } - DEF SO_9 Pioneer3DistanceSensor { - translation 0.117975 0.137 0.19254 - rotation 0 -1 0 0.698131 - name "so9" - } - DEF SONAR_RING_SHAPE Shape { - appearance PBRAppearance { - baseColor 1 0 0 - metalness 0 - roughness 0.3 - } - geometry IndexedFaceSet { - coord Coordinate { - point [ - 0.135387 0.112153 -0.119802, 0.135387 0.112153 -0.173993 0.100554 0.112153 -0.215505, 0.0534035 0.112153 -0.242465 -3.57e-05 0.112153 -0.251581, -0.0534037 0.112153 -0.242171 -0.100554 0.112153 -0.215212, -0.135387 0.112153 -0.173699 -0.135387 0.112153 -0.119508, -0.135387 0.162005 -0.119508 -0.135387 0.162005 -0.173699, -0.100554 0.162005 -0.215212 -0.0534037 0.162005 -0.242171, -3.57e-05 0.162005 -0.251581 0.0534035 0.162005 -0.242465, 0.100554 0.162005 -0.215505 0.135387 0.162005 -0.173993, 0.135387 0.162005 -0.119802 0.135391 0.162005 0.117593, 0.135391 0.162005 0.171784 0.100558 0.162005 0.213297, 0.0534075 0.162005 0.240256 -3.2e-05 0.162005 0.249373, -0.0533997 0.162005 0.239962 -0.10055 0.162005 0.213003, -0.135383 0.162005 0.17149 -0.135383 0.162005 0.1173, -0.135383 0.112153 0.1173 -0.135383 0.112153 0.17149, -0.10055 0.112153 0.213003 -0.0533997 0.112153 0.239962, -3.2e-05 0.112153 0.249373 0.0534075 0.112153 0.240256, 0.100558 0.112153 0.213297 0.135391 0.112153 0.171784, 0.135391 0.112153 0.117593 - ] - } - coordIndex [ - 3 5 4 -1 2 5 3 -1 2 6 5 -1 1 6 2 -1 1 7 6 -1 0 7 1 -1 0 8 7 -1 0 17 9 8 -1 4 13 14 3 -1 7 10 11 6 -1 5 12 13 4 -1 6 11 12 5 -1 8 9 10 7 -1 17 0 1 16 -1 3 14 15 2 -1 2 15 16 1 -1 33 34 19 20 -1 32 33 20 21 -1 34 35 18 19 -1 27 28 25 26 -1 29 30 23 24 -1 30 31 22 23 -1 28 29 24 25 -1 31 32 21 22 -1 35 27 26 18 -1 35 28 27 -1 35 34 28 -1 34 29 28 -1 34 33 29 -1 33 30 29 -1 33 32 30 -1 32 31 30 -1 - ] - } - } - DEF TOP_SHAPE Shape { - appearance PBRAppearance { - roughness 0.3 - metalness 0 - baseColorMap ImageTexture { - url [ - "https://raw.githubusercontent.com/cyberbotics/webots/R2021a/projects/robots/adept/pioneer3/protos/textures/top_3at_base_color.jpg" - ] - } - } - geometry IndexedFaceSet { - coord Coordinate { - point [ - -0.161502 0.165434 0.0273416, -0.187667 0.165434 -0.0087482 -0.187667 0.165434 -0.139574, -0.181351 0.165434 -0.157619 -0.130826 0.165434 -0.209949, -0.0757887 0.165434 -0.239723 0.0013534 0.165434 -0.255061, -0.161502 0.165434 0.168092 -0.151577 0.165434 0.187942, -0.0775932 0.165434 0.237565 0.0013534 0.165434 0.254244, 0.0802999 0.165434 0.237565 0.154284 0.165434 0.187941, 0.164209 0.165434 0.168092 0.0784954 0.165434 -0.239723, 0.133532 0.165434 -0.209949 0.184058 0.165434 -0.157619, 0.190374 0.165434 -0.139574 0.190374 0.165434 -0.0087482, 0.164209 0.165434 0.0273416 0.164209 0.161275 0.0273416, 0.190374 0.161275 -0.0087482 0.190374 0.161275 -0.139574, 0.184058 0.161275 -0.157619 0.133532 0.161275 -0.209949, 0.0784954 0.161275 -0.239723 0.164209 0.161275 0.168092, 0.154284 0.161275 0.187941 0.0802999 0.161275 0.237565, 0.0013534 0.161275 0.254244 -0.0775932 0.161275 0.237565, -0.151577 0.161275 0.187942 -0.161502 0.161275 0.168092, 0.0013534 0.161275 -0.255061 -0.0757887 0.161275 -0.239723, -0.130826 0.161275 -0.209949 -0.181351 0.161275 -0.157619, -0.187667 0.161275 -0.139574 -0.187667 0.161275 -0.0087482, -0.161502 0.161275 0.0273416 - ] - } - texCoord TextureCoordinate { - point [ - 0.661139 0.955673, 0.50258 0.98603, 0.344021 0.955673 0.661139 0.955673, 0.344021 0.955673, 0.230898 0.896745 0.774262 0.896745, 0.661139 0.955673, 0.230898 0.896745 0.774262 0.896745, 0.230898 0.896745, 0.127046 0.793173 0.878114 0.793173, 0.774262 0.896745, 0.127046 0.793173 0.878114 0.793173, 0.127046 0.793173, 0.114065 0.757459 0.891095 0.757459, 0.878114 0.793173, 0.114065 0.757459 0.891095 0.757459, 0.114065 0.757459, 0.114065 0.49853 0.891095 0.49853, 0.891095 0.757459, 0.114065 0.49853 0.114065 0.49853, 0.167845 0.427102, 0.891095 0.49853 0.891095 0.49853, 0.167845 0.427102, 0.837315 0.427102 0.837315 0.427102, 0.167845 0.427102, 0.167845 0.14853 0.837315 0.14853, 0.837315 0.427102, 0.167845 0.14853 0.837315 0.14853, 0.167845 0.14853, 0.188244 0.109244 0.816916 0.109244, 0.837315 0.14853, 0.188244 0.109244 0.664848 0.0110302, 0.816916 0.109244, 0.188244 0.109244 0.664848 0.0110302, 0.188244 0.109244, 0.340312 0.0110302 0.664848 0.0110302, 0.340312 0.0110302, 0.50258 0.0023283 0.1032 0.92567, 0.13135 0.92567, 0.13135 0.961898 0.1032 0.92567, 0.13135 0.92567, 0.13135 0.961898 0.1032 0.92567, 0.13135 0.92567, 0.13135 0.961898 0.1032 0.92567, 0.13135 0.92567, 0.13135 0.961898 0.1032 0.92567, 0.13135 0.92567, 0.13135 0.961898 0.1032 0.92567, 0.13135 0.92567, 0.13135 0.961898 0.1032 0.92567, 0.13135 0.92567, 0.13135 0.961898 0.1032 0.92567, 0.13135 0.92567, 0.13135 0.961898 0.1032 0.92567, 0.13135 0.92567, 0.13135 0.961898 0.1032 0.92567, 0.13135 0.92567, 0.13135 0.961898 0.1032 0.92567, 0.13135 0.92567, 0.13135 0.961898 0.1032 0.92567, 0.13135 0.92567, 0.13135 0.961898 0.1032 0.92567, 0.13135 0.92567, 0.13135 0.961898 0.1032 0.92567, 0.13135 0.92567, 0.13135 0.961898 0.1032 0.92567, 0.13135 0.92567, 0.13135 0.961898 0.1032 0.92567, 0.13135 0.92567, 0.13135 0.961898 0.1032 0.92567, 0.13135 0.92567, 0.13135 0.961898 0.1032 0.92567, 0.13135 0.92567, 0.13135 0.961898 0.167845 0.427102, 0.114065 0.49853, 0.114065 0.49853 0.167845 0.427102, 0.114065 0.49853, 0.167845 0.427102 0.114065 0.49853, 0.114065 0.757459, 0.114065 0.757459 0.114065 0.49853, 0.114065 0.757459, 0.114065 0.49853 0.114065 0.757459, 0.127046 0.793173, 0.127046 0.793173 0.114065 0.757459, 0.127046 0.793173, 0.114065 0.757459 0.127046 0.793173, 0.230898 0.896745, 0.230898 0.896745 0.127046 0.793173, 0.230898 0.896745, 0.127046 0.793173 0.230898 0.896745, 0.344021 0.955673, 0.344021 0.955673 0.230898 0.896745, 0.344021 0.955673, 0.230898 0.896745 0.344021 0.955673, 0.50258 0.98603, 0.50258 0.98603 0.344021 0.955673, 0.50258 0.98603, 0.344021 0.955673 0.188244 0.109244, 0.167845 0.14853, 0.167845 0.14853 0.188244 0.109244, 0.167845 0.14853, 0.188244 0.109244 0.340312 0.0110302, 0.188244 0.109244, 0.188244 0.109244 0.340312 0.0110302, 0.188244 0.109244, 0.340312 0.0110302 0.50258 0.0023283, 0.340312 0.0110302, 0.340312 0.0110302 0.50258 0.0023283, 0.340312 0.0110302, 0.50258 0.0023283 0.167845 0.14853, 0.167845 0.427102, 0.167845 0.427102 0.167845 0.14853, 0.167845 0.427102, 0.167845 0.14853 0.837315 0.427102, 0.837315 0.14853, 0.837315 0.14853 0.837315 0.427102, 0.837315 0.14853, 0.837315 0.427102 0.816916 0.109244, 0.664848 0.0110302, 0.664848 0.0110302 0.816916 0.109244, 0.664848 0.0110302, 0.816916 0.109244 0.837315 0.14853, 0.816916 0.109244, 0.816916 0.109244 0.837315 0.14853, 0.816916 0.109244, 0.837315 0.14853 0.661139 0.955673, 0.774262 0.896745, 0.774262 0.896745 0.661139 0.955673, 0.774262 0.896745, 0.661139 0.955673 0.774262 0.896745, 0.878114 0.793173, 0.878114 0.793173 0.774262 0.896745, 0.878114 0.793173, 0.774262 0.896745 0.878114 0.793173, 0.891095 0.757459, 0.891095 0.757459 0.878114 0.793173, 0.891095 0.757459, 0.878114 0.793173 0.891095 0.757459, 0.891095 0.49853, 0.891095 0.49853 0.891095 0.757459, 0.891095 0.49853, 0.891095 0.757459 0.891095 0.49853, 0.837315 0.427102, 0.837315 0.427102 0.891095 0.49853, 0.837315 0.427102, 0.891095 0.49853 0.50258 0.98603, 0.661139 0.955673, 0.661139 0.955673 0.50258 0.98603, 0.661139 0.955673, 0.50258 0.98603 0.664848 0.0110302, 0.50258 0.0023283, 0.50258 0.0023283 0.664848 0.0110302, 0.50258 0.0023283, 0.664848 0.0110302 - ] - } - coordIndex [ - 14 6 5 -1 14 5 4 -1 15 14 4 -1 15 4 3 -1 16 15 3 -1 16 3 2 -1 17 16 2 -1 17 2 1 -1 18 17 1 -1 1 0 18 -1 18 0 19 -1 19 0 7 -1 13 19 7 -1 13 7 8 -1 12 13 8 -1 11 12 8 -1 11 8 9 -1 11 9 10 -1 34 33 25 -1 35 34 25 -1 35 25 24 -1 36 35 24 -1 36 24 23 -1 37 36 23 -1 37 23 22 -1 38 37 22 -1 38 22 21 -1 39 38 21 -1 39 21 20 -1 32 39 20 -1 32 20 26 -1 31 32 26 -1 31 26 27 -1 31 27 28 -1 30 31 28 -1 29 30 28 -1 0 1 38 -1 0 38 39 -1 1 2 37 -1 1 37 38 -1 2 3 36 -1 2 36 37 -1 3 4 35 -1 3 35 36 -1 4 5 34 -1 4 34 35 -1 5 6 33 -1 5 33 34 -1 8 7 32 -1 8 32 31 -1 9 8 31 -1 9 31 30 -1 10 9 30 -1 10 30 29 -1 7 0 39 -1 7 39 32 -1 19 13 26 -1 19 26 20 -1 12 11 28 -1 12 28 27 -1 13 12 27 -1 13 27 26 -1 14 15 24 -1 14 24 25 -1 15 16 23 -1 15 23 24 -1 16 17 22 -1 16 22 23 -1 17 18 21 -1 17 21 22 -1 18 19 20 -1 18 20 21 -1 6 14 25 -1 6 25 33 -1 11 10 29 -1 11 29 28 -1 - ] - texCoordIndex [ - 0 1 2 -1 3 4 5 -1 6 7 8 -1 9 10 11 -1 12 13 14 -1 15 16 17 -1 18 19 20 -1 21 22 23 -1 24 25 26 -1 27 28 29 -1 30 31 32 -1 33 34 35 -1 36 37 38 -1 39 40 41 -1 42 43 44 -1 45 46 47 -1 48 49 50 -1 51 52 53 -1 54 55 56 -1 57 58 59 -1 60 61 62 -1 63 64 65 -1 66 67 68 -1 69 70 71 -1 72 73 74 -1 75 76 77 -1 78 79 80 -1 81 82 83 -1 84 85 86 -1 87 88 89 -1 90 91 92 -1 93 94 95 -1 96 97 98 -1 99 100 101 -1 102 103 104 -1 105 106 107 -1 108 109 110 -1 111 112 113 -1 114 115 116 -1 117 118 119 -1 120 121 122 -1 123 124 125 -1 126 127 128 -1 129 130 131 -1 132 133 134 -1 135 136 137 -1 138 139 140 -1 141 142 143 -1 144 145 146 -1 147 148 149 -1 150 151 152 -1 153 154 155 -1 156 157 158 -1 159 160 161 -1 162 163 164 -1 165 166 167 -1 168 169 170 -1 171 172 173 -1 174 175 176 -1 177 178 179 -1 180 181 182 -1 183 184 185 -1 186 187 188 -1 189 190 191 -1 192 193 194 -1 195 196 197 -1 198 199 200 -1 201 202 203 -1 204 205 206 -1 207 208 209 -1 210 211 212 -1 213 214 215 -1 216 217 218 -1 219 220 221 -1 222 223 224 -1 225 226 227 -1 - ] - creaseAngle 0.6 - } - } - DEF ANTENNAS_SHAPES Shape { - appearance PBRAppearance { - baseColor 0.9 0.9 0.9 - roughness 0.3 - } - geometry IndexedFaceSet { - coord Coordinate { - point [ - -0.0783471 0.165559 0.0121679, -0.0769598 0.165559 0.0142442 -0.0764726 0.165559 0.0166934, -0.0769598 0.165559 0.0191425 -0.0783471 0.165559 0.0212188, -0.0804234 0.165559 0.0226062 -0.0828726 0.165559 0.0230934, -0.0853217 0.165559 0.0226062 -0.0873981 0.165559 0.0212188, -0.0887854 0.165559 0.0191425 -0.0892726 0.165559 0.0166934, -0.0887854 0.165559 0.0142442 -0.0873981 0.165559 0.0121679, -0.0853217 0.165559 0.0107805 -0.0828726 0.165559 0.0102934, -0.0804234 0.165559 0.0107805 -0.0804234 0.170194 0.0107805, -0.0828726 0.170194 0.0102934 -0.0853217 0.170194 0.0107805, -0.0873981 0.170194 0.0121679 -0.0887854 0.170194 0.0142442, -0.0892726 0.170194 0.0166934 -0.0887854 0.170194 0.0191425, -0.0873981 0.170194 0.0212188 -0.0853217 0.170194 0.0226062, -0.0828726 0.170194 0.0230934 -0.0804234 0.170194 0.0226062, -0.0783471 0.170194 0.0212188 -0.0769598 0.170194 0.0191425, -0.0764726 0.170194 0.0166934 -0.0769598 0.170194 0.0142442, -0.0783471 0.170194 0.0121679 -0.0798949 0.170194 0.0137156, -0.078982 0.170194 0.0150818 -0.0786614 0.170194 0.0166934, -0.078982 0.170194 0.0183049 -0.0798949 0.170194 0.0196711, -0.081261 0.170194 0.0205839 -0.0828726 0.170194 0.0209045, -0.0844841 0.170194 0.0205839 -0.0858503 0.170194 0.0196711, -0.0867632 0.170194 0.0183049 -0.0870837 0.170194 0.0166934, -0.0867632 0.170194 0.0150818 -0.0858503 0.170194 0.0137156, -0.0844841 0.170194 0.0128028 -0.0828726 0.170194 0.0124822, -0.081261 0.170194 0.0128028 -0.081261 0.172863 0.0128028, -0.0828726 0.172863 0.0124822 -0.0844841 0.172863 0.0128028, -0.0858503 0.172863 0.0137156 -0.0867632 0.172863 0.0150818, -0.0870837 0.172863 0.0166934 -0.0867632 0.172863 0.0183049, -0.0858503 0.172863 0.0196711 -0.0844841 0.172863 0.0205839, -0.0828726 0.172863 0.0209045 -0.081261 0.172863 0.0205839, -0.0798949 0.172863 0.0196711 -0.078982 0.172863 0.0183049, -0.0786614 0.172863 0.0166934 -0.078982 0.172863 0.0150818, -0.0798949 0.172863 0.0137156 -0.0817977 0.172863 0.0140985, -0.0828726 0.172863 0.0138847 -0.0839474 0.172863 0.0140985, -0.0848586 0.172863 0.0147073 -0.0854675 0.172863 0.0156185, -0.0856813 0.172863 0.0166934 -0.0854675 0.172863 0.0177682, -0.0848586 0.172863 0.0186794 -0.0839474 0.172863 0.0192882, -0.0828726 0.172863 0.019502 -0.0817977 0.172863 0.0192882, -0.0808865 0.172863 0.0186794 -0.0802777 0.172863 0.0177682, -0.0800639 0.172863 0.0166934 -0.0802777 0.172863 0.0156185, -0.0808865 0.172863 0.0147073 -0.0808865 0.178136 0.0147073, -0.0802777 0.178136 0.0156185 -0.0800639 0.178136 0.0166934, -0.0802777 0.178136 0.0177682 -0.0808865 0.178136 0.0186794, -0.0817977 0.178136 0.0192882 -0.0828726 0.178136 0.019502, -0.0839474 0.178136 0.0192882 -0.0848586 0.178136 0.0186794, -0.0854675 0.178136 0.0177682 -0.0856813 0.178136 0.0166934, -0.0854675 0.178136 0.0156185 -0.0848586 0.178136 0.0147073, -0.0839474 0.178136 0.0140985 -0.0828726 0.178136 0.0138847, -0.0817977 0.178136 0.0140985 -0.0799784 0.178136 0.0137992, -0.0790911 0.178136 0.015127 -0.0787795 0.178136 0.0166934, -0.0790911 0.178136 0.0182597 -0.0799784 0.178136 0.0195876, -0.0813062 0.178136 0.0204748 -0.0828726 0.178136 0.0207864, -0.0844389 0.178136 0.0204748 -0.0857668 0.178136 0.0195876, -0.086654 0.178136 0.0182597 -0.0869656 0.178136 0.0166934, -0.086654 0.178136 0.015127 -0.0857668 0.178136 0.0137992, -0.0844389 0.178136 0.0129119 -0.0828726 0.178136 0.0126003, -0.0813062 0.178136 0.0129119 -0.0813062 0.180074 0.0129119, -0.0828726 0.180074 0.0126003 -0.0844389 0.180074 0.0129119, -0.0857668 0.180074 0.0137992 -0.086654 0.180074 0.015127, -0.0869656 0.180074 0.0166934 -0.086654 0.180074 0.0182597, -0.0857668 0.180074 0.0195876 -0.0844389 0.180074 0.0204748, -0.0828726 0.180074 0.0207864 -0.0813062 0.180074 0.0204748, -0.0799784 0.180074 0.0195876 -0.0790911 0.180074 0.0182597, -0.0787795 0.180074 0.0166934 -0.0790911 0.180074 0.015127, -0.0799784 0.180074 0.0137992 -0.0783919 0.180074 0.0122126, -0.0770182 0.180074 0.0142684 -0.0765359 0.180074 0.0166934, -0.0770182 0.180074 0.0191183 -0.0783919 0.180074 0.0211741, -0.0804476 0.180074 0.0225477 -0.0828726 0.180074 0.0230301, -0.0852975 0.180074 0.0225477 -0.0873533 0.180074 0.0211741, -0.0887269 0.180074 0.0191183 -0.0892093 0.180074 0.0166934, -0.0887269 0.180074 0.0142684 -0.0873533 0.180074 0.0122126, -0.0852975 0.180074 0.010839 -0.0828726 0.180074 0.0103567, -0.0804476 0.180074 0.010839 -0.0804476 0.181528 0.010839, -0.0828726 0.181528 0.0103567 -0.0852975 0.181528 0.010839, -0.0873533 0.181528 0.0122126 -0.0887269 0.181528 0.0142684, -0.0892093 0.181528 0.0166934 -0.0887269 0.181528 0.0191183, -0.0873533 0.181528 0.0211741 -0.0852975 0.181528 0.0225477, -0.0828726 0.181528 0.0230301 -0.0804476 0.181528 0.0225477, -0.0783919 0.181528 0.0211741 -0.0770182 0.181528 0.0191183, -0.0765359 0.181528 0.0166934 -0.0770182 0.181528 0.0142684, -0.0783919 0.181528 0.0122126 0.0900723 0.165559 0.0125202, 0.0914596 0.165559 0.0145965 0.0919468 0.165559 0.0170457, 0.0914596 0.165559 0.0194949 0.0900723 0.165559 0.0215712, 0.087996 0.165559 0.0229585 0.0855468 0.165559 0.0234457, 0.0830976 0.165559 0.0229585 0.0810213 0.165559 0.0215712, 0.0796339 0.165559 0.0194949 0.0791468 0.165559 0.0170457, 0.0796339 0.165559 0.0145965 0.0810213 0.165559 0.0125202, 0.0830976 0.165559 0.0111329 0.0855468 0.165559 0.0106457, 0.087996 0.165559 0.0111329 0.087996 0.170194 0.0111329, 0.0855468 0.170194 0.0106457 0.0830976 0.170194 0.0111329, 0.0810213 0.170194 0.0125202 0.0796339 0.170194 0.0145965, 0.0791468 0.170194 0.0170457 0.0796339 0.170194 0.0194949, 0.0810213 0.170194 0.0215712 0.0830976 0.170194 0.0229585, 0.0855468 0.170194 0.0234457 0.087996 0.170194 0.0229585, 0.0900723 0.170194 0.0215712 0.0914596 0.170194 0.0194949, 0.0919468 0.170194 0.0170457 0.0914596 0.170194 0.0145965, 0.0900723 0.170194 0.0125202 0.0885245 0.170194 0.014068, 0.0894374 0.170194 0.0154342 0.0897579 0.170194 0.0170457, 0.0894374 0.170194 0.0186572 0.0885245 0.170194 0.0200234, 0.0871583 0.170194 0.0209363 0.0855468 0.170194 0.0212568, 0.0839353 0.170194 0.0209363 0.0825691 0.170194 0.0200234, 0.0816562 0.170194 0.0186572 0.0813356 0.170194 0.0170457, 0.0816562 0.170194 0.0154342 0.0825691 0.170194 0.014068, 0.0839353 0.170194 0.0131551 0.0855468 0.170194 0.0128346, 0.0871583 0.170194 0.0131551 0.0871583 0.172863 0.0131551, 0.0855468 0.172863 0.0128346 0.0839353 0.172863 0.0131551, 0.0825691 0.172863 0.014068 0.0816562 0.172863 0.0154342, 0.0813356 0.172863 0.0170457 0.0816562 0.172863 0.0186572, 0.0825691 0.172863 0.0200234 0.0839353 0.172863 0.0209363, 0.0855468 0.172863 0.0212568 0.0871583 0.172863 0.0209363, 0.0885245 0.172863 0.0200234 0.0894374 0.172863 0.0186572, 0.0897579 0.172863 0.0170457 0.0894374 0.172863 0.0154342, 0.0885245 0.172863 0.014068 0.0866216 0.172863 0.0144508, 0.0855468 0.172863 0.014237 0.0844719 0.172863 0.0144508, 0.0835607 0.172863 0.0150597 0.0829519 0.172863 0.0159709, 0.0827381 0.172863 0.0170457 0.0829519 0.172863 0.0181205, 0.0835607 0.172863 0.0190317 0.0844719 0.172863 0.0196406, 0.0855468 0.172863 0.0198544 0.0866216 0.172863 0.0196406, 0.0875328 0.172863 0.0190317 0.0881416 0.172863 0.0181205, 0.0883554 0.172863 0.0170457 0.0881416 0.172863 0.0159709, 0.0875328 0.172863 0.0150597 0.0875328 0.178136 0.0150597, 0.0881416 0.178136 0.0159709 0.0883554 0.178136 0.0170457, 0.0881416 0.178136 0.0181205 0.0875328 0.178136 0.0190317, 0.0866216 0.178136 0.0196406 0.0855468 0.178136 0.0198544, 0.0844719 0.178136 0.0196406 0.0835607 0.178136 0.0190317, 0.0829519 0.178136 0.0181205 0.0827381 0.178136 0.0170457, 0.0829519 0.178136 0.0159709 0.0835607 0.178136 0.0150597, 0.0844719 0.178136 0.0144508 0.0855468 0.178136 0.014237, 0.0866216 0.178136 0.0144508 0.088441 0.178136 0.0141515, 0.0893282 0.178136 0.0154794 0.0896398 0.178136 0.0170457, 0.0893282 0.178136 0.018612 0.088441 0.178136 0.0199399, 0.0871131 0.178136 0.0208272 0.0855468 0.178136 0.0211387, 0.0839804 0.178136 0.0208272 0.0826526 0.178136 0.0199399, 0.0817653 0.178136 0.018612 0.0814537 0.178136 0.0170457, 0.0817653 0.178136 0.0154794 0.0826526 0.178136 0.0141515, 0.0839804 0.178136 0.0132642 0.0855468 0.178136 0.0129527, 0.0871131 0.178136 0.0132642 0.0871131 0.180074 0.0132642, 0.0855468 0.180074 0.0129527 0.0839804 0.180074 0.0132642, 0.0826526 0.180074 0.0141515 0.0817653 0.180074 0.0154794, 0.0814537 0.180074 0.0170457 0.0817653 0.180074 0.018612, 0.0826526 0.180074 0.0199399 0.0839804 0.180074 0.0208272, 0.0855468 0.180074 0.0211387 0.0871131 0.180074 0.0208272, 0.088441 0.180074 0.0199399 0.0893282 0.180074 0.018612, 0.0896398 0.180074 0.0170457 0.0893282 0.180074 0.0154794, 0.088441 0.180074 0.0141515 0.0900275 0.180074 0.012565, 0.0914011 0.180074 0.0146208 0.0918835 0.180074 0.0170457, 0.0914011 0.180074 0.0194706 0.0900275 0.180074 0.0215264, 0.0879717 0.180074 0.0229 0.0855468 0.180074 0.0233824, 0.0831218 0.180074 0.0229001 0.0810661 0.180074 0.0215264, 0.0796924 0.180074 0.0194707 0.0792101 0.180074 0.0170457, 0.0796924 0.180074 0.0146207 0.0810661 0.180074 0.012565, 0.0831218 0.180074 0.0111914 0.0855468 0.180074 0.010709, 0.0879717 0.180074 0.0111914 0.0879717 0.181528 0.0111914, 0.0855468 0.181528 0.010709 0.0831218 0.181528 0.0111914, 0.0810661 0.181528 0.012565 0.0796924 0.181528 0.0146207, 0.0792101 0.181528 0.0170457 0.0796924 0.181528 0.0194707, 0.0810661 0.181528 0.0215264 0.0831218 0.181528 0.0229001, 0.0855468 0.181528 0.0233824 0.0879717 0.181528 0.0229, 0.0900275 0.181528 0.0215264 0.0914011 0.181528 0.0194706, 0.0918835 0.181528 0.0170457 0.0914011 0.181528 0.0146208, 0.0900275 0.181528 0.012565 - ] - } - coordIndex [ - 15 13 14 -1 15 12 13 -1 0 12 15 -1 0 11 12 -1 11 0 1 -1 1 10 11 -1 2 10 1 -1 2 9 10 -1 3 9 2 -1 4 9 3 -1 4 8 9 -1 5 8 4 -1 5 7 8 -1 6 7 5 -1 146 144 145 -1 147 144 146 -1 147 159 144 -1 148 159 147 -1 148 158 159 -1 149 158 148 -1 149 157 158 -1 150 157 149 -1 150 156 157 -1 150 155 156 -1 151 155 150 -1 151 154 155 -1 152 154 151 -1 152 153 154 -1 175 173 174 -1 175 172 173 -1 160 172 175 -1 160 171 172 -1 161 171 160 -1 161 170 171 -1 162 170 161 -1 162 169 170 -1 163 169 162 -1 164 169 163 -1 164 168 169 -1 165 168 164 -1 165 167 168 -1 166 167 165 -1 306 304 305 -1 307 304 306 -1 307 319 304 -1 308 319 307 -1 308 318 319 -1 309 318 308 -1 309 317 318 -1 310 317 309 -1 310 316 317 -1 310 315 316 -1 311 315 310 -1 311 314 315 -1 312 314 311 -1 312 313 314 -1 288 318 289 -1 288 319 318 -1 289 317 290 -1 289 318 317 -1 290 316 291 -1 290 317 316 -1 291 315 292 -1 291 316 315 -1 292 314 293 -1 292 315 314 -1 293 313 294 -1 293 314 313 -1 294 312 295 -1 294 313 312 -1 295 311 296 -1 295 312 311 -1 296 310 297 -1 296 311 310 -1 297 309 298 -1 297 310 309 -1 298 308 299 -1 298 309 308 -1 299 307 300 -1 299 308 307 -1 300 306 301 -1 300 307 306 -1 301 305 302 -1 301 306 305 -1 302 304 303 -1 302 305 304 -1 303 319 288 -1 303 304 319 -1 272 288 287 -1 272 303 288 -1 273 303 272 -1 273 302 303 -1 274 302 273 -1 274 301 302 -1 275 301 274 -1 275 300 301 -1 276 300 275 -1 276 299 300 -1 277 299 276 -1 277 298 299 -1 278 298 277 -1 278 297 298 -1 279 297 278 -1 279 296 297 -1 280 296 279 -1 280 295 296 -1 281 295 280 -1 281 294 295 -1 282 294 281 -1 282 293 294 -1 283 293 282 -1 283 292 293 -1 284 292 283 -1 284 291 292 -1 285 291 284 -1 285 290 291 -1 286 290 285 -1 286 289 290 -1 287 289 286 -1 287 288 289 -1 256 286 257 -1 256 287 286 -1 257 285 258 -1 257 286 285 -1 258 284 259 -1 258 285 284 -1 259 283 260 -1 259 284 283 -1 260 282 261 -1 260 283 282 -1 261 281 262 -1 261 282 281 -1 262 280 263 -1 262 281 280 -1 263 279 264 -1 263 280 279 -1 264 278 265 -1 264 279 278 -1 265 277 266 -1 265 278 277 -1 266 276 267 -1 266 277 276 -1 267 275 268 -1 267 276 275 -1 268 274 269 -1 268 275 274 -1 269 273 270 -1 269 274 273 -1 270 272 271 -1 270 273 272 -1 271 287 256 -1 271 272 287 -1 255 256 240 -1 255 271 256 -1 254 271 255 -1 254 270 271 -1 253 270 254 -1 253 269 270 -1 252 269 253 -1 252 268 269 -1 251 267 252 -1 252 267 268 -1 250 267 251 -1 250 266 267 -1 249 266 250 -1 249 265 266 -1 248 265 249 -1 248 264 265 -1 247 264 248 -1 247 263 264 -1 246 263 247 -1 246 262 263 -1 245 262 246 -1 245 261 262 -1 244 261 245 -1 244 260 261 -1 243 259 244 -1 244 259 260 -1 242 259 243 -1 242 258 259 -1 241 258 242 -1 241 257 258 -1 240 257 241 -1 240 256 257 -1 224 240 239 -1 224 255 240 -1 225 255 224 -1 225 254 255 -1 226 254 225 -1 226 253 254 -1 227 253 226 -1 227 252 253 -1 228 252 227 -1 228 251 252 -1 229 251 228 -1 229 250 251 -1 230 250 229 -1 230 249 250 -1 231 249 230 -1 231 248 249 -1 232 248 231 -1 232 247 248 -1 233 247 232 -1 233 246 247 -1 234 246 233 -1 234 245 246 -1 235 245 234 -1 235 244 245 -1 236 244 235 -1 236 243 244 -1 237 243 236 -1 237 242 243 -1 238 242 237 -1 238 241 242 -1 239 241 238 -1 239 240 241 -1 223 239 222 -1 222 239 238 -1 222 237 221 -1 222 238 237 -1 221 236 220 -1 221 237 236 -1 220 235 219 -1 220 236 235 -1 219 234 218 -1 219 235 234 -1 218 233 217 -1 218 234 233 -1 217 232 216 -1 217 233 232 -1 216 232 215 -1 215 232 231 -1 215 230 214 -1 215 231 230 -1 214 229 213 -1 214 230 229 -1 213 228 212 -1 213 229 228 -1 212 227 211 -1 212 228 227 -1 211 226 210 -1 211 227 226 -1 210 225 209 -1 210 226 225 -1 209 224 208 -1 209 225 224 -1 208 239 223 -1 208 224 239 -1 192 222 193 -1 192 223 222 -1 193 221 194 -1 193 222 221 -1 194 220 195 -1 194 221 220 -1 195 219 196 -1 195 220 219 -1 196 218 197 -1 196 219 218 -1 197 217 198 -1 197 218 217 -1 198 216 199 -1 198 217 216 -1 199 215 200 -1 199 216 215 -1 200 214 201 -1 200 215 214 -1 201 213 202 -1 201 214 213 -1 202 212 203 -1 202 213 212 -1 203 211 204 -1 203 212 211 -1 204 210 205 -1 204 211 210 -1 205 209 206 -1 205 210 209 -1 206 208 207 -1 206 209 208 -1 207 223 192 -1 207 208 223 -1 176 192 191 -1 176 207 192 -1 177 207 176 -1 177 206 207 -1 178 206 177 -1 178 205 206 -1 179 205 178 -1 179 204 205 -1 180 204 179 -1 180 203 204 -1 181 203 180 -1 181 202 203 -1 182 202 181 -1 182 201 202 -1 183 201 182 -1 183 200 201 -1 184 200 183 -1 184 199 200 -1 185 199 184 -1 185 198 199 -1 186 198 185 -1 186 197 198 -1 187 197 186 -1 187 196 197 -1 188 196 187 -1 188 195 196 -1 189 195 188 -1 189 194 195 -1 190 194 189 -1 190 193 194 -1 191 193 190 -1 191 192 193 -1 160 190 161 -1 160 191 190 -1 161 189 162 -1 161 190 189 -1 162 188 163 -1 162 189 188 -1 163 187 164 -1 163 188 187 -1 164 186 165 -1 164 187 186 -1 165 185 166 -1 165 186 185 -1 166 184 167 -1 166 185 184 -1 167 183 168 -1 167 184 183 -1 168 182 169 -1 168 183 182 -1 169 181 170 -1 169 182 181 -1 170 180 171 -1 170 181 180 -1 171 179 172 -1 171 180 179 -1 172 178 173 -1 172 179 178 -1 173 177 174 -1 173 178 177 -1 174 176 175 -1 174 177 176 -1 175 191 160 -1 175 176 191 -1 31 0 15 -1 15 16 31 -1 14 16 15 -1 14 17 16 -1 13 17 14 -1 13 18 17 -1 12 18 13 -1 12 19 18 -1 11 19 12 -1 11 20 19 -1 10 20 11 -1 10 21 20 -1 9 21 10 -1 9 22 21 -1 8 22 9 -1 8 23 22 -1 7 23 8 -1 7 24 23 -1 6 24 7 -1 6 25 24 -1 5 25 6 -1 5 26 25 -1 4 26 5 -1 4 27 26 -1 3 27 4 -1 3 28 27 -1 2 28 3 -1 2 29 28 -1 1 29 2 -1 1 30 29 -1 0 30 1 -1 0 31 30 -1 31 33 30 -1 31 32 33 -1 30 34 29 -1 30 33 34 -1 29 35 28 -1 29 34 35 -1 28 36 27 -1 28 35 36 -1 27 37 26 -1 27 36 37 -1 26 38 25 -1 26 37 38 -1 25 39 24 -1 25 38 39 -1 24 40 23 -1 24 39 40 -1 23 41 22 -1 23 40 41 -1 22 42 21 -1 22 41 42 -1 21 43 20 -1 21 42 43 -1 20 44 19 -1 20 43 44 -1 19 45 18 -1 19 44 45 -1 18 46 17 -1 18 45 46 -1 17 47 16 -1 17 46 47 -1 16 32 31 -1 16 47 32 -1 47 63 32 -1 47 48 63 -1 46 48 47 -1 46 49 48 -1 45 49 46 -1 45 50 49 -1 44 50 45 -1 44 51 50 -1 43 51 44 -1 43 52 51 -1 42 52 43 -1 42 53 52 -1 41 53 42 -1 41 54 53 -1 40 54 41 -1 40 55 54 -1 39 55 40 -1 39 56 55 -1 38 56 39 -1 38 57 56 -1 37 57 38 -1 37 58 57 -1 36 58 37 -1 36 59 58 -1 35 59 36 -1 35 60 59 -1 34 60 35 -1 34 61 60 -1 33 61 34 -1 33 62 61 -1 32 62 33 -1 32 63 62 -1 48 79 63 -1 48 64 79 -1 49 64 48 -1 49 65 64 -1 50 65 49 -1 50 66 65 -1 51 66 50 -1 51 67 66 -1 52 67 51 -1 52 68 67 -1 53 68 52 -1 53 69 68 -1 54 69 53 -1 54 70 69 -1 55 70 54 -1 55 71 70 -1 56 71 55 -1 56 72 71 -1 57 72 56 -1 57 73 72 -1 58 73 57 -1 58 74 73 -1 59 74 58 -1 59 75 74 -1 60 75 59 -1 60 76 75 -1 61 76 60 -1 61 77 76 -1 62 77 61 -1 62 78 77 -1 63 78 62 -1 63 79 78 -1 79 81 78 -1 79 80 81 -1 78 82 77 -1 78 81 82 -1 77 83 76 -1 77 82 83 -1 76 84 75 -1 76 83 84 -1 75 85 74 -1 75 84 85 -1 74 86 73 -1 74 85 86 -1 73 87 72 -1 73 86 87 -1 72 88 71 -1 72 87 88 -1 71 89 70 -1 71 88 89 -1 70 90 69 -1 70 89 90 -1 69 91 68 -1 69 90 91 -1 68 92 67 -1 68 91 92 -1 67 93 66 -1 67 92 93 -1 66 94 65 -1 66 93 94 -1 65 95 64 -1 65 94 95 -1 64 80 79 -1 64 95 80 -1 80 97 81 -1 80 96 97 -1 81 98 82 -1 81 97 98 -1 82 99 83 -1 82 98 99 -1 83 99 84 -1 84 99 100 -1 84 101 85 -1 84 100 101 -1 85 102 86 -1 85 101 102 -1 86 103 87 -1 86 102 103 -1 87 104 88 -1 87 103 104 -1 88 105 89 -1 88 104 105 -1 89 106 90 -1 89 105 106 -1 90 107 91 -1 90 106 107 -1 91 107 92 -1 92 107 108 -1 92 109 93 -1 92 108 109 -1 93 110 94 -1 93 109 110 -1 94 111 95 -1 94 110 111 -1 95 96 80 -1 95 111 96 -1 111 127 96 -1 111 112 127 -1 110 112 111 -1 110 113 112 -1 109 113 110 -1 109 114 113 -1 108 114 109 -1 108 115 114 -1 107 115 108 -1 107 116 115 -1 106 116 107 -1 106 117 116 -1 105 117 106 -1 105 118 117 -1 104 118 105 -1 104 119 118 -1 103 119 104 -1 103 120 119 -1 102 120 103 -1 102 121 120 -1 101 121 102 -1 101 122 121 -1 100 122 101 -1 100 123 122 -1 99 123 100 -1 99 124 123 -1 98 124 99 -1 98 125 124 -1 97 125 98 -1 97 126 125 -1 96 126 97 -1 96 127 126 -1 127 129 126 -1 127 128 129 -1 126 130 125 -1 126 129 130 -1 125 131 124 -1 125 130 131 -1 124 132 123 -1 124 131 132 -1 123 133 122 -1 123 132 133 -1 122 134 121 -1 122 133 134 -1 121 135 120 -1 121 134 135 -1 120 136 119 -1 120 135 136 -1 119 137 118 -1 119 136 137 -1 118 138 117 -1 118 137 138 -1 117 139 116 -1 117 138 139 -1 116 140 115 -1 116 139 140 -1 115 141 114 -1 115 140 141 -1 114 142 113 -1 114 141 142 -1 113 143 112 -1 113 142 143 -1 112 128 127 -1 112 143 128 -1 143 159 128 -1 143 144 159 -1 142 144 143 -1 142 145 144 -1 141 145 142 -1 141 146 145 -1 140 146 141 -1 140 147 146 -1 139 147 140 -1 139 148 147 -1 138 148 139 -1 138 149 148 -1 137 149 138 -1 137 150 149 -1 136 150 137 -1 136 151 150 -1 135 151 136 -1 135 152 151 -1 134 152 135 -1 134 153 152 -1 133 153 134 -1 133 154 153 -1 132 154 133 -1 132 155 154 -1 131 155 132 -1 131 156 155 -1 130 156 131 -1 130 157 156 -1 129 157 130 -1 129 158 157 -1 128 158 129 -1 128 159 158 -1 - ] - creaseAngle 1 - } - } - DEF UPPER_BLACK_PANEL_SHAPE Shape { - appearance PBRAppearance { - baseColor 0.2 0.2 0.2 - roughness 0.3 - metalness 0 - } - geometry IndexedFaceSet { - coord Coordinate { - point [ - -0.0294513 0.168028 -0.0961899, 0.0338456 0.168028 -0.0961899 0.0338456 0.168028 -0.0645414, -0.0294513 0.168028 -0.0645414 -0.0294513 0.165223 -0.0645414, 0.0338456 0.165223 -0.0645414 0.0338456 0.165223 -0.0961899, -0.0294513 0.165223 -0.0961899 - ] - } - coordIndex [ - 5 4 6 -1 4 7 6 -1 7 0 1 -1 7 1 6 -1 2 5 6 -1 2 6 1 -1 3 4 5 -1 3 5 2 -1 0 7 4 -1 0 4 3 -1 0 3 1 -1 3 2 1 -1 - ] - } - } - ] - } - ] - name IS name - model "Adept Pioneer 3-AT" - boundingObject Group { - children [ - DEF MAIN_BODY_BOX Transform { - rotation -0.5774 0.5773 0.5774 -2.094 - translation 0.009 -0.0 0.063 - children [ - Box { - size 0.197 0.2 0.402 - } - ] - } - DEF BODY_BOX Transform { - rotation -0.5774 0.5773 0.5774 -2.094 - translation 0.01 -0.0 0.063 - children [ - Box { - size 0.264 0.2 0.332 - } - ] - } - DEF FRONT_TOP_CYLINDER Transform { - rotation -0.5774 0.5773 0.5774 -2.094 - translation 0.064 -0.0 0.163 - children [ - Cylinder { - height 0.004 - radius 0.19 - subdivision 24 - } - ] - } - DEF BACK_TOP_CYLINDER Transform { - rotation -0.5774 0.5773 0.5774 -2.094 - translation -0.084 -0.0 0.163 - children [ - Cylinder { - height 0.004 - radius 0.17 - subdivision 24 - } - ] - } - DEF FRONT_RING_CYLINDER Transform { - rotation -0.5774 0.5773 0.5774 -2.094 - translation 0.106 -0.0 0.137 - children [ - Cylinder { - height 0.05 - radius 0.145 - subdivision 24 - } - ] - } - DEF BACK_RING_CYLINDER Transform { - rotation -0.5774 0.5773 0.5774 -2.094 - translation -0.104 -0.0 0.137 - children [ - Cylinder { - height 0.05 - radius 0.145 - subdivision 24 - } - ] - } - DEF LEFT_ANTENNA_CYLINDER Transform { - rotation -0.5774 0.5773 0.5774 -2.094 - translation -0.017 0.082 0.174 - children [ - Cylinder { - height 0.016 - radius 0.0058 - subdivision 24 - } - ] - } - DEF RIGHT_ANTENNA_CYLINDER Transform { - rotation -0.5774 0.5773 0.5774 -2.094 - translation -0.017 -0.087 0.174 - children [ - Cylinder { - height 0.016 - radius 0.0058 - subdivision 24 - } - ] - } - DEF FRONT_LEFT_CORNER_BOX Transform { - translation 0.176 0.097 0.038 - rotation 0.8735 -0.3442 -0.3442 1.706 - children [ - Box { - size 0.049 0.15 0.05 - } - ] - } - DEF FRONT_RIGHT_CORNER_BOX Transform { - translation 0.176 -0.097 0.038 - rotation 0.2684 -0.6812 -0.6812 2.617 - children [ - Box { - size 0.049 0.15 0.05 - } - ] - } - DEF BACK_RIGHT_CORNER_BOX Transform { - translation -0.156 -0.097 0.038 - rotation 0.8735 -0.3442 -0.3442 1.706 - children [ - Box { - size 0.049 0.15 0.05 - } - ] - } - DEF BACK_LEFT_CORNER_BOX Transform { - translation -0.156 0.098 0.038 - rotation 0.2684 -0.6812 -0.6812 2.617 - children [ - Box { - size 0.049 0.15 0.05 - } - ] - } - DEF FRONT_AXLE Transform { - translation 0.134 -0.0 0.001 - rotation -0.0 1.0 0.0 -1.571 - children [ - DEF AXLE_CYLINDER Cylinder { - height 0.4 - radius 0.025 - subdivision 24 - } - ] - } - DEF BACK_AXLE Transform { - translation -0.132 0.0 0.001 - rotation -0.0 1.0 0.0 -1.571 - children [ - USE AXLE_CYLINDER - ] - } - ] - } - physics Physics { - density -1 - mass 8.8 - } - controller IS controller - controllerArgs IS controllerArgs - customData IS customData - supervisor IS supervisor - synchronization IS synchronization - } -} diff --git a/worlds/pioneer3at.wbt b/worlds/pioneer3at.wbt index c91f423..f14eab6 100644 --- a/worlds/pioneer3at.wbt +++ b/worlds/pioneer3at.wbt @@ -711,8 +711,8 @@ DEF KITCHEN Transform { } ] } -DEF PIONEER_3AT Pioneer3at_ENU { - translation 1.69482 -4.10279 0.11 +DEF PIONEER_3AT Pioneer3at { + translation 1.69482 -4.10279 0 rotation 1 0 0 4.692820414042842e-06 controller "ros" controllerArgs [ @@ -722,13 +722,6 @@ DEF PIONEER_3AT Pioneer3at_ENU { "--robot-description" ] extensionSlot [ - Camera { - translation 0 0.17 -0.22 - width 256 - height 128 - motionBlur 500 - noise 0.02 - } Accelerometer { lookupTable [ -39.24 -39.24 0.005 @@ -742,13 +735,13 @@ DEF PIONEER_3AT Pioneer3at_ENU { ] } SickLms291 { - translation 0 0.23 -0.136 + translation 0.136 0 0.35 + rotation -0.5773509358554485 0.5773489358556708 0.5773509358554485 -2.094395307179586 noise 0.1 } GPS { } InertialUnit { - rotation -0.577349935856137 0.577349935856137 0.5773509358560258 2.09439 } ] } From 20db75fe547b4b67ab3bc8051eb187a0d8b82b27 Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Tue, 16 Mar 2021 19:54:53 +0100 Subject: [PATCH 28/72] Integrate MoveIt config --- CMakeLists.txt | 16 +- config/tiago_steel.srdf | 326 ++++++++++++++++++++++++++++++++++++++++ launch/tiago.launch | 45 +++++- src/tiago.cpp | 69 --------- 4 files changed, 370 insertions(+), 86 deletions(-) create mode 100644 config/tiago_steel.srdf delete mode 100644 src/tiago.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index ebc29ef..4db48b8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -224,19 +224,6 @@ target_link_libraries(catch_the_bird ${catkin_LIBRARIES} ) -add_executable(tiago src/tiago.cpp) - -include_directories( - tiago - ${catkin_INCLUDE_DIRS} -) - -add_dependencies(tiago ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -target_link_libraries(tiago - ${catkin_LIBRARIES} -) - #instructions for panoramic_view_recorder node add_executable(panoramic_view_recorder src/panoramic_view_recorder.cpp) @@ -287,3 +274,6 @@ install(DIRECTORY plugins install(DIRECTORY worlds DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +install(DIRECTORY config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/config/tiago_steel.srdf b/config/tiago_steel.srdf new file mode 100644 index 0000000..121fc80 --- /dev/null +++ b/config/tiago_steel.srdf @@ -0,0 +1,326 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tiago.launch b/launch/tiago.launch index e7d98bd..7220deb 100644 --- a/launch/tiago.launch +++ b/launch/tiago.launch @@ -35,9 +35,46 @@ - + - + + + + robot_description_kinematics: + arm: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + move_group: + planning_plugin: ompl_interface/OMPLPlanner + request_adapters: + default_planner_request_adapters/AddTimeParameterization + default_planner_request_adapters/ResolveConstraintFrames + default_planner_request_adapters/FixWorkspaceBounds + default_planner_request_adapters/FixStartStateBounds + default_planner_request_adapters/FixStartStateCollision + default_planner_request_adapters/FixStartStatePathConstraints + start_state_max_bounds_error: 0.1 + moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager + moveit_manage_controllers: true + controller_list: + - name: arm_controller + action_ns: follow_joint_trajectory + default: True + type: FollowJointTrajectory + joints: + - arm_1_joint + - arm_2_joint + - arm_3_joint + - arm_4_joint + - arm_5_joint + - arm_6_joint + - arm_7_joint + planning_scene_monitor: + publish_planning_scene: true + publish_geometry_updates: true + publish_state_updates: true + publish_transforms_updates: true + + diff --git a/src/tiago.cpp b/src/tiago.cpp deleted file mode 100644 index f55d41f..0000000 --- a/src/tiago.cpp +++ /dev/null @@ -1,69 +0,0 @@ -#include -#include -#include -#include -#include -#include - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "plan_arm_torso_ik"); - - if (argc < 7) - { - ROS_INFO(" "); - ROS_INFO("\tUsage:"); - ROS_INFO(" "); - ROS_INFO("\trosrun tiago_moveit_tutorial plan_arm_torso_ik x y z r p y"); - ROS_INFO(" "); - ROS_INFO("\twhere the list of arguments specify the target pose of /arm_tool_link expressed in /base_footprint"); - ROS_INFO(" "); - return EXIT_FAILURE; - } - - geometry_msgs::PoseStamped goal_pose; - goal_pose.header.frame_id = "base_link"; - goal_pose.pose.position.x = atof(argv[1]); - goal_pose.pose.position.y = atof(argv[2]); - goal_pose.pose.position.z = atof(argv[3]); - goal_pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(atof(argv[4]), atof(argv[5]), atof(argv[6])); - - ros::NodeHandle nh; - ros::AsyncSpinner spinner(1); - spinner.start(); - - std::vector torso_arm_joint_names; - //select group of joints - moveit::planning_interface::MoveGroupInterface group_arm_torso("arm"); - //choose your preferred planner - group_arm_torso.setPlannerId("SBLkConfigDefault"); - group_arm_torso.setPoseReferenceFrame("base_link"); - group_arm_torso.setPoseTarget(goal_pose); - - ROS_INFO_STREAM("Planning to move " << group_arm_torso.getEndEffectorLink() << " to a target pose expressed in " << group_arm_torso.getPlanningFrame()); - - group_arm_torso.setStartStateToCurrentState(); - group_arm_torso.setMaxVelocityScalingFactor(1.0); - - moveit::planning_interface::MoveGroupInterface::Plan my_plan; - //set maximum time to find a plan - group_arm_torso.setPlanningTime(5.0); - - moveit::planning_interface::MoveItErrorCode success = group_arm_torso.plan(my_plan); - - if (!success) - throw std::runtime_error("No plan found"); - - ROS_INFO_STREAM("Plan found in " << my_plan.planning_time_ << " seconds"); - - // Execute the plan - ros::Time start = ros::Time::now(); - - group_arm_torso.move(); - - ROS_INFO_STREAM("Motion duration: " << (ros::Time::now() - start).toSec()); - - spinner.stop(); - - return EXIT_SUCCESS; -} From 7f16915d523a4cd0d10dc4b8a463d19af66c4a64 Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Tue, 16 Mar 2021 22:09:14 +0100 Subject: [PATCH 29/72] Relax constraints --- launch/tiago.launch | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/launch/tiago.launch b/launch/tiago.launch index 7220deb..f460fe7 100644 --- a/launch/tiago.launch +++ b/launch/tiago.launch @@ -4,7 +4,7 @@ arm_controller: - type: "position_controllers/JointTrajectoryController" + type: position_controllers/JointTrajectoryController joints: - arm_1_joint - arm_2_joint @@ -13,6 +13,8 @@ - arm_5_joint - arm_6_joint - arm_7_joint + constraints: + stopped_velocity_tolerance: 1 joint_controller: type: joint_state_controller/JointStateController publish_rate: 50 @@ -35,7 +37,7 @@ - + From d85c727183e523e5c0b812600dc705ca46498839 Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Tue, 16 Mar 2021 22:12:11 +0100 Subject: [PATCH 30/72] Update --- launch/tiago.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/tiago.launch b/launch/tiago.launch index f460fe7..9400586 100644 --- a/launch/tiago.launch +++ b/launch/tiago.launch @@ -37,7 +37,7 @@ - + From 574bdbe8a015ab47e3e88a050dfb28e30d652812 Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Wed, 17 Mar 2021 09:00:01 +0100 Subject: [PATCH 31/72] Add passive joints --- config/tiago_steel.srdf | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/config/tiago_steel.srdf b/config/tiago_steel.srdf index 121fc80..abfffb8 100644 --- a/config/tiago_steel.srdf +++ b/config/tiago_steel.srdf @@ -21,6 +21,14 @@ + + + + + + + + From 33375836572cb16af7e767de60878ecabb2ef643 Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Wed, 17 Mar 2021 09:09:58 +0100 Subject: [PATCH 32/72] Better world --- worlds/.tiago.wbproj | 11 + worlds/tiago.wbt | 928 +++++++++++++++++++++++-------------------- 2 files changed, 518 insertions(+), 421 deletions(-) create mode 100644 worlds/.tiago.wbproj diff --git a/worlds/.tiago.wbproj b/worlds/.tiago.wbproj new file mode 100644 index 0000000..b876f99 --- /dev/null +++ b/worlds/.tiago.wbproj @@ -0,0 +1,11 @@ +Webots Project File version R2021b +perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff000000010000011c0000038ffc0200000001fb0000001400540065007800740045006400690074006f007200000000170000038f0000003f00ffffff000000030000078000000087fc0100000001fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000007800000034100000001000000020000000100000008fc00000000 +simulationViewPerspectives: 000000ff000000010000000200000100000003820100000002010000000100 +sceneTreePerspectives: 000000ff0000000100000002000000e0000000fa0100000002010000000200 +maximizedDockId: -1 +centralWidgetVisible: 1 +orthographicViewHeight: 1 +textFiles: -1 +consoles: Console:All:All +renderingDevicePerspectives: e-puck:camera;0;1;0;0 +renderingDevicePerspectives: television:display;1;1;0;0 diff --git a/worlds/tiago.wbt b/worlds/tiago.wbt index f2785e9..3124fa7 100644 --- a/worlds/tiago.wbt +++ b/worlds/tiago.wbt @@ -1,455 +1,541 @@ #VRML_SIM R2021b utf8 WorldInfo { info [ - "This simulation shows a Tiago Steel making hello in an industrial environment. It can also been controlled using the keyboard arrows." + "TIAGo Steel simulation with MoveIt and differential drive integration (ROS)." ] - title "Simulation of PAL Robotics' TIAGo Steel robot" - coordinateSystem "NUE" + title "TIAGo Steel" + basicTimeStep 16 } Viewpoint { - orientation -0.13870670326979867 -0.9256806146864625 -0.3519600120208505 2.431279426949263 - position -9.395105901154711 9.204146785481651 -9.007040615249405 - follow "TIAGo Steel" + orientation 0.4925721069109625 0.36674151919891895 0.7892232748652084 1.5125517327991547 + position 15.600358335111576 -8.652535001134972 11.754842948879098 + exposure 0.5 } TexturedBackground { - texture "factory" + texture "empty_office" skybox FALSE + skyColor [ + 0.2 0.2 0.2 + ] } -TexturedBackgroundLight { - texture "factory" +DEF FLOOR Solid { + translation 3.3 -4.95 0 + rotation 1 0 0 1.5707963267948966 + children [ + Shape { + appearance Parquetry { + textureTransform TextureTransform { + scale 6 9 + } + } + geometry DEF FLOOR_PLANE Plane { + size 6.6 9.9 + } + } + ] + name "floor" + boundingObject USE FLOOR_PLANE + locked TRUE +} +DEF ROOF Solid { + translation 3.3 -4.95 2.4 + rotation 1 0 0 4.712386326794896 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.8 0.8 0.8 + baseColorMap ImageTexture { + url [ + "textures/roughcast.jpg" + ] + } + roughness 0.5 + metalness 0 + textureTransform TextureTransform { + scale 8 8 + } + } + geometry USE FLOOR_PLANE + } + ] + name "roof" + boundingObject USE FLOOR_PLANE + locked TRUE +} +Wall { + translation 0.65 0 0 + rotation 1 0 0 1.5707963267948966 + name "wall 1" + size 1 2.4 0.3 +} +Window { + translation 1.65 0 0 + rotation 1 0 0 1.5707963267948966 + name "window 1" + size 1 2.4 0.3 + frameAppearance PaintedWood { + colorOverride 0.13333333333333333 0.13333333333333333 0.13333333333333333 + } } -RectangleArena { - floorSize 15 15 - floorTileSize 1 1 - floorAppearance ThreadMetalPlate { +Wall { + translation 4.3 0 0 + rotation 1 0 0 1.5707963267948966 + name "wall 2" + size 4.3 2.4 0.3 +} +Wall { + translation 2.3 -9.9 0 + rotation 1 0 0 1.5707963267948966 + name "wall 3" + size 4.3 2.4 0.3 +} +Wall { + translation 5.95 -9.9 0 + rotation 1 0 0 1.5707963267948966 + name "wall 4" + size 1 2.4 0.3 +} +Door { + translation 4.95 -9.9 0 + rotation 9.38185669e-07 0.707106781186236 0.707106781186236 3.141590777218456 + name "door 1" + size 1 2.4 0.3 + canBeOpen FALSE + frameAppearance PaintedWood { + colorOverride 0.13333333333333333 0.13333333333333333 0.13333333333333333 } - wallHeight 2.5 - wallAppearance Roughcast { - colorOverride 0.533333 0.541176 0.521569 - textureTransform TextureTransform { - scale 5 1.75 - } + doorHandle DoorLever { + hasStaticParent TRUE } } -DEF ROOF RectangleArena { - translation 0 2.5 0 - rotation 1 0 0 3.141592653589793 - name "rectangle arena(1)" - floorSize 15 15 - floorTileSize 1 1 - floorAppearance Roughcast { - colorOverride 0.533333 0.541176 0.521569 - textureTransform TextureTransform { - scale 5 1.75 - } +Wall { + translation 0 -3.8 0 + rotation 0.577348855372322 0.577350976096979 0.577350976096979 2.094397223120449 + name "wall 5" + size 7.9 2.4 0.3 +} +Wall { + translation 0 -9.4 0 + rotation 0.577348855372322 0.577350976096979 0.577350976096979 2.094397223120449 + name "wall 6" + size 1.3 2.4 0.3 +} +Window { + translation 0 -8.25 0 + rotation 0.577348855372322 0.577350976096979 0.577350976096979 2.094397223120449 + name "window 2" + size 1 2.4 0.3 + frameAppearance PaintedWood { + colorOverride 0.13333333333333333 0.13333333333333333 0.13333333333333333 } - wallHeight 2.5 - wallAppearance Roughcast { - colorOverride 0.533333 0.541176 0.521569 - textureTransform TextureTransform { - scale 5 1.75 - } +} +Wall { + translation 6.6 -9.4 0 + rotation 0.577348855372322 0.577350976096979 0.577350976096979 2.094397223120449 + name "wall 7" + size 1.3 2.4 0.3 +} +Door { + translation 6.6 -8.25 0 + rotation 0.577348855372322 -0.577350976096979 -0.577350976096979 2.094397223120449 + name "door 2" + size 1 2.4 0.3 + canBeOpen FALSE + frameAppearance PaintedWood { + colorOverride 0.13333333333333333 0.13333333333333333 0.13333333333333333 + } + doorHandle DoorLever { + rotation -0.0012868889344011497 0.9999991719580925 0 0 + hasStaticParent TRUE } } -TiagoSteel { - translation -4.416 0.095 -1.063 - controller "ros" - controllerArgs [ - "--name=tiago" - "--use-ros-control" - "--auto-publish" - "--robot-description" +Wall { + translation 6.6 -3.8 0 + rotation 0.577348855372322 0.577350976096979 0.577350976096979 2.094397223120449 + name "wall 8" + size 7.9 2.4 0.3 +} +Wall { + translation 1.8 -3.3 0 + rotation 1 0 0 1.5707963267948966 + name "wall 9" + size 3.3 2.4 0.3 +} +CeilingLight { + translation 2.47061 -1.3341 2.4 + rotation 1 0 0 1.5707963267948966 + name "ceiling light 1" + pointLightIntensity 5 +} +CeilingLight { + translation 2.44317 -7.10107 2.4 + rotation 1 0 0 1.5707963267948966 + name "ceiling light 2" + pointLightIntensity 8 +} +FloorLight { + translation 0.745582 -4.00427 0 + rotation 1 0 0 1.5707963267948966 + pointLightIntensity 2 +} +Fridge { + translation 0.5 -0.52 0 + rotation 1 0 0 1.5707963267948966 + mainColor 0.6666666666666666 0 0 +} +Cabinet { + translation 0.15 -1.31 0 + rotation 0.577348855372322 0.577350976096979 0.577350976096979 2.094397223120449 + name "cabinet 1" + depth 0.68 + outerThickness 0.02 + rowsHeights [ + 0.22, 0.21, 0.21 + ] + columnsWidths [ + 0.42, 0.42 ] + layout [ + "Drawer (1, 1, 1, 1, 1.5)" + "Drawer (1, 2, 1, 1, 1.5)" + "Drawer (1, 3, 1, 1, 1.5)" + "Shelf (1, 2, 2, 0)" + "Shelf (1, 3, 2, 0)" + "Shelf (1, 1, 0, 3)" + ] + handle CabinetHandle { + handleLength 0.09 + handleRadius 0.008 + } + primaryAppearance PaintedWood { + colorOverride 0.13333333333333333 0.13333333333333333 0.13333333333333333 + } + secondaryAppearance PaintedWood { + colorOverride 0.13333333333333333 0.13333333333333333 0.13333333333333333 + } } -WoodenBox { - translation -6.2 0.405 -7 -} -WoodenBox { - translation -4.6 0.405 -7 - name "wooden box(1)" -} -WoodenBox { - translation -0.189 0.3 -0.174 - name "wooden box(2)" -} -WoodenBox { - translation -0.098 0.3 -0.994 - rotation 0 1 0 -2.618 - name "wooden box(3)" -} -WoodenBox { - translation 0.248 0.901 -0.48 - rotation 0 1 0 -0.524 - name "wooden box(4)" -} -WoodenBox { - translation 0.631 0.3 -0.397 - rotation 0 1 0 0.262 - name "wooden box(5)" -} -WoodenBox { - translation -2.99 0.44 4 - name "wooden box(6)" -} -WoodenBox { - translation 0 1.1 4 - name "wooden box(7)" -} -WoodenBox { - translation 3.6 1.1 4 - name "wooden box(8)" -} -WoodenBox { - translation 7.18 1.1 4 - name "wooden box(9)" -} -WoodenBox { - translation 0 1.1 6 - name "wooden box(10)" -} -WoodenBox { - translation 3.6 1.1 6 - name "wooden box(11)" -} -WoodenBox { - translation 7.18 1.1 6 - name "wooden box(12)" -} -SolidBox { - translation 7.49 0.85 4 - size 0.03 1.7 1 - appearance PBRAppearance { - baseColor 0 0 0 - baseColorMap ImageTexture { - url [ - "textures/tagged_wall.jpg" - ] - } - roughness 0.5 - metalness 0 +Cabinet { + translation 0.150001 -1.31 1.12 + rotation -0.577350661639742 -0.577350072964468 -0.577350072964468 4.188790793461465 + name "cabinet 2" + outerThickness 0.02 + rowsHeights [ + 0.22, 0.21, 0.21 + ] + columnsWidths [ + 0.42, 0.42 + ] + layout [ + "RightSidedDoor (1, 1, 1, 3, 1.5)" + "LeftSidedDoor (2, 1, 1, 3, 1.5)" + "Shelf (1, 2, 2, 0)" + "Shelf (1, 3, 2, 0)" + "Shelf (1, 1, 0, 3)" + ] + handle CabinetHandle { + translation -0.2 0 0 + handleLength 0.09 + handleRadius 0.008 + } + primaryAppearance PaintedWood { + colorOverride 0.13333333333333333 0.13333333333333333 0.13333333333333333 + } + secondaryAppearance PaintedWood { + colorOverride 0.13333333333333333 0.13333333333333333 0.13333333333333333 } } -SolidBox { - translation 7.49 0.85 6 - name "box(1)" - size 0.03 1.7 1 - appearance PBRAppearance { - baseColor 0 0 0 - baseColorMap ImageTexture { - url [ - "textures/tagged_wall.jpg" - ] - } - roughness 0.5 - metalness 0 +Cabinet { + translation 0.15 -2.19 0 + rotation -0.577350661639742 -0.577350072964468 -0.577350072964468 4.188790793461465 + name "cabinet 3" + depth 0.68 + outerThickness 0.02 + rowsHeights [ + 0.22, 0.21, 0.21 + ] + columnsWidths [ + 0.42, 0.42 + ] + layout [ + "LeftSidedDoor (2, 1, 1, 3, 1.5)" + "Shelf (1, 2, 2, 0)" + "Shelf (1, 3, 2, 0)" + "Shelf (1, 1, 0, 3)" + ] + handle CabinetHandle { + translation 0.2 0 0 + handleLength 0.09 + handleRadius 0.008 + } + primaryAppearance PaintedWood { + colorOverride 0.13333333333333333 0.13333333333333333 0.13333333333333333 + } + secondaryAppearance PaintedWood { + colorOverride 0.13333333333333333 0.13333333333333333 0.13333333333333333 } } -ConveyorBelt { - translation 2.47 0 4 - rotation 1 0 0 0 - size 10 0.8 0.8 - speed 0 -} -ConveyorBelt { - translation 2.47 0 6 - rotation 1 0 0 0 - name "conveyor belt(1)" - size 10 0.8 0.8 - speed 0 -} -FireExtinguisher { - translation -1.18 0 -7.38 -} -WoodenPalletStack { - translation -7 0 -6.85 -} -WoodenPalletStack { - translation -5.4 0 -6.85 - name "wooden pallet stack(1)" -} -WoodenPalletStack { - translation -3.8 0 -6.85 - name "wooden pallet stack(2)" -} -WoodenPalletStack { - translation -2.2 0 -6.85 - name "wooden pallet stack(3)" -} -WoodenPallet { - translation -6.24 0 5.27 - rotation 0 1 0 1.8326 -} -WoodenPallet { - translation -3.11 0 4 - rotation 0 1 0 1.5708 - name "wooden pallet(1)" -} -WoodenPallet { - translation -3.112 0 6 - rotation 0 1 0 1.5708 - name "wooden pallet(2)" -} -WoodenPallet { - translation -6.274 0.141 5.328 - rotation 0 1 0 -0.524 - name "wooden pallet(3)" -} -WoodenPallet { - translation -6.318 0.282 5.083 - rotation 0 1 0 0.524 - name "wooden pallet(4)" -} -WoodenPallet { - translation -6.326 0.423 5.112 - name "wooden pallet(5)" -} -PlatformCart { - translation 0.472 0 0.498 - rotation 0 -1 0 -0.524 -} -PlatformCart { - translation -4.6 0 -7 - rotation 0 1 0 1.5708 - name "platform cart(1)" -} -PlatformCart { - translation -6.2 0 -7 - rotation 0 -1 0 -1.571 - name "platform cart(2)" +Oven { + translation 0.58 -2.85 0.34 + rotation 1 0 0 1.5707963267948966 +} +Worktop { + translation 0.5 -1.31 0.71 + rotation 0.577348855372322 0.577350976096979 0.577350976096979 2.094397223120449 + name "worktop 1" + size 0.88 0.06 0.7 +} +Worktop { + translation 0.807 -1.97 0.71 + rotation 0.577348855372322 0.577350976096979 0.577350976096979 2.094397223120449 + name "worktop 2" + size 0.44 0.06 0.086 +} +Worktop { + translation 0.272 -1.97 0.71 + rotation 0.577348855372322 0.577350976096979 0.577350976096979 2.094397223120449 + name "worktop 3" + size 0.44 0.06 0.244 +} +Worktop { + translation 0.5 -2.63 0.71 + rotation 0.577348855372322 0.577350976096979 0.577350976096979 2.094397223120449 + name "worktop 4" + size 0.88 0.06 0.7 +} +Sink { + translation 0.48 -1.97 0.715 + rotation -0.577350661639742 0.577350072964468 0.577350072964468 4.188790793461465 +} +HotPlate { + translation 0.5 -2.85 0.71 + rotation -0.577350661639742 0.577350072964468 0.577350072964468 4.188790793461465 +} +Can { + translation 0.632793 -0.566328 0.841066 + rotation 1 0 0 1.5707963267948966 + name "can 1" } -Sofa { - translation 5.586 0 -7.001 - rotation 0 1 0 -1.571 +Table { + translation 4.94438 -1.07424 0 + rotation 0.577348855372322 0.577350976096979 0.577350976096979 2.094397223120449 + name "table(1)" +} +Chair { + translation 5.41278 -1.46063 -0.00224453 + rotation 3.2757990828e-05 0.707106780807154 0.707106780807154 3.141527137608161 + name "chair 1" + color 0.13333333333333333 0.13333333333333333 0.13333333333333333 + physics Physics { + centerOfMass [ + 0 0.45 0 + ] + } } -Sofa { - translation 6.99 0 -5.543 - rotation 0 1 0 3.1416 - name "sofa(1)" +Chair { + translation 4.44435 -0.642495 -0.00224453 + rotation 1 0 0 1.5707963267948966 + name "chair 2" + color 0.13333333333333333 0.13333333333333333 0.13333333333333333 + physics Physics { + centerOfMass [ + 0 0.45 0 + ] + } +} +Chair { + translation 4.48748 -1.39428 -0.00224453 + rotation 1 0 0 1.5707963267948966 + name "chair 3" + color 0.13333333333333333 0.13333333333333333 0.13333333333333333 + physics Physics { + centerOfMass [ + 0 0.45 0 + ] + } +} +Chair { + translation 5.38086 -0.706899 -0.00224453 + rotation 3.2757990828e-05 0.707106780807154 0.707106780807154 3.141527137608161 + name "chair 4" + color 0.13333333333333333 0.13333333333333333 0.13333333333333333 + physics Physics { + centerOfMass [ + 0 0.45 0 + ] + } +} +FruitBowl { + translation 4.88063 -0.715471 0.739784 + rotation 0.810579112476972 -0.414102343881343 -0.414102343881343 1.779275824851248 + color 0.6666666666666666 0 0 +} +Orange { + translation 4.86999 -0.775472 0.799671 + rotation 1 0 0 1.5707963267948966 +} +Orange { + translation 4.80474 -0.699693 0.799666 + rotation 1 0 0 1.5707963267948966 + name "orange 2" +} +Apple { + translation 4.95672 -0.722921 0.799667 + rotation 1 0 0 1.5707963267948966 +} +Apple { + translation 4.89356 -0.635999 0.79966 + rotation 1 0 0 1.5707963267948966 + name "apple 2" +} +Desk { + translation 0.512574 -5.12497 0 + rotation 0.577348855372322 0.577350976096979 0.577350976096979 2.094397223120449 +} +WoodenChair { + translation 0.9 -5.15 0 + rotation 0.577348855372322 -0.577350976096979 -0.577350976096979 2.094397223120449 + physics Physics { + centerOfMass [ + 0 0.45 0 + ] + } +} +Book { + translation 0.592219 -4.69758 0.719882 + rotation -3.2225617226e-05 5.987464127e-05 0.999999997688268 1.143847227025768 + name "book(1)" } Table { - translation 5.507 0 -5.65 - size 1 0.4 1 + translation 2.55544 -7.16302 0.02 + rotation 1 0 0 1.5707963267948966 + size 1.2 0.53 0.8 } -FloorLight { - translation 7.073 0 -7.105 -} -PanelWithTubes { - translation 7.1 0 -3.65 - rotation 0 1 0 3.14159 -} -PanelWithTubes { - translation 6.32 0 -3.65 - rotation 0 1 0 3.14159 - name "panel with tubes(1)" -} -PanelWithTubes { - translation 5.54 0 -3.65 - rotation 0 1 0 3.14159 - name "panel with tubes(2)" -} -PanelWithTubes { - translation 4.76 0 -3.65 - rotation 0 1 0 3.14159 - name "panel with tubes(3)" -} -PanelWithTubes { - translation 3.98 0 -3.65 - rotation 0 1 0 3.14159 - name "panel with tubes(4)" -} -PanelWithTubes { - translation 3.2 0 -4.47 - rotation 0 1 0 1.57059 - name "panel with tubes(5)" -} -PanelWithTubes { - translation 3.2 0 -5.25 - rotation 0 1 0 1.57059 - name "panel with tubes(6)" -} -PanelWithTubes { - translation 3.2 0 -6.03 - rotation 0 1 0 1.57059 - name "panel with tubes(7)" -} -PanelWithTubes { - translation 2.77 0 -6.45 - rotation 0 1 0 -3.1456 - name "panel with tubes(8)" +Armchair { + translation 3.82671 -5.8922 0 + rotation 0.299531095036754 -0.674641061271134 -0.674641061271134 2.559539551711696 + color 0.13333333333333333 0.13333333333333333 0.13333333333333333 } -PottedTree { - translation 3.339 0 -3.785 -} -DoubleFluorescentLamp { - translation -5 2.5 5 - rotation -0.7071067811865475 0 0.7071067811865475 3.1416 -} -DoubleFluorescentLamp { - translation -5 2.5 0 - rotation -0.7071067811865475 0 0.7071067811865475 3.1416 - name "double fuorescent lamp(1)" -} -DoubleFluorescentLamp { - translation -5 2.5 -5 - rotation -0.7071067811865475 0 0.7071067811865475 3.1416 - name "double fuorescent lamp(2)" -} -DoubleFluorescentLamp { - translation 0 2.5 5 - rotation -0.7071067811865475 0 0.7071067811865475 3.1416 - name "double fuorescent lamp(3)" -} -DoubleFluorescentLamp { - translation 0 2.5 0 - rotation -0.7071067811865475 0 0.7071067811865475 3.1416 - name "double fuorescent lamp(4)" -} -DoubleFluorescentLamp { - translation 0 2.5 -5 - rotation -0.7071067811865475 0 0.7071067811865475 3.1416 - name "double fuorescent lamp(5)" -} -DoubleFluorescentLamp { - translation 5 2.5 5 - rotation -0.7071067811865475 0 0.7071067811865475 3.1416 - name "double fuorescent lamp(6)" -} -DoubleFluorescentLamp { - translation 5 2.5 0 - rotation -0.7071067811865475 0 0.7071067811865475 3.1416 - name "double fuorescent lamp(7)" -} -DoubleFluorescentLamp { - translation 5 2.5 -5 - rotation -0.7071067811865475 0 0.7071067811865475 3.1416 - name "double fuorescent lamp(8)" -} -StraightStairs { - translation -6.92 0 0.6 - rotation 0 1 0 1.5708 - stepSize 0.2 0.03 1.1 - stepRise 0.13 - nSteps 20 - stringerWidth 0.03 - stepAppearance ThreadMetalPlate { - textureTransform TextureTransform { - scale 0.4 2.2 +Sofa { + translation 0.804228 -7.05325 0 + rotation 1 0 0 1.5707963267948966 + color 0.13333333333333333 0.13333333333333333 0.13333333333333333 +} +Sofa { + translation 2.49729 -8.95734 0 + rotation 0.577348855372322 0.577350976096979 0.577350976096979 2.094397223120449 + name "sofa 2" + color 0.13333333333333333 0.13333333333333333 0.13333333333333333 +} +Carpet { + translation 2.55076 -7.14218 -0.015 + rotation 1 0 0 1.5707963267948966 + color 0.13725490196078433 0.13725490196078433 0.13725490196078433 +} +BunchOfSunFlowers { + translation 3.9144 -9.05979 0 + rotation 1 0 0 1.5707963267948966 +} +Book { + translation 0.596131 -4.69173 0.739852 + rotation -0.00015528242257 0.000149611007892 0.999999976751958 0.516204925088763 +} +Can { + translation 2.74188 -7.22627 0.611066 + rotation 0.683001177874697 -0.516483006023323 -0.516483006023323 1.943140607471219 + name "can 2" +} +Can { + translation 2.7233 -7.34994 0.611066 + rotation 1 0 0 1.5707963267948966 + name "can 3" +} +Can { + translation 2.8744 -7.20688 0.611066 + rotation 0.672788985489524 0.5231419410657 0.5231419410657 1.957134326001526 + name "can 4" +} +E-puck { + translation 2.69279 -7.16904 0.549968 + rotation 0.53756105030843 -0.597868502276827 -0.594627085808039 2.159614696165064 + groundSensorsSlot [ + E-puckGroundSensors { } + ] +} +Cabinet { + translation 6.4326 -3.46807 0 + rotation -0.577350661639742 0.577350072964468 0.577350072964468 4.188790793461465 + name "cabinet 4" + depth 0.4 + outerThickness 0.02 + rowsHeights [ + 0.52, 0.44, 0.44, 0.44, 0.44 + ] + columnsWidths [ + 0.96 + ] + layout [ + "Shelf (1, 2, 1, 0)" + "Shelf (1, 3, 1, 0)" + "Shelf (1, 4, 1, 0)" + "Shelf (1, 5, 1, 0)" + ] + primaryAppearance PaintedWood { + colorOverride 0.13333333333333333 0.13333333333333333 0.13333333333333333 } - stringerAppearance BrushedAluminium { - colorOverride 0.533333 0.541176 0.521569 - textureTransform TextureTransform { - scale 20 20 - } + secondaryAppearance PaintedWood { + colorOverride 0.13333333333333333 0.13333333333333333 0.13333333333333333 } - leftRail [] - rightRail [ - StraightStairsRail { - translation -0.02 -0.13 0 - run 3.84 - rise 2.5 - newelHeight 0.8 - balusterHeight 0.62 - appearance Rubber { - } - } - ] } -OilBarrel { - translation -7.18 0.44 -1.203 -} -OilBarrel { - translation -7.18 0.45 -1.9 - name "oil barrel(1)" -} -OilBarrel { - translation -7.18 0.44 -2.57 - name "oil barrel(2)" -} -OilBarrel { - translation -7.18 0.44 -3.25 - name "oil barrel(3)" -} -OilBarrel { - translation -7.18 0.44 -3.95 - name "oil barrel(4)" -} -OilBarrel { - translation -6.55 0.44 -3.64 - name "oil barrel(5)" -} -OilBarrel { - translation -6.55 0.44 -2.95 - name "oil barrel(6)" -} -OilBarrel { - translation -6.55 0.44 -2.26 - name "oil barrel(7)" -} -OilBarrel { - translation -6.55 0.44 -1.58 - name "oil barrel(8)" -} -OilBarrel { - translation 5.826 0.305 0.356 - rotation -0.6778359835355757 0.5198742056615036 -0.5198742056615036 -1.95 - name "oil barrel(9)" -} -OilBarrel { - translation 5.892 0.305 -1.534 - rotation -0.9353625232302415 -0.25009693134498434 0.25009693134498434 -1.638 - name "oil barrel(10)" -} -OilBarrel { - translation 1.31 0.44 -5.783 - name "oil barrel(11)" -} -OilBarrel { - translation 1.017 0.44 -6.932 - name "oil barrel(12)" -} -OilBarrel { - translation -0.08 0.44 -6.53 - name "oil barrel(13)" -} -TrafficCone { - translation -5.892 0 1.397 -} -TrafficCone { - translation -5.045 0 2.377 - rotation 0 1 0 -0.785 - name "traffic cone(1)" -} -TrafficCone { - translation -3.996 0 3.072 - rotation 0 1 0 0.262 - name "traffic cone(2)" -} -TrafficCone { - translation 6.425 0 2.282 - rotation 0 1 0 0.262 - name "traffic cone(3)" -} -TrafficCone { - translation 5.034 0 2.12 - name "traffic cone(4)" -} -TrafficCone { - translation 4.128 0 1.239 - rotation 0 1 0 -0.262 - name "traffic cone(5)" -} -TrafficCone { - translation 4.289 0 -0.082 - rotation 0 -1 0 0.524 - name "traffic cone(6)" -} -TrafficCone { - translation 4.158 0 -1.273 - rotation 0 1 0 0.524 - name "traffic cone(7)" -} -TrafficCone { - translation 4.317 0 -2.52 - name "traffic cone(8)" +Book { + translation 6.15092 -3.02231 1.52924 + rotation 1 0 0 1.322112326794896 + name "book 2" +} +Book { + translation 6.15759 -3.04469 1.529 + rotation 1 0 0 1.305368326794896 + name "book 3" +} +Book { + translation 6.16417 -3.06681 1.52878 + rotation 1 0 0 1.292116326794897 + name "book 4" +} +Book { + translation 6.15821 -3.0912 1.52813 + rotation 1 0 0 1.259078326794896 + name "book 5" +} +Book { + translation 6.16234 -3.11388 1.52779 + rotation 1 0 0 1.244082326794896 + name "book 6" +} +PottedTree { + translation 6.08049 -4.51701 0 + rotation 1 0 0 1.5707963267948966 +} +PortraitPainting { + translation 2.47 -9.73 1.6 + rotation 9.38185669e-07 0.707106781186236 0.707106781186236 3.141590777218456 +} +LandscapePainting { + translation 4.92523 -0.184484 1.62868 + rotation 1 0 0 1.5707963267948966 +} +CreateWall { + translation 3.53117 -3.235 0 + rotation 1 0 0 1.5707963267948966 +} +TiagoSteel { + translation 2.194 -4.717 0.095 + rotation 1 0 0 4.692820414042842e-06 + controller "ros" + controllerArgs [ + "--name=tiago" + "--use-ros-control" + "--auto-publish" + "--robot-description" + ] } From 5e3de2f68d300d54a77622d7f1b9ba7dcef344d7 Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Wed, 17 Mar 2021 09:16:57 +0100 Subject: [PATCH 33/72] Update passive --- config/tiago_steel.srdf | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/config/tiago_steel.srdf b/config/tiago_steel.srdf index abfffb8..52ec381 100644 --- a/config/tiago_steel.srdf +++ b/config/tiago_steel.srdf @@ -21,14 +21,14 @@ - - - - - - - - + + + + + + + + From a630d8b7db2d0c8c06b1ab66493f6196d14fc798 Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Wed, 17 Mar 2021 11:49:12 +0100 Subject: [PATCH 34/72] Update srdf --- config/tiago_steel.srdf | 348 ++++++++++++++++++++-------------------- 1 file changed, 175 insertions(+), 173 deletions(-) diff --git a/config/tiago_steel.srdf b/config/tiago_steel.srdf index 52ec381..458a0ac 100644 --- a/config/tiago_steel.srdf +++ b/config/tiago_steel.srdf @@ -3,7 +3,7 @@ This is a format for representing semantic information about the robot structure. A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined --> - + @@ -19,27 +19,28 @@ - + - - - - - - - - - + + + + + + + + + + - - + + - - - + + + @@ -55,56 +56,57 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -120,98 +122,99 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -246,50 +249,49 @@ - + - + - - + - + - + - + - + - + @@ -316,12 +318,12 @@ - - - - - - + + + + + + From 4d9c91e42e6feecc13ff0e46667c8e481bbb2c95 Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Wed, 17 Mar 2021 12:37:25 +0100 Subject: [PATCH 35/72] Add mapping --- launch/tiago.launch | 3 +++ worlds/tiago.wbt | 5 +++++ 2 files changed, 8 insertions(+) diff --git a/launch/tiago.launch b/launch/tiago.launch index 9400586..d016296 100644 --- a/launch/tiago.launch +++ b/launch/tiago.launch @@ -79,4 +79,7 @@ publish_transforms_updates: true + + + diff --git a/worlds/tiago.wbt b/worlds/tiago.wbt index 3124fa7..c99e5bf 100644 --- a/worlds/tiago.wbt +++ b/worlds/tiago.wbt @@ -5,6 +5,11 @@ WorldInfo { ] title "TIAGo Steel" basicTimeStep 16 + contactProperties [ + ContactProperties { + softCFM 1e-05 + } + ] } Viewpoint { orientation 0.4925721069109625 0.36674151919891895 0.7892232748652084 1.5125517327991547 From 7660348b5d9aec0ae84c2566aac4047dd089df0a Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Thu, 1 Apr 2021 12:36:59 +0200 Subject: [PATCH 36/72] Add relative pose --- CMakeLists.txt | 1 + src/complete_test.cpp | 25 +++++++++++++++++++++++++ srv/node_get_relative_pose.srv | 4 ++++ 3 files changed, 30 insertions(+) create mode 100644 srv/node_get_relative_pose.srv diff --git a/CMakeLists.txt b/CMakeLists.txt index 4db48b8..fdacd44 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -99,6 +99,7 @@ find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs sensor_msgs messag node_get_number_of_contact_points.srv node_get_name.srv node_get_orientation.srv + node_get_relative_pose.srv node_get_parent_node.srv node_get_position.srv node_get_static_balance.srv diff --git a/src/complete_test.cpp b/src/complete_test.cpp index 774ba96..68d63e7 100644 --- a/src/complete_test.cpp +++ b/src/complete_test.cpp @@ -105,6 +105,7 @@ #include #include #include +#include #include #include #include @@ -2984,6 +2985,7 @@ int main(int argc, char **argv) { supervisor_node_get_position_client.shutdown(); time_step_client.call(time_step_srv); + // supervisor_node_get_orientation ros::ServiceClient supervisor_node_get_orientation_client; webots_ros::node_get_orientation supervisor_node_get_orientation_srv; supervisor_node_get_orientation_client = @@ -2999,6 +3001,29 @@ int main(int argc, char **argv) { supervisor_node_get_orientation_client.shutdown(); time_step_client.call(time_step_srv); + // supervisor_node_get_relative_pose + ros::ServiceClient supervisor_node_get_relative_pose_client; + webots_ros::node_get_relative_pose supervisor_node_get_relative_pose_srv; + supervisor_node_get_relative_pose_client = + n.serviceClient(model_name + "/supervisor/node/get_relative_pose"); + + supervisor_node_get_relative_pose_srv.request.node_from = def_node; + supervisor_node_get_relative_pose_srv.request.node_to = def_node; + supervisor_node_get_relative_pose_client.call(supervisor_node_get_relative_pose_srv); + ROS_INFO("From_def get_relative_pose rotation is:\nw=%f x=%f y=%f z=%f.", + supervisor_node_get_relative_pose_srv.response.pose.rotation.w, + supervisor_node_get_relative_pose_srv.response.pose.rotation.x, + supervisor_node_get_relative_pose_srv.response.pose.rotation.y, + supervisor_node_get_relative_pose_srv.response..pose.rotation.z); + ROS_INFO("From_def get_relative_pose translation is:\nw=%f x=%f y=%f.", + supervisor_node_get_relative_pose_srv.response.pose.translation.x, + supervisor_node_get_relative_pose_srv.response.pose.translation.y, + supervisor_node_get_relative_pose_srv.response..pose.translation.z); + + supervisor_node_get_relative_pose_client.shutdown(); + time_step_client.call(time_step_srv); + + // supervisor_node_get_center_of_mass ros::ServiceClient supervisor_node_get_center_of_mass_client; webots_ros::node_get_center_of_mass supervisor_node_get_center_of_mass_srv; supervisor_node_get_center_of_mass_client = diff --git a/srv/node_get_relative_pose.srv b/srv/node_get_relative_pose.srv new file mode 100644 index 0000000..e52d209 --- /dev/null +++ b/srv/node_get_relative_pose.srv @@ -0,0 +1,4 @@ +uint64 node_from +uint64 node_to +--- +geometry_msgs/Transform pose From fb3486b47401ef301dfbc7a8e23387fa295ab3b5 Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Tue, 6 Apr 2021 10:10:39 +0200 Subject: [PATCH 37/72] Fix --- src/complete_test.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/complete_test.cpp b/src/complete_test.cpp index 68d63e7..3f1bbfc 100644 --- a/src/complete_test.cpp +++ b/src/complete_test.cpp @@ -3007,18 +3007,18 @@ int main(int argc, char **argv) { supervisor_node_get_relative_pose_client = n.serviceClient(model_name + "/supervisor/node/get_relative_pose"); - supervisor_node_get_relative_pose_srv.request.node_from = def_node; - supervisor_node_get_relative_pose_srv.request.node_to = def_node; + supervisor_node_get_relative_pose_srv.request.node_from = from_def_node; + supervisor_node_get_relative_pose_srv.request.node_to = from_def_node; supervisor_node_get_relative_pose_client.call(supervisor_node_get_relative_pose_srv); ROS_INFO("From_def get_relative_pose rotation is:\nw=%f x=%f y=%f z=%f.", supervisor_node_get_relative_pose_srv.response.pose.rotation.w, supervisor_node_get_relative_pose_srv.response.pose.rotation.x, supervisor_node_get_relative_pose_srv.response.pose.rotation.y, - supervisor_node_get_relative_pose_srv.response..pose.rotation.z); + supervisor_node_get_relative_pose_srv.response.pose.rotation.z); ROS_INFO("From_def get_relative_pose translation is:\nw=%f x=%f y=%f.", supervisor_node_get_relative_pose_srv.response.pose.translation.x, supervisor_node_get_relative_pose_srv.response.pose.translation.y, - supervisor_node_get_relative_pose_srv.response..pose.translation.z); + supervisor_node_get_relative_pose_srv.response.pose.translation.z); supervisor_node_get_relative_pose_client.shutdown(); time_step_client.call(time_step_srv); From a8b372cd8f2654cd5d39e70ae3635693087187ac Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Tue, 6 Apr 2021 16:33:15 +0200 Subject: [PATCH 38/72] Rename get pose --- CMakeLists.txt | 2 +- src/complete_test.cpp | 42 +++++++++---------- ...et_relative_pose.srv => node_get_pose.srv} | 2 +- 3 files changed, 23 insertions(+), 23 deletions(-) rename srv/{node_get_relative_pose.srv => node_get_pose.srv} (76%) diff --git a/CMakeLists.txt b/CMakeLists.txt index fdacd44..b5952a5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -99,7 +99,7 @@ find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs sensor_msgs messag node_get_number_of_contact_points.srv node_get_name.srv node_get_orientation.srv - node_get_relative_pose.srv + node_get_pose.srv node_get_parent_node.srv node_get_position.srv node_get_static_balance.srv diff --git a/src/complete_test.cpp b/src/complete_test.cpp index 3f1bbfc..7ae1b86 100644 --- a/src/complete_test.cpp +++ b/src/complete_test.cpp @@ -105,7 +105,7 @@ #include #include #include -#include +#include #include #include #include @@ -3001,26 +3001,26 @@ int main(int argc, char **argv) { supervisor_node_get_orientation_client.shutdown(); time_step_client.call(time_step_srv); - // supervisor_node_get_relative_pose - ros::ServiceClient supervisor_node_get_relative_pose_client; - webots_ros::node_get_relative_pose supervisor_node_get_relative_pose_srv; - supervisor_node_get_relative_pose_client = - n.serviceClient(model_name + "/supervisor/node/get_relative_pose"); - - supervisor_node_get_relative_pose_srv.request.node_from = from_def_node; - supervisor_node_get_relative_pose_srv.request.node_to = from_def_node; - supervisor_node_get_relative_pose_client.call(supervisor_node_get_relative_pose_srv); - ROS_INFO("From_def get_relative_pose rotation is:\nw=%f x=%f y=%f z=%f.", - supervisor_node_get_relative_pose_srv.response.pose.rotation.w, - supervisor_node_get_relative_pose_srv.response.pose.rotation.x, - supervisor_node_get_relative_pose_srv.response.pose.rotation.y, - supervisor_node_get_relative_pose_srv.response.pose.rotation.z); - ROS_INFO("From_def get_relative_pose translation is:\nw=%f x=%f y=%f.", - supervisor_node_get_relative_pose_srv.response.pose.translation.x, - supervisor_node_get_relative_pose_srv.response.pose.translation.y, - supervisor_node_get_relative_pose_srv.response.pose.translation.z); - - supervisor_node_get_relative_pose_client.shutdown(); + // supervisor_node_get_pose + ros::ServiceClient supervisor_node_get_pose_client; + webots_ros::node_get_pose supervisor_node_get_pose_srv; + supervisor_node_get_pose_client = + n.serviceClient(model_name + "/supervisor/node/get_pose"); + + supervisor_node_get_pose_srv.request.node_from = from_def_node; + supervisor_node_get_pose_srv.request.node_to = from_def_node; + supervisor_node_get_pose_client.call(supervisor_node_get_pose_srv); + ROS_INFO("From_def get_pose rotation is:\nw=%f x=%f y=%f z=%f.", + supervisor_node_get_pose_srv.response.pose.rotation.w, + supervisor_node_get_pose_srv.response.pose.rotation.x, + supervisor_node_get_pose_srv.response.pose.rotation.y, + supervisor_node_get_pose_srv.response.pose.rotation.z); + ROS_INFO("From_def get_pose translation is:\nw=%f x=%f y=%f.", + supervisor_node_get_pose_srv.response.pose.translation.x, + supervisor_node_get_pose_srv.response.pose.translation.y, + supervisor_node_get_pose_srv.response.pose.translation.z); + + supervisor_node_get_pose_client.shutdown(); time_step_client.call(time_step_srv); // supervisor_node_get_center_of_mass diff --git a/srv/node_get_relative_pose.srv b/srv/node_get_pose.srv similarity index 76% rename from srv/node_get_relative_pose.srv rename to srv/node_get_pose.srv index e52d209..7eed83c 100644 --- a/srv/node_get_relative_pose.srv +++ b/srv/node_get_pose.srv @@ -1,4 +1,4 @@ +uint64 node uint64 node_from -uint64 node_to --- geometry_msgs/Transform pose From fb78c3ba4696fd2b34cfa2a39d8a4a9bd193ae16 Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Tue, 6 Apr 2021 17:59:38 +0200 Subject: [PATCH 39/72] Fix --- src/complete_test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/complete_test.cpp b/src/complete_test.cpp index 7ae1b86..d930c77 100644 --- a/src/complete_test.cpp +++ b/src/complete_test.cpp @@ -3008,7 +3008,7 @@ int main(int argc, char **argv) { n.serviceClient(model_name + "/supervisor/node/get_pose"); supervisor_node_get_pose_srv.request.node_from = from_def_node; - supervisor_node_get_pose_srv.request.node_to = from_def_node; + supervisor_node_get_pose_srv.request.node = from_def_node; supervisor_node_get_pose_client.call(supervisor_node_get_pose_srv); ROS_INFO("From_def get_pose rotation is:\nw=%f x=%f y=%f z=%f.", supervisor_node_get_pose_srv.response.pose.rotation.w, From 95710f9dccc9b41c1ca48e9d580b07daa5e05156 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Darko=20Luki=C4=87?= Date: Mon, 12 Apr 2021 08:33:57 +0200 Subject: [PATCH 40/72] Update src/complete_test.cpp Co-authored-by: Olivier Michel --- src/complete_test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/complete_test.cpp b/src/complete_test.cpp index d930c77..6f1c0b3 100644 --- a/src/complete_test.cpp +++ b/src/complete_test.cpp @@ -3015,7 +3015,7 @@ int main(int argc, char **argv) { supervisor_node_get_pose_srv.response.pose.rotation.x, supervisor_node_get_pose_srv.response.pose.rotation.y, supervisor_node_get_pose_srv.response.pose.rotation.z); - ROS_INFO("From_def get_pose translation is:\nw=%f x=%f y=%f.", + ROS_INFO("From_def get_pose translation is:\nx=%f y=%f z=%f.", supervisor_node_get_pose_srv.response.pose.translation.x, supervisor_node_get_pose_srv.response.pose.translation.y, supervisor_node_get_pose_srv.response.pose.translation.z); From 9e2472b19b4165fe06e6bf1cb80a389766b2bbcf Mon Sep 17 00:00:00 2001 From: Sis Hamilton <69378550+sishamilton@users.noreply.github.com> Date: Tue, 18 May 2021 09:22:33 -0700 Subject: [PATCH 41/72] Add altimeter callback function --- src/complete_test.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/complete_test.cpp b/src/complete_test.cpp index cd2678b..4d3f6e0 100644 --- a/src/complete_test.cpp +++ b/src/complete_test.cpp @@ -140,6 +140,7 @@ static vector imageColor; static vector imageRangeFinder; static int connectorPresence = 0; static double accelerometerValues[3] = {0, 0, 0}; +static double altimeterValue = 0; static double compassValues[3] = {0, 0, 0}; static double GPSValues[3] = {0, 0, 0}; static double GyroValues[3] = {0, 0, 0}; @@ -238,6 +239,11 @@ void accelerometerCallback(const sensor_msgs::Imu::ConstPtr &values) { callbackCalled = true; } +void altimeterCallback(const webots_ros::Float64Stamped::ConstPtr &value) { + ROS_INFO("Altimeter value is z=%f (time: %d:%d).", value->data, value->header.stamp.sec, value->header.stamp.nsec); + callbackCalled = true; +} + void battery_sensorCallback(const webots_ros::Float64Stamped::ConstPtr &value) { ROS_INFO("Battery level is %f (time: %d:%d).", value->data, value->header.stamp.sec, value->header.stamp.nsec); callbackCalled = true; From 29310145e8e533598952f7a8ee215170fb22c8bc Mon Sep 17 00:00:00 2001 From: Sis Hamilton <69378550+sishamilton@users.noreply.github.com> Date: Tue, 18 May 2021 10:22:06 -0700 Subject: [PATCH 42/72] Add altimeter methods test --- src/complete_test.cpp | 52 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 52 insertions(+) diff --git a/src/complete_test.cpp b/src/complete_test.cpp index 4d3f6e0..e37bdad 100644 --- a/src/complete_test.cpp +++ b/src/complete_test.cpp @@ -1028,6 +1028,58 @@ int main(int argc, char **argv) { sampling_period_accelerometer_client.shutdown(); time_step_client.call(time_step_srv); + //////////////////////////// + // ALTIMETER METHODS TEST // + //////////////////////////// + + ros::ServiceClient set_altimeter_client; + webots::set_int altimeter_srv; + ros::subscriber sub_altimeter_32; + + set_altimeter_client = n.serviceClinet(model_name + "/altimeter/enable"); + + ros::ServiceClient sampling_period_altimeter_client; + webot_ros::get_int sampling_period_altimeter_srv; + sampling_period_altimeter_client = + n.serviceClient(model_name + "/altimeter/get_sampling_period"); + + altimeter_srv.request.value = 32; + if (set_altimeter_client.call(altimeter_srv) && altimeter_srv.response.succes) { + ROS_INFO("Altimeter enabled."); + sub_altimeter_32 = n.subscribe(model_name + "/altimeter/value", 1, altimeterCallback); + callbackCalled = false; + while (sub_altimeter_32.getNumPublishers() == 0 && !callbackCalled) { + ros::spinOnce(); + time_step_client.call(time_step_srv); + } + } else { + if (!altimeter_srv.response.success) + ROS_ERROR("Sampling period is not valid."); + ROS_ERROR("Failed to enable altimeter."); + return 1; + } + + sub_altimeter_32.shutdown(); + + time_step_client.call(time_step_srv); + + sampling_period_altimeter_client.call(sampling_period_altimeter_srv); + ROS_INFO("Altimeter is enabled with a sampling period of %d.", sampling_period_altimeter_srv.response.value); + + time_step_client.call(time_step_srv); + + time_step_client.call(time_step_srv); + time_step_client.call(time_step_srv); + time_step_client.call(time_step_srv); + + sampling_period_altimeter_client.call(sampling_period_altimeter_srv); + ROS_INFO("Altimeter is disabled (sampling period is %d).", sampling_period_altimeter_srv.response.value); + + set_altimeter_client.shutdown(); + sampling_period_altimeter_client.shutdown(); + time_step_client.call(time_step_srv); + + ///////////////////////////////// // BATTERY SENSOR METHODS TEST // ///////////////////////////////// From 70d9b457e62696ebbbf96c851d565b0823c6f8e7 Mon Sep 17 00:00:00 2001 From: Sis Hamilton <69378550+sishamilton@users.noreply.github.com> Date: Tue, 18 May 2021 10:34:02 -0700 Subject: [PATCH 43/72] Remove unused altimeterValue variable --- src/complete_test.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/complete_test.cpp b/src/complete_test.cpp index 6041d42..b15b531 100644 --- a/src/complete_test.cpp +++ b/src/complete_test.cpp @@ -143,7 +143,6 @@ static vector imageColor; static vector imageRangeFinder; static int connectorPresence = 0; static double accelerometerValues[3] = {0, 0, 0}; -static double altimeterValue = 0; static double compassValues[3] = {0, 0, 0}; static double GPSValues[3] = {0, 0, 0}; static double GyroValues[3] = {0, 0, 0}; From 20de4790bc2a729c50cf9cea16984a5fe54b957b Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Mon, 7 Jun 2021 17:47:07 +0200 Subject: [PATCH 44/72] Init --- src/complete_test.cpp | 30 ++++++++++++++++++++++++++++++ srv/field_disable_sf_tracking.srv | 3 +++ srv/field_enable_sf_tracking.srv | 4 ++++ srv/node_disable_pose_tracking.srv | 4 ++++ srv/node_enable_pose_tracking.srv | 5 +++++ 5 files changed, 46 insertions(+) create mode 100644 srv/field_disable_sf_tracking.srv create mode 100644 srv/field_enable_sf_tracking.srv create mode 100644 srv/node_disable_pose_tracking.srv create mode 100644 srv/node_enable_pose_tracking.srv diff --git a/src/complete_test.cpp b/src/complete_test.cpp index 6f1c0b3..0554808 100644 --- a/src/complete_test.cpp +++ b/src/complete_test.cpp @@ -68,6 +68,8 @@ #include #include #include +#include +#include #include #include #include @@ -106,6 +108,8 @@ #include #include #include +#include +#include #include #include #include @@ -3023,6 +3027,32 @@ int main(int argc, char **argv) { supervisor_node_get_pose_client.shutdown(); time_step_client.call(time_step_srv); + // supervisor_node_enable_pose_tracking + ros::ServiceClient supervisor_node_enable_pose_tracking_client; + webots_ros::node_enable_pose_tracking supervisor_node_enable_pose_tracking_srv; + supervisor_node_enable_pose_tracking_client = + n.serviceClient(model_name + "/supervisor/node/enable_pose_tracking"); + + supervisor_node_enable_pose_tracking_srv.request.node_from = from_def_node; + supervisor_node_enable_pose_tracking_srv.request.node = from_def_node; + supervisor_node_enable_pose_tracking_srv.request.sampling_period = 32; + supervisor_node_enable_pose_tracking_client.call(supervisor_node_enable_pose_tracking_srv); + supervisor_node_enable_pose_tracking_client.shutdown(); + time_step_client.call(time_step_srv); + + // supervisor_node_disable_pose_tracking + ros::ServiceClient supervisor_node_disable_pose_tracking_client; + webots_ros::node_disable_pose_tracking supervisor_node_disable_pose_tracking_srv; + supervisor_node_disable_pose_tracking_client = + n.serviceClient(model_name + "/supervisor/node/disable_pose_tracking"); + + supervisor_node_disable_pose_tracking_srv.request.node_from = from_def_node; + supervisor_node_disable_pose_tracking_srv.request.node = from_def_node; + supervisor_node_disable_pose_tracking_srv.request.sampling_period = 32; + supervisor_node_disable_pose_tracking_client.call(supervisor_node_disable_pose_tracking_srv); + supervisor_node_disable_pose_tracking_client.shutdown(); + time_step_client.call(time_step_srv); + // supervisor_node_get_center_of_mass ros::ServiceClient supervisor_node_get_center_of_mass_client; webots_ros::node_get_center_of_mass supervisor_node_get_center_of_mass_srv; diff --git a/srv/field_disable_sf_tracking.srv b/srv/field_disable_sf_tracking.srv new file mode 100644 index 0000000..c46a9cb --- /dev/null +++ b/srv/field_disable_sf_tracking.srv @@ -0,0 +1,3 @@ +uint64 field +--- +int8 success diff --git a/srv/field_enable_sf_tracking.srv b/srv/field_enable_sf_tracking.srv new file mode 100644 index 0000000..d7a45b5 --- /dev/null +++ b/srv/field_enable_sf_tracking.srv @@ -0,0 +1,4 @@ +uint64 field +uint64 sampling_period +--- +int8 success diff --git a/srv/node_disable_pose_tracking.srv b/srv/node_disable_pose_tracking.srv new file mode 100644 index 0000000..08d21f1 --- /dev/null +++ b/srv/node_disable_pose_tracking.srv @@ -0,0 +1,4 @@ +uint64 node +uint64 node_from +--- +int8 success diff --git a/srv/node_enable_pose_tracking.srv b/srv/node_enable_pose_tracking.srv new file mode 100644 index 0000000..e18025a --- /dev/null +++ b/srv/node_enable_pose_tracking.srv @@ -0,0 +1,5 @@ +uint64 node +uint64 sampling_period +uint64 node_from +--- +int8 success From 915e132cbb4ca3845d59aa115cdb2028a37c4333 Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Mon, 7 Jun 2021 17:55:02 +0200 Subject: [PATCH 45/72] add --- src/complete_test.cpp | 35 +++++++++++++++++++++++++++++++++++ 1 file changed, 35 insertions(+) diff --git a/src/complete_test.cpp b/src/complete_test.cpp index 0554808..84cfe74 100644 --- a/src/complete_test.cpp +++ b/src/complete_test.cpp @@ -3258,6 +3258,41 @@ int main(int argc, char **argv) { supervisor_field_set_string_client.shutdown(); time_step_client.call(time_step_srv); + // supervisor_field_enable_sf_tracking + ros::ServiceClient supervisor_field_enable_sf_tracking_client; + webots_ros::field_enable_sf_tracking supervisor_field_enable_sf_tracking_srv; + supervisor_field_enable_sf_tracking_client = + n.serviceClient(model_name + "/supervisor/field/enable_sf_tracking"); + + supervisor_field_enable_sf_tracking_srv.request.field = field; + supervisor_field_enable_sf_tracking_srv.request.sampling_period = 32; + if (supervisor_field_enable_sf_tracking_client.call(supervisor_field_enable_sf_tracking_srv) && + supervisor_field_enable_sf_tracking_srv.response.success == 1) + ROS_INFO("Field is successfully tracked."); + else + ROS_ERROR("Failed to call service field_enable_sf_tracking."); + + supervisor_field_enable_sf_tracking_client.shutdown(); + time_step_client.call(time_step_srv); + + // supervisor_field_disable_sf_tracking + ros::ServiceClient supervisor_field_disable_sf_tracking_client; + webots_ros::field_disable_sf_tracking supervisor_field_disable_sf_tracking_srv; + supervisor_field_disable_sf_tracking_client = + n.serviceClient(model_name + "/supervisor/field/disable_sf_tracking"); + + supervisor_field_disable_sf_tracking_srv.request.field = field; + supervisor_field_disable_sf_tracking_srv.request.sampling_period = 32; + if (supervisor_field_disable_sf_tracking_client.call(supervisor_field_disable_sf_tracking_srv) && + supervisor_field_disable_sf_tracking_srv.response.success == 1) + ROS_INFO("Field is successfully tracked."); + else + ROS_ERROR("Failed to call service field_disable_sf_tracking."); + + supervisor_field_disable_sf_tracking_client.shutdown(); + time_step_client.call(time_step_srv); + + // supervisor_field_get_string_client ros::ServiceClient supervisor_field_get_string_client; webots_ros::field_get_string supervisor_field_get_string_srv; supervisor_field_get_string_client = From 415ef22c7b425e29be2a6adbde74a1a78fdbccfc Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Mon, 7 Jun 2021 17:57:15 +0200 Subject: [PATCH 46/72] Fix --- src/complete_test.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/complete_test.cpp b/src/complete_test.cpp index 84cfe74..14bc295 100644 --- a/src/complete_test.cpp +++ b/src/complete_test.cpp @@ -68,8 +68,8 @@ #include #include #include -#include -#include +#include +#include #include #include #include From 607c17730556b42180d9e518c0b4a85277d2adf7 Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Mon, 7 Jun 2021 17:58:03 +0200 Subject: [PATCH 47/72] fix --- src/complete_test.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/complete_test.cpp b/src/complete_test.cpp index 14bc295..84cfe74 100644 --- a/src/complete_test.cpp +++ b/src/complete_test.cpp @@ -68,8 +68,8 @@ #include #include #include -#include -#include +#include +#include #include #include #include From e6ee426b948a8cd1f60a9d7a6a223c1321620c11 Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Mon, 7 Jun 2021 17:59:52 +0200 Subject: [PATCH 48/72] Fix --- CMakeLists.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index b5952a5..f9b9460 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -51,6 +51,8 @@ find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs sensor_msgs messag display_image_paste.srv display_image_save.srv display_set_font.srv + field_disable_sf_tracking.srv + field_enable_sf_tracking.srv field_get_bool.srv field_get_color.srv field_get_count.srv @@ -91,6 +93,8 @@ find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs sensor_msgs messag mouse_get_state.srv node_add_force_or_torque.srv node_add_force_with_offset.srv + node_disable_pose_tracking.srv + node_enable_pose_tracking.srv node_get_center_of_mass.srv node_get_contact_point.srv node_get_contact_point_node.srv From d294e7237daec9801be9755aa30a8496841e1f0c Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Mon, 7 Jun 2021 18:00:44 +0200 Subject: [PATCH 49/72] fix --- src/complete_test.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/complete_test.cpp b/src/complete_test.cpp index 84cfe74..7e3e982 100644 --- a/src/complete_test.cpp +++ b/src/complete_test.cpp @@ -3048,7 +3048,6 @@ int main(int argc, char **argv) { supervisor_node_disable_pose_tracking_srv.request.node_from = from_def_node; supervisor_node_disable_pose_tracking_srv.request.node = from_def_node; - supervisor_node_disable_pose_tracking_srv.request.sampling_period = 32; supervisor_node_disable_pose_tracking_client.call(supervisor_node_disable_pose_tracking_srv); supervisor_node_disable_pose_tracking_client.shutdown(); time_step_client.call(time_step_srv); From e57e24aa66af72a875fe43add146fd5e44b5eb02 Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Mon, 7 Jun 2021 18:01:15 +0200 Subject: [PATCH 50/72] Fix --- src/complete_test.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/complete_test.cpp b/src/complete_test.cpp index 7e3e982..6b5e810 100644 --- a/src/complete_test.cpp +++ b/src/complete_test.cpp @@ -3281,7 +3281,6 @@ int main(int argc, char **argv) { n.serviceClient(model_name + "/supervisor/field/disable_sf_tracking"); supervisor_field_disable_sf_tracking_srv.request.field = field; - supervisor_field_disable_sf_tracking_srv.request.sampling_period = 32; if (supervisor_field_disable_sf_tracking_client.call(supervisor_field_disable_sf_tracking_srv) && supervisor_field_disable_sf_tracking_srv.response.success == 1) ROS_INFO("Field is successfully tracked."); From ed8bd85d227888462e33e39f2524ca7af28d0d5d Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Tue, 8 Jun 2021 09:25:09 +0200 Subject: [PATCH 51/72] Consistency --- src/complete_test.cpp | 6 +++--- srv/node_disable_pose_tracking.srv | 2 +- srv/node_enable_pose_tracking.srv | 2 +- srv/node_get_pose.srv | 2 +- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/complete_test.cpp b/src/complete_test.cpp index 6b5e810..a495aa3 100644 --- a/src/complete_test.cpp +++ b/src/complete_test.cpp @@ -3011,7 +3011,7 @@ int main(int argc, char **argv) { supervisor_node_get_pose_client = n.serviceClient(model_name + "/supervisor/node/get_pose"); - supervisor_node_get_pose_srv.request.node_from = from_def_node; + supervisor_node_get_pose_srv.request.from_node = from_def_node; supervisor_node_get_pose_srv.request.node = from_def_node; supervisor_node_get_pose_client.call(supervisor_node_get_pose_srv); ROS_INFO("From_def get_pose rotation is:\nw=%f x=%f y=%f z=%f.", @@ -3033,7 +3033,7 @@ int main(int argc, char **argv) { supervisor_node_enable_pose_tracking_client = n.serviceClient(model_name + "/supervisor/node/enable_pose_tracking"); - supervisor_node_enable_pose_tracking_srv.request.node_from = from_def_node; + supervisor_node_enable_pose_tracking_srv.request.from_node = from_def_node; supervisor_node_enable_pose_tracking_srv.request.node = from_def_node; supervisor_node_enable_pose_tracking_srv.request.sampling_period = 32; supervisor_node_enable_pose_tracking_client.call(supervisor_node_enable_pose_tracking_srv); @@ -3046,7 +3046,7 @@ int main(int argc, char **argv) { supervisor_node_disable_pose_tracking_client = n.serviceClient(model_name + "/supervisor/node/disable_pose_tracking"); - supervisor_node_disable_pose_tracking_srv.request.node_from = from_def_node; + supervisor_node_disable_pose_tracking_srv.request.from_node = from_def_node; supervisor_node_disable_pose_tracking_srv.request.node = from_def_node; supervisor_node_disable_pose_tracking_client.call(supervisor_node_disable_pose_tracking_srv); supervisor_node_disable_pose_tracking_client.shutdown(); diff --git a/srv/node_disable_pose_tracking.srv b/srv/node_disable_pose_tracking.srv index 08d21f1..ac43d79 100644 --- a/srv/node_disable_pose_tracking.srv +++ b/srv/node_disable_pose_tracking.srv @@ -1,4 +1,4 @@ uint64 node -uint64 node_from +uint64 from_node --- int8 success diff --git a/srv/node_enable_pose_tracking.srv b/srv/node_enable_pose_tracking.srv index e18025a..3d0f1b4 100644 --- a/srv/node_enable_pose_tracking.srv +++ b/srv/node_enable_pose_tracking.srv @@ -1,5 +1,5 @@ uint64 node uint64 sampling_period -uint64 node_from +uint64 from_node --- int8 success diff --git a/srv/node_get_pose.srv b/srv/node_get_pose.srv index 7eed83c..bc6d15c 100644 --- a/srv/node_get_pose.srv +++ b/srv/node_get_pose.srv @@ -1,4 +1,4 @@ uint64 node -uint64 node_from +uint64 from_node --- geometry_msgs/Transform pose From d9369a956279c993423db2d8302d7ae46303da90 Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Wed, 9 Jun 2021 17:28:56 +0200 Subject: [PATCH 52/72] Fix Tiago MoveIt --- config/tiago_steel.srdf | 2 +- package.xml | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/config/tiago_steel.srdf b/config/tiago_steel.srdf index 458a0ac..91afebf 100644 --- a/config/tiago_steel.srdf +++ b/config/tiago_steel.srdf @@ -3,7 +3,7 @@ This is a format for representing semantic information about the robot structure. A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined --> - + diff --git a/package.xml b/package.xml index 1d6b474..8e5e237 100644 --- a/package.xml +++ b/package.xml @@ -11,7 +11,7 @@ Apache 2.0 - moveit_ros_planning_interface + moveit catkin rospy roscpp @@ -25,7 +25,7 @@ sensor_msgs message_runtime tf - moveit_ros_planning_interface + moveit ros_control ros_controllers robot_state_publisher From ca0072c183a64a3257a996e75437b196b03b7870 Mon Sep 17 00:00:00 2001 From: ad-daniel Date: Fri, 11 Jun 2021 12:14:27 +0200 Subject: [PATCH 53/72] add test --- CMakeLists.txt | 1 + src/complete_test.cpp | 21 +++++++++++++++++++++ 2 files changed, 22 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index f9b9460..ee4e353 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -89,6 +89,7 @@ find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs sensor_msgs messag lidar_get_info.srv lidar_get_layer_point_cloud.srv lidar_get_layer_range_image.srv + motor_get_multiplier.srv motor_set_control_pid.srv mouse_get_state.srv node_add_force_or_torque.srv diff --git a/src/complete_test.cpp b/src/complete_test.cpp index a495aa3..b933713 100644 --- a/src/complete_test.cpp +++ b/src/complete_test.cpp @@ -94,6 +94,7 @@ #include #include #include +#include #include #include #include @@ -2343,6 +2344,26 @@ int main(int argc, char **argv) { get_max_torque_client.shutdown(); time_step_client.call(time_step_srv); + ros::ServiceClient rotational_motor_get_multiplier_client; + webots_ros::get_float get_multiplier_srv; + rotational_motor_get_multiplier_client = n.serviceClient(model_name + "/rotational_motor/get_multiplier"); + + rotational_motor_get_multiplier_client.call(get_multiplier_srv); + ROS_INFO("Multiplier for rotational_motor is %f.", get_multiplier_srv.response.value); + + rotational_motor_get_multiplier_client.shutdown(); + time_step_client.call(time_step_srv); + + ros::ServiceClient linear_motor_get_multiplier_client; + webots_ros::get_float get_multiplier_srv; + linear_motor_get_multiplier_client = n.serviceClient(model_name + "/linear_motor/get_multiplier"); + + linear_motor_get_multiplier_client.call(get_multiplier_srv); + ROS_INFO("Multiplier for linear_motor is %f.", get_multiplier_srv.response.value); + + linear_motor_get_multiplier_client.shutdown(); + time_step_client.call(time_step_srv); + ros::ServiceClient set_motor_feedback_client; webots_ros::set_int motor_feedback_srv; ros::Subscriber sub_motor_feedback_32; From 3f1c29033da5e63169a9f3d740009dd8f7bdfd70 Mon Sep 17 00:00:00 2001 From: Ben Evans <69378550+sishamilton@users.noreply.github.com> Date: Tue, 15 Jun 2021 12:16:20 -0700 Subject: [PATCH 54/72] Fix typos in altimeter tests --- src/complete_test.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/complete_test.cpp b/src/complete_test.cpp index b15b531..d30ea7c 100644 --- a/src/complete_test.cpp +++ b/src/complete_test.cpp @@ -1035,18 +1035,18 @@ int main(int argc, char **argv) { //////////////////////////// ros::ServiceClient set_altimeter_client; - webots::set_int altimeter_srv; - ros::subscriber sub_altimeter_32; + webots_ros::set_int altimeter_srv; + ros::Subscriber sub_altimeter_32; - set_altimeter_client = n.serviceClinet(model_name + "/altimeter/enable"); + set_altimeter_client = n.serviceClient(model_name + "/altimeter/enable"); ros::ServiceClient sampling_period_altimeter_client; - webot_ros::get_int sampling_period_altimeter_srv; + webots_ros::get_int sampling_period_altimeter_srv; sampling_period_altimeter_client = n.serviceClient(model_name + "/altimeter/get_sampling_period"); altimeter_srv.request.value = 32; - if (set_altimeter_client.call(altimeter_srv) && altimeter_srv.response.succes) { + if (set_altimeter_client.call(altimeter_srv) && altimeter_srv.response.success) { ROS_INFO("Altimeter enabled."); sub_altimeter_32 = n.subscribe(model_name + "/altimeter/value", 1, altimeterCallback); callbackCalled = false; From 56dd25b580273fa503d8c399fa3c060050e18b58 Mon Sep 17 00:00:00 2001 From: Ben Evans <69378550+sishamilton@users.noreply.github.com> Date: Tue, 15 Jun 2021 12:17:06 -0700 Subject: [PATCH 55/72] Add altimeter node to robot --- worlds/complete_test.wbt | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/worlds/complete_test.wbt b/worlds/complete_test.wbt index 3080d81..25c6a1a 100644 --- a/worlds/complete_test.wbt +++ b/worlds/complete_test.wbt @@ -78,6 +78,8 @@ Robot { translation 0 0 0.1 rotation 1 0 0 1.5707963267948966 children [ + Altimeter { + } SliderJoint { device [ Brake { @@ -171,8 +173,9 @@ Robot { Camera { translation 0.14 0 0 model "camera model" + fieldOfView 1.33 focus Focus { - focalDistance 0.2 + focalDistance 0.33 focalLength 0.3 maxFocalDistance 0.4 minFocalDistance 0.1 @@ -222,7 +225,7 @@ Robot { } InertialUnit { name "inertial_unit" - noise 0.00001 + noise 1e-05 } LED { translation 0.1 0.05 0.1 @@ -284,7 +287,7 @@ Robot { "--clock" "--synchronize" ] - customData "INITIAL" + customData "OVERWRITTEN" supervisor TRUE battery [ 50, 50, 2 From 4f8fcf1a9c6c484a0b3b4b5d4cbe9a1f90ed54e5 Mon Sep 17 00:00:00 2001 From: ad-daniel Date: Tue, 15 Jun 2021 22:53:22 +0200 Subject: [PATCH 56/72] fix test --- CMakeLists.txt | 1 - src/complete_test.cpp | 2 -- worlds/complete_test.wbt | 2 ++ 3 files changed, 2 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ee4e353..f9b9460 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -89,7 +89,6 @@ find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs sensor_msgs messag lidar_get_info.srv lidar_get_layer_point_cloud.srv lidar_get_layer_range_image.srv - motor_get_multiplier.srv motor_set_control_pid.srv mouse_get_state.srv node_add_force_or_torque.srv diff --git a/src/complete_test.cpp b/src/complete_test.cpp index b933713..ae630dc 100644 --- a/src/complete_test.cpp +++ b/src/complete_test.cpp @@ -94,7 +94,6 @@ #include #include #include -#include #include #include #include @@ -2355,7 +2354,6 @@ int main(int argc, char **argv) { time_step_client.call(time_step_srv); ros::ServiceClient linear_motor_get_multiplier_client; - webots_ros::get_float get_multiplier_srv; linear_motor_get_multiplier_client = n.serviceClient(model_name + "/linear_motor/get_multiplier"); linear_motor_get_multiplier_client.call(get_multiplier_srv); diff --git a/worlds/complete_test.wbt b/worlds/complete_test.wbt index 3080d81..38fdb14 100644 --- a/worlds/complete_test.wbt +++ b/worlds/complete_test.wbt @@ -86,6 +86,7 @@ Robot { LinearMotor { name "linear_motor" maxPosition 100 + multiplier 1.5 } ] endPoint Solid { @@ -143,6 +144,7 @@ Robot { name "rotational_motor" acceleration 5 maxVelocity 2 + multiplier 2.5 } ] endPoint DEF TEST2 Solid { From 039668bfea5faae6f5e0ead986ebd0caaf989928 Mon Sep 17 00:00:00 2001 From: Stefania Pedrazzi Date: Wed, 16 Jun 2021 16:15:05 +0200 Subject: [PATCH 57/72] Add supervisor_node_set_joint_position API function (#68) * Add new service for supervisor_node_set_joint_position * Add node_set_joint_position test * Improve logged message --- CMakeLists.txt | 1 + src/complete_test.cpp | 30 +++++++++++++++++++++++++++++- srv/node_set_joint_position.srv | 5 +++++ worlds/complete_test.wbt | 6 ++++-- 4 files changed, 39 insertions(+), 3 deletions(-) create mode 100644 srv/node_set_joint_position.srv diff --git a/CMakeLists.txt b/CMakeLists.txt index f9b9460..84e6ba8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -115,6 +115,7 @@ find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs sensor_msgs messag node_remove.srv node_reset_functions.srv node_move_viewpoint.srv + node_set_joint_position.srv node_set_visibility.srv node_set_velocity.srv pen_set_ink_color.srv diff --git a/src/complete_test.cpp b/src/complete_test.cpp index ae630dc..253dae7 100644 --- a/src/complete_test.cpp +++ b/src/complete_test.cpp @@ -106,8 +106,8 @@ #include #include #include -#include #include +#include #include #include #include @@ -117,6 +117,7 @@ #include #include #include +#include #include #include #include @@ -3350,6 +3351,16 @@ int main(int argc, char **argv) { } else ROS_ERROR("could not get CONE node from DEF."); + supervisor_get_from_def_srv.request.name = "HINGE_JOINT"; + supervisor_get_from_def_srv.request.proto = 0; + supervisor_get_from_def_client.call(supervisor_get_from_def_srv); + uint64_t hinge_joint_node = 0; + if (supervisor_get_from_def_srv.response.node != 0) { + ROS_INFO("Got HINGE_JOINT node from DEF: %lu.", supervisor_get_from_def_srv.response.node); + hinge_joint_node = supervisor_get_from_def_srv.response.node; + } else + ROS_ERROR("could not get HINGE_JOINT node from DEF."); + supervisor_node_get_type_name_client.shutdown(); supervisor_get_from_def_client.shutdown(); supervisor_field_get_node_client.shutdown(); @@ -3486,6 +3497,23 @@ int main(int argc, char **argv) { node_get_parent_node_client.shutdown(); time_step_client.call(time_step_srv); + // node_set_joint_position + ros::ServiceClient node_set_joint_position_client; + webots_ros::node_set_joint_position node_set_joint_position_srv; + node_set_joint_position_client = + n.serviceClient(model_name + "/supervisor/node/set_joint_position"); + node_set_joint_position_srv.request.node = hinge_joint_node; + node_set_joint_position_srv.request.position = 0.6; + node_set_joint_position_srv.request.index = 1; + node_set_joint_position_client.call(node_set_joint_position_srv); + if (node_set_joint_position_srv.response.success == 1) + ROS_INFO("Joint position set successfully."); + else + ROS_ERROR("Failed to call service node_set_joint_position."); + + node_set_joint_position_client.shutdown(); + time_step_client.call(time_step_srv); + // movie ros::ServiceClient supervisor_movie_is_ready_client; webots_ros::node_get_status supervisor_movie_is_ready_srv; diff --git a/srv/node_set_joint_position.srv b/srv/node_set_joint_position.srv new file mode 100644 index 0000000..282eb06 --- /dev/null +++ b/srv/node_set_joint_position.srv @@ -0,0 +1,5 @@ +uint64 node +float64 position +int32 index +--- +bool success diff --git a/worlds/complete_test.wbt b/worlds/complete_test.wbt index 38fdb14..9b0a453 100644 --- a/worlds/complete_test.wbt +++ b/worlds/complete_test.wbt @@ -110,7 +110,9 @@ Robot { } } } - HingeJoint { + DEF HINGE_JOINT HingeJoint { + jointParameters HingeJointParameters { + } device [ PositionSensor { name "position_sensor" @@ -224,7 +226,7 @@ Robot { } InertialUnit { name "inertial_unit" - noise 0.00001 + noise 1e-05 } LED { translation 0.1 0.05 0.1 From 55281217a24c265da50316631fe9bdc7972e1b89 Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Thu, 17 Jun 2021 11:50:36 +0200 Subject: [PATCH 58/72] Add contact point tracking --- srv/node_enable_contact_points_tracking.srv | 5 +++++ srv/node_enable_pose_tracking.srv | 2 +- 2 files changed, 6 insertions(+), 1 deletion(-) create mode 100644 srv/node_enable_contact_points_tracking.srv diff --git a/srv/node_enable_contact_points_tracking.srv b/srv/node_enable_contact_points_tracking.srv new file mode 100644 index 0000000..00106fe --- /dev/null +++ b/srv/node_enable_contact_points_tracking.srv @@ -0,0 +1,5 @@ +uint64 node +int32 sampling_period +bool include_descendants +--- +int8 success diff --git a/srv/node_enable_pose_tracking.srv b/srv/node_enable_pose_tracking.srv index 3d0f1b4..cbf9ff9 100644 --- a/srv/node_enable_pose_tracking.srv +++ b/srv/node_enable_pose_tracking.srv @@ -1,5 +1,5 @@ uint64 node -uint64 sampling_period +int32 sampling_period uint64 from_node --- int8 success From 8269ee96c4f0cb51cc12a91885bf657efccf3c19 Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Thu, 17 Jun 2021 12:07:33 +0200 Subject: [PATCH 59/72] Add types --- msg/ContactPoint.msg | 2 ++ srv/node_disable_contact_point_tracking.srv | 4 ++++ srv/node_get_contact_points.srv | 4 ++++ 3 files changed, 10 insertions(+) create mode 100644 msg/ContactPoint.msg create mode 100644 srv/node_disable_contact_point_tracking.srv create mode 100644 srv/node_get_contact_points.srv diff --git a/msg/ContactPoint.msg b/msg/ContactPoint.msg new file mode 100644 index 0000000..bea3324 --- /dev/null +++ b/msg/ContactPoint.msg @@ -0,0 +1,2 @@ +geometry_msgs/Point point +int32 node_id diff --git a/srv/node_disable_contact_point_tracking.srv b/srv/node_disable_contact_point_tracking.srv new file mode 100644 index 0000000..654faaa --- /dev/null +++ b/srv/node_disable_contact_point_tracking.srv @@ -0,0 +1,4 @@ +uint64 node +bool include_descendants +--- +int8 success diff --git a/srv/node_get_contact_points.srv b/srv/node_get_contact_points.srv new file mode 100644 index 0000000..212cac0 --- /dev/null +++ b/srv/node_get_contact_points.srv @@ -0,0 +1,4 @@ +uint64 node +bool include_descendants +--- +webots_ros/ContactPoint[] points From f634c2bf0f114b4f0caf298ff3dcba8d95b39556 Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Thu, 17 Jun 2021 12:28:17 +0200 Subject: [PATCH 60/72] Better --- ...nt_tracking.srv => node_disable_contact_points_tracking.srv} | 0 srv/node_get_contact_points.srv | 2 +- 2 files changed, 1 insertion(+), 1 deletion(-) rename srv/{node_disable_contact_point_tracking.srv => node_disable_contact_points_tracking.srv} (100%) diff --git a/srv/node_disable_contact_point_tracking.srv b/srv/node_disable_contact_points_tracking.srv similarity index 100% rename from srv/node_disable_contact_point_tracking.srv rename to srv/node_disable_contact_points_tracking.srv diff --git a/srv/node_get_contact_points.srv b/srv/node_get_contact_points.srv index 212cac0..f52d36b 100644 --- a/srv/node_get_contact_points.srv +++ b/srv/node_get_contact_points.srv @@ -1,4 +1,4 @@ uint64 node bool include_descendants --- -webots_ros/ContactPoint[] points +webots_ros/ContactPoint[] contact_points From 40f133ed3d531972c42253b2b2234783c7437471 Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Thu, 17 Jun 2021 16:01:58 +0200 Subject: [PATCH 61/72] Add test --- src/complete_test.cpp | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/src/complete_test.cpp b/src/complete_test.cpp index 253dae7..592a902 100644 --- a/src/complete_test.cpp +++ b/src/complete_test.cpp @@ -98,6 +98,9 @@ #include #include #include +#include +#include +#include #include #include #include @@ -3129,6 +3132,21 @@ int main(int argc, char **argv) { supervisor_node_get_contact_point_node_client.shutdown(); time_step_client.call(time_step_srv); + // wb_supervisor_node_get_contact_points + ros::ServiceClient supervisor_node_get_contact_points_client; + webots_ros::node_get_contact_points supervisor_node_get_contact_points_srv; + supervisor_node_get_contact_points_client = n.serviceClient( + model_name + "/supervisor/node/get_contact_points"); + + supervisor_node_get_contact_points_srv.request.node = from_def_node; + supervisor_node_get_contact_points_srv.request.include_descendants = false; + supervisor_node_get_contact_points_client.call(supervisor_node_get_contact_points_srv); + ROS_INFO("From_def node got %d contact points.", + supervisor_node_get_contact_points_srv.response.contact_points.size()); + + supervisor_node_get_contact_points_client.shutdown(); + time_step_client.call(time_step_srv); + // test get_static_balance // if the node isn't a top Solid webots will throw a warning but still return true to ros ros::ServiceClient supervisor_node_get_static_balance_client; From ffd01387e2fa35a42222760a418f7acd2d1ca690 Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Thu, 17 Jun 2021 16:06:50 +0200 Subject: [PATCH 62/72] Tracking test --- src/complete_test.cpp | 65 +++++++++++++++++++++++++++++-------------- 1 file changed, 44 insertions(+), 21 deletions(-) diff --git a/src/complete_test.cpp b/src/complete_test.cpp index 592a902..9459c2b 100644 --- a/src/complete_test.cpp +++ b/src/complete_test.cpp @@ -68,8 +68,8 @@ #include #include #include -#include #include +#include #include #include #include @@ -97,12 +97,14 @@ #include #include #include -#include -#include -#include #include +#include +#include +#include +#include #include #include +#include #include #include #include @@ -111,8 +113,6 @@ #include #include #include -#include -#include #include #include #include @@ -2349,7 +2349,8 @@ int main(int argc, char **argv) { ros::ServiceClient rotational_motor_get_multiplier_client; webots_ros::get_float get_multiplier_srv; - rotational_motor_get_multiplier_client = n.serviceClient(model_name + "/rotational_motor/get_multiplier"); + rotational_motor_get_multiplier_client = + n.serviceClient(model_name + "/rotational_motor/get_multiplier"); rotational_motor_get_multiplier_client.call(get_multiplier_srv); ROS_INFO("Multiplier for rotational_motor is %f.", get_multiplier_srv.response.value); @@ -3031,21 +3032,16 @@ int main(int argc, char **argv) { // supervisor_node_get_pose ros::ServiceClient supervisor_node_get_pose_client; webots_ros::node_get_pose supervisor_node_get_pose_srv; - supervisor_node_get_pose_client = - n.serviceClient(model_name + "/supervisor/node/get_pose"); + supervisor_node_get_pose_client = n.serviceClient(model_name + "/supervisor/node/get_pose"); supervisor_node_get_pose_srv.request.from_node = from_def_node; supervisor_node_get_pose_srv.request.node = from_def_node; supervisor_node_get_pose_client.call(supervisor_node_get_pose_srv); - ROS_INFO("From_def get_pose rotation is:\nw=%f x=%f y=%f z=%f.", - supervisor_node_get_pose_srv.response.pose.rotation.w, - supervisor_node_get_pose_srv.response.pose.rotation.x, - supervisor_node_get_pose_srv.response.pose.rotation.y, + ROS_INFO("From_def get_pose rotation is:\nw=%f x=%f y=%f z=%f.", supervisor_node_get_pose_srv.response.pose.rotation.w, + supervisor_node_get_pose_srv.response.pose.rotation.x, supervisor_node_get_pose_srv.response.pose.rotation.y, supervisor_node_get_pose_srv.response.pose.rotation.z); - ROS_INFO("From_def get_pose translation is:\nx=%f y=%f z=%f.", - supervisor_node_get_pose_srv.response.pose.translation.x, - supervisor_node_get_pose_srv.response.pose.translation.y, - supervisor_node_get_pose_srv.response.pose.translation.z); + ROS_INFO("From_def get_pose translation is:\nx=%f y=%f z=%f.", supervisor_node_get_pose_srv.response.pose.translation.x, + supervisor_node_get_pose_srv.response.pose.translation.y, supervisor_node_get_pose_srv.response.pose.translation.z); supervisor_node_get_pose_client.shutdown(); time_step_client.call(time_step_srv); @@ -3132,17 +3128,44 @@ int main(int argc, char **argv) { supervisor_node_get_contact_point_node_client.shutdown(); time_step_client.call(time_step_srv); + // wb_supervisor_node_enable_contact_points_tracking + ros::ServiceClient supervisor_node_enable_contact_points_tracking_client; + webots_ros::node_get_contact_points supervisor_node_enable_contact_points_tracking_srv; + supervisor_node_enable_contact_points_tracking_client = n.serviceClient( + model_name + "/supervisor/node/enable_contact_points_tracking"); + + supervisor_node_enable_contact_points_tracking_srv.request.node = from_def_node; + supervisor_node_enable_contact_points_tracking_srv.request.include_descendants = false; + supervisor_node_enable_contact_points_tracking_client.call(supervisor_node_enable_contact_points_tracking_srv); + ROS_INFO("Contact point tracking is success = ", supervisor_node_enable_contact_points_tracking_srv.response.success); + + supervisor_node_enable_contact_points_tracking_client.shutdown(); + time_step_client.call(time_step_srv); + + // wb_supervisor_node_disable_contact_points_tracking + ros::ServiceClient supervisor_node_disable_contact_points_tracking_client; + webots_ros::node_get_contact_points supervisor_node_disable_contact_points_tracking_srv; + supervisor_node_disable_contact_points_tracking_client = n.serviceClient( + model_name + "/supervisor/node/disable_contact_points_tracking"); + + supervisor_node_disable_contact_points_tracking_srv.request.node = from_def_node; + supervisor_node_disable_contact_points_tracking_srv.request.include_descendants = false; + supervisor_node_disable_contact_points_tracking_client.call(supervisor_node_disable_contact_points_tracking_srv); + ROS_INFO("Contact point tracking is success = ", supervisor_node_disable_contact_points_tracking_srv.response.success); + + supervisor_node_disable_contact_points_tracking_client.shutdown(); + time_step_client.call(time_step_srv); + // wb_supervisor_node_get_contact_points ros::ServiceClient supervisor_node_get_contact_points_client; webots_ros::node_get_contact_points supervisor_node_get_contact_points_srv; - supervisor_node_get_contact_points_client = n.serviceClient( - model_name + "/supervisor/node/get_contact_points"); + supervisor_node_get_contact_points_client = + n.serviceClient(model_name + "/supervisor/node/get_contact_points"); supervisor_node_get_contact_points_srv.request.node = from_def_node; supervisor_node_get_contact_points_srv.request.include_descendants = false; supervisor_node_get_contact_points_client.call(supervisor_node_get_contact_points_srv); - ROS_INFO("From_def node got %d contact points.", - supervisor_node_get_contact_points_srv.response.contact_points.size()); + ROS_INFO("From_def node got %d contact points.", supervisor_node_get_contact_points_srv.response.contact_points.size()); supervisor_node_get_contact_points_client.shutdown(); time_step_client.call(time_step_srv); From 5e1ebdb412db2f43236e757bda35e0831df5c220 Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Thu, 17 Jun 2021 16:09:02 +0200 Subject: [PATCH 63/72] msgs --- CMakeLists.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 84e6ba8..02ef63d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -29,6 +29,7 @@ find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs sensor_msgs messag RadarTarget.msg RecognitionObject.msg StringStamped.msg + ContactPoint.msg ) ## Generate services in the 'srv' folder @@ -93,10 +94,13 @@ find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs sensor_msgs messag mouse_get_state.srv node_add_force_or_torque.srv node_add_force_with_offset.srv + node_disable_contact_points_tracking.srv node_disable_pose_tracking.srv + node_enable_contact_points_tracking.srv node_enable_pose_tracking.srv node_get_center_of_mass.srv node_get_contact_point.srv + node_get_contact_points.srv node_get_contact_point_node.srv node_get_field.srv node_get_id.srv From 579f945c4d97cf710d3ac37161635e8d4dd21aa4 Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Thu, 17 Jun 2021 16:11:09 +0200 Subject: [PATCH 64/72] Fix --- src/complete_test.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/complete_test.cpp b/src/complete_test.cpp index 9459c2b..6d3cd3d 100644 --- a/src/complete_test.cpp +++ b/src/complete_test.cpp @@ -3130,28 +3130,28 @@ int main(int argc, char **argv) { // wb_supervisor_node_enable_contact_points_tracking ros::ServiceClient supervisor_node_enable_contact_points_tracking_client; - webots_ros::node_get_contact_points supervisor_node_enable_contact_points_tracking_srv; + webots_ros::node_enable_contact_points_tracking supervisor_node_enable_contact_points_tracking_srv; supervisor_node_enable_contact_points_tracking_client = n.serviceClient( model_name + "/supervisor/node/enable_contact_points_tracking"); supervisor_node_enable_contact_points_tracking_srv.request.node = from_def_node; supervisor_node_enable_contact_points_tracking_srv.request.include_descendants = false; supervisor_node_enable_contact_points_tracking_client.call(supervisor_node_enable_contact_points_tracking_srv); - ROS_INFO("Contact point tracking is success = ", supervisor_node_enable_contact_points_tracking_srv.response.success); + ROS_INFO("Contact point tracking is success = %d", supervisor_node_enable_contact_points_tracking_srv.response.success); supervisor_node_enable_contact_points_tracking_client.shutdown(); time_step_client.call(time_step_srv); // wb_supervisor_node_disable_contact_points_tracking ros::ServiceClient supervisor_node_disable_contact_points_tracking_client; - webots_ros::node_get_contact_points supervisor_node_disable_contact_points_tracking_srv; + webots_ros::node_disable_contact_points_tracking supervisor_node_disable_contact_points_tracking_srv; supervisor_node_disable_contact_points_tracking_client = n.serviceClient( model_name + "/supervisor/node/disable_contact_points_tracking"); supervisor_node_disable_contact_points_tracking_srv.request.node = from_def_node; supervisor_node_disable_contact_points_tracking_srv.request.include_descendants = false; supervisor_node_disable_contact_points_tracking_client.call(supervisor_node_disable_contact_points_tracking_srv); - ROS_INFO("Contact point tracking is success = ", supervisor_node_disable_contact_points_tracking_srv.response.success); + ROS_INFO("Contact point tracking is success = %d", supervisor_node_disable_contact_points_tracking_srv.response.success); supervisor_node_disable_contact_points_tracking_client.shutdown(); time_step_client.call(time_step_srv); @@ -3165,7 +3165,7 @@ int main(int argc, char **argv) { supervisor_node_get_contact_points_srv.request.node = from_def_node; supervisor_node_get_contact_points_srv.request.include_descendants = false; supervisor_node_get_contact_points_client.call(supervisor_node_get_contact_points_srv); - ROS_INFO("From_def node got %d contact points.", supervisor_node_get_contact_points_srv.response.contact_points.size()); + ROS_INFO("From_def node got %lu contact points.", supervisor_node_get_contact_points_srv.response.contact_points.size()); supervisor_node_get_contact_points_client.shutdown(); time_step_client.call(time_step_srv); From b8493a87cdbc9a41c123ec957e2f93f48d334372 Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Thu, 17 Jun 2021 16:12:18 +0200 Subject: [PATCH 65/72] Pretty --- src/complete_test.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/complete_test.cpp b/src/complete_test.cpp index 6d3cd3d..d8b3e10 100644 --- a/src/complete_test.cpp +++ b/src/complete_test.cpp @@ -3137,7 +3137,7 @@ int main(int argc, char **argv) { supervisor_node_enable_contact_points_tracking_srv.request.node = from_def_node; supervisor_node_enable_contact_points_tracking_srv.request.include_descendants = false; supervisor_node_enable_contact_points_tracking_client.call(supervisor_node_enable_contact_points_tracking_srv); - ROS_INFO("Contact point tracking is success = %d", supervisor_node_enable_contact_points_tracking_srv.response.success); + ROS_INFO("Contact point tracking success = %d", supervisor_node_enable_contact_points_tracking_srv.response.success); supervisor_node_enable_contact_points_tracking_client.shutdown(); time_step_client.call(time_step_srv); @@ -3151,7 +3151,7 @@ int main(int argc, char **argv) { supervisor_node_disable_contact_points_tracking_srv.request.node = from_def_node; supervisor_node_disable_contact_points_tracking_srv.request.include_descendants = false; supervisor_node_disable_contact_points_tracking_client.call(supervisor_node_disable_contact_points_tracking_srv); - ROS_INFO("Contact point tracking is success = %d", supervisor_node_disable_contact_points_tracking_srv.response.success); + ROS_INFO("Contact point tracking success = %d", supervisor_node_disable_contact_points_tracking_srv.response.success); supervisor_node_disable_contact_points_tracking_client.shutdown(); time_step_client.call(time_step_srv); From 74c8b67117bbef632dc62d9633527cd68ea2e5fc Mon Sep 17 00:00:00 2001 From: sishamilton <69378550+sishamilton@users.noreply.github.com> Date: Thu, 17 Jun 2021 11:42:15 -0700 Subject: [PATCH 66/72] Update README.md to develop branch version --- README.md | 24 ++++-------------------- 1 file changed, 4 insertions(+), 20 deletions(-) diff --git a/README.md b/README.md index c37185d..e63199d 100644 --- a/README.md +++ b/README.md @@ -12,34 +12,18 @@ ROS tutorial for Webots: - https://www.cyberbotics.com/doc/guide/using-ros -## Acknowledgements +## Acknowledgement - rosin_logo
Supported by ROSIN - ROS-Industrial Quality-Assured Robot Software Components. More information: rosin-project.eu -eu_flag This project has received funding from the European Union’s Horizon 2020 -research and innovation programme under grant agreement no. 732287. - -
- - - opendr_logo -
- -Supported by OpenDR - Open Deep Learning Toolkit for Robotics. -More information: opendr.eu - -eu_flag - -This project has received funding from the European Union’s Horizon 2020 -research and innovation programme under grant agreement no. 871449. +research and innovation programme under grant agreement no. 732287. From 59bf51dddcf93d15e0ba0af78758e5827801ae6b Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Fri, 18 Jun 2021 17:12:53 +0200 Subject: [PATCH 67/72] Test --- package.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/package.xml b/package.xml index 8e5e237..4348c75 100644 --- a/package.xml +++ b/package.xml @@ -11,7 +11,7 @@ Apache 2.0 - moveit + moveit_ros catkin rospy roscpp @@ -25,7 +25,7 @@ sensor_msgs message_runtime tf - moveit + moveit_ros ros_control ros_controllers robot_state_publisher From 4003bae7742764aa3056c70d828cea8f73eac8fa Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Mon, 21 Jun 2021 08:43:10 +0200 Subject: [PATCH 68/72] Test --- package.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/package.xml b/package.xml index 4348c75..1d6b474 100644 --- a/package.xml +++ b/package.xml @@ -11,7 +11,7 @@ Apache 2.0 - moveit_ros + moveit_ros_planning_interface catkin rospy roscpp @@ -25,7 +25,7 @@ sensor_msgs message_runtime tf - moveit_ros + moveit_ros_planning_interface ros_control ros_controllers robot_state_publisher From 42f8f1ebcb27d3db6e9af2d0cdba7c400b52cc83 Mon Sep 17 00:00:00 2001 From: Stefania Pedrazzi Date: Thu, 24 Jun 2021 08:56:52 +0200 Subject: [PATCH 69/72] Add Supervisor functions to get field by index (#71) * Initial implementation of ROS API * Comlete ROS API * Remove debug change * Add missing services * Fix test --- CMakeLists.txt | 4 +- src/complete_test.cpp | 40 +++++++++++++++++-- src/panoramic_view_recorder.cpp | 2 +- ...d_get_type_name.srv => field_get_name.srv} | 0 srv/node_get_field_by_index.srv | 5 +++ srv/node_get_number_of_fields.srv | 4 ++ 6 files changed, 50 insertions(+), 5 deletions(-) rename srv/{field_get_type_name.srv => field_get_name.srv} (100%) create mode 100644 srv/node_get_field_by_index.srv create mode 100644 srv/node_get_number_of_fields.srv diff --git a/CMakeLists.txt b/CMakeLists.txt index 02ef63d..a315e1a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -63,7 +63,7 @@ find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs sensor_msgs messag field_get_rotation.srv field_get_string.srv field_get_type.srv - field_get_type_name.srv + field_get_name.srv field_get_vec2f.srv field_get_vec3f.srv field_import_node.srv @@ -103,8 +103,10 @@ find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs sensor_msgs messag node_get_contact_points.srv node_get_contact_point_node.srv node_get_field.srv + node_get_field_by_index.srv node_get_id.srv node_get_number_of_contact_points.srv + node_get_number_of_fields.srv node_get_name.srv node_get_orientation.srv node_get_pose.srv diff --git a/src/complete_test.cpp b/src/complete_test.cpp index d8b3e10..2025ed8 100644 --- a/src/complete_test.cpp +++ b/src/complete_test.cpp @@ -75,11 +75,11 @@ #include #include #include +#include #include #include #include #include -#include #include #include #include @@ -106,9 +106,11 @@ #include #include #include +#include #include #include #include +#include #include #include #include @@ -3260,6 +3262,38 @@ int main(int argc, char **argv) { supervisor_node_get_field_client.shutdown(); time_step_client.call(time_step_srv); + ros::ServiceClient wb_supervisor_node_get_number_of_fields_client; + webots_ros::node_get_number_of_fields wb_supervisor_node_get_number_of_fields_srv; + wb_supervisor_node_get_number_of_fields_client = + n.serviceClient(model_name + "/supervisor/node/get_field_by_index"); + wb_supervisor_node_get_number_of_fields_srv.request.node = root_node; + wb_supervisor_node_get_number_of_fields_srv.request.proto = 0; + wb_supervisor_node_get_number_of_fields_client.call(wb_supervisor_node_get_number_of_fields_srv); + ROS_INFO("World's root Group node have %d fields.", wb_supervisor_node_get_number_of_fields_srv.response.value); + wb_supervisor_node_get_number_of_fields_client.shutdown(); + time_step_client.call(time_step_srv); + + ros::ServiceClient wb_supervisor_node_get_field_by_index_client; + webots_ros::node_get_field_by_index wb_supervisor_node_get_field_by_index_srv; + wb_supervisor_node_get_field_by_index_client = + n.serviceClient(model_name + "/supervisor/node/get_field_by_index"); + wb_supervisor_node_get_field_by_index_srv.request.node = root_node; + wb_supervisor_node_get_field_by_index_srv.request.index = 0; + wb_supervisor_node_get_field_by_index_client.call(wb_supervisor_node_get_field_by_index_srv); + ROS_INFO("World's root Group node has a single 'children' field: %d.", + wb_supervisor_node_get_field_by_index_srv.response.field == field); + wb_supervisor_node_get_field_by_index_client.shutdown(); + time_step_client.call(time_step_srv); + + ros::ServiceClient supervisor_field_get_name_client; + webots_ros::field_get_name supervisor_field_get_name_srv; + supervisor_field_get_name_client = n.serviceClient(model_name + "/supervisor/field/get_name"); + supervisor_field_get_name_srv.request.field = field; + supervisor_field_get_name_client.call(supervisor_field_get_name_srv); + ROS_INFO("World's children field has name '%s'.", supervisor_field_get_name_srv.response.name.c_str()); + supervisor_field_get_name_client.shutdown(); + time_step_client.call(time_step_srv); + ros::ServiceClient supervisor_field_get_type_client; webots_ros::field_get_type supervisor_field_get_type_srv; supervisor_field_get_type_client = n.serviceClient(model_name + "/supervisor/field/get_type"); @@ -3272,9 +3306,9 @@ int main(int argc, char **argv) { time_step_client.call(time_step_srv); ros::ServiceClient supervisor_field_get_type_name_client; - webots_ros::field_get_type_name supervisor_field_get_type_name_srv; + webots_ros::field_get_name supervisor_field_get_type_name_srv; supervisor_field_get_type_name_client = - n.serviceClient(model_name + "/supervisor/field/get_type_name"); + n.serviceClient(model_name + "/supervisor/field/get_type_name"); supervisor_field_get_type_name_srv.request.field = field; supervisor_field_get_type_name_client.call(supervisor_field_get_type_name_srv); diff --git a/src/panoramic_view_recorder.cpp b/src/panoramic_view_recorder.cpp index 9b3bf02..7f69439 100644 --- a/src/panoramic_view_recorder.cpp +++ b/src/panoramic_view_recorder.cpp @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include #include diff --git a/srv/field_get_type_name.srv b/srv/field_get_name.srv similarity index 100% rename from srv/field_get_type_name.srv rename to srv/field_get_name.srv diff --git a/srv/node_get_field_by_index.srv b/srv/node_get_field_by_index.srv new file mode 100644 index 0000000..4a93929 --- /dev/null +++ b/srv/node_get_field_by_index.srv @@ -0,0 +1,5 @@ +uint64 node +uint32 index +bool proto +--- +uint64 field diff --git a/srv/node_get_number_of_fields.srv b/srv/node_get_number_of_fields.srv new file mode 100644 index 0000000..9533e56 --- /dev/null +++ b/srv/node_get_number_of_fields.srv @@ -0,0 +1,4 @@ +uint64 node +bool proto +--- +int32 value From cbe1ed1aef37864f0802fc138cb3e9cda23a12e2 Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Tue, 29 Jun 2021 16:30:19 +0200 Subject: [PATCH 70/72] Revert non-related changes --- worlds/complete_test.wbt | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/worlds/complete_test.wbt b/worlds/complete_test.wbt index e6ea571..0c82524 100644 --- a/worlds/complete_test.wbt +++ b/worlds/complete_test.wbt @@ -177,9 +177,8 @@ Robot { Camera { translation 0.14 0 0 model "camera model" - fieldOfView 1.33 focus Focus { - focalDistance 0.33 + focalDistance 0.2 focalLength 0.3 maxFocalDistance 0.4 minFocalDistance 0.1 @@ -291,7 +290,7 @@ Robot { "--clock" "--synchronize" ] - customData "OVERWRITTEN" + customData "INITIAL" supervisor TRUE battery [ 50, 50, 2 From ed784fb90ae0f1b7d44ce3bb47d9035812809d2a Mon Sep 17 00:00:00 2001 From: Stefania Pedrazzi Date: Tue, 29 Jun 2021 19:48:26 +0200 Subject: [PATCH 71/72] Improve camera recognition objects message (#72) * Publish single message containing all the recognized objects * Adjust test * Remove header from RecognitionObject message * Update RecognitionObjects message --- CMakeLists.txt | 1 + msg/RecognitionObject.msg | 1 - msg/RecognitionObjects.msg | 2 ++ src/complete_test.cpp | 10 +++++++--- 4 files changed, 10 insertions(+), 4 deletions(-) create mode 100644 msg/RecognitionObjects.msg diff --git a/CMakeLists.txt b/CMakeLists.txt index a315e1a..d6d4442 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -28,6 +28,7 @@ find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs sensor_msgs messag Int8Stamped.msg RadarTarget.msg RecognitionObject.msg + RecognitionObjects.msg StringStamped.msg ContactPoint.msg ) diff --git a/msg/RecognitionObject.msg b/msg/RecognitionObject.msg index 25906e1..1e357d8 100644 --- a/msg/RecognitionObject.msg +++ b/msg/RecognitionObject.msg @@ -1,4 +1,3 @@ -Header header geometry_msgs/Vector3 position geometry_msgs/Quaternion orientation geometry_msgs/Vector3 position_on_image diff --git a/msg/RecognitionObjects.msg b/msg/RecognitionObjects.msg new file mode 100644 index 0000000..8011f19 --- /dev/null +++ b/msg/RecognitionObjects.msg @@ -0,0 +1,2 @@ +Header header +webots_ros/RecognitionObject[] objects diff --git a/src/complete_test.cpp b/src/complete_test.cpp index 2025ed8..556ffa4 100644 --- a/src/complete_test.cpp +++ b/src/complete_test.cpp @@ -34,6 +34,7 @@ #include #include #include +#include #include #include #include "ros/ros.h" @@ -180,9 +181,12 @@ void cameraCallback(const sensor_msgs::Image::ConstPtr &values) { callbackCalled = true; } -void cameraRecognitionCallback(const webots_ros::RecognitionObject::ConstPtr &object) { - ROS_INFO("Camera recognition saw a '%s' (time: %d:%d).", object->model.c_str(), object->header.stamp.sec, - object->header.stamp.nsec); +void cameraRecognitionCallback(const webots_ros::RecognitionObjects::ConstPtr &objects) { + const int objectsCount = objects->objects.size(); + ROS_INFO("Camera recognition saw %d objects (time: %d:%d).", objectsCount, objects->header.stamp.sec, + objects->header.stamp.nsec); + for (int i = 0; i < objectsCount; i++) + ROS_INFO(" Recognition object %d: '%s'.", i, objects->objects[i].model.c_str()); callbackCalled = true; } From 0811e80f069d66c0e2ff63ea9eddcf195aac311a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Benjamin=20D=C3=A9l=C3=A8ze?= Date: Tue, 13 Jul 2021 11:56:23 +0200 Subject: [PATCH 72/72] Update e-puck_line.wbt --- worlds/e-puck_line.wbt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/worlds/e-puck_line.wbt b/worlds/e-puck_line.wbt index ea5a426..13fc50d 100644 --- a/worlds/e-puck_line.wbt +++ b/worlds/e-puck_line.wbt @@ -16,7 +16,7 @@ RectangleArena { floorAppearance PBRAppearance { baseColorMap ImageTexture { url [ - "https://raw.githubusercontent.com/cyberbotics/webots/R2021a/projects/robots/gctronic/e-puck/worlds/textures/floor.png" + "https://raw.githubusercontent.com/cyberbotics/webots/R2021b/projects/robots/gctronic/e-puck/worlds/textures/floor.png" ] } roughness 1