#include <tesseract_common/macros.h>
#include <gtest/gtest.h>
#include <algorithm>
#include <vector>
#include <omp.h>
#include <cmath>
#include <fstream>
#include <console_bridge/console.h>
#include <tesseract_urdf/urdf_parser.h>
#include <tesseract_srdf/srdf_model.h>
#include <tesseract_geometry/impl/box.h>
#include <tesseract_geometry/impl/sphere.h>
#include <tesseract_common/resource_locator.h>
#include <tesseract_common/manipulator_info.h>
#include <tesseract_common/utils.h>
#include <tesseract_scene_graph/graph.h>
#include <tesseract_scene_graph/link.h>
#include <tesseract_scene_graph/joint.h>
#include <tesseract_scene_graph/scene_state.h>
#include <tesseract_state_solver/state_solver.h>
#include <tesseract_state_solver/kdl/kdl_state_solver.h>
#include <tesseract_kinematics/core/joint_group.h>
#include <tesseract_kinematics/core/kinematic_group.h>
#include <tesseract_collision/core/discrete_contact_manager.h>
#include <tesseract_collision/core/continuous_contact_manager.h>
#include <tesseract_collision/core/types.h>
#include <tesseract_environment/environment.h>
#include <tesseract_environment/command.h>
#include <tesseract_environment/commands.h>
#include <tesseract_environment/utils.h>

Include dependency graph for tesseract_environment_unit.cpp:

Enumerations
enum	EnvironmentInitType { EnvironmentInitType::OBJECT, EnvironmentInitType::STRING, EnvironmentInitType::FILEPATH }

Functions
void	checkProcessInterpolatedResults (const std::vector< tesseract_collision::ContactResultMap > &contacts)

void	checkProcessInterpolatedResultsNoTime0 (const tesseract_collision::ContactResultMap &contacts)
	Verify that no contact results is at Time0. More...

void	checkProcessInterpolatedResultsNoTime1 (const tesseract_collision::ContactResultMap &contacts)
	Verify that no contact results is at Time1. More...

long	getContactCount (const std::vector< tesseract_collision::ContactResultMap > &contacts)
	Get the total number of contacts. More...

Environment::Ptr	getEnvironment (EnvironmentInitType init_type=EnvironmentInitType::OBJECT)

Environment::Ptr	getEnvironmentURDFOnly (EnvironmentInitType init_type)

SceneGraph::Ptr	getSceneGraph (const tesseract_common::ResourceLocator &locator)

std::string	getSceneGraphString (const tesseract_common::ResourceLocator &locator)

SRDFModel::Ptr	getSRDFModel (const SceneGraph &scene_graph, const tesseract_common::ResourceLocator &locator)

std::string	getSRDFModelString (const tesseract_common::ResourceLocator &locator)

SceneGraph::Ptr	getSubSceneGraph ()

bool	hasProcessInterpolatedResultsTime0 (const tesseract_collision::ContactResultMap &contacts)
	Verify that the results contain Time0. More...

bool	hasProcessInterpolatedResultsTime1 (const tesseract_collision::ContactResultMap &contacts)
	Verify that the results contain Time1. More...

int	main (int argc, char **argv)

void	runCompareSceneStates (const SceneState &base_state, const SceneState &compare_state)

void	runCompareStateSolver (const StateSolver &base_solver, StateSolver &comp_solver)

void	runGetLinkTransformsTest (Environment &env)

Eigen::Isometry3d	tcpCallback (const tesseract_common::ManipulatorInfo &mi)

	TEST (TesseractEnvironmentUnit, checkTrajectoryUnit)

	TEST (TesseractEnvironmentUnit, EnvAddAndRemoveAllowedCollisionCommandUnit)

	TEST (TesseractEnvironmentUnit, EnvAddandRemoveLink)

	TEST (TesseractEnvironmentUnit, EnvAddandRemoveTrajectoryLink)

	TEST (TesseractEnvironmentUnit, EnvAddKinematicsInformationCommandUnit)

	TEST (TesseractEnvironmentUnit, EnvAddSceneGraphCommandUnit)

	TEST (TesseractEnvironmentUnit, EnvApplyCommandsStateSolverCompareUnit)

	TEST (TesseractEnvironmentUnit, EnvChangeCollisionMarginsCommandUnit)

	TEST (TesseractEnvironmentUnit, EnvChangeJointLimitsCommandUnit)

	TEST (TesseractEnvironmentUnit, EnvChangeJointOriginCommandUnit)

	TEST (TesseractEnvironmentUnit, EnvChangeLinkCollisionEnabledCommandUnit)

	TEST (TesseractEnvironmentUnit, EnvChangeLinkOriginCommandUnit)

	TEST (TesseractEnvironmentUnit, EnvChangeLinkVisibilityCommandUnit)

	TEST (TesseractEnvironmentUnit, EnvChangeNameUnit)

	TEST (TesseractEnvironmentUnit, EnvClone)

	TEST (TesseractEnvironmentUnit, EnvCloneContactManagerUnit)

	TEST (TesseractEnvironmentUnit, EnvCurrentStatePreservedWhenEnvChanges)

	TEST (TesseractEnvironmentUnit, EnvFindTCPUnit)

	TEST (TesseractEnvironmentUnit, EnvInitFailuresUnit)

	TEST (TesseractEnvironmentUnit, EnvInitURDFOnlyUnit)

	TEST (TesseractEnvironmentUnit, EnvMoveJointCommandUnit)

	TEST (TesseractEnvironmentUnit, EnvMoveLinkCommandUnit)

	TEST (TesseractEnvironmentUnit, EnvMultithreadedApplyCommandsTest)

	TEST (TesseractEnvironmentUnit, EnvResetUnit)

	TEST (TesseractEnvironmentUnit, EnvSetActiveContinuousContactManagerCommandUnit)

	TEST (TesseractEnvironmentUnit, EnvSetActiveDiscreteContactManagerCommandUnit)

	TEST (TesseractEnvironmentUnit, EnvSetState)

	TEST (TesseractEnvironmentUnit, EnvSetState2)

	TEST (TesseractEnvironmentUnit, getActiveLinkNamesRecursiveUnit)

Enumeration Type Documentation

◆ EnvironmentInitType

enum EnvironmentInitType

strong

Enumerator
OBJECT
STRING
FILEPATH

Definition at line 158 of file tesseract_environment_unit.cpp.

Function Documentation

◆ checkProcessInterpolatedResults()

void checkProcessInterpolatedResults ( const std::vector< tesseract_collision::ContactResultMap > & contacts )

Definition at line 2760 of file tesseract_environment_unit.cpp.

◆ checkProcessInterpolatedResultsNoTime0()

void checkProcessInterpolatedResultsNoTime0 ( const tesseract_collision::ContactResultMap & contacts )

Verify that no contact results is at Time0.

Parameters

contacts The contacts to check

Definition at line 2796 of file tesseract_environment_unit.cpp.

◆ checkProcessInterpolatedResultsNoTime1()

void checkProcessInterpolatedResultsNoTime1 ( const tesseract_collision::ContactResultMap & contacts )

Verify that no contact results is at Time1.

Parameters

contacts The contacts to check

Definition at line 2812 of file tesseract_environment_unit.cpp.

◆ getContactCount()

long getContactCount ( const std::vector< tesseract_collision::ContactResultMap > & contacts )

Get the total number of contacts.

Definition at line 2865 of file tesseract_environment_unit.cpp.

◆ getEnvironment()

Environment::Ptr getEnvironment ( EnvironmentInitType init_type = EnvironmentInitType::OBJECT )

Definition at line 165 of file tesseract_environment_unit.cpp.

◆ getEnvironmentURDFOnly()

Environment::Ptr getEnvironmentURDFOnly ( EnvironmentInitType init_type )

Definition at line 352 of file tesseract_environment_unit.cpp.

◆ getSceneGraph()

SceneGraph::Ptr getSceneGraph ( const tesseract_common::ResourceLocator & locator )

Definition at line 57 of file tesseract_environment_unit.cpp.

◆ getSceneGraphString()

std::string getSceneGraphString ( const tesseract_common::ResourceLocator & locator )

Definition at line 73 of file tesseract_environment_unit.cpp.

◆ getSRDFModel()

SRDFModel::Ptr getSRDFModel	(	const SceneGraph &	scene_graph,
		const tesseract_common::ResourceLocator &	locator
	)

Definition at line 63 of file tesseract_environment_unit.cpp.

◆ getSRDFModelString()

std::string getSRDFModelString ( const tesseract_common::ResourceLocator & locator )

Definition at line 83 of file tesseract_environment_unit.cpp.

◆ getSubSceneGraph()

SceneGraph::Ptr getSubSceneGraph ( )

Definition at line 93 of file tesseract_environment_unit.cpp.

◆ hasProcessInterpolatedResultsTime0()

bool hasProcessInterpolatedResultsTime0 ( const tesseract_collision::ContactResultMap & contacts )

Verify that the results contain Time0.

Parameters

contacts The contacts to check

Definition at line 2828 of file tesseract_environment_unit.cpp.

◆ hasProcessInterpolatedResultsTime1()

bool hasProcessInterpolatedResultsTime1 ( const tesseract_collision::ContactResultMap & contacts )

Verify that the results contain Time1.

Parameters

contacts The contacts to check

Definition at line 2848 of file tesseract_environment_unit.cpp.

◆ main()

int main	(	int	argc,
		char **	argv
	)

Definition at line 4648 of file tesseract_environment_unit.cpp.

◆ runCompareSceneStates()

void runCompareSceneStates	(	const SceneState &	base_state,
		const SceneState &	compare_state
	)

Definition at line 1900 of file tesseract_environment_unit.cpp.

◆ runCompareStateSolver()

void runCompareStateSolver	(	const StateSolver &	base_solver,
		StateSolver &	comp_solver
	)

Definition at line 1922 of file tesseract_environment_unit.cpp.

◆ runGetLinkTransformsTest()

void runGetLinkTransformsTest ( Environment & env )

Definition at line 125 of file tesseract_environment_unit.cpp.

◆ tcpCallback()

Eigen::Isometry3d tcpCallback ( const tesseract_common::ManipulatorInfo & mi )

Definition at line 48 of file tesseract_environment_unit.cpp.

◆ TEST() [1/29]

TEST	(	TesseractEnvironmentUnit	,
		checkTrajectoryUnit
	)

<

Todo:: we are getting 9 instead of 6 because duplicate contacts at states

<

Todo:: we are getting 9 instead of 6 because duplicate contacts at states

<

Todo:: we are getting 7 instead of 4 because duplicate contacts at states

<

Todo:: we are getting 9 instead of 6 because duplicate contacts at states

<

Todo:: we are getting 9 instead of 6 because duplicate contacts at states

<

Todo:: we are getting 7 instead of 4 because duplicate contacts at states

<

Todo:: we are getting 288 instead of 285 because duplicate contacts at states

<

Todo:: we are getting 9 instead of 6 because duplicate contacts at states

<

Todo:: we are getting 7 instead of 4 because duplicate contacts at states

<

Todo:: we are getting 9 instead of 6 because duplicate contacts at states

<

Todo:: we are getting 9 instead of 6 because duplicate contacts at states

<

Todo:: we are getting 288 instead of 285 because duplicate contacts at states

Definition at line 2874 of file tesseract_environment_unit.cpp.

◆ TEST() [2/29]

TEST	(	TesseractEnvironmentUnit	,
		EnvAddAndRemoveAllowedCollisionCommandUnit
	)

Definition at line 553 of file tesseract_environment_unit.cpp.

◆ TEST() [3/29]

TEST	(	TesseractEnvironmentUnit	,
		EnvAddandRemoveLink
	)

Definition at line 646 of file tesseract_environment_unit.cpp.

◆ TEST() [4/29]

TEST	(	TesseractEnvironmentUnit	,
		EnvAddandRemoveTrajectoryLink
	)

Definition at line 812 of file tesseract_environment_unit.cpp.

◆ TEST() [5/29]

TEST	(	TesseractEnvironmentUnit	,
		EnvAddKinematicsInformationCommandUnit
	)

Definition at line 904 of file tesseract_environment_unit.cpp.

◆ TEST() [6/29]

TEST	(	TesseractEnvironmentUnit	,
		EnvAddSceneGraphCommandUnit
	)

Definition at line 929 of file tesseract_environment_unit.cpp.

◆ TEST() [7/29]

TEST	(	TesseractEnvironmentUnit	,
		EnvApplyCommandsStateSolverCompareUnit
	)

Definition at line 1944 of file tesseract_environment_unit.cpp.

◆ TEST() [8/29]

TEST	(	TesseractEnvironmentUnit	,
		EnvChangeCollisionMarginsCommandUnit
	)

Definition at line 1369 of file tesseract_environment_unit.cpp.

◆ TEST() [9/29]

TEST	(	TesseractEnvironmentUnit	,
		EnvChangeJointLimitsCommandUnit
	)

Definition at line 1052 of file tesseract_environment_unit.cpp.

◆ TEST() [10/29]

TEST	(	TesseractEnvironmentUnit	,
		EnvChangeJointOriginCommandUnit
	)

Definition at line 1186 of file tesseract_environment_unit.cpp.

◆ TEST() [11/29]

TEST	(	TesseractEnvironmentUnit	,
		EnvChangeLinkCollisionEnabledCommandUnit
	)

Todo:: update contact manager to have function to check collision object enabled state

Definition at line 1256 of file tesseract_environment_unit.cpp.

◆ TEST() [12/29]

TEST	(	TesseractEnvironmentUnit	,
		EnvChangeLinkOriginCommandUnit
	)

Definition at line 1237 of file tesseract_environment_unit.cpp.

◆ TEST() [13/29]

TEST	(	TesseractEnvironmentUnit	,
		EnvChangeLinkVisibilityCommandUnit
	)

Todo:: update contact manager to have function to check collision object enabled state

Definition at line 1299 of file tesseract_environment_unit.cpp.

◆ TEST() [14/29]

TEST	(	TesseractEnvironmentUnit	,
		EnvChangeNameUnit
	)

Definition at line 1888 of file tesseract_environment_unit.cpp.

◆ TEST() [15/29]

TEST	(	TesseractEnvironmentUnit	,
		EnvClone
	)

Definition at line 2337 of file tesseract_environment_unit.cpp.

◆ TEST() [16/29]

TEST	(	TesseractEnvironmentUnit	,
		EnvCloneContactManagerUnit
	)

Definition at line 508 of file tesseract_environment_unit.cpp.

◆ TEST() [17/29]

TEST	(	TesseractEnvironmentUnit	,
		EnvCurrentStatePreservedWhenEnvChanges
	)

Definition at line 1743 of file tesseract_environment_unit.cpp.

◆ TEST() [18/29]

TEST	(	TesseractEnvironmentUnit	,
		EnvFindTCPUnit
	)

Definition at line 2714 of file tesseract_environment_unit.cpp.

◆ TEST() [19/29]

TEST	(	TesseractEnvironmentUnit	,
		EnvInitFailuresUnit
	)

Definition at line 429 of file tesseract_environment_unit.cpp.

◆ TEST() [20/29]

TEST	(	TesseractEnvironmentUnit	,
		EnvInitURDFOnlyUnit
	)

Definition at line 422 of file tesseract_environment_unit.cpp.

◆ TEST() [21/29]

TEST	(	TesseractEnvironmentUnit	,
		EnvMoveJointCommandUnit
	)

Definition at line 1571 of file tesseract_environment_unit.cpp.

◆ TEST() [22/29]

TEST	(	TesseractEnvironmentUnit	,
		EnvMoveLinkCommandUnit
	)

Definition at line 1654 of file tesseract_environment_unit.cpp.

◆ TEST() [23/29]

TEST	(	TesseractEnvironmentUnit	,
		EnvMultithreadedApplyCommandsTest
	)

Definition at line 2304 of file tesseract_environment_unit.cpp.

◆ TEST() [24/29]

TEST	(	TesseractEnvironmentUnit	,
		EnvResetUnit
	)

Definition at line 1831 of file tesseract_environment_unit.cpp.

◆ TEST() [25/29]

TEST	(	TesseractEnvironmentUnit	,
		EnvSetActiveContinuousContactManagerCommandUnit
	)

Todo:: update contact manager to have function to check collision object enabled state

Definition at line 1325 of file tesseract_environment_unit.cpp.

◆ TEST() [26/29]

TEST	(	TesseractEnvironmentUnit	,
		EnvSetActiveDiscreteContactManagerCommandUnit
	)

Todo:: update contact manager to have function to check collision object enabled state

Definition at line 1347 of file tesseract_environment_unit.cpp.

◆ TEST() [27/29]

TEST	(	TesseractEnvironmentUnit	,
		EnvSetState
	)

Definition at line 2451 of file tesseract_environment_unit.cpp.

◆ TEST() [28/29]

TEST	(	TesseractEnvironmentUnit	,
		EnvSetState2
	)

Definition at line 2612 of file tesseract_environment_unit.cpp.

◆ TEST() [29/29]

TEST	(	TesseractEnvironmentUnit	,
		getActiveLinkNamesRecursiveUnit
	)

Definition at line 2749 of file tesseract_environment_unit.cpp.

Enumerations

Functions

Enumeration Type Documentation

◆ EnvironmentInitType

Function Documentation

◆ checkProcessInterpolatedResults()

◆ checkProcessInterpolatedResultsNoTime0()

◆ checkProcessInterpolatedResultsNoTime1()

◆ getContactCount()

◆ getEnvironment()

◆ getEnvironmentURDFOnly()

◆ getSceneGraph()

◆ getSceneGraphString()

◆ getSRDFModel()

◆ getSRDFModelString()

◆ getSubSceneGraph()

◆ hasProcessInterpolatedResultsTime0()

◆ hasProcessInterpolatedResultsTime1()

◆ main()

◆ runCompareSceneStates()

◆ runCompareStateSolver()

◆ runGetLinkTransformsTest()

◆ tcpCallback()

◆ TEST() [1/29]

◆ TEST() [2/29]

◆ TEST() [3/29]

◆ TEST() [4/29]

◆ TEST() [5/29]

◆ TEST() [6/29]

◆ TEST() [7/29]

◆ TEST() [8/29]

◆ TEST() [9/29]

◆ TEST() [10/29]

◆ TEST() [11/29]

◆ TEST() [12/29]

◆ TEST() [13/29]

◆ TEST() [14/29]

◆ TEST() [15/29]

◆ TEST() [16/29]

◆ TEST() [17/29]

◆ TEST() [18/29]

◆ TEST() [19/29]

◆ TEST() [20/29]

◆ TEST() [21/29]

◆ TEST() [22/29]

◆ TEST() [23/29]

◆ TEST() [24/29]

◆ TEST() [25/29]

◆ TEST() [26/29]

◆ TEST() [27/29]

◆ TEST() [28/29]

◆ TEST() [29/29]