robotology · lucapa17 · Aug 11, 2023 · Aug 9, 2023 · Aug 9, 2023 · Aug 10, 2023
diff --git a/.github/workflows/apt.yml b/.github/workflows/apt.yml
@@ -48,6 +48,7 @@ jobs:
  key: source-deps-${{ runner.os }}-os-${{ matrix.os }}-build-type-${{ matrix.build_type }}-ycm-${{ matrix.ycm_tag }}-yarp-${{ matrix.yarp_tag }}
 
  - name: Source-based Dependencies
+ if: steps.cache-source-deps.outputs.cache-hit != 'true'
  run: |
  # YCM
  git clone https://github.com/robotology/ycm
@@ -65,7 +66,12 @@ jobs:
  mkdir -p build
  cd build
  cmake -GNinja -DCMAKE_PREFIX_PATH=${GITHUB_WORKSPACE}/install/deps -DCMAKE_INSTALL_PREFIX=${GITHUB_WORKSPACE}/install/deps ..
- cmake --build . --config ${{ matrix.build_type }} --target install 
+ cmake --build . --config ${{ matrix.build_type }} --target install
+
+ - name: Configure Environment variables for Source-based Dependencies
+ run: |
+ # Configure environment
+ echo "YARP_DATA_DIRS=${GITHUB_WORKSPACE}/install/deps/share/yarp" >> $GITHUB_ENV
 
  # ===================
  # CMAKE-BASED PROJECT

diff --git a/.github/workflows/conda-forge.yml b/.github/workflows/conda-forge.yml
@@ -67,6 +67,7 @@ jobs:
  cmake --build . --config ${{ matrix.build_type }} 
 
  - name: Test
+ if: contains(matrix.os, 'macos') || contains(matrix.os, 'ubuntu')
  shell: bash -l {0}
  run: |
  cd build

diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -12,9 +12,18 @@ set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR})
 gz_find_package(gz-sim7 REQUIRED)
 set(GZ_SIM_VER ${gz-sim7_VERSION_MAJOR})
 
+set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_BINDIR}")
+set(CMAKE_LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}")
+set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}")
+
 # Support shared libraries on Windows
 set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)
 
 add_subdirectory(libraries)
 add_subdirectory(plugins)
+option(BUILD_TESTING "Create tests using CMake" ON)
+if(BUILD_TESTING)
+ include(CTest)
+ add_subdirectory(tests)
+endif()
 
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
@@ -0,0 +1,2 @@
+add_subdirectory(forcetorque)
+
diff --git a/tests/forcetorque/CMakeLists.txt b/tests/forcetorque/CMakeLists.txt
@@ -0,0 +1,24 @@
+# Fetch and configure GTest
+include(FetchContent)
+FetchContent_Declare(
+ googletest
+ URL https://github.com/google/googletest/archive/609281088cfefc76f9d0ce82e1ff6c30cc3591e5.zip
+)
+set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
+FetchContent_MakeAvailable(googletest)
+
+add_executable(ForceTorqueTest ForceTorqueTest.cc)
+target_link_libraries(ForceTorqueTest
+ gtest_main
+ gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}
+ gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER}
+ ${YARP_LIBRARIES}
+)
+add_test(NAME ForceTorqueTest
+ COMMAND ForceTorqueTest)
+
+set(_env_vars)
+list(APPEND _env_vars "GZ_SIM_SYSTEM_PLUGIN_PATH=$<TARGET_FILE_DIR:GazeboYarpForceTorque>")
+
+set_tests_properties(ForceTorqueTest PROPERTIES
+ ENVIRONMENT "${_env_vars}")
diff --git a/tests/forcetorque/ForceTorqueTest.cc b/tests/forcetorque/ForceTorqueTest.cc
@@ -0,0 +1,59 @@
+#include <gtest/gtest.h>
+#include <yarp/dev/PolyDriver.h>
+#include <yarp/dev/MultipleAnalogSensorsInterfaces.h>
+#include <yarp/os/Network.h>
+#include <gz/sim/TestFixture.hh>
+
+TEST(ForceTorqueTest, PluginTest)
+{
+ yarp::os::NetworkBase::setLocalMode(true);
+
+ // Maximum verbosity helps with debugging
+ gz::common::Console::SetVerbosity(4);
+
+ // Instantiate test fixture
+ gz::sim::TestFixture fixture("../../../tests/forcetorque/model.sdf");
+
+ int iterations = 1000;
+ fixture.Server()->Run(/*_blocking=*/true, iterations, /*_paused=*/false);
+
+ yarp::os::Property option;
+ option.put("device","multipleanalogsensorsclient");
+ option.put("remote","/forcetorque");
+ option.put("timeout",1.0);
+ option.put("local", "/ForceTorqueTest");
+ yarp::dev::PolyDriver driver;
+
+ ASSERT_TRUE(driver.open(option));
+
+ yarp::dev::ISixAxisForceTorqueSensors* isixaxis = nullptr;
+
+ ASSERT_TRUE(driver.view(isixaxis));
+
+ fixture.Server()->Run(/*_blocking=*/true, iterations, /*_paused=*/false);
+ yarp::sig::Vector measure(6);
+ std::string sensorName;
+ double timestamp;
+
+ isixaxis->getSixAxisForceTorqueSensorName(0, sensorName);
+ size_t maxNrOfReadingAttempts = 20;
+ bool readSuccessful = false;
+ for (size_t i=0; (i < maxNrOfReadingAttempts) && !readSuccessful ; i++)
+ {
+ readSuccessful = isixaxis->getSixAxisForceTorqueSensorMeasure(0, measure, timestamp);
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
+ }
+ ASSERT_TRUE(readSuccessful);
+
+ yarp::dev::MAS_status status;
+ status = isixaxis->getSixAxisForceTorqueSensorStatus(0);
+ ASSERT_EQ(status, yarp::dev::MAS_OK);
+
+ EXPECT_NEAR(measure(0), 0.0, 1e-2);
+ EXPECT_NEAR(measure(1), 0.0, 1e-2);
+ EXPECT_NEAR(measure(2), -9.8*10, 1e-2);
+ EXPECT_NEAR(measure(3), 0.0, 1e-2);
+ EXPECT_NEAR(measure(4), 0.0, 1e-2);
+ EXPECT_NEAR(measure(5), 0.0, 1e-2);
+ EXPECT_GT(timestamp, 0.0);
+}
diff --git a/tests/forcetorque/forcetorque_nws.xml b/tests/forcetorque/forcetorque_nws.xml
@@ -0,0 +1,15 @@
+<?xml version="1.0" encoding="UTF-8" ?>
+<!DOCTYPE robot PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
+
+<robot name="forcetorque" portprefix="forcetorque" build="0" xmlns:xi="http://www.w3.org/2001/XInclude">
+ <devices>
+ <device name="forcetorque_nws_yarp" type="multipleanalogsensorsserver">
+ <param name="name"> /forcetorque </param>
+ <param name="period"> 100 </param>
+ <action phase="startup" level="5" type="attach">
+ <param name="device"> forcetorque_plugin_device </param>
+ </action>
+ <action phase="shutdown" level="5" type="detach" />
+ </device>
+ </devices>
+</robot>
diff --git a/tests/forcetorque/model.sdf b/tests/forcetorque/model.sdf
@@ -0,0 +1,156 @@
+<?xml version="1.0" ?>
+
+<sdf version="1.6">
+ <world name="sensors">
+ <physics name="1ms" type="ignored">
+ <max_step_size>0.001</max_step_size>
+ <real_time_factor>1.0</real_time_factor>
+ </physics>
+ <plugin
+ filename="gz-sim-physics-system"
+ name="gz::sim::systems::Physics">
+ </plugin>
+ <plugin
+ filename="gz-sim-forcetorque-system"
+ name="gz::sim::systems::ForceTorque">
+ </plugin>
+ <plugin
+ filename="gz-sim-user-commands-system"
+ name="gz::sim::systems::UserCommands">
+ </plugin>
+ <plugin
+ filename="gz-sim-scene-broadcaster-system"
+ name="gz::sim::systems::SceneBroadcaster">
+ </plugin>
+
+ <light type="directional" name="sun">
+ <cast_shadows>true</cast_shadows>
+ <pose>0 0 10 0 0 0</pose>
+ <diffuse>0.8 0.8 0.8 1</diffuse>
+ <specular>0.2 0.2 0.2 1</specular>
+ <attenuation>
+ <range>1000</range>
+ <constant>0.9</constant>
+ <linear>0.01</linear>
+ <quadratic>0.001</quadratic>
+ </attenuation>
+ <direction>-0.5 0.1 -0.9</direction>
+ </light>
+
+ <model name="ground_plane">
+ <static>true</static>
+ <link name="link">
+ <collision name="collision">
+ <geometry>
+ <plane>
+ <normal>0 0 1</normal>
+ <size>100 100</size>
+ </plane>
+ </geometry>
+ </collision>
+ <visual name="visual">
+ <geometry>
+ <plane>
+ <normal>0 0 1</normal>
+ <size>100 100</size>
+ </plane>
+ </geometry>
+ <material>
+ <ambient>0.8 0.8 0.8 1</ambient>
+ <diffuse>0.8 0.8 0.8 1</diffuse>
+ <specular>0.8 0.8 0.8 1</specular>
+ </material>
+ </visual>
+ </link>
+ </model>
+
+ <model name="force_torque_example">
+ <plugin name="GazeboYarpForceTorque" filename="GazeboYarpForceTorque">
+ <yarpConfigurationString>(yarpDeviceName forcetorque_plugin_device) (jointName joint_12) (sensorName force_torque_sensor)</yarpConfigurationString>
+ </plugin>
+ <link name="link_1">
+ <inertial>
+ <pose>0 0 0.05 0 0 0</pose>
+ <inertia>
+ <ixx>0.020000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>0.020000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>0.020000</izz>
+ </inertia>
+ <mass>10.000000</mass>
+ </inertial>
+ <visual name="visual_cylinder">
+ <pose>0 0 0.05 0 0 0</pose>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.100000</length>
+ </cylinder>
+ </geometry>
+ </visual>
+ <collision name="collision_cylinder">
+ <pose>0 0 0.05 0 0 0</pose>
+ <max_contacts>250</max_contacts>
+ <geometry>
+ <cylinder>
+ <radius>0.100000</radius>
+ <length>0.100000</length>
+ </cylinder>
+ </geometry>
+ </collision>
+ </link>
+ <link name="link_2">
+ <pose>0 0 0.15 0 0 0</pose>
+ <inertial>
+ <pose>0 0 0.0 0 0 0</pose>
+ <inertia>
+ <ixx>0.020000</ixx>
+ <ixy>0.000000</ixy>
+ <ixz>0.000000</ixz>
+ <iyy>0.020000</iyy>
+ <iyz>0.000000</iyz>
+ <izz>0.020000</izz>
+ </inertia>
+ <mass>10.000000</mass>
+ </inertial>
+ <visual name="visual_box">
+ <pose>0 0 0.0 0 0 0</pose>
+ <geometry>
+ <box>
+ <size>0.1 0.1 0.1</size>
+ </box>
+ </geometry>
+ </visual>
+ <collision name="collision_box">
+ <pose>0 0 0.0 0 0 0</pose>
+ <geometry>
+ <box>
+ <size>0.1 0.1 0.1</size>
+ </box>
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="joint_01" type="fixed">
+ <parent>world</parent>
+ <child>link_1</child>
+ <pose>0 0 0.0 0 0 0</pose>
+ </joint>
+ <joint name="joint_12" type="revolute">
+ <parent>link_1</parent>
+ <child>link_2</child>
+ <pose>0 0 0 0 0 0</pose>
+ <sensor name="force_torque_sensor" type="force_torque">
+ <always_on>true</always_on>
+ <visualize>true</visualize>
+ <update_rate>30</update_rate>
+ </sensor>
+ </joint>
+ <plugin name="GazeboYarpRobotInterface" filename="GazeboYarpRobotInterface">
+ <yarpRobotInterfaceConfigurationFile>../../../tests/forcetorque/forcetorque_nws.xml</yarpRobotInterfaceConfigurationFile>
+ </plugin>
+ </model>
+ </world>
+</sdf>
diff --git a/tutorial/camera/model/README.md b/tutorial/camera/model/README.md
@@ -8,9 +8,9 @@
  ~~~
 - 2nd terminal:
  ~~~
- export LIBGL_ALWAYS_SOFTWARE=1 
- cd ../../../build
- export GZ_SIM_SYSTEM_PLUGIN_PATH=`pwd`
+ export LIBGL_ALWAYS_SOFTWARE=1
+ cd build
+ export GZ_SIM_SYSTEM_PLUGIN_PATH=`pwd`:`pwd`/lib
  cd ../tutorial/camera/model
  gz sim model.sdf
  ~~~

diff --git a/tutorial/camera/model/model.sdf b/tutorial/camera/model/model.sdf
@@ -65,7 +65,7 @@
  </model>
 
  <model name="camera_model">
- <plugin name="GazeboYarpCamera" filename="plugins/camera/libGazeboYarpCamera.so">
+ <plugin name="GazeboYarpCamera" filename="GazeboYarpCamera">
  <yarpConfigurationString>(yarpDeviceName camera_plugin_device) (parentLinkName link_1) (sensorName camera_sensor) (display_timestamp)</yarpConfigurationString>
  </plugin>
  <static>true</static>
@@ -95,7 +95,7 @@
  <visualize>true</visualize>
  </sensor>
  </link>
- <plugin name="GazeboYarpRobotInterface" filename="plugins/robotinterface/libGazeboYarpRobotInterface.so">
+ <plugin name="GazeboYarpRobotInterface" filename="GazeboYarpRobotInterface">
  <yarpRobotInterfaceConfigurationFile>camera_nws.xml</yarpRobotInterfaceConfigurationFile>
  </plugin>
  </model>

diff --git a/tutorial/camera/model_horizontal_flip/README.md b/tutorial/camera/model_horizontal_flip/README.md
@@ -9,8 +9,8 @@
 - 2nd terminal:
  ~~~
  export LIBGL_ALWAYS_SOFTWARE=1 
- cd ../../../build
- export GZ_SIM_SYSTEM_PLUGIN_PATH=`pwd`
+ cd build
+ export GZ_SIM_SYSTEM_PLUGIN_PATH=`pwd`:`pwd`/lib
  cd ../tutorial/camera/model_horizontal_flip
  gz sim model.sdf
  ~~~

diff --git a/tutorial/camera/model_horizontal_flip/model.sdf b/tutorial/camera/model_horizontal_flip/model.sdf
@@ -65,7 +65,7 @@
  </model>
 
  <model name="camera_model">
- <plugin name="GazeboYarpCamera" filename="plugins/camera/libGazeboYarpCamera.so">
+ <plugin name="GazeboYarpCamera" filename="GazeboYarpCamera">
  <yarpConfigurationString>(yarpDeviceName camera_plugin_device) (parentLinkName link_1) (sensorName camera_sensor) (display_timestamp) (horizontal_flip)</yarpConfigurationString>
  </plugin>
  <static>true</static>
@@ -95,7 +95,7 @@
  <visualize>true</visualize>
  </sensor>
  </link>
- <plugin name="GazeboYarpRobotInterface" filename="plugins/robotinterface/libGazeboYarpRobotInterface.so">
+ <plugin name="GazeboYarpRobotInterface" filename="GazeboYarpRobotInterface">
  <yarpRobotInterfaceConfigurationFile>camera_nws.xml</yarpRobotInterfaceConfigurationFile>
  </plugin>
  </model>

diff --git a/tutorial/camera/model_vertical_flip/README.md b/tutorial/camera/model_vertical_flip/README.md
@@ -9,8 +9,8 @@
 - 2nd terminal:
  ~~~
  export LIBGL_ALWAYS_SOFTWARE=1 
- cd ../../../build
- export GZ_SIM_SYSTEM_PLUGIN_PATH=`pwd`
+ cd build
+ export GZ_SIM_SYSTEM_PLUGIN_PATH=`pwd`:`pwd`/lib
  cd ../tutorial/camera/model_vertical_flip
  gz sim model.sdf
  ~~~

diff --git a/tutorial/camera/model_vertical_flip/model.sdf b/tutorial/camera/model_vertical_flip/model.sdf
@@ -65,7 +65,7 @@
  </model>
 
  <model name="camera_model">
- <plugin name="GazeboYarpCamera" filename="plugins/camera/libGazeboYarpCamera.so">
+ <plugin name="GazeboYarpCamera" filename="GazeboYarpCamera">
  <yarpConfigurationString>(yarpDeviceName camera_plugin_device) (parentLinkName link_1) (sensorName camera_sensor) (display_timestamp) (vertical_flip)</yarpConfigurationString>
  </plugin>
  <static>true</static>
@@ -95,7 +95,7 @@
  <visualize>true</visualize>
  </sensor>
  </link>
- <plugin name="GazeboYarpRobotInterface" filename="plugins/robotinterface/libGazeboYarpRobotInterface.so">
+ <plugin name="GazeboYarpRobotInterface" filename="GazeboYarpRobotInterface">
  <yarpRobotInterfaceConfigurationFile>camera_nws.xml</yarpRobotInterfaceConfigurationFile>
  </plugin>
  </model>