Go to the documentation of this file.00001
00002
00003
00004
00005 #include <ros/ros.h>
00006 #include <sensor_msgs/Joy.h>
00007 #include <moveit/move_group_interface/move_group.h>
00008 #include <control_msgs/GripperCommandAction.h>
00009 #include <actionlib/client/simple_action_client.h>
00010 #include <control_msgs/GripperCommandAction.h>
00011
00012 #define DEBUG_JOY_ARM
00013
00014 typedef actionlib::SimpleActionClient<control_msgs::GripperCommandAction> GripperClient;
00015
00016 class ArmJoyNode {
00017 private:
00018 ros::NodeHandle _nodeHandle;
00019 ros::AsyncSpinner _spinner;
00020 moveit::planning_interface::MoveGroup _group;
00021 float _rotation1IncreamentValue;
00022 float _rotation2IncreamentValue;
00023 float _shoulder1IncreamentValue;
00024 float _shoulder2IncreamentValue;
00025 float _shoulder3IncreamentValue;
00026 float _wristIncreamentValue;
00027
00028 int _rotation1State;
00029 int _rotation2State;
00030 int _shoulder1State;
00031 int _shoulder2State;
00032 int _shoulder3State;
00033 int _wristState;
00034 int _gripperState;
00035
00036
00037 int _deadManIndex;
00038 int _hardProfileIndex;
00039 int _softProfileIndex;
00040 int _xButtonIndex;
00041 int _yButtonIndex;
00042 int _aButtonIndex;
00043 int _bButtonIndex;
00044 int _upDownAxis;
00045 int _rightLeftAxis;
00046 bool _gripperWorking;
00047
00048 ros::Subscriber _joySub;
00049
00050 void doneCb(const actionlib::SimpleClientGoalState& state,
00051 const control_msgs::GripperCommandResultConstPtr& result)
00052 {
00053 _gripperWorking = false;
00054 }
00055
00056 void joyCallback(const sensor_msgs::Joy::ConstPtr &msg) {
00057 bool isDeadManActive = msg->buttons[_deadManIndex] == 1;
00058
00059 if(!isDeadManActive) {
00060 if(msg->axes[_hardProfileIndex] == -1.0) {
00061 if(msg->axes[_rightLeftAxis] == 1) {
00062 _rotation1State = 1;
00063 } else if(msg->axes[_rightLeftAxis] == -1) {
00064 _rotation1State = -1;
00065 }
00066
00067 if(msg->axes[_upDownAxis] == 1) {
00068 _shoulder1State = -1;
00069 } else if(msg->axes[_upDownAxis] == -1) {
00070 _shoulder1State = 1;
00071 }
00072
00073 if(msg->buttons[_xButtonIndex] == 1) {
00074 _rotation2State = -1;
00075 } else if(msg->buttons[_bButtonIndex] == 1) {
00076 _rotation2State = 1;
00077 }
00078 if(msg->buttons[_yButtonIndex] == 1) {
00079 _shoulder2State = -1;
00080 } else if(msg->buttons[_aButtonIndex] == 1) {
00081 _shoulder2State = 1;
00082 }
00083
00084
00085 } else if(msg->axes[_softProfileIndex] == -1.0) {
00086 if(msg->axes[_rightLeftAxis] == 1) {
00087 _wristState = -1;
00088 } else if(msg->axes[_rightLeftAxis] == -1) {
00089 _wristState = 1;
00090 }
00091
00092 if(msg->axes[_upDownAxis] == 1) {
00093 _shoulder3State = -1;
00094 } else if (msg->axes[_upDownAxis] == -1) {
00095 _shoulder3State = 1;
00096 }
00097
00098 if(msg->buttons[_yButtonIndex] == 1) {
00099 _gripperState = 1;
00100 } else if(msg->buttons[_aButtonIndex] == 1) {
00101 _gripperState = -1;
00102 }
00103 }
00104 }
00105
00106 }
00107 bool haveMoveGoal() {
00108 return _rotation1State == 1 || _rotation1State == -1
00109 || _rotation2State == 1 || _rotation2State == -1
00110 || _shoulder1State == 1 || _shoulder1State == -1
00111 || _shoulder2State == 1 || _shoulder2State == -1
00112 || _shoulder3State == 1 || _shoulder3State == -1
00113 || _wristState == 1 || _wristState == -1
00114 || _gripperState == 1 || _gripperState == -1;
00115 }
00116 public:
00117 ArmJoyNode(): _nodeHandle(), _spinner(2), _group("arm") {
00118 ROS_INFO("[%s]: Arm joy node is active", ros::this_node::getName().c_str());
00119 _rotation1State = 0;
00120 _rotation2State = 0;
00121 _shoulder1State = 0;
00122 _shoulder2State = 0;
00123 _shoulder3State = 0;
00124 _wristState = 0;
00125 _gripperState = 0;
00126 _gripperWorking = false;
00127 _joySub = _nodeHandle.subscribe<sensor_msgs::Joy>("joy", 10, &ArmJoyNode::joyCallback, this);
00128 _group.setPlannerId("RRTConnectkConfigDefault");
00129
00130
00131
00132 ros::param::param<int>("drive_joy_teleop_deadman_button", _deadManIndex, 8);
00133 ros::param::param<int>("arm_joy_node_hard_profile_index", _hardProfileIndex, 4);
00134 ros::param::param<int>("arm_joy_node_soft_profile_index", _softProfileIndex, 5);
00135 ros::param::param<int>("arm_joy_node_x_button_index", _xButtonIndex, 3);
00136 ros::param::param<int>("arm_joy_node_y_button_index", _yButtonIndex, 4);
00137 ros::param::param<int>("arm_joy_node_a_button_index", _aButtonIndex, 0);
00138 ros::param::param<int>("arm_joy_node_b_button_index", _bButtonIndex, 1);
00139 ros::param::param<int>("arm_joy_node_up_down_index", _upDownAxis, 7);
00140 ros::param::param<int>("arm_joy_node_left_right_index", _rightLeftAxis, 6);
00141
00142 ros::param::param<float>("arm_joy_node_rotation1", _rotation1IncreamentValue, 0.05);
00143 ros::param::param<float>("arm_joy_node_rotation2", _rotation2IncreamentValue, 0.1);
00144 ros::param::param<float>("arm_joy_node_shoulder1", _shoulder1IncreamentValue, 0.05);
00145 ros::param::param<float>("arm_joy_node_shoulder2", _shoulder2IncreamentValue, 0.05);
00146 ros::param::param<float>("arm_joy_node_shoulder3", _shoulder3IncreamentValue, 0.05);
00147 ros::param::param<float>("arm_joy_node_wrist", _wristIncreamentValue, 0.1);
00148
00149 _spinner.start();
00150 }
00151
00152 void run() {
00153 ros::Rate loopRate(50);
00154 std::vector<double> group_variable_values;
00155 _group.getCurrentState()->copyJointGroupPositions(
00156 _group.getCurrentState()->getRobotModel()->getJointModelGroup(_group.getName()),
00157 group_variable_values);
00158
00159 while(ros::ok()) {
00160 if(haveMoveGoal()) {
00161
00162
00163 if(_rotation1State == -1) {
00164 _rotation1State = 0;
00165 group_variable_values[0] -= _rotation1IncreamentValue;
00166 } else if(_rotation1State == 1) {
00167 _rotation1State = 0;
00168 group_variable_values[0] += _rotation1IncreamentValue;
00169 }
00170
00171 if(_shoulder1State == -1) {
00172 _shoulder1State = 0;
00173 group_variable_values[1] -= _shoulder1IncreamentValue;
00174 } else if(_shoulder1State == 1) {
00175 _shoulder1State = 0;
00176 group_variable_values[1] += _shoulder1IncreamentValue;
00177 }
00178
00179 if(_shoulder2State == -1) {
00180 _shoulder2State = 0;
00181 group_variable_values[2] -= _shoulder2IncreamentValue;
00182 } else if(_shoulder2State == 1) {
00183 _shoulder2State = 0;
00184 group_variable_values[2] += _shoulder2IncreamentValue;
00185 }
00186
00187 if(_rotation2State == -1) {
00188 _rotation2State = 0;
00189 group_variable_values[3] -= _rotation2IncreamentValue;
00190 } else if(_rotation2State == 1) {
00191 _rotation2State = 0;
00192 group_variable_values[3] += _rotation2IncreamentValue;
00193 }
00194
00195 if(_shoulder3State == -1) {
00196 _shoulder3State = 0;
00197 group_variable_values[4] -= _shoulder3IncreamentValue;
00198 } else if(_shoulder3State == 1) {
00199 _shoulder3State = 0;
00200 group_variable_values[4] += _shoulder3IncreamentValue;
00201 }
00202
00203 if(_wristState == -1) {
00204 _wristState = 0;
00205 group_variable_values[5] -= _wristIncreamentValue;
00206 } else if(_wristState == 1) {
00207 _wristState = 0;
00208 group_variable_values[5] += _wristIncreamentValue;
00209 }
00210
00211 if(_gripperState == -1) {
00212 _gripperState = 0;
00213 if(!_gripperWorking) {
00214
00215 } else {
00216 ROS_WARN("[%s]: Gripper already have a goal please wait", ros::this_node::getName().c_str());
00217 }
00218
00219 } else if(_gripperState == 1) {
00220 _gripperState = 0;
00221 if (!_gripperWorking) {
00222
00223 } else {
00224 ROS_WARN("[%s]: Gripper already have a goal please wait", ros::this_node::getName().c_str());
00225 }
00226
00227 }
00228
00229
00230
00231
00232 _group.setJointValueTarget(group_variable_values);
00233 moveit::planning_interface::MoveGroup::Plan my_plan;
00234 bool success = _group.plan(my_plan);
00235 if (success) {
00236 _group.move();
00237 }
00238 else {
00239 ROS_WARN("[%s]: Invalid goal", ros::this_node::getName().c_str());
00240 }
00241 }
00242
00243 loopRate.sleep();
00244 }
00245 }
00246
00247
00248
00249 };
00250
00251 int main(int argc, char** argv) {
00252
00253 ros::init(argc, argv, "arm_joy_node");
00254 ArmJoyNode armJoy;
00255 armJoy.run();
00256
00257 return 0;
00258
00259 }