Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #ifndef ROS_CLIENT_HPP
00036 #define ROS_CLIENT_HPP
00037
00038 #include <ros/ros.h>
00039 #include <control_msgs/FollowJointTrajectoryAction.h>
00040 #include <control_msgs/FollowJointTrajectoryActionGoal.h>
00041 #include <control_msgs/FollowJointTrajectoryGoal.h>
00042 #include <trajectory_msgs/JointTrajectoryPoint.h>
00043 #include <actionlib/client/simple_action_client.h>
00044 #include <string>
00045 #include <vector>
00046 #include <cmath>
00047
00048 class ROS_Client
00049 {
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064 public:
00065 typedef actionlib::SimpleActionClient<control_msgs::FollowJointTrajectoryAction> TrajClient;
00066
00067 control_msgs::FollowJointTrajectoryGoal goal;
00068
00069 ROS_Client() :
00070 GR_TORSO("torso"), GR_HEAD("head"), GR_LARM("larm"), GR_RARM("rarm"), MSG_ASK_ISSUEREPORT(
00071 "Your report to https://github.com/start-jsk/rtmros_hironx/issues "
00072 "about the issue you are seeing is appreciated.")
00073 {
00074 }
00075
00076 explicit ROS_Client(std::vector<std::string> joingroups) :
00077 MSG_ASK_ISSUEREPORT(
00078 "Your report to https://github.com/start-jsk/rtmros_hironx/issues "
00079 "about the issue you are seeing is appreciated.")
00080 {
00081 set_groupnames(joingroups);
00082 }
00083
00084 ROS_Client(const ROS_Client& obj)
00085 {
00086 GR_TORSO = obj.GR_TORSO;
00087 GR_HEAD = obj.GR_HEAD;
00088 GR_LARM = obj.GR_LARM;
00089 GR_RARM = obj.GR_RARM;
00090 }
00091
00092 ROS_Client& operator=(const ROS_Client& obj)
00093 {
00094 GR_TORSO = obj.GR_TORSO;
00095 GR_HEAD = obj.GR_HEAD;
00096 GR_LARM = obj.GR_LARM;
00097 GR_RARM = obj.GR_RARM;
00098 return *this;
00099 }
00100
00101 ~ROS_Client()
00102 {
00103 }
00104
00105 void init_action_clients()
00106 {
00107 static TrajClient aclient_larm("/larm_controller/joint_trajectory_action", true);
00108 static TrajClient aclient_rarm("/rarm_controller/joint_trajectory_action", true);
00109 static TrajClient aclient_head("/head_controller/joint_trajectory_action", true);
00110 static TrajClient aclient_torso("/torso_controller/joint_trajectory_action", true);
00111
00112 aclient_larm.waitForServer();
00113 ROS_INFO("ros_client; 1");
00114 goal_larm = control_msgs::FollowJointTrajectoryGoal();
00115 ROS_INFO("ros_client; 2");
00116 goal_larm.trajectory.joint_names.push_back("LARM_JOINT0");
00117 goal_larm.trajectory.joint_names.push_back("LARM_JOINT1");
00118 goal_larm.trajectory.joint_names.push_back("LARM_JOINT2");
00119 goal_larm.trajectory.joint_names.push_back("LARM_JOINT3");
00120 goal_larm.trajectory.joint_names.push_back("LARM_JOINT4");
00121 goal_larm.trajectory.joint_names.push_back("LARM_JOINT5");
00122 ROS_INFO("ros_client; 3");
00123
00124 aclient_rarm.waitForServer();
00125 goal_rarm = control_msgs::FollowJointTrajectoryGoal();
00126 goal_rarm.trajectory.joint_names.push_back("RARM_JOINT0");
00127 goal_rarm.trajectory.joint_names.push_back("RARM_JOINT1");
00128 goal_rarm.trajectory.joint_names.push_back("RARM_JOINT2");
00129 goal_rarm.trajectory.joint_names.push_back("RARM_JOINT3");
00130 goal_rarm.trajectory.joint_names.push_back("RARM_JOINT4");
00131 goal_rarm.trajectory.joint_names.push_back("RARM_JOINT5");
00132
00133 aclient_head.waitForServer();
00134 goal_head = control_msgs::FollowJointTrajectoryGoal();
00135 goal_head.trajectory.joint_names.push_back("HEAD_JOINT0");
00136 goal_head.trajectory.joint_names.push_back("HEAD_JOINT1");
00137
00138 aclient_torso.waitForServer();
00139 goal_torso = control_msgs::FollowJointTrajectoryGoal();
00140 goal_torso.trajectory.joint_names.push_back("CHEST_JOINT0");
00141
00142
00143 ROS_INFO("\nJoint names;");
00144 std::cout << "Torso: [";
00145 for (std::vector<std::string>::iterator name = goal_torso.trajectory.joint_names.begin();
00146 name != goal_torso.trajectory.joint_names.end(); ++name)
00147 std::cout << *name << " ";
00148 std::cout << "]\nHead: [";
00149 for (std::vector<std::string>::iterator name = goal_head.trajectory.joint_names.begin();
00150 name != goal_head.trajectory.joint_names.end(); ++name)
00151 std::cout << *name << " ";
00152 std::cout << "]\nL-Arm: [";
00153 for (std::vector<std::string>::iterator name = goal_larm.trajectory.joint_names.begin();
00154 name != goal_larm.trajectory.joint_names.end(); ++name)
00155 std::cout << *name << " ";
00156 std::cout << "]\nR-Arm: [";
00157 for (std::vector<std::string>::iterator name = goal_rarm.trajectory.joint_names.begin();
00158 name != goal_rarm.trajectory.joint_names.end(); ++name)
00159 std::cout << *name << " ";
00160 std::cout << "]" << std::endl;
00161 }
00162
00163
00164
00165
00166 void go_init(double task_duration = 7.0)
00167 {
00168 ROS_INFO("*** go_init begins ***");
00169 std::vector<double> POSITIONS_TORSO_DEG(1, 0.0);
00170 set_joint_angles_deg(GR_TORSO, POSITIONS_TORSO_DEG, task_duration);
00171
00172 std::vector<double> POSITIONS_HEAD_DEG(2, 0.0);
00173 set_joint_angles_deg(GR_HEAD, POSITIONS_HEAD_DEG, task_duration);
00174
00175 std::vector<double> POSITIONS_LARM_DEG(6);
00176 POSITIONS_LARM_DEG[0] = 0.6;
00177 POSITIONS_LARM_DEG[1] = 0.0;
00178 POSITIONS_LARM_DEG[2] = -100;
00179 POSITIONS_LARM_DEG[3] = -15.2;
00180 POSITIONS_LARM_DEG[4] = 9.4;
00181 POSITIONS_LARM_DEG[5] = -3.2;
00182 set_joint_angles_deg(GR_LARM, POSITIONS_LARM_DEG, task_duration);
00183
00184 std::vector<double> POSITIONS_RARM_DEG(6);
00185 POSITIONS_RARM_DEG[0] = -0.6;
00186 POSITIONS_RARM_DEG[1] = 0;
00187 POSITIONS_RARM_DEG[2] = -100;
00188 POSITIONS_RARM_DEG[3] = 15.2;
00189 POSITIONS_RARM_DEG[4] = 9.4;
00190 POSITIONS_RARM_DEG[5] = 3.2;
00191 set_joint_angles_deg(GR_RARM, POSITIONS_RARM_DEG, task_duration, true);
00192
00193 ROS_INFO("*** go_init_ends ***");
00194 }
00195
00196
00197
00198 void set_joint_angles_rad(std::string groupname, std::vector<double> positions_radian, double duration = 7.0,
00199 bool wait = false)
00200 {
00201 TrajClient* action_client;
00202
00203 if (groupname == GR_TORSO)
00204 {
00205 action_client = new TrajClient("torso_controller/joint_trajectory_action", true);
00206 goal = goal_torso;
00207 }
00208 else if (groupname == GR_HEAD)
00209 {
00210 action_client = new TrajClient("head_controller/joint_trajectory_action", true);
00211 goal = goal_head;
00212 }
00213 else if (groupname == GR_LARM)
00214 {
00215 action_client = new TrajClient("larm_controller/joint_trajectory_action", true);
00216 goal = goal_larm;
00217 }
00218 else if (groupname == GR_RARM)
00219 {
00220 action_client = new TrajClient("rarm_controller/joint_trajectory_action", true);
00221 goal = goal_rarm;
00222 }
00223
00224 try
00225 {
00226 trajectory_msgs::JointTrajectoryPoint pt;
00227 pt.positions = positions_radian;
00228 pt.time_from_start = ros::Duration(duration);
00229 goal.trajectory.points.clear();
00230 goal.trajectory.points.push_back(pt);
00231 goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(duration);
00232 action_client->sendGoal(goal);
00233 if (wait)
00234 {
00235 ROS_INFO("waiting......");
00236 ROS_INFO("wait_for_result for the joint group %s = %d", groupname.c_str(), action_client->waitForResult());
00237 }
00238 }
00239 catch (const ros::Exception& e)
00240 {
00241 ROS_INFO("%s", e.what());
00242 }
00243 catch (...)
00244 {
00245 ROS_INFO("Exception");
00246 }
00247 delete action_client;
00248 }
00249
00250
00251 void set_joint_angles_deg(std::string groupname, std::vector<double> positions_deg, double duration = 7.0, bool wait =
00252 false)
00253 {
00254 set_joint_angles_rad(groupname, to_rad_list(positions_deg), duration, wait);
00255 }
00256
00257 private:
00258 std::string GR_TORSO;
00259 std::string GR_HEAD;
00260 std::string GR_LARM;
00261 std::string GR_RARM;
00262
00263 const std::string MSG_ASK_ISSUEREPORT;
00264
00265 control_msgs::FollowJointTrajectoryGoal goal_larm;
00266 control_msgs::FollowJointTrajectoryGoal goal_rarm;
00267 control_msgs::FollowJointTrajectoryGoal goal_head;
00268 control_msgs::FollowJointTrajectoryGoal goal_torso;
00269
00270
00271
00272
00273
00274
00275
00276
00277
00278
00279 void set_groupnames(std::vector<std::string> groupnames)
00280 {
00281
00282 ROS_INFO("set_groupnames");
00283 for (std::vector<std::string>::iterator name = groupnames.begin(); name != groupnames.end(); ++name)
00284 std::cout << *name << " ";
00285 std::cout << std::endl;
00286
00287 GR_TORSO = groupnames[0];
00288 GR_HEAD = groupnames[1];
00289 GR_RARM = groupnames[2];
00290 GR_LARM = groupnames[3];
00291 }
00292
00293
00294
00295 std::vector<double> to_rad_list(std::vector<double> list_degree)
00296 {
00297 std::vector<double> list_rad;
00298 list_rad.reserve(list_degree.size());
00299 for (std::vector<double>::iterator deg = list_degree.begin(); deg != list_degree.end(); ++deg)
00300 {
00301 double rad = *deg * M_PI / 180.0;
00302 list_rad.push_back(rad);
00303 ROS_INFO("Position deg=%lf rad=%lf", *deg, rad);
00304 }
00305 return list_rad;
00306 }
00307 };
00308
00309 #endif