test_gripper_joint_trajectory.cpp
Go to the documentation of this file.
00001 /*
00002  * UOS-ROS packages - Robot Operating System code by the University of Osnabrück
00003  * Copyright (C) 2011  University of Osnabrück
00004  *
00005  * This program is free software; you can redistribute it and/or
00006  * modify it under the terms of the GNU General Public License
00007  * as published by the Free Software Foundation; either version 2
00008  * of the License, or (at your option) any later version.
00009  *
00010  * This program is distributed in the hope that it will be useful,
00011  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00012  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00013  * GNU General Public License for more details.
00014  *
00015  * You should have received a copy of the GNU General Public License
00016  * along with this program; if not, write to the Free Software
00017  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
00018  *
00019  * test_gripper_joint_trajectory.cpp
00020  *
00021  *  Created on: 24.10.2011
00022  *      Author: Karl Glatz <glatz@hs-weingarten.de>
00023  *              Ravensburg-Weingarten, University of Applied Sciences
00024  *
00025  *    based on: min_max_trajectory.cpp
00026  */
00027 #include <ros/ros.h>
00028 #include <control_msgs/JointTrajectoryAction.h>
00029 #include <actionlib/client/simple_action_client.h>
00030 #include <sensor_msgs/JointState.h>
00031 
00032 typedef actionlib::SimpleActionClient<control_msgs::JointTrajectoryAction> TrajClient;
00033 
00035 static const double GRIPPER_OPEN_ANGLE = 0.30;
00036 
00038 static const double GRIPPER_CLOSED_ANGLE = -0.44;
00039 
00041 //static const double DEFAULT_GRIPPER_OBJECT_PRESENCE_THRESHOLD = -0.43;
00042 
00044 //static const double GRIPPER_OPENING_CLOSING_DURATION = 6.0;
00045 
00046 class TestGripperJointTrajectory
00047 {
00048 private:
00049   // Action client for the joint trajectory action
00050   // used to trigger the arm movement action
00051   TrajClient* traj_client_;
00052 
00053   ros::NodeHandle nh_;
00054 
00055   ros::Subscriber joint_state_sub_;
00056   std::vector<std::string> joint_names_;
00057   bool got_joint_state_;
00058   std::vector<double> current_joint_state_;
00059   ros::AsyncSpinner spinner_;
00060 
00061 
00062   void jointStateCB(const sensor_msgs::JointState::ConstPtr &msg)
00063   {
00064     std::vector<double> ordered_js;
00065 
00066     ordered_js.resize(joint_names_.size());
00067 
00068     for (size_t i = 0; i < joint_names_.size(); ++i)
00069     {
00070       bool found = false;
00071       for (size_t j = 0; j < msg->name.size(); ++j)
00072       {
00073         if (joint_names_[i] == msg->name[j])
00074         {
00075           ordered_js[i] = msg->position[j];
00076           found = true;
00077           break;
00078         }
00079       }
00080       if (!found)
00081         return;
00082     }
00083 
00084     ROS_INFO_ONCE("Got joint state!");
00085     current_joint_state_ = ordered_js;
00086     got_joint_state_ = true;
00087   }
00088 
00089 public:
00090   TestGripperJointTrajectory() :
00091     got_joint_state_(false), spinner_(1)
00092   {
00093     joint_names_.push_back("katana_l_finger_joint");
00094     joint_names_.push_back("katana_r_finger_joint");
00095 
00096     joint_state_sub_ = nh_.subscribe("/joint_states", 1, &TestGripperJointTrajectory::jointStateCB, this);
00097     spinner_.start();
00098 
00099     // tell the action client that we want to spin a thread by default
00100     traj_client_ = new TrajClient("katana_arm_controller/gripper_joint_trajectory_action", true);
00101 
00102     // wait for action server to come up
00103     while (!traj_client_->waitForServer(ros::Duration(5.0)) && ros::ok())
00104     {
00105       ROS_INFO("Waiting for the joint_trajectory_action server");
00106     }
00107 
00108     ROS_INFO("The joint_trajectory_action server is available");
00109 
00110   }
00111 
00112   ~TestGripperJointTrajectory()
00113   {
00114     delete traj_client_;
00115   }
00116 
00118   void startTrajectory(control_msgs::JointTrajectoryGoal goal)
00119   {
00120     // When to start the trajectory: 1s from now
00121     goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(1.0);
00122     traj_client_->sendGoal(goal);
00123   }
00124 
00125   control_msgs::JointTrajectoryGoal closeGoal()
00126   {
00127     control_msgs::JointTrajectoryGoal goal;
00128 
00129     // set up the joint names
00130     goal.trajectory.joint_names = joint_names_;
00131 
00132     // set some values
00133 
00134     // num of points
00135     goal.trajectory.points.resize(2);
00136 
00137     // number of joints in each point
00138     goal.trajectory.points[0].positions.resize(2);
00139     goal.trajectory.points[0].velocities.resize(2);
00140 
00141     goal.trajectory.points[0].positions[0] = current_joint_state_[0];
00142     goal.trajectory.points[0].velocities[0] = 0.0;
00143 
00144     goal.trajectory.points[0].positions[1] = current_joint_state_[1];
00145     goal.trajectory.points[0].velocities[1] = 0.0;
00146 
00147     goal.trajectory.points[0].time_from_start = ros::Duration(0.0);
00148 
00149     // close
00150 
00151     // number of joints in each point
00152     goal.trajectory.points[1].positions.resize(2);
00153     goal.trajectory.points[1].velocities.resize(2);
00154 
00155     goal.trajectory.points[1].positions[0] = GRIPPER_CLOSED_ANGLE;
00156     goal.trajectory.points[1].velocities[0] = 0.0;
00157 
00158     goal.trajectory.points[1].positions[1] = GRIPPER_CLOSED_ANGLE;
00159     goal.trajectory.points[1].velocities[1] = 0.0;
00160 
00161     goal.trajectory.points[1].time_from_start = ros::Duration(3.0);
00162 
00163 
00164     return goal;
00165   }
00166 
00167   control_msgs::JointTrajectoryGoal openGoal()
00168   {
00169     control_msgs::JointTrajectoryGoal goal;
00170 
00171     // set up the joint names
00172     goal.trajectory.joint_names = joint_names_;
00173 
00174     // set some values
00175 
00176     // num of points
00177     goal.trajectory.points.resize(2);
00178 
00179     // number of joints in each point
00180     goal.trajectory.points[0].positions.resize(2);
00181     goal.trajectory.points[0].velocities.resize(2);
00182 
00183     goal.trajectory.points[0].positions[0] = current_joint_state_[0];
00184     goal.trajectory.points[0].velocities[0] = 0.0;
00185 
00186     goal.trajectory.points[0].positions[1] = current_joint_state_[1];
00187     goal.trajectory.points[0].velocities[1] = 0.0;
00188 
00189     goal.trajectory.points[0].time_from_start = ros::Duration(0.0);
00190 
00191     // close
00192 
00193     // number of joints in each point
00194     goal.trajectory.points[1].positions.resize(2);
00195     goal.trajectory.points[1].velocities.resize(2);
00196 
00197     goal.trajectory.points[1].positions[0] = GRIPPER_OPEN_ANGLE;
00198     goal.trajectory.points[1].velocities[0] = 0.0;
00199 
00200     goal.trajectory.points[1].positions[1] = GRIPPER_OPEN_ANGLE;
00201     goal.trajectory.points[1].velocities[1] = 0.0;
00202 
00203     goal.trajectory.points[1].time_from_start = ros::Duration(3.0);
00204 
00205     return goal;
00206   }
00207 
00209   actionlib::SimpleClientGoalState getState()
00210   {
00211     return traj_client_->getState();
00212   }
00213 
00214 };
00215 
00216 int main(int argc, char** argv)
00217 {
00218 
00219   // Init the ROS node
00220   ros::init(argc, argv, "test_gripper_joint_trajectory");
00221 
00222   TestGripperJointTrajectory test;
00223 
00224   ROS_INFO("Start, press CRTL+C to stop!");
00225   while (ros::ok())
00226   {
00227 
00228     ROS_INFO("Send closeGoal");
00229     test.startTrajectory(test.closeGoal());
00230     while (!test.getState().isDone() && ros::ok())
00231     {
00232       usleep(50000);
00233     }
00234 
00235     ROS_INFO("closeGoal %s", test.getState().toString().c_str());
00236 
00237     usleep(1000000); // 1 sec
00238 
00239     ROS_INFO("Send openGoal");
00240     test.startTrajectory(test.openGoal());
00241     while (!test.getState().isDone() && ros::ok())
00242     {
00243       usleep(50000);
00244     }
00245 
00246     ROS_INFO("openGoal %s", test.getState().toString().c_str());
00247 
00248     usleep(1000000); // 1 sec
00249 
00250   }
00251 
00252   return 0;
00253 
00254 }
00255 


katana_gazebo_plugins
Author(s): Martin Günther
autogenerated on Thu Jun 6 2019 21:42:23