00001 /* 00002 * basic_move_controller.hpp 00003 * LICENSE : BSD - https://raw.github.com/yujinrobot/yujin_ocs/license/LICENSE 00004 */ 00005 00006 #include <ros/ros.h> 00007 #include <tf/tf.h> 00008 00009 #include <yocs_math_toolkit/common.hpp> 00010 #include <yocs_math_toolkit/geometry.hpp> 00011 00012 #include <nav_msgs/Odometry.h> 00013 #include <geometry_msgs/Twist.h> 00014 00015 #include "yocs_navigator/default_params.h" 00016 00017 #ifndef _YOCS_BASIC_MOVE_CONTROLLER_HPP_ 00018 #define _YOCS_BASIC_MOVE_CONTROLLER_HPP_ 00019 00020 namespace yocs_navigator { 00021 class BasicMoveController { 00022 public: 00023 BasicMoveController(ros::NodeHandle& n); 00024 BasicMoveController(ros::NodeHandle& n, const std::string& twist_topic, const std::string& odometry_topic); 00025 virtual ~BasicMoveController(); 00026 00027 void init(); 00028 00029 // linear, angualr, time 00030 void moveAt(double v, double w, double t); 00031 00032 void slowForward(); 00033 void slowBackward(); 00034 void turnClockwise(); 00035 void turnCounterClockwise(); 00036 void stop(); 00037 00038 void forward(double distance); 00039 void backward(double distance); 00040 void turn(double angle); 00041 00042 void turn2(double angle); 00043 void spinClockwise(); 00044 void spinCounterClockwise(); 00045 protected: 00046 void processOdometry(const nav_msgs::Odometry::ConstPtr& msg); 00047 00048 private: 00049 ros::NodeHandle nh_; 00050 ros::Publisher pub_cmd_vel_; 00051 ros::Subscriber sub_odom_; 00052 00053 nav_msgs::Odometry odometry_; 00054 std::string cmd_vel_topic_; 00055 std::string odometry_topic_; 00056 }; 00057 } 00058 #endif