Skip to content
Snippets Groups Projects
Select Git revision
  • 0769ee2db5d32cbd4708e16d305801a1a543288b
  • master default
2 results

appmod.cpp

Blame
  • appmod.cpp 13.16 KiB
    #include <ros/ros.h>
    #include "datalink/abstractobject.h"
    #include "datalink/graspobject.h"
    #include "datalink/hand.h"
    #include "datalink/humanspace.h"
    #include "datalink/joint.h"
    #include "datalink/obstacle.h"
    #include "datalink/robot.h"
    #include "datalink/robotfpe.h"
    #include "datalink/world.h"
    #include "datalink/worldobject.h"
    #include "datalink/quaternion.h"
    #include <vector>
    #include <iostream>
    #include <tf2_ros/transform_listener.h>
    #include <geometry_msgs/TransformStamped.h>
    #include <geometry_msgs/PoseStamped.h>
    #include <tf2_ros/static_transform_broadcaster.h>
    #include <cstdio>
    #include <tf2/LinearMath/Quaternion.h>
    #include "tf2_ros/message_filter.h"
    #include "message_filters/subscriber.h"
    #include "tf2_geometry_msgs/tf2_geometry_msgs.h"
    #include "geometry_msgs/PointStamped.h"
    #include <tf2_ros/transform_broadcaster.h>
    #include <tf2/LinearMath/Quaternion.h>
    #include <moveit/move_group_interface/move_group_interface.h>
    #include <moveit/planning_scene_interface/planning_scene_interface.h>
    #include <moveit_msgs/CollisionObject.h>
    #include <moveit_visual_tools/moveit_visual_tools.h>
    #include <moveit/trajectory_processing/iterative_time_parameterization.h>
    #include <trajectory_msgs/JointTrajectoryPoint.h>
    
    int main(int argc, char** argv) {
    
        ros::init(argc, argv, "Application Model");
        ros::NodeHandle node_handle;
        ros::AsyncSpinner spinner(1);
        spinner.start();
    
        // setup the planning environment
        static const std::string PLANNING_GROUP = "panda_arm";
        moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
        moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
    
        ros::Duration(3.0).sleep();
    
        move_group.setStartStateToCurrentState();
    
        geometry_msgs::Pose another_pose;
        another_pose.orientation.w = 1.0;
        another_pose.position.x = 0.4;
        another_pose.position.y = -0.4;
        another_pose.position.z = 0.9;
        move_group.setPoseTarget(another_pose);
    
        moveit::planning_interface::MoveGroupInterface::Plan my_plan;
        bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
        ROS_INFO("Visualizing plan %s", success ? "" : "FAILED");
    
        // Move the (simulated) robot in gazebo
        move_group.move();
    
        if(another_pose.position.z==0.9)
        ROS_INFO(" THE ROBOT HAS MOVED TO POSITION AS INTENDED ");
        else
            ROS_INFO("THE ROBOT HAS NOT MOVED TO THE POSITION AS INTENDED");
    
    
        ros::shutdown();