test_platformInfo.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include "sawyer_base/sawyer_hardware.h"
00003 
00004 #include <boost/assign/list_of.hpp>
00005 #include <float.h>
00006 #define polling_timeout_ 20
00007 using namespace std;
00008 void publishRawData()
00009 {
00010  
00011         std::ostringstream ostream;
00012       sawyer::base_data::RawData data =sawyer::Transport::instance().getdata();
00013       ostream << "command: { " ;
00014       ostream << std::setfill('0') << std::uppercase;
00015       for (unsigned int i=0; i < data.length; i++)
00016           ostream << std::hex << std::setw(2) << static_cast<unsigned int>(data.data[i]) << " " << std::dec;
00017       ostream << "}";
00018       std_msgs::StringPtr msg(new std_msgs::String);
00019       msg->data = ostream.str();
00020       ROS_INFO("reveive command:%s",msg->data.c_str());
00021 }
00022 
00023 void getPlatformName()
00024 { 
00025   core::Channel<sawyer::DataPlatformName>::Ptr getPlatformName = 
00026   core::Channel<sawyer::DataPlatformName>::requestData(polling_timeout_);
00027   publishRawData();
00028   if(getPlatformName){
00029   std::cout<<"Received platform name:"<<getPlatformName->getName()<<std::endl;
00030     
00031   }
00032 }
00033 
00034 void getDifferentControlConstantData()
00035 {
00036 
00037   core::Channel<sawyer::DataDifferentialControl>::Ptr getDifferentControlConstantData = 
00038   core::Channel<sawyer::DataDifferentialControl>::requestData(polling_timeout_);
00039   publishRawData();
00040   if(getDifferentControlConstantData){ 
00041      ROS_INFO("Received Data of Differential Control Data, Left_P:%.2lf, Left_I:%.2lf, Left_D:%.2lf,"
00042                "right_P:%.2lf, right_I:%.2lf, right_D:%.2lf.",
00043            getDifferentControlConstantData->getLeftP(),getDifferentControlConstantData->getLeftI(),getDifferentControlConstantData->getLeftD(),
00044            getDifferentControlConstantData->getRightP(),getDifferentControlConstantData->getRightI(),getDifferentControlConstantData->getRightD());
00045    }
00046     
00047  }
00048 namespace
00049 {
00050   const uint8_t LEFT = 0, RIGHT = 1;
00051 };
00052 int main(int argc, char** argv){
00053   
00054   std::string port = "/dev/roch";
00055   core::connect(port);
00056   while(1){
00057     ROS_INFO("begin send requestData");
00058     core::Channel<sawyer::DataEncoders>::Ptr enc = core::Channel<sawyer::DataEncoders>::requestData(
00059       20);
00060     publishRawData();
00061       
00062     if (enc)
00063     {
00064                 for (int i = 0; i < 2; i++)//新本 双驱
00065       {
00066         //joints_[i].position_offset = linearToAngular(enc->getTravel(i % 2));
00067         ROS_INFO("joints_[%d].distance.x:%.2lf  ",i,enc->getTravel(i % 2));
00068       }
00069     }
00070     else
00071     {
00072       ROS_ERROR("Could not get encoder data to calibrate travel offset");
00073     }
00074   }
00075   return 0;
00076 }


roch_base
Author(s): Mike Purvis , Paul Bovbel , Carl
autogenerated on Sat Jun 8 2019 20:32:33