Select Git revision
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();