Skip to content

Rbs se3 viz #153

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 5 commits into
base: master
Choose a base branch
from
+1,331 −82
Open

Rbs se3 viz #153

Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
67bd987
added visualizer for RigidBodyStateSE3 and Wrench
mmaurus Sep 7, 2020
ae55026
using osgviz PrimitivesFactory for displaying arrow and circular arro…
mmaurus Jan 19, 2021
e6b0c18
added test script vor wrench visualization
mmaurus Feb 2, 2021
3374a61
fixed intendation and empty lines
mmaurus Feb 2, 2021
3b0eaea
addded test for WrenchVisualization
mmaurus Feb 4, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
126 changes: 126 additions & 0 deletions test/viz/test_wrench_visualization.rb
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,126 @@
require_relative 'helpers'
require 'orocos'
Orocos.load_typekit 'base'

module Vizkit
describe 'WrenchVisualization' do
include TestHelpers

before do
register_widget(@vizkit3d_widget =
Vizkit.default_loader.create_plugin("vizkit3d::Vizkit3DWidget"))
@viz = @vizkit3d_widget.createPlugin('base', 'WrenchVisualization')

@wrench = Types.base.samples.Wrench.new
@wrench.frame_id = "test_frame"
@wrench.force = Types.base.Vector3d.new(1,1,1)
@wrench.torque = Types.base.Vector3d.new(-1,-1,1)
@viz.updateData(@wrench)

@viz.setResolution(1.0)

@vizkit3d_widget.setCameraEye(5, 4, 5)
@vizkit3d_widget.setCameraLookAt(0, 0, 0)
@vizkit3d_widget.show
end

# describe "updateData(Wrench)" do
# @wrench = Types.base.samples.Wrench.new
# @wrench.frame_id = "test_frame"
# @wrench.force = Types.base.Vector3d.new(1,1,1)
# @wrench.torque = Types.base.Vector3d.new(-1,-1,1)

# it "updates wrench" do
# @viz.updateData(@wrench)
# confirm 'Force should be displayed as a red arrow from (0,0,0) to (1,1,1) and torque as a green arrow from (0,0,0) to (-1,-1,1) with a circular arrow rotating counter-clockwise around axis'
# end

# it "updates wrench" do
# @wrench.force = Types.base.Vector3d.new(1,0,0)
# @wrench.torque = Types.base.Vector3d.new(0,1,0)
# @viz.updateData(@wrench)
# confirm 'Force should be displayed as a red arrow from (0,0,0) to (1,0,0) and torque as a green arrow from (0,0,0) to (0,1,0) with a circular arrow rotating counter-clockwise around axis'
# end

# it "updates wrench" do
# @wrench.force = Types.base.Vector3d.new(0,1,0)
# @wrench.torque = Types.base.Vector3d.new(0,0,1)
# @viz.updateData(@wrench)
# confirm 'Force should be displayed as a red arrow from (0,0,0) to (0,1,0) and torque as a green arrow from (0,0,0) to (0,0,1) with a circular arrow rotating counter-clockwise around axis'
# end

# it "updates wrench" do
# @wrench.force = Types.base.Vector3d.new(0,0,1)
# @wrench.torque = Types.base.Vector3d.new(1,0,0)
# @viz.updateData(@wrench)
# confirm 'Force should be displayed as a red arrow from (0,0,0) to (0,0,1) and torque as a green arrow from (0,0,0) to (1,0,0) with a circular arrow rotating counter-clockwise around axis'
# end

# it "updates wrench" do
# @wrench.force = Types.base.Vector3d.new(0,0,0)
# @wrench.torque = Types.base.Vector3d.new(0,0,0)
# @viz.updateData(@wrench)
# confirm 'Nothing should be displayed'
# end

# it "updates wrench" do
# @wrench.force = Types.base.Vector3d.new(0.1,0,0)
# @wrench.torque = Types.base.Vector3d.new(0.3,0,0)
# @viz.updateData(@wrench)
# confirm 'Force should be displayed as a red arrow from (0,0,0) to (0,0,0.1) and torque as a green arrow from (0,0,0) to (0.3,0,0) with a circular arrow rotating counter-clockwise around axis. Both arrows should be displayed without the arrow head'
# end
# end

describe "setSeperateAxesTorque" do
it "displays torque axis component" do
torque = Types.base.Vector3d.new(1,1,1)
@viz.setTorque(torque)
@viz.setSeperateAxesTorque(true)
confirm 'Torque should be displayed as three arrows along axes each with length 1 and a circular arrow rotating counter-clockwise'

end

it "displays torque" do
torque = Types.base.Vector3d.new(1,1,1)
@viz.setTorque(torque)
@viz.setSeperateAxesTorque(false)
confirm 'Torque should be displayed as one arrows starting from (0,0,0) to (1,1,1) with circular arrow rotating counter-clockwise'
end
end

describe "setSeperateAxesForce" do
it "displays force axis component" do
force = Types.base.Vector3d.new(1,1,1)
@viz.setForce(force)
@viz.setSeperateAxesForce(true)
confirm 'Force should be displayed as three arrows along axes each with length 1'
end

it "displays force" do
force = Types.base.Vector3d.new(1,1,1)
@viz.setForce(force)
@viz.setSeperateAxesForce(false)
confirm 'Force should be displayed as one arrows starting from (0,0,0) to (1,1,1)'
end
end

# describe "setResolution" do
# wrench = Types.base.samples.Wrench.new
# wrench.frame_id = "test_frame"
# wrench.force = Types.base.Vector3d.new(1,0,0)
# wrench.torque = Types.base.Vector3d.new(0,1,0)

# it "sets resolution to 1" do
# @viz.updateData(wrench)
# @viz.setResolution(1.0)
# confirm "Force and torque will be displayed as arrows from (0,0,0) to (1,0,0) / (0,1,0)"
# end
# it "it sets resolution to 0.1" do
# @viz.updateData(wrench)
# @viz.setResolution(0.1)
# confirm "Force and torque will be displayed as arrows from (0,0,0) to (0.1,0,0) / (0,0.1,0) without arrow head"
# end
# end
end
end

8 changes: 7 additions & 1 deletion viz/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -22,6 +22,9 @@ rock_vizkit_plugin(base-viz
SonarVisualization.cpp
PointcloudVisualization.cpp
DepthMapVisualization.cpp
RigidBodyStateSE3Visualization.cpp
WrenchModel.cpp
WrenchVisualization.cpp
${OPTIONAL_CPP}
HEADERS
Uncertainty.hpp
@@ -37,8 +40,11 @@ rock_vizkit_plugin(base-viz
SonarVisualization.hpp
PointcloudVisualization.hpp
DepthMapVisualization.hpp
RigidBodyStateSE3Visualization.hpp
WrenchModel.hpp
WrenchVisualization.hpp
${OPTIONAL_HPP}
DEPS base-types
LIBS ${Boost_SYSTEM_LIBRARY}
DEPS_PKGCONFIG base-logging
DEPS_PKGCONFIG base-logging PrimitivesFactory osgViz
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I really believe we should consider other alternatives before adding osgViz as a "hard" dependency in base/types.

This is also breaking rock-core's build.

@doudou

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

In practice, base/types already depends on gui/vizkit3d which depends on osgViz. The (optional) dependency should definitely be explicitly added to the manifest, though.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes, that will be a problem in constrained environments (i.e embedded computers running iodrivers_base based drivers). Can we please don't do that?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The keyword was optional ;-). The dependency on osgViz that this PR adds is also optional as the whole viz/ folder is disabled by the Rock CMake macros if vizkit3d is not available or if ROCK_VIZ_ENABLED is set to OFF.

base/types already depends optionally on gui/vizkit3d. In constrained environments, you usually exclude gui/.* which removes that particular dependency

Copy link
Contributor

@g-arjones g-arjones Feb 9, 2021 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

the whole viz/ folder is disabled by the Rock CMake macros if vizkit3d is not available or if ROCK_VIZ_ENABLED is set to OFF

I missed that, sorry. I guess I was misled by the breaking build (which is probably breaking for some other reason I didn't look into). Did this pass on your CI?

Copy link
Contributor

@g-arjones g-arjones Feb 9, 2021 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more. 

-- Checking for module 'PrimitivesFactory'
--   No package 'PrimitivesFactory' found
CMake Error at /usr/share/cmake-3.10/Modules/FindPkgConfig.cmake:415 (message):
  A required package was not found
Call Stack (most recent call first):
  /usr/share/cmake-3.10/Modules/FindPkgConfig.cmake:593 (_pkg_check_modules_internal)
  /buildbot/squidbot-build/build/install/base/cmake/share/rock/cmake/Rock.cmake:288 (pkg_check_modules)
  /buildbot/squidbot-build/build/install/base/cmake/share/rock/cmake/Rock.cmake:383 (rock_find_pkgconfig)
  /buildbot/squidbot-build/build/install/base/cmake/share/rock/cmake/Rock.cmake:649 (rock_target_definition)
  /buildbot/squidbot-build/build/install/base/cmake/share/rock/cmake/Rock.cmake:811 (rock_library_common)
  viz/CMakeLists.txt:11 (rock_vizkit_plugin)

This is when building base/types

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't know what PrimitivesFactory is or where it comes from. I was looking at osgViz since it was what you mentioned in your comment.

Didn't check CI yet.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

PrimitivesFactory is a module in osgViz

)
174 changes: 93 additions & 81 deletions viz/PluginLoader.cpp
View file
Original file line number Diff line number Diff line change
@@ -11,94 +11,106 @@
#include "SonarVisualization.hpp"
#include "DepthMapVisualization.hpp"
#include "DistanceImageVisualization.hpp"
#include "RigidBodyStateSE3Visualization.hpp"
#include "WrenchVisualization.hpp"

namespace vizkit3d {
class QtPluginVizkitBase : public vizkit3d::VizkitPluginFactory {
private:
public:

QtPluginVizkitBase() {
}

/**
* Returns a list of all available visualization plugins.
* @return list of plugin names
*/
virtual QStringList* getAvailablePlugins() const
{
QStringList *pluginNames = new QStringList();
pluginNames->push_back("WaypointVisualization");
pluginNames->push_back("TrajectoryVisualization");
pluginNames->push_back("MotionCommandVisualization");
pluginNames->push_back("RigidBodyStateVisualization");
pluginNames->push_back("BodyStateVisualization");
pluginNames->push_back("LaserScanVisualization");
pluginNames->push_back("SonarGroundDistanceVisualization");
pluginNames->push_back("PointcloudVisualization");
pluginNames->push_back("SonarBeamVisualization");
pluginNames->push_back("SonarVisualization");
pluginNames->push_back("DepthMapVisualization");
pluginNames->push_back("DistanceImageVisualization");
return pluginNames;
}

virtual QObject* createPlugin(QString const& pluginName)

QtPluginVizkitBase() {
}

/**
* Returns a list of all available visualization plugins.
* @return list of plugin names
*/
virtual QStringList* getAvailablePlugins() const
{
QStringList *pluginNames = new QStringList();
pluginNames->push_back("WaypointVisualization");
pluginNames->push_back("TrajectoryVisualization");
pluginNames->push_back("MotionCommandVisualization");
pluginNames->push_back("RigidBodyStateVisualization");
pluginNames->push_back("BodyStateVisualization");
pluginNames->push_back("LaserScanVisualization");
pluginNames->push_back("SonarGroundDistanceVisualization");
pluginNames->push_back("PointcloudVisualization");
pluginNames->push_back("SonarBeamVisualization");
pluginNames->push_back("SonarVisualization");
pluginNames->push_back("DepthMapVisualization");
pluginNames->push_back("DistanceImageVisualization");
pluginNames->push_back("RigidBodyStateSE3Visualization");
pluginNames->push_back("WrenchVisualization");
return pluginNames;
}

virtual QObject* createPlugin(QString const& pluginName)
{
vizkit3d::VizPluginBase* plugin = 0;
if (pluginName == "WaypointVisualization")
{
plugin = new vizkit3d::WaypointVisualization();
}
else if (pluginName == "MotionCommandVisualization")
{
plugin = new vizkit3d::MotionCommandVisualization();
}
else if (pluginName == "TrajectoryVisualization")
{
plugin = new vizkit3d::TrajectoryVisualization();
}
else if (pluginName == "RigidBodyStateVisualization")
{
plugin = new vizkit3d::RigidBodyStateVisualization();
}
else if (pluginName == "BodyStateVisualization")
{
plugin = new vizkit3d::BodyStateVisualization();
}
else if (pluginName == "LaserScanVisualization")
{
plugin = new vizkit3d::LaserScanVisualization();
}
else if (pluginName == "SonarGroundDistanceVisualization")
{
vizkit3d::VizPluginBase* plugin = 0;
if (pluginName == "WaypointVisualization")
{
plugin = new vizkit3d::WaypointVisualization();
}
else if (pluginName == "MotionCommandVisualization")
{
plugin = new vizkit3d::MotionCommandVisualization();
}
else if (pluginName == "TrajectoryVisualization")
{
plugin = new vizkit3d::TrajectoryVisualization();
}
else if (pluginName == "RigidBodyStateVisualization")
{
plugin = new vizkit3d::RigidBodyStateVisualization();
}
else if (pluginName == "BodyStateVisualization")
{
plugin = new vizkit3d::BodyStateVisualization();
}
else if (pluginName == "LaserScanVisualization")
{
plugin = new vizkit3d::LaserScanVisualization();
}
else if (pluginName == "SonarGroundDistanceVisualization")
{
plugin = new vizkit3d::SonarGroundDistanceVisualization();
}
else if (pluginName == "PointcloudVisualization")
{
plugin = new vizkit3d::PointcloudVisualization();
}
else if (pluginName == "SonarBeamVisualization")
{
plugin = new vizkit3d::SonarBeamVisualization();
}
else if (pluginName == "SonarVisualization")
{
plugin = new vizkit3d::SonarVisualization();
}
else if (pluginName == "DepthMapVisualization")
{
plugin = new vizkit3d::DepthMapVisualization();
}
else if (pluginName == "DistanceImageVisualization")
{
plugin = new vizkit3d::DistanceImageVisualization();
}
plugin = new vizkit3d::SonarGroundDistanceVisualization();
}
else if (pluginName == "PointcloudVisualization")
{
plugin = new vizkit3d::PointcloudVisualization();
}
else if (pluginName == "SonarBeamVisualization")
{
plugin = new vizkit3d::SonarBeamVisualization();
}
else if (pluginName == "SonarVisualization")
{
plugin = new vizkit3d::SonarVisualization();
}
else if (pluginName == "DepthMapVisualization")
{
plugin = new vizkit3d::DepthMapVisualization();
}
else if (pluginName == "DistanceImageVisualization")
{
plugin = new vizkit3d::DistanceImageVisualization();
}
else if (pluginName == "RigidBodyStateSE3Visualization")
{
plugin = new vizkit3d::RigidBodyStateSE3Visualization();
}
else if (pluginName == "WrenchVisualization")
{
plugin = new vizkit3d::WrenchVisualization();
}

if (plugin)
{
return plugin;
}
return NULL;
if (plugin)
{
return plugin;
}
return NULL;
};
};
Q_EXPORT_PLUGIN2(QtPluginVizkitBase, QtPluginVizkitBase)
Loading