mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
remove tests that do not test anything
This commit is contained in:
parent
9e5e098339
commit
4fcac84d76
@ -21,15 +21,3 @@ if (CATKIN_ENABLE_TESTING)
|
|||||||
catkin_add_gtest(${PROJECT_NAME}-test-cost_queue test_cost_queue.cpp)
|
catkin_add_gtest(${PROJECT_NAME}-test-cost_queue test_cost_queue.cpp)
|
||||||
target_link_libraries(${PROJECT_NAME}-test-cost_queue ${PROJECT_NAME} gtest_main)
|
target_link_libraries(${PROJECT_NAME}-test-cost_queue ${PROJECT_NAME} gtest_main)
|
||||||
endif()
|
endif()
|
||||||
|
|
||||||
add_executable(test_plan_current_state test_plan_current_state.cpp)
|
|
||||||
target_link_libraries(test_plan_current_state ${PROJECT_NAME}_stages ${PROJECT_NAME})
|
|
||||||
|
|
||||||
add_executable(test_plan_gripper test_plan_gripper.cpp)
|
|
||||||
target_link_libraries(test_plan_gripper ${PROJECT_NAME}_stages ${PROJECT_NAME})
|
|
||||||
|
|
||||||
add_executable(test_plan_generate_grasp_pose test_plan_generate_grasp_pose.cpp)
|
|
||||||
target_link_libraries(test_plan_generate_grasp_pose ${PROJECT_NAME}_stages ${PROJECT_NAME})
|
|
||||||
|
|
||||||
add_executable(test_plan_cartesian_forward test_plan_cartesian_forward.cpp)
|
|
||||||
target_link_libraries(test_plan_cartesian_forward ${PROJECT_NAME}_stages ${PROJECT_NAME})
|
|
||||||
|
|||||||
@ -1,49 +0,0 @@
|
|||||||
#include <ros/ros.h>
|
|
||||||
|
|
||||||
#include <moveit/task_constructor/task.h>
|
|
||||||
|
|
||||||
#include <moveit/task_constructor/stages/current_state.h>
|
|
||||||
#include <moveit/task_constructor/stages/cartesian_position_motion.h>
|
|
||||||
|
|
||||||
using namespace moveit::task_constructor;
|
|
||||||
|
|
||||||
int main(int argc, char** argv){
|
|
||||||
ros::init(argc, argv, "test_plan_current_state");
|
|
||||||
|
|
||||||
Task t;
|
|
||||||
|
|
||||||
t.add( std::make_unique<stages::CurrentState>("current state") );
|
|
||||||
|
|
||||||
{
|
|
||||||
auto move= std::make_unique<stages::CartesianPositionMotion>("lift object");
|
|
||||||
move->setGroup("arm");
|
|
||||||
move->setLink("s_model_tool0");
|
|
||||||
move->setMinMaxDistance(0.05, 0.2);
|
|
||||||
move->setCartesianStepSize(0.05);
|
|
||||||
|
|
||||||
geometry_msgs::Vector3Stamped direction;
|
|
||||||
direction.header.frame_id= "world";
|
|
||||||
direction.vector.x= 1.0;
|
|
||||||
move->along(direction);
|
|
||||||
t.add(std::move(move));
|
|
||||||
}
|
|
||||||
|
|
||||||
{
|
|
||||||
auto move= std::make_unique<stages::CartesianPositionMotion>("lift object");
|
|
||||||
move->setGroup("arm");
|
|
||||||
move->setLink("s_model_tool0");
|
|
||||||
move->setMinDistance(0.01);
|
|
||||||
move->setCartesianStepSize(0.01);
|
|
||||||
|
|
||||||
geometry_msgs::PointStamped target;
|
|
||||||
target.header.frame_id= "world";
|
|
||||||
target.point.y= 0.7;
|
|
||||||
target.point.z= 1.5;
|
|
||||||
move->towards(target);
|
|
||||||
t.add(std::move(move));
|
|
||||||
}
|
|
||||||
|
|
||||||
t.plan();
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
@ -1,19 +0,0 @@
|
|||||||
#include <ros/ros.h>
|
|
||||||
|
|
||||||
#include <moveit/task_constructor/task.h>
|
|
||||||
|
|
||||||
#include <moveit/task_constructor/stages/current_state.h>
|
|
||||||
|
|
||||||
using namespace moveit::task_constructor;
|
|
||||||
|
|
||||||
int main(int argc, char** argv){
|
|
||||||
ros::init(argc, argv, "test_plan_current_state");
|
|
||||||
|
|
||||||
Task t;
|
|
||||||
|
|
||||||
t.add( std::make_unique<stages::CurrentState>("current state") );
|
|
||||||
|
|
||||||
t.plan();
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
@ -1,51 +0,0 @@
|
|||||||
#include <moveit/task_constructor/task.h>
|
|
||||||
#include <moveit/task_constructor/stages/generate_grasp_pose.h>
|
|
||||||
|
|
||||||
#include <ros/ros.h>
|
|
||||||
#include <moveit_msgs/CollisionObject.h>
|
|
||||||
#include <shape_msgs/SolidPrimitive.h>
|
|
||||||
|
|
||||||
#include <moveit/planning_scene_interface/planning_scene_interface.h>
|
|
||||||
|
|
||||||
using namespace moveit::task_constructor;
|
|
||||||
|
|
||||||
void spawnObject(){
|
|
||||||
moveit::planning_interface::PlanningSceneInterface psi;
|
|
||||||
|
|
||||||
moveit_msgs::CollisionObject o;
|
|
||||||
o.id= "object";
|
|
||||||
o.header.frame_id= "world";
|
|
||||||
o.primitive_poses.resize(1);
|
|
||||||
o.primitive_poses[0].position.x= 0.53;
|
|
||||||
o.primitive_poses[0].position.y= 0.55;
|
|
||||||
o.primitive_poses[0].position.z= 0.84;
|
|
||||||
o.primitive_poses[0].orientation.w= 1.0;
|
|
||||||
o.primitives.resize(1);
|
|
||||||
o.primitives[0].type= shape_msgs::SolidPrimitive::SPHERE;
|
|
||||||
o.primitives[0].dimensions.resize(1, 0.03);
|
|
||||||
psi.applyCollisionObject(o);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
int main(int argc, char** argv){
|
|
||||||
ros::init(argc, argv, "test_plan_current_state");
|
|
||||||
ros::AsyncSpinner spinner(1);
|
|
||||||
spinner.start();
|
|
||||||
|
|
||||||
spawnObject();
|
|
||||||
|
|
||||||
Task t;
|
|
||||||
|
|
||||||
auto st= std::make_unique<stages::GenerateGraspPose>("generate grasp candidates");
|
|
||||||
|
|
||||||
st->setEndEffector("gripper");
|
|
||||||
st->setObject("object");
|
|
||||||
st->setAngleDelta(0.1);
|
|
||||||
st->setToolToGraspTF(Eigen::Translation3d(.03,0,0));
|
|
||||||
|
|
||||||
t.add(std::move(st));
|
|
||||||
|
|
||||||
t.plan();
|
|
||||||
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
@ -1,38 +0,0 @@
|
|||||||
#include <ros/ros.h>
|
|
||||||
|
|
||||||
#include <moveit/task_constructor/task.h>
|
|
||||||
|
|
||||||
#include <moveit/task_constructor/stages/current_state.h>
|
|
||||||
#include <moveit/task_constructor/stages/gripper.h>
|
|
||||||
|
|
||||||
using namespace moveit::task_constructor;
|
|
||||||
|
|
||||||
int main(int argc, char** argv){
|
|
||||||
ros::init(argc, argv, "test_plan_gripper");
|
|
||||||
ros::AsyncSpinner spinner(1);
|
|
||||||
spinner.start();
|
|
||||||
|
|
||||||
Task t;
|
|
||||||
|
|
||||||
t.add( std::make_unique<stages::CurrentState>("current state") );
|
|
||||||
{
|
|
||||||
auto gripper= std::make_unique<stages::Gripper>("close gripper");
|
|
||||||
gripper->setEndEffector("gripper");
|
|
||||||
gripper->setTo("closed");
|
|
||||||
t.add(std::move( gripper ));
|
|
||||||
}
|
|
||||||
t.plan();
|
|
||||||
|
|
||||||
/*TODO currently not implemented in gripper*/
|
|
||||||
/*
|
|
||||||
{
|
|
||||||
auto gripper= std::make_unique<stages::Gripper>("close gripper");
|
|
||||||
gripper->setEndEffector("gripper");
|
|
||||||
gripper->setTo("closed");
|
|
||||||
t.add(std::move( gripper ));
|
|
||||||
}
|
|
||||||
t.add( std::make_unique<stages::CurrentState>("current state") );
|
|
||||||
t.plan();
|
|
||||||
*/
|
|
||||||
return 0;
|
|
||||||
}
|
|
||||||
Loading…
Reference in New Issue
Block a user