00001 /****************************************************** 00002 * This is CIR-KIT 3rd robot control driver. 00003 * Author : Arita Yuta(Kyutech) 00004 ******************************************************/ 00005 00006 // inclusive dependency 00007 #include "ThirdRobotInterface/ThirdRobotInterface.h" 00008 00009 #include <ros/ros.h> 00010 #include <geometry_msgs/Twist.h> // cmd_vel 00011 #include <tf/transform_broadcaster.h> 00012 00013 #include <string> 00014 00015 namespace cirkit { 00016 class CirkitUnit03Driver { 00017 public: 00018 CirkitUnit03Driver(const std::string&, const ros::NodeHandle&); 00019 ~CirkitUnit03Driver(); 00020 void resetCommunication(); 00021 void run(); 00022 private: 00023 // Callback function 00024 void cmdVelReceived(const geometry_msgs::Twist::ConstPtr& cmd_vel); 00025 00026 ros::NodeHandle nh_; 00027 ros::Rate rate_; 00028 // ROS topic trader 00029 ros::Publisher odom_pub_; 00030 ros::Publisher steer_pub_; 00031 ros::Subscriber cmd_vel_sub_; 00032 // cirkit unit03 interface object 00033 cirkit::ThirdRobotInterface cirkit_unit03_; 00034 // self member 00035 tf::TransformBroadcaster odom_broadcaster_; 00036 std::string imcs01_port_; 00037 ros::Time current_time_, last_time_; 00038 geometry_msgs::Twist steer_dir_; 00039 }; 00040 }