ros_client.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  * Copyright (c) 2014, TORK (Tokyo Opensource Robotics Kyokai Association)
00005  * All rights reserved.
00006  *
00007  * Redistribution and use in source and binary forms, with or without
00008  * modification, are permitted provided that the following conditions
00009  * are met:
00010  *
00011  *  * Redistributions of source code must retain the above copyright
00012  *    notice, this list of conditions and the following disclaimer.
00013  *  * Redistributions in binary form must reproduce the above
00014  *    copyright notice, this list of conditions and the following
00015  *    disclaimer in the documentation and/or other materials provided
00016  *    with the distribution.
00017  *  * Neither the name of JSK Lab, University of Tokyo. nor the
00018  *    names of its contributors may be used to endorse or promote products
00019  *    derived from this software without specific prior written permission.
00020  *
00021  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  * POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00035 #ifndef ROS_CLIENT_HPP
00036 #define ROS_CLIENT_HPP
00037 
00038 #include <ros/ros.h>
00039 #include <pr2_controllers_msgs/JointTrajectoryAction.h>
00040 #include <pr2_controllers_msgs/JointTrajectoryActionGoal.h>
00041 #include <pr2_controllers_msgs/JointTrajectoryGoal.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    This class:
00052 
00053    - holds methods that are specific to Kawada Industries' dual-arm
00054    robot called Hiro, via ROS.
00055    - As of July 2014, this class is only intended to be used through HIRONX
00056    class.
00057    */
00058 
00059   // TODO(@iory): Address the following concern.
00060   // This duplicates "Group" definition in HIRONX, which is bad.
00061   // Need to consider consolidating the definition either in hironx_ros_bridge
00062   // or somewhere in the upstream, e.g.:
00063   // https://github.com/fkanehiro/hrpsys-base/pull/253
00064 public:
00065   typedef actionlib::SimpleActionClient<pr2_controllers_msgs::JointTrajectoryAction> TrajClient;
00066 
00067   pr2_controllers_msgs::JointTrajectoryGoal 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 = pr2_controllers_msgs::JointTrajectoryGoal();
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 = pr2_controllers_msgs::JointTrajectoryGoal();
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 = pr2_controllers_msgs::JointTrajectoryGoal();
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 = pr2_controllers_msgs::JointTrajectoryGoal();
00140     goal_torso.trajectory.joint_names.push_back("CHEST_JOINT0");
00141 
00142     // Display Joint names
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   // Init positions are taken from HIRONX.
00164   // TODO(@iory): Need to add factory position too that's so convenient when
00165   // working with the manufacturer.
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   // This method takes the angles(radian) and
00197   // changes the angle of the joints of the robot.
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   //  groupname: This should exist in groupnames.
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   pr2_controllers_msgs::JointTrajectoryGoal goal_larm;
00266   pr2_controllers_msgs::JointTrajectoryGoal goal_rarm;
00267   pr2_controllers_msgs::JointTrajectoryGoal goal_head;
00268   pr2_controllers_msgs::JointTrajectoryGoal goal_torso;
00269 
00270   /*
00271    param groupnames: List of the joint group names. Assumes to be in the
00272    following order:
00273    torso, head, right arm, left arm.
00274    This current setting is derived from the order of
00275    Groups argument in HIRONX class. If other groups
00276    need to be defined in the future, this method may
00277    need to be modified.
00278    */
00279   void set_groupnames(std::vector<std::string> groupnames)
00280   {
00281     // output group name
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   // This method change degree to radian.
00294   // param list_degree: A list length of the number of joints.
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


hironx_ros_bridge
Author(s): Kei Okada
autogenerated on Sun Sep 13 2015 23:21:39