fetch_scenes_visualize.cpp File Reference

#include <robowflex_library/builder.h>
#include <robowflex_library/detail/fetch.h>
#include <robowflex_library/io/visualization.h>
#include <robowflex_library/log.h>
#include <robowflex_library/scene.h>
#include <robowflex_library/util.h>

Go to the source code of this file.

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

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

Function Documentation

main()

int main	(	int	argc,
		char **	argv
	)

Definition at line 23 of file fetch_scenes_visualize.cpp.

{
    // Startup ROS
    ROS ros(argc, argv);
 
    // Create the default Fetch robot.
    auto fetch = std::make_shared<FetchRobot>();
 
    // False does not add the virtual joint since the real robot does not have one.
    fetch->initialize(false);
 
    // Create an RViz visualization helper.
    // Publishes all topics and parameter under `/robowflex` by default.
    IO::RVIZHelper rviz(fetch);
 
    RBX_INFO("RViz Initialized! Press enter to continue (after your RViz is setup)...");
    std::cin.get();
 
    const int start = 1;
    const int end = 10;
    for (int i = start; i <= end; i++)
    {
        const auto &is = std::to_string(i);
        const auto &index = std::string(4 - is.size(), '0') + is;
 
        const auto &scene_file =
            "package://robowflex_library/yaml/fetch_scenes/scene_vicon" + index + ".yaml";
        const auto &request_file = "package://robowflex_library/yaml/fetch_scenes/request" + index + ".yaml";
 
        // Create an empty Scene.
        auto scene = std::make_shared<Scene>(fetch);
        if (not scene->fromYAMLFile(scene_file))
        {
            RBX_ERROR("Failed to read file: %s for scene", scene_file);
            continue;
        }
 
        // Create the default planner for the Fetch.
        auto planner = std::make_shared<OMPL::FetchOMPLPipelinePlanner>(fetch, "default");
        planner->initialize();
 
        // Create an empty motion planning request.
        auto request = std::make_shared<robowflex::MotionRequestBuilder>(planner, GROUP);
        if (not request->fromYAMLFile(request_file))
        {
            RBX_ERROR("Failed to read file: %s for request", request_file);
            continue;
        }
 
        // Visualize the scene.
        rviz.updateScene(scene);
        rviz.updateMarkers();
 
        RBX_INFO("Scene displayed! Press enter to plan...");
        std::cin.get();
 
        // Do motion planning!
        planning_interface::MotionPlanResponse res = planner->plan(scene, request->getRequest());
        if (res.error_code_.val != moveit_msgs::MoveItErrorCodes::SUCCESS)
            return 1;
 
        // Publish the trajectory to a topic to display in RViz
        rviz.updateTrajectory(res);
 
        RBX_INFO("Press enter to remove the scene.");
        std::cin.get();
 
        rviz.removeScene();
    }
}

Variable Documentation

GROUP

const std::string GROUP = "arm_with_torso"

static

Definition at line 21 of file fetch_scenes_visualize.cpp.