Merge develop into master branch (#145)

* Merge `master` into `develop` (#129)

* Add controllers directory to CMakeLists (#122)

* Feature worlds using extern proto (#117)

* adding EXTERNPROTOs to the webots_ros worlds

* resolving merge conflicts

* resolving merge conflicts

* resolving merge conflicts

* adding the EXTERNPROTOS in the webots_ros worlds

* adding the EXTERNPROTOS in the webots_ros worlds

* comment

* Update worlds/tiago.wbt

Co-authored-by: Yannick Goumaz <[email protected]>

* Create update_urls.py (#123)

* Fix hard code urls (#124)

* Hard coded URLs

* Delete update_urls.py

* Fixed ROS service (#126)

* Fix import node test (#127)

* Fixed ROS service

* Removed import node from complete_test

Co-authored-by: Yannick Goumaz <[email protected]>
Co-authored-by: carikaser <[email protected]>
Co-authored-by: Olivier Michel <[email protected]>

* Merge master into develop (#132)

* Add controllers directory to CMakeLists (#122)

* Feature worlds using extern proto (#117)

* adding EXTERNPROTOs to the webots_ros worlds

* resolving merge conflicts

* resolving merge conflicts

* resolving merge conflicts

* adding the EXTERNPROTOS in the webots_ros worlds

* adding the EXTERNPROTOS in the webots_ros worlds

* comment

* Update worlds/tiago.wbt

Co-authored-by: Yannick Goumaz <[email protected]>

* Create update_urls.py (#123)

* Fix hard code urls (#124)

* Hard coded URLs

* Delete update_urls.py

* Fixed ROS service (#126)

* Fix import node test (#127)

* Fixed ROS service

* Removed import node from complete_test

* Add missing declarations (#130)

Co-authored-by: carikaser <[email protected]>
Co-authored-by: Olivier Michel <[email protected]>
Co-authored-by: ad-daniel <[email protected]>

* Update node_disable_contact_points_tracking.srv (#138) (#139)

* Merge master into develop (#141)

* Update node_disable_contact_points_tracking.srv (#138)

* Update complete_test.cpp (#140)

* Feature VacuumGripper device (#144)

* Test VacuumCup API

* Rename device

* Fix test

* Fix srv type

* Update src/complete_test.cpp

---------

Co-authored-by: Olivier Michel <[email protected]>

* Update TIAGo PROTO name (#143)

---------

Co-authored-by: Yannick Goumaz <[email protected]>
Co-authored-by: ad-daniel <[email protected]>
Co-authored-by: carikaser <[email protected]>
Co-authored-by: Olivier Michel <[email protected]>
Co-authored-by: ad-daniel <[email protected]>

Author: 5 people

src/complete_test.cpp

@@ -161,6 +161,7 @@ static double GPSSpeedVectorValues[3] = {0, 0, 0};
 static double GyroValues[3] = {0, 0, 0};
 static double inertialUnitValues[4] = {0, 0, 0, 0};
 static double touchSensorValues[3] = {0, 0, 0};
+static bool vacuumGripperPresence = 0;
 static bool callbackCalled = false;
 
 ros::ServiceClient time_step_client;
@@ -368,6 +369,14 @@ void touchSensor3DCallback(const geometry_msgs::WrenchStamped::ConstPtr &values)
   callbackCalled = true;
 }
 
+void vacuumGripperCallback(const webots_ros::BoolStamped::ConstPtr &value) {
+  vacuumGripperPresence = value->data;
+
+  ROS_INFO("VacuumGripper presence: %d (time: %d:%d).", vacuumGripperPresence, value->header.stamp.sec,
+           value->header.stamp.nsec);
+  callbackCalled = true;
+}
+
 void receiverCallback(const webots_ros::StringStamped::ConstPtr &value) {
   char *message = const_cast<char *>(value->data.c_str());
   ROS_INFO("Received a message %s.", message);
@@ -3025,6 +3034,75 @@ int main(int argc, char **argv) {
   sampling_period_touch_sensor_client.shutdown();
   time_step_client.call(time_step_srv);
 
+  /////////////////////////////////
+  // VACUUM GRIPPER METHODS TEST //
+  /////////////////////////////////
+
+  ros::ServiceClient vacuum_gripper_enable_presence_client;
+  webots_ros::set_int vacuum_gripper_srv;
+  ros::Subscriber sub_vacuum_gripper;
+  vacuum_gripper_enable_presence_client =
+    n.serviceClient<webots_ros::set_int>(model_name + "/vacuum_gripper/presence_sensor/enable");
+
+  vacuum_gripper_srv.request.value = 32;
+  if (vacuum_gripper_enable_presence_client.call(vacuum_gripper_srv) && vacuum_gripper_srv.response.success) {
+    ROS_INFO("Vacuum gripper's presence sensor enabled.");
+    sub_vacuum_gripper = n.subscribe(model_name + "/vacuum_gripper/presence", 1, vacuumGripperCallback);
+    callbackCalled = false;
+    while (sub_vacuum_gripper.getNumPublishers() == 0 || !callbackCalled) {
+      ros::spinOnce();
+      time_step_client.call(time_step_srv);
+    }
+  } else {
+    if (!vacuum_gripper_srv.response.success)
+      ROS_ERROR("Sampling period is not valid.");
+    ROS_ERROR("Failed to enable vacuum gripper's presence sensor.");
+    return 1;
+  }
+
+  sub_vacuum_gripper.shutdown();
+
+  time_step_client.call(time_step_srv);
+  time_step_client.call(time_step_srv);
+  time_step_client.call(time_step_srv);
+
+  vacuum_gripper_srv.request.value = 0;
+  if (vacuum_gripper_enable_presence_client.call(vacuum_gripper_srv) && vacuum_gripper_srv.response.success)
+    ROS_INFO("Vacuum gripper's presence sensor disabled.");
+  else {
+    if (!vacuum_gripper_srv.response.success)
+      ROS_ERROR("Sampling period is not valid.");
+    ROS_ERROR("Failed to disable vacuum gripper's presence sensor.");
+    return 1;
+  }
+
+  ros::ServiceClient vacuum_gripper_turn_on_client;
+  webots_ros::set_bool vacuum_gripper_turn_on_srv;
+  vacuum_gripper_turn_on_client = n.serviceClient<webots_ros::set_bool>(model_name + "/vacuum_gripper/turn_on");
+
+  vacuum_gripper_turn_on_srv.request.value = true;
+  if (vacuum_gripper_turn_on_client.call(vacuum_gripper_turn_on_srv) && vacuum_gripper_turn_on_srv.response.success)
+    ROS_INFO("Vacuum gripper has been turned on.");
+  else
+    ROS_INFO("Failed to turn on vacuum gripper.");
+
+  vacuum_gripper_turn_on_client.shutdown();
+  vacuum_gripper_enable_presence_client.shutdown();
+  time_step_client.call(time_step_srv);
+
+  ros::ServiceClient vacuum_gripper_is_on_client;
+  webots_ros::get_bool vacuum_gripper_is_on_srv;
+  vacuum_gripper_is_on_client = n.serviceClient<webots_ros::get_bool>(model_name + "/vacuum_gripper/is_on");
+
+  if (vacuum_gripper_is_on_client.call(vacuum_gripper_is_on_srv))
+    ROS_INFO("Vacuum gripper is on: %d", vacuum_gripper_is_on_srv.response.value);
+  else
+    ROS_INFO("Failed to call is_on for vacuum gripper.");
+
+  vacuum_gripper_turn_on_client.shutdown();
+  vacuum_gripper_enable_presence_client.shutdown();
+  time_step_client.call(time_step_srv);
+
   /////////////////////////////
   // SUPERVISOR METHODS TEST //
   /////////////////////////////

worlds/complete_test.wbt

@@ -283,6 +283,8 @@ Robot {
     TouchSensor {
       name "touch_sensor"
     }
+    VacuumGripper {
+    }
     Shape {
       appearance PBRAppearance {
         baseColor 0.8 0 0.0117647

worlds/tiago.wbt

@@ -1,50 +1,50 @@
-#VRML_SIM R2023a utf8
+#VRML_SIM R2023b utf8
 
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/backgrounds/protos/TexturedBackground.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/floors/protos/RectangleArena.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/cabinet/protos/Cabinet.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/cabinet/protos/CabinetHandle.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/kitchen/oven/protos/Oven.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/kitchen/fridge/protos/Fridge.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/tables/protos/Table.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/chairs/protos/Chair.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/plants/protos/PottedTree.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/plants/protos/BunchOfSunFlowers.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/living_room_furniture/protos/Sofa.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/living_room_furniture/protos/Carpet.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/living_room_furniture/protos/Armchair.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/apartment_structure/protos/Door.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/apartment_structure/protos/DoorLever.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/apartment_structure/protos/Wall.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/apartment_structure/protos/Window.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/kitchen/components/protos/Sink.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/kitchen/components/protos/Worktop.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/kitchen/components/protos/HotPlate.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/chairs/protos/WoodenChair.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/drinks/protos/Can.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/fruits/protos/FruitBowl.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/school_furniture/protos/Book.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/robots/pal_robotics/tiago_steel/protos/TiagoSteel.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/lights/protos/CeilingLight.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/lights/protos/FloorLight.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/paintings/protos/PortraitPainting.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/paintings/protos/LandscapePainting.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/create_wall/protos/CreateWall.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/appearances/protos/PaintedWood.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/appearances/protos/Parquetry.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/appearances/protos/Roughcast.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/fruits/protos/Orange.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/fruits/protos/Apple.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/tables/protos/Desk.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/robots/gctronic/e-puck/protos/E-puck.proto"
-EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/robots/gctronic/e-puck/protos/E-puckGroundSensors.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/backgrounds/protos/TexturedBackground.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/floors/protos/RectangleArena.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/cabinet/protos/Cabinet.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/cabinet/protos/CabinetHandle.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/kitchen/oven/protos/Oven.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/kitchen/fridge/protos/Fridge.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/tables/protos/Table.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/chairs/protos/Chair.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/plants/protos/PottedTree.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/plants/protos/BunchOfSunFlowers.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/living_room_furniture/protos/Sofa.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/living_room_furniture/protos/Carpet.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/living_room_furniture/protos/Armchair.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/apartment_structure/protos/Door.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/apartment_structure/protos/DoorLever.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/apartment_structure/protos/Wall.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/apartment_structure/protos/Window.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/kitchen/components/protos/Sink.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/kitchen/components/protos/Worktop.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/kitchen/components/protos/HotPlate.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/chairs/protos/WoodenChair.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/drinks/protos/Can.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/fruits/protos/FruitBowl.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/school_furniture/protos/Book.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/robots/pal_robotics/tiago/protos/Tiago.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/lights/protos/CeilingLight.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/lights/protos/FloorLight.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/paintings/protos/PortraitPainting.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/paintings/protos/LandscapePainting.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/create_wall/protos/CreateWall.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/appearances/protos/PaintedWood.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/appearances/protos/Parquetry.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/appearances/protos/Roughcast.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/fruits/protos/Orange.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/fruits/protos/Apple.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/tables/protos/Desk.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/robots/gctronic/e-puck/protos/E-puck.proto"
+EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/robots/gctronic/e-puck/protos/E-puckGroundSensors.proto"
 
 WorldInfo {
   info [
-    "TIAGo Steel simulation with MoveIt and differential drive integration (ROS)."
+    "TIAGo simulation with MoveIt and differential drive integration (ROS)."
   ]
-  title "TIAGo Steel"
+  title "TIAGo"
   basicTimeStep 16
   contactProperties [
     ContactProperties {
@@ -534,7 +534,7 @@ LandscapePainting {
 CreateWall {
   translation 3.53117 -3.235 0
 }
-TiagoSteel {
+Tiago {
   translation 2.194 -4.717 0.095
   controller "ros"
   controllerArgs [