23 priv_node_handle_(
"~")
31 ROS_INFO(
"OpenManipulator initialization");
58 std::vector<double> temp_angle;
60 for(std::vector<int>::size_type i = 0; i < msg->name.size(); i ++)
62 if(!msg->name.at(i).compare(
"joint1")) temp_angle.at(0) = (msg->position.at(i));
63 else if(!msg->name.at(i).compare(
"joint2")) temp_angle.at(1) = (msg->position.at(i));
64 else if(!msg->name.at(i).compare(
"joint3")) temp_angle.at(2) = (msg->position.at(i));
65 else if(!msg->name.at(i).compare(
"joint4")) temp_angle.at(3) = (msg->position.at(i));
73 std::vector<double> temp_position;
74 temp_position.push_back(msg->pose.position.x);
75 temp_position.push_back(msg->pose.position.y);
76 temp_position.push_back(msg->pose.position.z);
91 open_manipulator_msgs::SetJointPosition srv;
92 srv.request.joint_position.joint_name = joint_name;
93 srv.request.joint_position.position = joint_angle;
94 srv.request.path_time = path_time;
98 return srv.response.is_planned;
105 open_manipulator_msgs::SetJointPosition srv;
106 srv.request.joint_position.joint_name = joint_name;
107 srv.request.joint_position.position = joint_angle;
108 srv.request.path_time = path_time;
112 return srv.response.is_planned;
119 open_manipulator_msgs::SetJointPosition srv;
120 srv.request.joint_position.joint_name.push_back(
priv_node_handle_.
param<std::string>(
"end_effector_name",
"gripper"));
121 srv.request.joint_position.position = joint_angle;
125 return srv.response.is_planned;
132 open_manipulator_msgs::SetKinematicsPose srv;
134 srv.request.kinematics_pose.pose.position.x = kinematics_pose.at(0);
135 srv.request.kinematics_pose.pose.position.y = kinematics_pose.at(1);
136 srv.request.kinematics_pose.pose.position.z = kinematics_pose.at(2);
137 srv.request.path_time = path_time;
141 return srv.response.is_planned;
149 printf(
"---------------------------\n");
150 printf(
"Control Your OpenManipulator!\n");
151 printf(
"---------------------------\n");
152 printf(
"w : increase x axis in task space\n");
153 printf(
"s : decrease x axis in task space\n");
154 printf(
"a : increase y axis in task space\n");
155 printf(
"d : decrease y axis in task space\n");
156 printf(
"z : increase z axis in task space\n");
157 printf(
"x : decrease z axis in task space\n");
159 printf(
"y : increase joint 1 angle\n");
160 printf(
"h : decrease joint 1 angle\n");
161 printf(
"u : increase joint 2 angle\n");
162 printf(
"j : decrease joint 2 angle\n");
163 printf(
"i : increase joint 3 angle\n");
164 printf(
"k : decrease joint 3 angle\n");
165 printf(
"o : increase joint 4 angle\n");
166 printf(
"l : decrease joint 4 angle\n");
168 printf(
"g : gripper open\n");
169 printf(
"f : gripper close\n");
171 printf(
"1 : init pose\n");
172 printf(
"2 : home pose\n");
174 printf(
"q to quit\n");
175 printf(
"---------------------------\n");
177 printf(
"Present Joint Angle J1: %.3lf J2: %.3lf J3: %.3lf J4: %.3lf\n",
182 printf(
"Present Kinematics Position X: %.3lf Y: %.3lf Z: %.3lf\n",
186 printf(
"---------------------------\n");
192 std::vector<double> goalPose; goalPose.resize(3, 0.0);
193 std::vector<double> goalJoint; goalJoint.resize(4, 0.0);
195 if(ch ==
'w' || ch ==
'W')
197 printf(
"input : w \tincrease(++) x axis in task space\n");
198 goalPose.at(0) =
DELTA;
201 else if(ch ==
's' || ch ==
'S')
203 printf(
"input : s \tdecrease(--) x axis in task space\n");
204 goalPose.at(0) = -
DELTA;
207 else if(ch ==
'a' || ch ==
'A')
209 printf(
"input : a \tincrease(++) y axis in task space\n");
210 goalPose.at(1) =
DELTA;
213 else if(ch ==
'd' || ch ==
'D')
215 printf(
"input : d \tdecrease(--) y axis in task space\n");
216 goalPose.at(1) = -
DELTA;
219 else if(ch ==
'z' || ch ==
'Z')
221 printf(
"input : z \tincrease(++) z axis in task space\n");
222 goalPose.at(2) =
DELTA;
225 else if(ch ==
'x' || ch ==
'X')
227 printf(
"input : x \tdecrease(--) z axis in task space\n");
228 goalPose.at(2) = -
DELTA;
231 else if(ch ==
'y' || ch ==
'Y')
233 printf(
"input : y \tincrease(++) joint 1 angle\n");
234 std::vector<std::string> joint_name;
235 joint_name.push_back(
"joint1"); goalJoint.at(0) =
JOINT_DELTA;
236 joint_name.push_back(
"joint2");
237 joint_name.push_back(
"joint3");
238 joint_name.push_back(
"joint4");
241 else if(ch ==
'h' || ch ==
'H')
243 printf(
"input : h \tdecrease(--) joint 1 angle\n");
244 std::vector<std::string> joint_name;
245 joint_name.push_back(
"joint1"); goalJoint.at(0) = -
JOINT_DELTA;
246 joint_name.push_back(
"joint2");
247 joint_name.push_back(
"joint3");
248 joint_name.push_back(
"joint4");
252 else if(ch ==
'u' || ch ==
'U')
254 printf(
"input : u \tincrease(++) joint 2 angle\n");
255 std::vector<std::string> joint_name;
256 joint_name.push_back(
"joint1");
257 joint_name.push_back(
"joint2"); goalJoint.at(1) =
JOINT_DELTA;
258 joint_name.push_back(
"joint3");
259 joint_name.push_back(
"joint4");
262 else if(ch ==
'j' || ch ==
'J')
264 printf(
"input : j \tdecrease(--) joint 2 angle\n");
265 std::vector<std::string> joint_name;
266 joint_name.push_back(
"joint1");
267 joint_name.push_back(
"joint2"); goalJoint.at(1) = -
JOINT_DELTA;
268 joint_name.push_back(
"joint3");
269 joint_name.push_back(
"joint4");
273 else if(ch ==
'i' || ch ==
'I')
275 printf(
"input : i \tincrease(++) joint 3 angle\n");
276 std::vector<std::string> joint_name;
277 joint_name.push_back(
"joint1");
278 joint_name.push_back(
"joint2");
279 joint_name.push_back(
"joint3"); goalJoint.at(2) =
JOINT_DELTA;
280 joint_name.push_back(
"joint4");
283 else if(ch ==
'k' || ch ==
'K')
285 printf(
"input : k \tdecrease(--) joint 3 angle\n");
286 std::vector<std::string> joint_name;
287 joint_name.push_back(
"joint1");
288 joint_name.push_back(
"joint2");
289 joint_name.push_back(
"joint3"); goalJoint.at(2) = -
JOINT_DELTA;
290 joint_name.push_back(
"joint4");
294 else if(ch ==
'o' || ch ==
'O')
296 printf(
"input : o \tincrease(++) joint 4 angle\n");
297 std::vector<std::string> joint_name;
298 joint_name.push_back(
"joint1");
299 joint_name.push_back(
"joint2");
300 joint_name.push_back(
"joint3");
301 joint_name.push_back(
"joint4"); goalJoint.at(3) =
JOINT_DELTA;
304 else if(ch ==
'l' || ch ==
'L')
306 printf(
"input : l \tdecrease(--) joint 4 angle\n");
307 std::vector<std::string> joint_name;
308 joint_name.push_back(
"joint1");
309 joint_name.push_back(
"joint2");
310 joint_name.push_back(
"joint3");
311 joint_name.push_back(
"joint4"); goalJoint.at(3) = -
JOINT_DELTA;
315 else if(ch ==
'g' || ch ==
'G')
317 printf(
"input : g \topen gripper\n");
318 std::vector<double> joint_angle;
320 joint_angle.push_back(0.01);
323 else if(ch ==
'f' || ch ==
'F')
325 printf(
"input : f \tclose gripper\n");
326 std::vector<double> joint_angle;
327 joint_angle.push_back(-0.01);
333 printf(
"input : 2 \thome pose\n");
334 std::vector<std::string> joint_name;
335 std::vector<double> joint_angle;
336 double path_time = 2.0;
338 joint_name.push_back(
"joint1"); joint_angle.push_back(0.0);
339 joint_name.push_back(
"joint2"); joint_angle.push_back(-1.05);
340 joint_name.push_back(
"joint3"); joint_angle.push_back(0.35);
341 joint_name.push_back(
"joint4"); joint_angle.push_back(0.70);
346 printf(
"input : 1 \tinit pose\n");
348 std::vector<std::string> joint_name;
349 std::vector<double> joint_angle;
350 double path_time = 2.0;
351 joint_name.push_back(
"joint1"); joint_angle.push_back(0.0);
352 joint_name.push_back(
"joint2"); joint_angle.push_back(0.0);
353 joint_name.push_back(
"joint3"); joint_angle.push_back(0.0);
354 joint_name.push_back(
"joint4"); joint_angle.push_back(0.0);
361 tcsetattr(0, TCSANOW, &
oldt_);
368 tcgetattr(0, &
oldt_);
370 newt.c_lflag &= ~(ICANON | ECHO);
371 tcsetattr(0, TCSANOW, &newt);
374 int main(
int argc,
char **argv)
377 ros::init(argc, argv,
"open_manipulator_teleop");
381 ROS_INFO(
"OpenManipulator teleoperation using keyboard start");
388 while (
ros::ok() && (ch = std::getchar()) !=
'q')
393 openManipulatorTeleop.
setGoal(ch);
396 printf(
"input : q \tTeleop. is finished\n");
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
std::vector< double > getPresentKinematicsPose()
ros::ServiceClient goal_joint_space_path_client_
void jointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg)
ros::ServiceClient goal_tool_control_client_
std::vector< double > getPresentJointAngle()
ros::ServiceClient goal_joint_space_path_from_present_client_
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
bool setTaskSpacePathFromPresentPositionOnly(std::vector< double > kinematics_pose, double path_time)
void restoreTerminalSettings(void)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool call(MReq &req, MRes &res)
void kinematicsPoseCallback(const open_manipulator_msgs::KinematicsPose::ConstPtr &msg)
std::vector< double > present_joint_angle_
bool setJointSpacePath(std::vector< std::string > joint_name, std::vector< double > joint_angle, double path_time)
std::vector< double > present_kinematic_position_
bool setJointSpacePathFromPresent(std::vector< std::string > joint_name, std::vector< double > joint_angle, double path_time)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
ros::ServiceClient goal_task_space_path_from_present_position_only_client_
ros::NodeHandle priv_node_handle_
void setGoal(const char *str)
ros::Subscriber joint_states_sub_
int main(int argc, char **argv)
bool setToolControl(std::vector< double > joint_angle)
ROSCPP_DECL bool isStarted()
ros::Subscriber kinematics_pose_sub_
ros::NodeHandle node_handle_
ROSCPP_DECL void shutdown()
void disableWaitingForEnter(void)
ROSCPP_DECL void spinOnce()
ROSCPP_DECL void waitForShutdown()