head_plugin.cpp
Go to the documentation of this file.
00001 
00002 #include "head_plugin.h"
00003 
00004 
00005 #define PODO_ADDR       "10.12.3.30"
00006 #define PODO_PORT       8888
00007 
00008 const float     D2Rf = 0.0174533;
00009 const float     R2Df = 57.2957802;
00010 
00011 namespace gazebo
00012 {
00013 
00014 void HeadPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
00015 {
00016     std::cout << "\033[1;32m===================================" << std::endl;
00017     std::cout << "   Now Start the HeadPlugin..!!" << std::endl << std::endl;
00018     std::cout << "   Developer: Jeongsoo Lim" << std::endl;
00019     std::cout << "   E-mail   : yjs0497@kaist.ac.kr" << std::endl;
00020     std::cout << "===================================\033[0m" << std::endl;
00021 
00022     // Make sure the ROS node for Gazebo has already been initialized
00023     if (!ros::isInitialized()){
00024         ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
00025                          << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
00026         return;
00027     }
00028     ros::NodeHandle n;
00029     joint_pub = n.advertise<sensor_msgs::JointState>("joint_states", 1);
00030     cloud_pub = n.advertise<sensor_msgs::PointCloud2>("point_cloud", 1);
00031     cmd_sub = n.subscribe("drc_head_cmd", 1, &HeadPlugin::head_cmd_callback, this);
00032     scan_sub = n.subscribe("scan", 2, &HeadPlugin::scan_callback, this);
00033 
00034     cloud.height = 1;
00035     cloud.fields.resize (3);
00036     cloud.fields[0].name = "x";
00037     cloud.fields[0].offset = 0;
00038     cloud.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
00039     cloud.fields[0].count = 1;
00040     cloud.fields[1].name = "y";
00041     cloud.fields[1].offset = 4;
00042     cloud.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
00043     cloud.fields[1].count = 1;
00044     cloud.fields[2].name = "z";
00045     cloud.fields[2].offset = 8;
00046     cloud.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
00047     cloud.fields[2].count = 1;
00048     cloud.point_step = 12;
00049 
00050     model = _model;
00051     JCon = new physics::JointController(model);
00052 
00053     std::cout << "Model Name : " << model->GetName() << std::endl;
00054 
00055     joint_state.name.resize(1);
00056     joint_state.position.resize(1);
00057 
00058 
00059     JPtr_Hokuyo = model->GetJoint("DRC_head::Head_Sensor");
00060     if(JPtr_Hokuyo == NULL){
00061         JPtr_Hokuyo = model->GetJoint("DRC_head_kinect::Head_Sensor");
00062     }
00063 
00064     joint_state.name[0] = "Head_Sensor";
00065     ref_Hokuyo = 0.0;
00066 
00067     toCamera.setOrigin(tf::Vector3(0.05, -0.043, -0.042));
00068     toCamera.setRotation(tf::Quaternion(0,0,0,1));
00069     toHokuyo.setOrigin(tf::Vector3(0, -0.042, 0.036));
00070     toHokuyo.setRotation(tf::Quaternion(0,0,0,1));
00071     toStreaming.setOrigin(tf::Vector3(0.082, -0.063, 0.253));
00072     toStreaming.setRotation(tf::Quaternion(tf::createQuaternionFromRPY(0.0, 0.1746, 0.0)));
00073 
00074 
00075     // Listen to the update event. This event is broadcast every simulation iteration.
00076     UpdateConnection = event::Events::ConnectWorldUpdateBegin(boost::bind(&HeadPlugin::OnUpdate, this, _1));
00077 }
00078 
00079 void HeadPlugin::scan_callback(const sensor_msgs::LaserScanPtr &scan){
00080     if(jointData.mode != SCAN_MODE_SCAN)
00081         return;
00082 
00083     cloud.data.resize(cloud.point_step * (cloud_accum_cnt + scan->ranges.size()));
00084     for(int i=0; i<scan->ranges.size(); i++){
00085         if(scan->ranges[i] <= scan->range_min || scan->ranges[i] >= scan->range_max)
00086             continue;
00087 
00088         float *pstep = (float*)&cloud.data[cloud_accum_cnt * cloud.point_step];
00089 
00090         pstep[0] = scan->ranges[i] * cos(scan->angle_min + (float)i * scan->angle_increment) * cos(ref_Hokuyo) + 0.036 * sin(ref_Hokuyo) + 0.085;
00091         pstep[1] = scan->ranges[i] * sin(scan->angle_min + (float)i * scan->angle_increment);
00092         pstep[2] = scan->ranges[i] * cos(scan->angle_min + (float)i * scan->angle_increment) * (-sin(ref_Hokuyo)) + 0.036 * cos(ref_Hokuyo) + 0.165;
00093 
00094         cloud_accum_cnt++;
00095     }
00096     cloud.width = cloud_accum_cnt;
00097     cloud.row_step = cloud.point_step * cloud.width;
00098     cloud.data.resize(cloud.row_step);
00099 }
00100 
00101 void HeadPlugin::head_cmd_callback(const drc_plugin::DRC_HEAD_CMD::ConstPtr &cmd){
00102     std::cout << "Got Head Command..!!" << std::endl;
00103     switch(cmd->cmd){
00104     case 1: // scan from A to B
00105         scanData.target = cmd->end_pos;
00106         scanData.timeMs = cmd->timeMs;
00107         setMoveJoint(cmd->start_pos, 500, SCAN_MODE_GOTO_START_POS);
00108         break;
00109     case 2: // go to target pos
00110         setMoveJoint(cmd->end_pos, 500, SCAN_MODE_JUST_MOVE);
00111         break;
00112     }
00113 }
00114 
00115 
00116 void HeadPlugin::OnUpdate(const common::UpdateInfo &){
00117     static int cnt = 0;
00118 
00119     moveJoint();
00120     ref_Hokuyo = jointData.current;
00121     JPtr_Hokuyo->SetPosition(0, ref_Hokuyo);
00122 
00123     joint_state.header.stamp = ros::Time::now();
00124     joint_state.position[0] = ref_Hokuyo;
00125     joint_pub.publish(joint_state);
00126 
00127     if(cnt%10 == 0){
00128         broadcaster.sendTransform(
00129                     tf::StampedTransform(
00130                         toHokuyo,
00131                         ros::Time::now(),
00132                         "Body_Hokuyo",
00133                         "Sensor_scan"));
00134         broadcaster.sendTransform(
00135                     tf::StampedTransform(
00136                         toCamera,
00137                         ros::Time::now(),
00138                         "Body_Hokuyo",
00139                         "Sensor_camera"));
00140         broadcaster.sendTransform(
00141                     tf::StampedTransform(
00142                         toStreaming,
00143                         ros::Time::now(),
00144                         "Body_Head",
00145                         "Sensor_streaming"));
00146     }
00147 
00148     cnt++;
00149 }
00150 
00151 void HeadPlugin::setMoveJoint(float target, long msTime, int mode){
00152     jointData.flag = false;
00153     jointData.togo = target;
00154     jointData.init = jointData.current;
00155     jointData.delta = jointData.togo - jointData.current;
00156     jointData.curCnt = 0;
00157     jointData.goalCnt = msTime;
00158     jointData.flag = true;
00159     jointData.mode = mode;
00160 }
00161 
00162 void HeadPlugin::moveJoint(){
00163     if(jointData.flag == true){
00164         jointData.curCnt++;
00165         if(jointData.goalCnt <= jointData.curCnt){
00166             jointData.goalCnt = jointData.curCnt = 0;
00167             jointData.current = jointData.togo;
00168             jointData.flag = false;
00169             if(jointData.mode == SCAN_MODE_GOTO_START_POS){
00170                 cloud_accum_cnt = 0;
00171                 setMoveJoint(scanData.target, scanData.timeMs, SCAN_MODE_SCAN);
00172             }else if(jointData.mode == SCAN_MODE_SCAN){
00173                 jointData.mode = SCAN_MODE_NONE;
00174 
00175                 cloud.header.frame_id = "Body_Head";
00176                 cloud.header.stamp = ros::Time::now();
00177                 cloud_pub.publish(cloud);
00178             }else{
00179                 jointData.mode = SCAN_MODE_NONE;
00180             }
00181         }else{
00182             jointData.current = jointData.init + jointData.delta*jointData.curCnt/jointData.goalCnt;
00183         }
00184     }
00185 }
00186 
00187 
00188 GZ_REGISTER_MODEL_PLUGIN(HeadPlugin)
00189 }
00190 


drc_plugin
Author(s): JeongsooLim , SeungwooHong
autogenerated on Sat Jul 22 2017 02:25:33