00001 #include "tibi_dabo_laser_3d_driver_node.h" 00002 00003 TibiDaboLaser3dDriverNode::TibiDaboLaser3dDriverNode(ros::NodeHandle &nh) : 00004 iri_base_driver::IriBaseNodeDriver<TibiDaboLaser3dDriver>(nh) 00005 { 00006 //init class attributes if necessary 00007 this->loop_rate_ = 100;//in [Hz] 00008 00009 // [init publishers] 00010 this->assembled_scans_publisher_ = this->public_node_handle_.advertise<sensor_msgs::PointCloud>("assembled_scans", 10); 00011 this->joint_states_publisher_ = this->public_node_handle_.advertise<sensor_msgs::JointState>("joint_states", 10); 00012 00013 this->JointState_msg_.name.resize(1); 00014 this->JointState_msg_.name[0] = "servo_laser"; 00015 this->JointState_msg_.position.resize(1); 00016 this->JointState_msg_.position[0]=0.0; 00017 this->JointState_msg_.velocity.resize(1); 00018 this->JointState_msg_.velocity[0]=0.0; 00019 this->JointState_msg_.effort.resize(1); 00020 this->JointState_msg_.effort[0]=0.0; 00021 00022 // [init subscribers] 00023 00024 // [init services] 00025 00026 // [init clients] 00027 assemble_scans_client_ = this->public_node_handle_.serviceClient<laser_assembler::AssembleScans>("assemble_scans"); 00028 00029 // [init action servers] 00030 00031 // [init action clients] 00032 } 00033 00034 void TibiDaboLaser3dDriverNode::mainNodeThread(void) 00035 { 00036 static ros::Time previous_time=ros::Time::now(); 00037 static bool first=true; 00038 00039 //lock access to driver if necessary 00040 this->driver_.lock(); 00041 00042 // [fill msg Header if necessary] 00043 //<publisher_name>.header.stamp = ros::Time::now(); 00044 //<publisher_name>.header.frame_id = "<publisher_topic_name>"; 00045 00046 // [fill msg structures] 00047 //this->PointCloud_msg_.data = my_var; 00048 //this->JointState_msg_.data = my_var; 00049 00050 // [fill srv structure and make request to the server] 00051 //assemble_scans_srv_.request.data = my_var; 00052 //ROS_INFO("TibiDaboLaser3dDriverNode:: Sending New Request!"); 00053 //if (assemble_scans_client_.call(assemble_scans_srv_)) 00054 //{ 00055 //ROS_INFO("TibiDaboLaser3dDriverNode:: Response: %s", assemble_scans_srv_.response.result); 00056 //} 00057 //else 00058 //{ 00059 //ROS_INFO("TibiDaboLaser3dDriverNode:: Failed to Call Server on topic assemble_scans "); 00060 //} 00061 00062 // [fill action structure and make request to the action server] 00063 00064 // [publish messages] 00065 //this->assembled_scans_publisher_.publish(this->PointCloud_msg_); 00066 this->JointState_msg_.header.stamp=ros::Time::now(); 00067 this->JointState_msg_.position[0]=-this->driver_.get_current_angle()*3.14159/180.0; 00068 this->JointState_msg_.velocity[0]=this->driver_.get_current_velocity()*3.14159/180.0; 00069 this->joint_states_publisher_.publish(this->JointState_msg_); 00070 if(this->driver_.is_scan_enabled()) 00071 { 00072 if(this->driver_.is_scan_done()) 00073 { 00074 assemble_scans_srv_.request.begin = previous_time; 00075 assemble_scans_srv_.request.end = ros::Time::now(); 00076 if (assemble_scans_client_.call(assemble_scans_srv_)) 00077 this->assembled_scans_publisher_.publish(assemble_scans_srv_.response.cloud); 00078 else 00079 ROS_ERROR("TibiDaboLaser2dTo3dAlgNode:: Failed to Call Server on topic assemble_scans "); 00080 previous_time=ros::Time::now(); 00081 this->driver_.start_scan(); 00082 } 00083 if(!first) 00084 first=true; 00085 } 00086 else 00087 { 00088 if(first) 00089 { 00090 first=false; 00091 this->driver_.move_absolute_angle(this->driver_.get_fixed_angle()); 00092 } 00093 } 00094 00095 //unlock access to driver if previously blocked 00096 this->driver_.unlock(); 00097 } 00098 00099 /* [subscriber callbacks] */ 00100 00101 /* [service callbacks] */ 00102 00103 /* [action callbacks] */ 00104 00105 /* [action requests] */ 00106 00107 void TibiDaboLaser3dDriverNode::postNodeOpenHook(void) 00108 { 00109 } 00110 00111 void TibiDaboLaser3dDriverNode::addNodeDiagnostics(void) 00112 { 00113 } 00114 00115 void TibiDaboLaser3dDriverNode::addNodeOpenedTests(void) 00116 { 00117 } 00118 00119 void TibiDaboLaser3dDriverNode::addNodeStoppedTests(void) 00120 { 00121 } 00122 00123 void TibiDaboLaser3dDriverNode::addNodeRunningTests(void) 00124 { 00125 } 00126 00127 void TibiDaboLaser3dDriverNode::reconfigureNodeHook(int level) 00128 { 00129 } 00130 00131 TibiDaboLaser3dDriverNode::~TibiDaboLaser3dDriverNode(void) 00132 { 00133 // [free dynamic memory] 00134 } 00135 00136 /* main function */ 00137 int main(int argc,char *argv[]) 00138 { 00139 return driver_base::main<TibiDaboLaser3dDriverNode>(argc, argv, "tibi_dabo_laser_3d_driver_node"); 00140 }