int main(int argc, char **argv)
{
/*Initialise Variables*/
CAPTURE_MOVEMENT=false;//know when you have reach the maximum of points to handle
//Creating the joint_msg_leap
joint_msg_leap.name.resize(8);
joint_msg_leap.position.resize(8);
joint_msg_leap.name[0]="arm_1_joint";
joint_msg_leap.name[1]="arm_2_joint";
joint_msg_leap.name[2]="arm_3_joint";
joint_msg_leap.name[3]="arm_4_joint";
joint_msg_leap.name[4]="arm_5_joint";
joint_msg_leap.name[5]="arm_6_joint";
joint_msg_leap.name[6]="base_joint_gripper_left";
joint_msg_leap.name[7]="base_joint_gripper_right";
aux_enter=1;
FIRST_VALUE=true;//Help knowing Initial Position of the hand
int arm_trajectory_point=1;
collision_detection::CollisionRequest collision_request;
collision_detection::CollisionResult collision_result;
/*Finish Variables Initialitation*/
//ROS DECLARATION
ros::init(argc, argv,"listener");
if (argc != 2)
{
ROS_WARN("WARNING: you should specify number of points to capture");
}
else
{
count=atoi(argv[1]);
ROS_INFO ("Number of points /n%d", count);
}
//Create an object of the LeapMotionListener Class
LeapMotionListener leapmotionlistener;
leapmotionlistener.Configure(count);
//ros::NodeHandle node_handle;
ros::NodeHandle node_handle("~");
// start a ROS spinning thread
ros::AsyncSpinner spinner(1);
spinner.start();
//we need this for leap
ros::Rate r(1);
//robo_pub = n.advertise<sensor_msgs::JointState>("joint_leap", 100);
//Creating a Robot Model
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();
//Initialise PlanningSceneMonitorPtr
/* MOVEIT Setup*/
// ^^^^^
moveit::planning_interface::MoveGroup group("arm");
group.setPlanningTime(0.5);
moveit::planning_interface::MoveGroup::Plan my_plan;
// We will use the :planning_scene_interface:`PlanningSceneInterface`
// class to deal directly with the world.
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
/* Adding/Removing Objects and Attaching/Detaching Objects*/
ROS_INFO("CREATING PLANNING_SCENE PUBLISHER");
ros::Publisher planning_scene_diff_publisher = node_handle.advertise<moveit_msgs::PlanningScene>("/planning_scene", 1);
while(planning_scene_diff_publisher.getNumSubscribers() < 1)
{
ros::WallDuration sleep_t(0.5);
sleep_t.sleep();
}
ROS_INFO("CREATING COLLISION OBJECT");
moveit_msgs::AttachedCollisionObject attached_object;
attached_object.link_name = "";
/* The header must contain a valid TF frame*/
attached_object.object.header.frame_id = "gripper_left";
/* The id of the object */
attached_object.object.id = "box";
/* A default pose */
geometry_msgs::Pose posebox;
posebox.orientation.w = 1.0;
/* Define a box to be attached */
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.1;
primitive.dimensions[1] = 0.1;
primitive.dimensions[2] = 0.1;
attached_object.object.primitives.push_back(primitive);
attached_object.object.primitive_poses.push_back(posebox);
attached_object.object.operation = attached_object.object.ADD;
ROS_INFO("ADDING COLLISION OBJECT TO THE WORLD");
moveit_msgs::PlanningScene planning_scene_msg;
planning_scene_msg.world.collision_objects.push_back(attached_object.object);
planning_scene_msg.is_diff = true;
planning_scene_diff_publisher.publish(planning_scene_msg);
//sleep_time.sleep();
/* First, define the REMOVE object message*/
moveit_msgs::CollisionObject remove_object;
remove_object.id = "box";
remove_object.header.frame_id = "odom_combined";
remove_object.operation = remove_object.REMOVE;
/* Carry out the REMOVE + ATTACH operation */
ROS_INFO("Attaching the object to the right wrist and removing it from the world.");
planning_scene_msg.world.collision_objects.clear();
planning_scene_msg.world.collision_objects.push_back(remove_object);
planning_scene_msg.robot_state.attached_collision_objects.push_back(attached_object);
planning_scene_diff_publisher.publish(planning_scene_msg);
//sleep_time.sleep();
// Create a publisher for visualizing plans in Rviz.
ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model,collision_detection::WorldPtr(new collision_detection::World())));
//Creating planning_scene_monitor
boost::shared_ptr<tf::TransformListener> tf(new tf::TransformListener(ros::Duration(2.0)));
planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor(new planning_scene_monitor::PlanningSceneMonitor(planning_scene,"robot_description", tf));
planning_scene_monitor->startSceneMonitor();
//planning_scene_monitor->initialize(planning_scene);
planning_pipeline::PlanningPipeline *planning_pipeline= new planning_pipeline::PlanningPipeline(robot_model,node_handle,"planning_plugin", "request_adapters");
/* Sleep a little to allow time to startup rviz, etc. */
ros::WallDuration sleep_time(20.0);
sleep_time.sleep();
/*end of MOVEIT Setup*/
// We can print the name of the reference frame for this robot.
ROS_INFO("Reference frame: %s", group.getPlanningFrame().c_str());
// We can also print the name of the end-effector link for this group.
ROS_INFO("Reference frame: %s", group.getEndEffectorLink().c_str());
// Planning to a Pose goal 1
// ^^^^^^^^^^^^^^^^^^^^^^^
// We can plan a motion for this group to a desired pose for the
// end-effector
ROS_INFO("Planning to INITIAL POSE");
planning_interface::MotionPlanRequest req;
planning_interface::MotionPlanResponse res;
geometry_msgs::PoseStamped pose;
pose.header.frame_id = "/odom_combined";
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.position.z = 1.15;
pose.pose.orientation.w = 1.0;
std::vector<double> tolerance_pose(3, 0.01);
std::vector<double> tolerance_angle(3, 0.01);
old_pose=pose;
// We will create the request as a constraint using a helper function available
req.group_name = "arm";
moveit_msgs::Constraints pose_goal = kinematic_constraints::constructGoalConstraints("gripper_base_link", pose, tolerance_pose, tolerance_angle);
req.goal_constraints.push_back(pose_goal);
// Now, call the pipeline and check whether planning was successful.
planning_pipeline->generatePlan(planning_scene, req, res);
/* Check that the planning was successful */
if(res.error_code_.val != res.error_code_.SUCCESS)
{
ROS_ERROR("Could not compute plan successfully");
return 0;
}
// Visualize the result
// ^^^^^^^^^^^^^^^^^^^^
/* Visualize the trajectory */
moveit_msgs::DisplayTrajectory display_trajectory;
ROS_INFO("Visualizing the trajectory 1");
moveit_msgs::MotionPlanResponse response;
res.getMessage(response);
display_trajectory.trajectory_start = response.trajectory_start;
display_trajectory.trajectory.push_back(response.trajectory);
display_publisher.publish(display_trajectory);
//sleep_time.sleep();
/* End Planning to a Pose goal 1*/
// First, set the state in the planning scene to the final state of the last plan
robot_state::RobotState& robot_state = planning_scene->getCurrentStateNonConst();
//planning_scene->setCurrentState(response.trajectory_start);
joint_model_group = robot_state.getJointModelGroup("arm");
robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
spinner.stop();
/*Capturing Stage*/
/*****************/
ROS_INFO("PRESH ENTER TO START CAPTURING POINTS");
while (getline(std::cin,s))
{
if ('\n' == getchar())
break;
}
/* SENSOR SUBSCRIBING */
//LEAP MOTION
ROS_INFO("SUBSCRIBING LEAPMOTION");
ros::Subscriber leapsub = node_handle.subscribe("/leapmotion/data", 1000, &LeapMotionListener::leapmotionCallback, &leapmotionlistener);
ros::Subscriber trajectorysub = node_handle.subscribe("/move_group/", 1000, &LeapMotionListener::leapmotionCallback, &leapmotionlistener);
while(!CAPTURE_MOVEMENT==true)
{
ros::spinOnce();
}
leapsub.shutdown();
ROS_INFO("CAPTURING POINTS FINISH...PROCESSING POINTS");
// End of Capturing Stage
/* Start Creating Arm Trajectory*/
/**********************************/
ROS_INFO("START CREATING ARM TRAJECTORY");
for (unsigned i=0; i<trajectory_hand.size(); i++)
{
//First we set the initial Position of the Hand
if (FIRST_VALUE)
{
dataLastHand_.palmpos.x=trajectory_hand.at(i).palmpos.x;
dataLastHand_.palmpos.y=trajectory_hand.at(i).palmpos.y;
dataLastHand_.palmpos.z=trajectory_hand.at(i).palmpos.z;
FIRST_VALUE=0;
ROS_INFO("ORIGINAL POSITION OF THE HAND SET TO \n X: %f\n Y: %f\n Z: %f\n ",trajectory_hand.at(i).palmpos.x,trajectory_hand.at(i).palmpos.y,trajectory_hand.at(i).palmpos.z);
sleep(2);
}
else
{
// Both limits for x,y,z to avoid small changes
Updifferencex=dataLastHand_.palmpos.x+10;
Downdifferencex=dataLastHand_.palmpos.x-10;
Updifferencez=dataLastHand_.palmpos.z+10;
Downdifferencez=dataLastHand_.palmpos.z-20;
Updifferencey=dataLastHand_.palmpos.y+20;
Downdifferencey=dataLastHand_.palmpos.y-20;
if ((trajectory_hand.at(i).palmpos.x<Downdifferencex)||(trajectory_hand.at(i).palmpos.x>Updifferencex)||(trajectory_hand.at(i).palmpos.y<Downdifferencey)||(trajectory_hand.at(i).palmpos.y>Updifferencey)||(trajectory_hand.at(i).palmpos.z<Downdifferencez)||(trajectory_hand.at(i).palmpos.z>Updifferencez))
{
ros::AsyncSpinner spinner(1);
spinner.start();
ROS_INFO("TRYING TO ADD POINT %d TO TRAJECTORY",arm_trajectory_point);
// Cartesian Paths
// ^^^^^^^^^^^^^^^
// You can plan a cartesian path directly by specifying a list of waypoints
// for the end-effector to go through. Note that we are starting
// from the new start state above. The initial pose (start state) does not
// need to be added to the waypoint list.
pose.header.frame_id = "/odom_combined";
pose.pose.orientation.w = 1.0;
pose.pose.position.y +=(trajectory_hand.at(i).palmpos.x-dataLastHand_.palmpos.x)/500 ;
pose.pose.position.z +=(trajectory_hand.at(i).palmpos.y-dataLastHand_.palmpos.y)/1000 ;
if(pose.pose.position.z>Uplimitez)
pose.pose.position.z=Uplimitez;
pose.pose.position.x +=-(trajectory_hand.at(i).palmpos.z-dataLastHand_.palmpos.z)/500 ;
ROS_INFO("END EFFECTOR POSITION \n X: %f\n Y: %f\n Z: %f\n", pose.pose.position.x,pose.pose.position.y,pose.pose.position.z);
ROS_INFO("Palmpos \n X: %f\n Y: %f\n Z: %f\n ",trajectory_hand.at(i).palmpos.x,trajectory_hand.at(i).palmpos.y,trajectory_hand.at(i).palmpos.z);
// We will create the request as a constraint using a helper function available
ROS_INFO("1");
pose_goal= kinematic_constraints::constructGoalConstraints("gripper_base_link", pose, tolerance_pose, tolerance_angle);
ROS_INFO("2");
req.goal_constraints.push_back(pose_goal);
// Now, call the pipeline and check whether planning was successful.
planning_pipeline->generatePlan(planning_scene, req, res);
ROS_INFO("3");
if(res.error_code_.val != res.error_code_.SUCCESS)
{
ROS_ERROR("Could not compute plan successfully");
pose=old_pose;
}
else
{
// Now when we plan a trajectory it will avoid the obstacle
res.getMessage(response);
collision_result.clear();
collision_detection::AllowedCollisionMatrix acm = planning_scene->getAllowedCollisionMatrix();
robot_state::RobotState copied_state = planning_scene->getCurrentState();
planning_scene->checkCollision(collision_request, collision_result, copied_state, acm);
ROS_INFO_STREAM("Collision Test "<< (collision_result.collision ? "in" : "not in")<< " collision");
arm_trajectory_point++;
// Visualize the trajectory
ROS_INFO("VISUALIZING NEW POINT");
//req.goal_constraints.clear();
display_trajectory.trajectory_start = response.trajectory_start;
ROS_INFO("AXIS 1 NEXT TRAJECTORY IS %f\n",response.trajectory_start.joint_state.position[1] );
display_trajectory.trajectory.push_back(response.trajectory);
// Now you should see two planned trajectories in series
display_publisher.publish(display_trajectory);
planning_scene->setCurrentState(response.trajectory_start);
if (planning_scene_monitor->updatesScene(planning_scene))
{
ROS_INFO("CHANGING STATE");
planning_scene_monitor->updateSceneWithCurrentState();
}
else
{
ROS_ERROR("NOT POSSIBLE TO CHANGE THE SCENE");
}
robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
req.goal_constraints.clear();
old_pose=pose;
dataLastHand_.palmpos.x=trajectory_hand.at(i).palmpos.x;
dataLastHand_.palmpos.y=trajectory_hand.at(i).palmpos.y;
dataLastHand_.palmpos.z=trajectory_hand.at(i).palmpos.z;
}
//sleep(2);
spinner.stop();
}
}
}
//ros::Subscriber myogestsub = n.subscribe("/myo_gest", 1000, myogestCallback);
return 0;
}
int main(int argc, char **argv)
{
ros::init (argc, argv, "planning_scene_ros_api_tutorial");
ros::AsyncSpinner spinner(1);
spinner.start();
ros::NodeHandle node_handle;
ros::Duration sleep_time(5.0);
sleep_time.sleep();
sleep_time.sleep();
// BEGIN_TUTORIAL
//
// ROS API
// ^^^^^^^
// The ROS API to the planning scene publisher is through a topic interface
// using "diffs". A planning scene diff is the difference between the current
// planning scene (maintained by the move_group node) and the new planning
// scene desired by the user.
// Advertise the required topic
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
// Note that this topic may need to be remapped in the launch file
ros::Publisher planning_scene_diff_publisher = node_handle.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
while(planning_scene_diff_publisher.getNumSubscribers() < 1)
{
ros::WallDuration sleep_t(0.5);
sleep_t.sleep();
}
// Define the attached object message
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
// We will use this message to add or
// subtract the object from the world
// and to attach the object to the robot
moveit_msgs::AttachedCollisionObject attached_object;
attached_object.link_name = "base_link";
/* The header must contain a valid TF frame*/
attached_object.object.header.frame_id = "base_link";
/* The id of the object */
attached_object.object.id = "box";
/* A default pose */
geometry_msgs::Pose pose;
pose.orientation.w = 1.0;
pose.position.x=0.0;
pose.position.y=-0.45;
pose.position.z=0.2;
/* Define a box to be attached */
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.04;
primitive.dimensions[1] = 0.04;
primitive.dimensions[2] = 0.04;
attached_object.object.primitives.push_back(primitive);
attached_object.object.primitive_poses.push_back(pose);
// Note that attaching an object to the robot requires
// the corresponding operation to be specified as an ADD operation
attached_object.object.operation = attached_object.object.ADD;
// Add an object into the environment
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
// Add the object into the environment by adding it to
// the set of collision objects in the "world" part of the
// planning scene. Note that we are using only the "object"
// field of the attached_object message here.
ROS_INFO("Adding the object into the world at the location of the right wrist.");
moveit_msgs::PlanningScene planning_scene;
planning_scene.world.collision_objects.push_back(attached_object.object);
planning_scene.is_diff = true;
planning_scene_diff_publisher.publish(planning_scene);
sleep_time.sleep();
// Attach an object to the robot
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
// When the robot picks up an object from the environment, we need to
// "attach" the object to the robot so that any component dealing with
// the robot model knows to account for the attached object, e.g. for
// collision checking.
// Attaching an object requires two operations
// * Removing the original object from the environment
// * Attaching the object to the robot
/* First, define the REMOVE object message*/
moveit_msgs::CollisionObject remove_object;
remove_object.id = "box";
remove_object.header.frame_id = "odom_combined";
remove_object.operation = remove_object.REMOVE;
// Note how we make sure that the diff message contains no other
// attached objects or collisions objects by clearing those fields
// first.
/* Carry out the REMOVE + ATTACH operation */
ROS_INFO("Attaching the object to the right wrist and removing it from the world.");
planning_scene.world.collision_objects.clear();
planning_scene.world.collision_objects.push_back(remove_object);
planning_scene.robot_state.attached_collision_objects.push_back(attached_object);
planning_scene_diff_publisher.publish(planning_scene);
sleep_time.sleep();
// Detach an object from the robot
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
// Detaching an object from the robot requires two operations
// * Detaching the object from the robot
// * Re-introducing the object into the environment
/* First, define the DETACH object message*/
moveit_msgs::AttachedCollisionObject detach_object;
detach_object.object.id = "box";
detach_object.link_name = "gripper_roll_link";
detach_object.object.operation = attached_object.object.REMOVE;
// Note how we make sure that the diff message contains no other
// attached objects or collisions objects by clearing those fields
// first.
/* Carry out the DETACH + ADD operation */
ROS_INFO("Detaching the object from the robot and returning it to the world.");
planning_scene.robot_state.attached_collision_objects.clear();
planning_scene.robot_state.attached_collision_objects.push_back(detach_object);
planning_scene.world.collision_objects.clear();
planning_scene.world.collision_objects.push_back(attached_object.object);
planning_scene_diff_publisher.publish(planning_scene);
sleep_time.sleep();
// REMOVE THE OBJECT FROM THE COLLISION WORLD
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
// Removing the object from the collision world just requires
// using the remove object message defined earlier.
// Note, also how we make sure that the diff message contains no other
// attached objects or collisions objects by clearing those fields
// first.
ROS_INFO("Removing the object from the world.");
planning_scene.robot_state.attached_collision_objects.clear();
planning_scene.world.collision_objects.clear();
planning_scene.world.collision_objects.push_back(remove_object);
planning_scene_diff_publisher.publish(planning_scene);
// END_TUTORIAL
ros::shutdown();
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "move_group_base");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
/* This sleep is ONLY to allow Rviz to come up */
sleep(10.0);
moveit::planning_interface::MoveGroup group("base");
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
moveit_msgs::DisplayTrajectory display_trajectory;
ros::Publisher chatter_pub = node_handle.advertise<pr2_moveit_ltl::plan_msg>("chatter", 1000);
ros::Publisher collision_object_publisher = node_handle.advertise<moveit_msgs::CollisionObject>("collision_object", 1);
while(collision_object_publisher.getNumSubscribers() < 1)
{
ros::WallDuration sleep_t(0.5);
sleep_t.sleep();
}
//Agregar objeto que colisiona
moveit_msgs::CollisionObject co;
co.header.frame_id = group.getPlanningFrame();
co.id= "muros";
shapes::Mesh* m = shapes::createMeshFromResource("package://pr2_moveit_ltl/models/ENV_6.dae");
shape_msgs::Mesh co_mesh;
shapes::ShapeMsg co_mesh_msg;
shapes::constructMsgFromShape(m,co_mesh_msg);
co_mesh = boost::get<shape_msgs::Mesh>(co_mesh_msg);
co.meshes.resize(1);
co.meshes[0] = co_mesh;
co.mesh_poses.resize(1);
co.mesh_poses[0].position.x = -5.0;
co.mesh_poses[0].position.y = -5.0;
co.mesh_poses[0].position.z = 0.0;
co.mesh_poses[0].orientation.w= 0.7071;
co.mesh_poses[0].orientation.x= 0.7071 ;
co.mesh_poses[0].orientation.y= 0.0;
co.mesh_poses[0].orientation.z= 0.0;
co.meshes.push_back(co_mesh);
co.mesh_poses.push_back(co.mesh_poses[0]);
co.operation = co.ADD;
/* Publish and sleep (to view the visualized results) */
collision_object_publisher.publish(co);
ros::WallDuration sleep_time(10.0);
sleep_time.sleep();
std::vector<moveit_msgs::CollisionObject> collision_objects;
collision_objects.push_back(co);
planning_scene_interface.addCollisionObjects(collision_objects);
sleep(10.0);
// First get the current set of joint values for the group.
std::vector<double> group_variable_values;
group.getCurrentState()->copyJointGroupPositions(group.getCurrentState()->getRobotModel()->getJointModelGroup(group.getName()), group_variable_values);
group_variable_values[0] = -4.0;
//group_variable_values[1] = -4.0;
//group_variable_values[2] = 1.5;
group.setJointValueTarget(group_variable_values);
std::cout << "Vector de joints en el grupo"<< group_variable_values.size() << std::endl;
moveit::planning_interface::MoveGroup::Plan my_plan;
group.setPlannerId("RRTstarkConfigDefault");
group.setPlanningTime(10.0);
bool success = group.plan(my_plan);
if (success){
//group.execute(my_plan);
moveit_msgs::RobotTrajectory trajState = my_plan.trajectory_;
trajectory_msgs::MultiDOFJointTrajectory traj = trajState.multi_dof_joint_trajectory;
}
std::vector<std::string> obj = planning_scene_interface.getKnownObjectNames();
if (obj.size() > 0)
{
std::cout << std::endl << "-- KNOWN COLLISION OBJECTS --" << std::endl;
for (int i = 0; i < obj.size(); ++i)
std::cout << obj[i] << std::endl;
}
moveit_msgs::RobotState stateTest = my_plan.start_state_;
sensor_msgs::JointState jointstatetest = stateTest.joint_state;
std::cout << std::endl << "-- Estado inicial --" << std::endl;
int size = sizeof(jointstatetest.name)/sizeof(std::string);
for (int i = 0; i < size; ++i){
std::cout << jointstatetest.name[i] << std::endl;
std::cout << jointstatetest.position[i] << std::endl;
std::cout << jointstatetest.velocity[i] << std::endl;
//std::cout << jointstatetest.effort[0] << std::endl;
std::cout << size << std::endl;
}
moveit_msgs::RobotTrajectory trajState = my_plan.trajectory_;
trajectory_msgs::JointTrajectory jointtraj = trajState.joint_trajectory;
int sizeString = sizeof(jointtraj.joint_names)/sizeof(std::string);
std::cout << sizeString << std::endl;
pr2_moveit_ltl::plan_msg msg;
msg.state_ = my_plan.start_state_;
msg.traj_ = my_plan.trajectory_;
chatter_pub.publish(msg);
// We can print the name of the reference frame for this robot.
ROS_INFO("Reference frame: %s", group.getPlanningFrame().c_str());
// We can also print the name of the end-effector link for this group.
ROS_INFO("Reference frame: %s", group.getEndEffectorLink().c_str());
ros::spin();
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "task_queue_client_ltl");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
sleep(10.0);
ros::ServiceClient client_a = node_handle.serviceClient<pr2_moveit_ltl::automaton_gen_msg>("automaton_gen_ltl");
ros::ServiceClient client_p = node_handle.serviceClient<pr2_moveit_ltl::task_queue_msg>("plan_gen_ltl");
pr2_moveit_ltl::automaton_gen_msg srv_auto;
srv_auto.request.client = "auto_gen_client_ltl";
pr2_moveit_ltl::automaton automata;
if (client_a.call(srv_auto))
{
automata = srv_auto.response.automata;
std::cout << "Automata:" << automata.name << std::endl;
}
else
{
ROS_ERROR("Failed to call service plan_gen_ltl");
return 1;
}
pr2_moveit_ltl::task_queue_msg srv;
srv.request.name = "task_queue_client_ltl";
srv.request.automata = automata;
pr2_moveit_ltl::proposition props[10];
bool plan = false;
int size = 0;
if (client_p.call(srv))
{
size = srv.response.size;
int i = 0;
for(std::vector<pr2_moveit_ltl::proposition>::const_iterator it = srv.response.propositions.begin(); it != srv.response.propositions.end(); ++it)
{
props[i] = *it;
i++;
}
ROS_INFO("service plan_gen_ltl on");
plan = true;
}
else
{
ROS_ERROR("Failed to call service plan_gen_ltl");
return 1;
}
//Para planear
moveit::planning_interface::MoveGroup group("base");
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
moveit_msgs::DisplayTrajectory display_trajectory;
//Collision objects
ros::Publisher collision_object_publisher = node_handle.advertise<moveit_msgs::CollisionObject>("collision_object", 1);
while(collision_object_publisher.getNumSubscribers() < 1)
{
ros::WallDuration sleep_t(0.5);
sleep_t.sleep();
}
//Agregar objeto que colisiona
moveit_msgs::CollisionObject co;
co.header.frame_id = group.getPlanningFrame();
co.id= "muros";
shapes::Mesh* m = shapes::createMeshFromResource("package://pr2_moveit_ltl/models/ENV_6.dae");
shape_msgs::Mesh co_mesh;
shapes::ShapeMsg co_mesh_msg;
shapes::constructMsgFromShape(m,co_mesh_msg);
co_mesh = boost::get<shape_msgs::Mesh>(co_mesh_msg);
co.meshes.resize(1);
co.meshes[0] = co_mesh;
co.mesh_poses.resize(1);
co.mesh_poses[0].position.x = -5.0;
co.mesh_poses[0].position.y = -5.0;
co.mesh_poses[0].position.z = 0.0;
co.mesh_poses[0].orientation.w= 0.7071;
co.mesh_poses[0].orientation.x= 0.7071 ;
co.mesh_poses[0].orientation.y= 0.0;
co.mesh_poses[0].orientation.z= 0.0;
co.meshes.push_back(co_mesh);
co.mesh_poses.push_back(co.mesh_poses[0]);
co.operation = co.ADD;
/* Publish and sleep (to view the visualized results) */
collision_object_publisher.publish(co);
ros::WallDuration sleep_time(10.0);
sleep_time.sleep();
std::vector<moveit_msgs::CollisionObject> collision_objects;
collision_objects.push_back(co);
planning_scene_interface.addCollisionObjects(collision_objects);
sleep(10.0);
/*
* Para planear
*/
if(plan){
for(int j = 0; j < size; j++){
// First get the current set of joint values for the group.
std::vector<double> group_variable_values;
group.getCurrentState()->copyJointGroupPositions(group.getCurrentState()->getRobotModel()->getJointModelGroup(group.getName()), group_variable_values);
// Now, let's modify one of the joints, plan to the new joint
// space goal and visualize the plan.
group_variable_values[0] = props[j].point.x;
group_variable_values[1] = props[j].point.y;
group_variable_values[2] = props[j].point.z;
group.setJointValueTarget(group_variable_values);
moveit::planning_interface::MoveGroup::Plan my_plan;
group.setPlannerId("RRTstarkConfigDefault");
group.setPlanningTime(20.0);
bool success = group.plan(my_plan);
//Mover el robot
if (success){
group.move();
}
sleep(10.0);
}
}
ros::spin();
return 0;
}
//引用传参比较好，不改变值的情况下生明为const安全。
void add_object(const moveit::planning_interface::MoveGroup &group)
{
ros::NodeHandle node_handle;
//添加物体
// Advertise the required topic
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
// Note that this topic may need to be remapped in the launch file
ros::Publisher planning_scene_diff_publisher = node_handle.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
while(planning_scene_diff_publisher.getNumSubscribers() < 1)
{
ros::WallDuration sleep_t(0.5);
sleep_t.sleep();
}
// Define the attached object message
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
// We will use this message to add or
// subtract the object from the world
// and to attach the object to the robot
moveit_msgs::AttachedCollisionObject attached_object;
attached_object.link_name = "lLink7";
/* The header must contain a valid TF frame*/
attached_object.object.header.frame_id = group.getPlanningFrame();
/* The id of the object */
attached_object.object.id = "box1";
/* A default left pose */
geometry_msgs::Pose pose1;
pose1.orientation.w = 1.0;
pose1.position.x=0.4;
pose1.position.y=0.64;
pose1.position.z=0.6;
/* Define a left box to be attached */
shape_msgs::SolidPrimitive primitive1;
primitive1.type = primitive1.BOX;
primitive1.dimensions.resize(3);
primitive1.dimensions[0] = 0.03;
primitive1.dimensions[1] = 0.03;
primitive1.dimensions[2] = 0.2;
/* A default right pose */
geometry_msgs::Pose pose2;
pose2.orientation.w = 1.0;
pose2.position.x=-0.4;
pose2.position.y=0.7;
pose2.position.z=0.4;
/* Define a right box to be attached */
shape_msgs::SolidPrimitive primitive2;
primitive2.type = primitive2.BOX;
primitive2.dimensions.resize(3);
primitive2.dimensions[0] = 0.3;
primitive2.dimensions[1] = 0.3;
primitive2.dimensions[2] = 0.4;
//容器使用push_back进行添加元素
attached_object.object.primitives.push_back(primitive1);
attached_object.object.primitive_poses.push_back(pose1);
attached_object.object.primitives.push_back(primitive2);
attached_object.object.primitive_poses.push_back(pose2);
// Note that attaching an object to the robot requires
// the corresponding operation to be specified as an ADD operation
attached_object.object.operation = attached_object.object.ADD;
// Add an object into the environment
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
// Add the object into the environment by adding it to
// the set of collision objects in the "world" part of the
// planning scene. Note that we are using only the "object"
// field of the attached_object message here.
ROS_INFO("Adding the object into the world ");
moveit_msgs::PlanningScene planning_scene;
planning_scene.world.collision_objects.push_back(attached_object.object);
planning_scene.is_diff = true;
planning_scene_diff_publisher.publish(planning_scene);
sleep(2);
}
