6 #include <sensor_msgs/Joy.h> 8 #include <control_msgs/GripperCommandAction.h> 10 #include <control_msgs/GripperCommandAction.h> 51 const control_msgs::GripperCommandResultConstPtr& result)
53 _gripperWorking =
false;
59 if(!isDeadManActive) {
60 if(msg->axes[_hardProfileIndex] == -1.0) {
61 if(msg->axes[_rightLeftAxis] == 1) {
63 }
else if(msg->axes[_rightLeftAxis] == -1) {
67 if(msg->axes[_upDownAxis] == 1) {
69 }
else if(msg->axes[_upDownAxis] == -1) {
73 if(msg->buttons[_xButtonIndex] == 1) {
75 }
else if(msg->buttons[_bButtonIndex] == 1) {
78 if(msg->buttons[_yButtonIndex] == 1) {
80 }
else if(msg->buttons[_aButtonIndex] == 1) {
85 }
else if(msg->axes[_softProfileIndex] == -1.0) {
86 if(msg->axes[_rightLeftAxis] == 1) {
88 }
else if(msg->axes[_rightLeftAxis] == -1) {
92 if(msg->axes[_upDownAxis] == 1) {
94 }
else if (msg->axes[_upDownAxis] == -1) {
98 if(msg->buttons[_yButtonIndex] == 1) {
100 }
else if(msg->buttons[_aButtonIndex] == 1) {
108 return _rotation1State == 1 || _rotation1State == -1
109 || _rotation2State == 1 || _rotation2State == -1
110 || _shoulder1State == 1 || _shoulder1State == -1
111 || _shoulder2State == 1 || _shoulder2State == -1
112 || _shoulder3State == 1 || _shoulder3State == -1
113 || _wristState == 1 || _wristState == -1
114 || _gripperState == 1 || _gripperState == -1;
126 _gripperWorking =
false;
132 ros::param::param<int>(
"drive_joy_teleop_deadman_button",
_deadManIndex, 8);
135 ros::param::param<int>(
"arm_joy_node_x_button_index",
_xButtonIndex, 3);
136 ros::param::param<int>(
"arm_joy_node_y_button_index",
_yButtonIndex, 4);
137 ros::param::param<int>(
"arm_joy_node_a_button_index",
_aButtonIndex, 0);
138 ros::param::param<int>(
"arm_joy_node_b_button_index",
_bButtonIndex, 1);
139 ros::param::param<int>(
"arm_joy_node_up_down_index",
_upDownAxis, 7);
140 ros::param::param<int>(
"arm_joy_node_left_right_index",
_rightLeftAxis, 6);
154 std::vector<double> group_variable_values;
157 group_variable_values);
163 if(_rotation1State == -1) {
166 }
else if(_rotation1State == 1) {
171 if(_shoulder1State == -1) {
174 }
else if(_shoulder1State == 1) {
179 if(_shoulder2State == -1) {
182 }
else if(_shoulder2State == 1) {
187 if(_rotation2State == -1) {
190 }
else if(_rotation2State == 1) {
195 if(_shoulder3State == -1) {
198 }
else if(_shoulder3State == 1) {
203 if(_wristState == -1) {
206 }
else if(_wristState == 1) {
211 if(_gripperState == -1) {
213 if(!_gripperWorking) {
219 }
else if(_gripperState == 1) {
221 if (!_gripperWorking) {
234 bool success = _group.
plan(my_plan);
251 int main(
int argc,
char** argv) {
ros::AsyncSpinner _spinner
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
bool setJointValueTarget(const std::vector< double > &group_variable_values)
float _shoulder2IncreamentValue
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ROSCPP_DECL const std::string & getName()
int main(int argc, char **argv)
float _shoulder3IncreamentValue
void setPlannerId(const std::string &planner_id)
float _rotation2IncreamentValue
const std::string & getName() const
void joyCallback(const sensor_msgs::Joy::ConstPtr &msg)
void doneCb(const actionlib::SimpleClientGoalState &state, const control_msgs::GripperCommandResultConstPtr &result)
float _rotation1IncreamentValue
ros::NodeHandle _nodeHandle
moveit::planning_interface::MoveGroup _group
actionlib::SimpleActionClient< control_msgs::GripperCommandAction > GripperClient
MoveItErrorCode plan(Plan &plan)
float _wristIncreamentValue
robot_state::RobotStatePtr getCurrentState()
float _shoulder1IncreamentValue