r2_gaze_controller.hpp
Go to the documentation of this file.
00001 #ifndef _R2_GAZE_CONTROLLER_HPP_
00002 #define _R2_GAZE_CONTROLLER_HPP_
00003 
00004 // My neck inverse_kinematics
00005 #include "r2_gaze_ik.hpp"
00006 
00007 // Boost
00008 #include <boost/scoped_ptr.hpp>
00009 
00010 // Standard libraries
00011 #include <string>
00012 
00013 // ROS core
00014 #include <ros/ros.h>
00015 #include <ros/node_handle.h>
00016 
00017 // ROS messages
00018 #include <message_filters/subscriber.h>
00019 #include <sensor_msgs/JointState.h>
00020 #include <geometry_msgs/PoseStamped.h>
00021 
00022 // Other ROS packages
00023 #include <tf/message_filter.h>
00024 #include <tf/transform_listener.h>
00025 
00026 namespace r2_gaze_controller
00027 {
00028 
00029 class R2GazeController
00030 {
00031 public:
00032     R2GazeController(const ros::NodeHandle& handle, const std::string& rootName, const std::string& tipName);
00033     ~R2GazeController();
00034 
00035     bool initialize();
00036         
00037 private:
00038     KDL::Segment createVirtualSegment();
00039     double getWeight(const std::string& paramName);
00040     Eigen::MatrixXd getWeightMatrix();
00041     void setupRosTopics(const std::string& lookAtTopicName, const std::string& neckJointCmdTopicName);
00042     void serializeToMsg(const KDL::JntArray& q, sensor_msgs::JointState& cmdMsg);
00043     void transformToRootFrame(const geometry_msgs::PoseStamped::ConstPtr& pose_msg, KDL::Frame& frame);
00044     void publishNeckCmd(const KDL::JntArray& q_cmd);
00045     void look_at_cb(const geometry_msgs::PoseStamped::ConstPtr& pose_msg);
00046     
00047     // ROS comm
00048     ros::NodeHandle nodeHandle;
00049     ros::Subscriber jointStateSubscriber;
00050     ros::Publisher neckCmdPublisher;
00051     message_filters::Subscriber<geometry_msgs::PoseStamped> lookAtSubscriber;
00052     boost::scoped_ptr<tf::MessageFilter<geometry_msgs::PoseStamped> > lookAtFilter;
00053     tf::TransformListener transformListener;
00054     
00055     // Inverse kinematics solver
00056     boost::scoped_ptr<r2_gaze_controller::R2GazeIK> ikPtr;
00057     
00058     // Tip and root names of the chain to create
00059     std::string rootName, tipName;
00060 
00061     // Temporaries to speed up computation
00062     KDL::JntArray q_init, q_cmd;
00063     KDL::Frame F_desired;
00064     sensor_msgs::JointState jntCmdMsg;
00065 };
00066 
00067 }
00068 
00069 #endif


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