fetch_tabletop_inits.cpp File Reference

#include <robowflex_library/util.h>
#include <robowflex_library/log.h>
#include <robowflex_library/detail/fetch.h>
#include <robowflex_library/io.h>
#include <robowflex_library/io/visualization.h>
#include <robowflex_library/scene.h>
#include <robowflex_library/yaml.h>
#include <robowflex_tesseract/trajopt_planner.h>

Go to the source code of this file.

Functions
int	main (int argc, char **argv)

Variables
static const std::string	GROUP = "arm"

Function Documentation

main()

int main	(	int	argc,
		char **	argv
	)

Definition at line 22 of file fetch_tabletop_inits.cpp.

{
    // Startup ROS
    ROS ros(argc, argv);
 
    // Create the default Fetch robot.
    auto fetch = std::make_shared<FetchRobot>();
    fetch->initialize(false);
 
    // Load tabletop scene.
    auto scene = std::make_shared<Scene>(fetch);
    scene->fromYAMLFile("package://robowflex_tesseract/scenes/table/scene.yaml");
 
    // Attach object to end effector.
    scene->attachObject(*fetch->getScratchState(), "Can1");
 
    // Create a TrajOpt planner for Fetch.
    auto planner = std::make_shared<TrajOptPlanner>(fetch, GROUP);
    planner->initialize("torso_lift_link", "gripper_link");
 
    // Set planner parameters.
    planner->options.num_waypoints = 8;  // Select number of waypoints in trajectory.
 
    // Load request.
    const auto &request = std::make_shared<MotionRequestBuilder>(fetch);
    request->fromYAMLFile("package://robowflex_tesseract/scenes/table/request.yaml");
 
    // RVIZ helper.
    const auto &rviz = std::make_shared<IO::RVIZHelper>(fetch);
    rviz->updateScene(scene);
    rviz->visualizeState(request->getStartConfiguration());
 
    RBX_INFO("Visualizing start state");
    RBX_INFO("Press Enter to try to plan with STATIONARY initialization");
    std::cin.ignore();
 
    // Initialize all waypoints at the start state. This is the default initialization.
    planner->setInitType(trajopt::InitInfo::Type::STATIONARY);
 
    // Do motion planning.
    auto res = planner->plan(scene, request->getRequest());
    if (res.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
        rviz->updateTrajectory(res);
 
    rviz->visualizeState(request->getGoalConfiguration());
 
    RBX_INFO("Visualizing goal state");
    RBX_INFO("Press Enter to try to plan with JOINT_INTERPOLATED initialization");
    std::cin.ignore();
 
    // Initialize using a straight-line between start and goal in C-Space.
    planner->setInitType(trajopt::InitInfo::Type::JOINT_INTERPOLATED);
 
    // Do motion planning.
    res = planner->plan(scene, request->getRequest());
    if (res.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
        rviz->updateTrajectory(res);
 
    rviz->visualizeState(request->getGoalConfiguration());
 
    RBX_INFO("Visualizing goal state");
    RBX_INFO("Press Enter to exit");
    std::cin.ignore();
 
    return 0;
}

Variable Documentation

GROUP

const std::string GROUP = "arm"

static

Definition at line 20 of file fetch_tabletop_inits.cpp.