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);
81 if(msg->axes.at(1) >= 0.9)
setGoal(
"x+");
82 else if(msg->axes.at(1) <= -0.9)
setGoal(
"x-");
83 else if(msg->axes.at(0) >= 0.9)
setGoal(
"y+");
84 else if(msg->axes.at(0) <= -0.9)
setGoal(
"y-");
85 else if(msg->buttons.at(3) == 1)
setGoal(
"z+");
86 else if(msg->buttons.at(0) == 1)
setGoal(
"z-");
87 else if(msg->buttons.at(5) == 1)
setGoal(
"home");
88 else if(msg->buttons.at(4) == 1)
setGoal(
"init");
90 if(msg->buttons.at(2) == 1)
setGoal(
"gripper close");
91 else if(msg->buttons.at(1) == 1)
setGoal(
"gripper open");
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;
148 std::vector<double> goalPose; goalPose.resize(3, 0.0);
149 std::vector<double> goalJoint; goalJoint.resize(4, 0.0);
153 printf(
"increase(++) x axis in cartesian space\n");
154 goalPose.at(0) =
DELTA;
159 printf(
"decrease(--) x axis in cartesian space\n");
160 goalPose.at(0) = -
DELTA;
165 printf(
"increase(++) y axis in cartesian space\n");
166 goalPose.at(1) =
DELTA;
171 printf(
"decrease(--) y axis in cartesian space\n");
172 goalPose.at(1) = -
DELTA;
177 printf(
"increase(++) z axis in cartesian space\n");
178 goalPose.at(2) =
DELTA;
183 printf(
"decrease(--) z axis in cartesian space\n");
184 goalPose.at(2) = -
DELTA;
188 else if(str ==
"gripper open")
190 printf(
"open gripper\n");
191 std::vector<double> joint_angle;
193 joint_angle.push_back(0.01);
196 else if(str ==
"gripper close")
198 printf(
"close gripper\n");
199 std::vector<double> joint_angle;
200 joint_angle.push_back(-0.01);
204 else if(str ==
"home")
206 printf(
"home pose\n");
207 std::vector<std::string> joint_name;
208 std::vector<double> joint_angle;
209 double path_time = 2.0;
211 joint_name.push_back(
"joint1"); joint_angle.push_back(0.0);
212 joint_name.push_back(
"joint2"); joint_angle.push_back(-1.05);
213 joint_name.push_back(
"joint3"); joint_angle.push_back(0.35);
214 joint_name.push_back(
"joint4"); joint_angle.push_back(0.70);
217 else if(str ==
"init")
219 printf(
"init pose\n");
221 std::vector<std::string> joint_name;
222 std::vector<double> joint_angle;
223 double path_time = 2.0;
224 joint_name.push_back(
"joint1"); joint_angle.push_back(0.0);
225 joint_name.push_back(
"joint2"); joint_angle.push_back(0.0);
226 joint_name.push_back(
"joint3"); joint_angle.push_back(0.0);
227 joint_name.push_back(
"joint4"); joint_angle.push_back(0.0);
232 int main(
int argc,
char **argv)
235 ros::init(argc, argv,
"open_manipulator_TELEOP");
238 ROS_INFO(
"OpenManipulator teleoperation using joystick start");
242 printf(
"Teleop. 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()
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)
int main(int argc, char **argv)
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 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_
ros::Subscriber joy_command_sub_
void joyCallback(const sensor_msgs::Joy::ConstPtr &msg)
void setGoal(const char *str)
ros::Subscriber joint_states_sub_
bool setToolControl(std::vector< double > joint_angle)
ROSCPP_DECL bool isStarted()
ros::Subscriber kinematics_pose_sub_
ros::NodeHandle node_handle_
ROSCPP_DECL void shutdown()
ROSCPP_DECL void waitForShutdown()