Go to the documentation of this file.00001
00002 #include <ros/ros.h>
00003 #include <control_msgs/FollowJointTrajectoryActionGoal.h>
00004 #include <control_msgs/FollowJointTrajectoryActionFeedback.h>
00005 #include <control_msgs/FollowJointTrajectoryAction.h>
00006 #include <moveit/move_group_interface/move_group.h>
00007 #include <actionlib/client/action_client.h>
00008
00009 bool debug=true;
00010
00011
00012 int main(int argc, char **argv) {
00013 ros::init(argc, argv,"moveit_group_goal");
00014 ros::AsyncSpinner spinner(3);
00015 spinner.start();
00016 ros::NodeHandle n;
00017
00018 moveit::planning_interface::MoveGroup group("arm");
00019
00020 actionlib::ActionClient<control_msgs::FollowJointTrajectoryAction> ac(n, "/arm_trajectory_controller/follow_joint_trajectory");
00021
00022 std::vector< double > current_vals = group.getCurrentJointValues();
00023 std::vector< std::string > joint_names = group.getJoints();
00024 joint_names.resize(6);
00025
00026 std::vector< double > vals;
00027 vals.resize(6);
00028
00029
00030 for (int i=0;i<vals.size();i++) {
00031 vals[i]=0;
00032 }
00033
00034
00035 for (int i=0;i<current_vals.size();i++) {
00036
00037 std::cout << joint_names[i] <<" "<<current_vals[i] << std::endl;
00038 }
00039
00040
00041 std::srand(std::time(0));
00042
00043 control_msgs::FollowJointTrajectoryActionGoal goal;
00044 goal.header.stamp = ros::Time::now();
00045
00046 goal.goal_id.stamp = ros::Time::now();
00047 std::ostringstream oss;
00048 oss << "goal_" << std::rand();
00049 goal.goal_id.id = oss.str();
00050
00051 goal.goal.trajectory.joint_names=joint_names;
00052 goal.goal.trajectory.header.frame_id="/map";
00053
00054 trajectory_msgs::JointTrajectoryPoint jtp;
00055 jtp.time_from_start = ros::Duration(5.0);
00056 jtp.positions=vals;
00057 jtp.velocities.resize(6);
00058 jtp.accelerations.resize(6);
00059
00060 goal.goal.trajectory.points.push_back(jtp);
00061
00062 for (int i=0;i<vals.size();i++) {
00063 vals[i]=-0.2;
00064 }
00065 jtp.time_from_start = ros::Duration(10.0);
00066 jtp.positions=vals;
00067 goal.goal.trajectory.points.push_back(jtp);
00068
00069 ac.sendGoal(goal.goal);
00070
00071
00072
00073 if (debug) {
00074 std::stringstream s;
00075 std::string indent("*> ");
00076 ros::message_operations::Printer<control_msgs::FollowJointTrajectoryActionGoal> p;
00077 p.stream(s, indent, goal);
00078 std::cout << s.str() << std::endl;
00079 }
00080
00081
00082 ros::spin();
00083
00084 return 0;
00085 }