r2_gaze_controller.cpp
Go to the documentation of this file.
00001 #include "r2_gaze_controller.hpp"
00002 
00003 // Boost
00004 #include <boost/thread/mutex.hpp>
00005 
00006 // KDL
00007 #include <kdl/tree.hpp>
00008 
00009 // Ros
00010 #include <kdl_parser/kdl_parser.hpp>
00011 #include <tf_conversions/tf_kdl.h>
00012 
00013 namespace r2_gaze_controller
00014 {
00015 
00016 int GetIndexFromName(const std::vector<std::string>& nameArray, const std::string& name)
00017 {
00018     for (unsigned int n=0; n < nameArray.size(); n++)
00019     {
00020         if (name == nameArray[n]) return n;
00021     }    
00022     return -1;
00023 }
00024 
00025 void print_pose(const KDL::Frame& frame, const std::string& name)
00026 {
00027         double r,p,y;
00028         frame.M.GetRPY(r, p, y);
00029         ROS_INFO("x_%s: [%.3f, %.3f, %.3f, %.3f, %.3f, %.3f]", name.c_str(), frame.p(0), frame.p(1), frame.p(2), r, p, y);
00030 }
00031 
00032 R2GazeController::R2GazeController(const ros::NodeHandle& handle, const std::string& rootName, const std::string& tipName) :
00033         nodeHandle(handle),
00034         ikPtr(NULL),
00035         rootName(rootName),
00036         tipName(tipName),
00037         q_init(4),
00038         q_cmd(4)
00039 {
00040     jntCmdMsg.name.push_back("r2/neck/joint0");
00041     jntCmdMsg.name.push_back("r2/neck/joint1");
00042     jntCmdMsg.name.push_back("r2/neck/joint2");
00043         jntCmdMsg.position.resize(3);
00044 
00045         q_init.data = Eigen::VectorXd::Zero(4);
00046         q_cmd.data = Eigen::VectorXd::Zero(4);
00047 }
00048 
00049 R2GazeController::~R2GazeController()
00050 {
00051     jointStateSubscriber.shutdown();
00052 }
00053 
00054 bool R2GazeController::initialize()
00055 {
00056     // Create a KDL tree from the robot_description parameter
00057         // TODO: Make robot_description a parameter
00058     KDL::Tree tree;
00059     if (!kdl_parser::treeFromParam("robot_description", tree))
00060     {
00061         ROS_ERROR("Failed to construct kdl tree from the robot_description");
00062         return false;
00063     }
00064    
00065     // Extract the 'neck' chain from the tree and then add our virtual segment
00066     KDL::Chain chain;
00067     tree.getChain(rootName, tipName, chain);
00068     chain.addSegment(createVirtualSegment());
00069     
00070     // With a augmented chain created, lets create a kinematics solver
00071     ikPtr.reset(new R2GazeIK(chain));
00072     ikPtr->setWeightMatrix(getWeightMatrix());
00073     
00074     // And then setup our ROS topics
00075     setupRosTopics("/r2_controller/gaze/pose_command", "/r2_controller/neck/joint_command");
00076 
00077         return true;
00078 }
00079 
00080 KDL::Segment R2GazeController::createVirtualSegment()
00081 {
00082         KDL::Joint virtualPrismaticJoint(KDL::Joint::TransX);
00083         KDL::Frame virtualFrame(KDL::Frame::Identity());
00084         return KDL::Segment(virtualPrismaticJoint, virtualFrame);
00085 }
00086 
00087 double R2GazeController::getWeight(const std::string& paramName)
00088 {
00089         double weight = 1.0;
00090         if (nodeHandle.getParam(paramName, weight))
00091         {
00092                 ROS_INFO("IN  HERE");
00093         }
00094         std::cout << paramName << ": " << weight << std::endl;
00095         //ROS_INFO("%s: %.1f", paramName.c_str(), weight);
00096         return weight;
00097 }
00098 
00099 Eigen::MatrixXd R2GazeController::getWeightMatrix()
00100 {
00101     /* Default weight matrix. A large weight means that joint will contribute
00102      * to the task-space solution and a small weight means the joint will
00103      * not be used to come up with the task-space solution. In our case we
00104      * want to penalize the use of the real joints in an effort to minimize
00105      * the robot 'chicken head' syndrome.
00106          */
00107     Eigen::VectorXd w(4);
00108     w(0) = getWeight("/r2_gaze_controller/weight_j0");
00109     w(1) = getWeight("/r2_gaze_controller/weight_j1");
00110     w(2) = getWeight("/r2_gaze_controller/weight_j2");
00111     w(3) = getWeight("/r2_gaze_controller/weight_jv");
00112     return w.asDiagonal();
00113 }
00114 
00115 void R2GazeController::setupRosTopics(const std::string& lookAtTopicName, const std::string& neckJointCmdTopicName)
00116 {
00117         // Subscribe to the joint_state topic
00118         //jointStateSubscriber = nodeHandle.subscribe("/joint_states", 1, &GazeController::R2GazeController::joint_state_cb, this);
00119 
00120         // Subscribe to a PoseStamped topic - this is our desired look-at point
00121         lookAtSubscriber.subscribe(nodeHandle, lookAtTopicName, 1);
00122         lookAtFilter.reset(new tf::MessageFilter<geometry_msgs::PoseStamped>(lookAtSubscriber, transformListener, rootName, 10, nodeHandle));
00123         lookAtFilter->registerCallback(boost::bind(&r2_gaze_controller::R2GazeController::look_at_cb, this, _1));
00124 
00125         // Publish our joint commands to the neck command topic
00126         neckCmdPublisher = nodeHandle.advertise<sensor_msgs::JointState>(neckJointCmdTopicName, 1);
00127 }
00128 
00129 void R2GazeController::serializeToMsg(const KDL::JntArray& q, sensor_msgs::JointState& cmdMsg)
00130 {
00131         for (int n=0; n < cmdMsg.position.size(); n++) cmdMsg.position[n] = q(n);
00132 }
00133 
00134 void R2GazeController::transformToRootFrame(const geometry_msgs::PoseStamped::ConstPtr& pose_msg, KDL::Frame& frame)
00135 {
00136         // Convert ROS pose message to KDL frame format
00137         tf::Stamped<tf::Pose> pose_stamped;
00138         poseStampedMsgToTF(*pose_msg, pose_stamped);
00139 
00140         // Convert to reference frame of root link of the controller chain
00141         //transformListener.waitForTransform(rootName, tipName, ros::Time::now(), ros::Duration(1.0));
00142         transformListener.transformPose(rootName, pose_stamped, pose_stamped);
00143         tf::PoseTFToKDL(pose_stamped, frame);
00144 }
00145 
00146 void R2GazeController::publishNeckCmd(const KDL::JntArray& q_cmd)
00147 {
00148         serializeToMsg(q_cmd, jntCmdMsg);
00149         neckCmdPublisher.publish(jntCmdMsg);
00150 }
00151 
00152 void R2GazeController::look_at_cb(const geometry_msgs::PoseStamped::ConstPtr& pose_msg)
00153 {
00154     // Transform msg pose into our root frame
00155         transformToRootFrame(pose_msg, F_desired);
00156 
00157     // Compute IK solution
00158     int ret = ikPtr->computeSolution(q_init, F_desired, q_cmd);
00159 
00160     // Publish... regardless if we converged (desired point may be unreachable)
00161     publishNeckCmd(q_cmd);
00162 
00163     if (ret < 0)
00164     {
00165         ROS_ERROR("ik did not converge: j1: %.3f, j2: %.3f, j3: %.3f, jv: %.3f!", q_cmd(0), q_cmd(1), q_cmd(2), q_cmd(3));
00166     }
00167 }
00168 
00169 } // end namespace
00170 
00171 
00172 int main( int argc, char **argv)
00173 {
00174     ros::init(argc, argv, "r2_gaze_controller");
00175     ros::NodeHandle nodeHandle;
00176     
00177     // TODO: Take root and tip names as parameters
00178     r2_gaze_controller::R2GazeController gazeController(nodeHandle, "waist_center", "vision_center_link");
00179     if (!gazeController.initialize())
00180     {
00181         ROS_ERROR_NAMED("r2_gaze_controller", "could not initialize");
00182         return -1;
00183     }
00184     
00185     ros::spin();
00186     return 0;
00187 }


r2_controllers_gazebo
Author(s): Stephen Hart
autogenerated on Mon Oct 6 2014 02:48:44