void processMocapData( const char** mocap_model ) { //UdpMulticastSocket multicast_client_socket( LOCAL_PORT, MULTICAST_IP ); MOCAPSocket Socket; static tf::TransformBroadcaster br; //ushort payload; //MoCapDataFormat format; while( true ) { //int numberOfPackets = 0; //bool packetread = false; //int trials = 0; //while( !packetread && trials < 100 ) //{ //int numBytes = 0; //do //{ // Receive data form mocap device //numBytes = multicast_client_socket.recv(); // Parse mocap data //if( numBytes > 0 ) //{ //const char* buffer = multicast_client_socket.getBuffer(); //unsigned short header = *((unsigned short*)(&buffer[0])); // Look for the beginning of a NatNet package //if (header == 7) //{ //payload = *((ushort*) &buffer[2]); //format.parse(buffer,payload); //packetread = true; //numberOfPackets++; //} // else skip packet //} //} while( numBytes > 0 ); // Don't try again immediately //if( !packetread ) //{ //usleep( 10 ); //trials++; //} //} // Once a mocap package has been received and parsed, publish the data using tf if(Socket.Read()>0) //if( packetread ) { ros::Time timestamp( ros::Time::now() ); //for( int i = 0; i < format.model[0].numRigidBodies; i++ ) //{ tf::Transform transform; // Translate mocap data from mm --> m to be compatible with rviz // Change y and z due to mocap calibration transform.setOrigin( tf::Vector3(-Socket.rigidBody[0].x / 1000.0f,Socket.rigidBody[0].z / 1000.0f,Socket.rigidBody[0].y / 1000.0f ) ); // Change y and z due to mocap calibration tf::Quaternion q( Socket.rigidBody[0].qx, Socket.rigidBody[0].qz, Socket.rigidBody[0].qy, Socket.rigidBody[0].qw ) ; transform.setRotation(q.inverse()); // Handle int rigid_body_id = abs(Socket.rigidBody[0].ID); const char* rigid_body_name = "OBJECT"; //mocap_model[rigid_body_id]; br.sendTransform(tf::StampedTransform(transform, timestamp, "base_link", std::string( rigid_body_name ) )); //} } } }
bool detectFiducials(cob_object_detection_msgs::DetectionArray& detection_array, cv::Mat& color_image) { int id_start_idx = 2351; unsigned int marker_array_size = 0; unsigned int pose_array_size = 0; // Detect fiducials std::vector<ipa_Fiducials::t_pose> tags_vec; std::vector<std::vector<double> >vec_vec7d; if (m_pi_tag->GetPose(color_image, tags_vec) & ipa_Utils::RET_OK) { pose_array_size = tags_vec.size(); // TODO: Average results for (unsigned int i=0; i<pose_array_size; i++) { cob_object_detection_msgs::Detection fiducial_instance; fiducial_instance.label = "pi-tag"; //tags_vec[i].id; fiducial_instance.detector = "Fiducial_PI"; fiducial_instance.score = 0; fiducial_instance.bounding_box_lwh.x = 0; fiducial_instance.bounding_box_lwh.y = 0; fiducial_instance.bounding_box_lwh.z = 0; // TODO: Set Mask cv::Mat frame(3,4, CV_64FC1); for (int k=0; k<3; k++) for (int l=0; l<3; l++) frame.at<double>(k,l) = tags_vec[i].rot.at<double>(k,l); frame.at<double>(0,3) = tags_vec[i].trans.at<double>(0,0); frame.at<double>(1,3) = tags_vec[i].trans.at<double>(1,0); frame.at<double>(2,3) = tags_vec[i].trans.at<double>(2,0); std::vector<double> vec7d = FrameToVec7(frame); vec_vec7d.push_back(vec7d); // Results are given in CfromO fiducial_instance.pose.pose.position.x = vec7d[0]; fiducial_instance.pose.pose.position.y = vec7d[1]; fiducial_instance.pose.pose.position.z = vec7d[2]; fiducial_instance.pose.pose.orientation.w = vec7d[3]; fiducial_instance.pose.pose.orientation.x = vec7d[4]; fiducial_instance.pose.pose.orientation.y = vec7d[5]; fiducial_instance.pose.pose.orientation.z = vec7d[6]; fiducial_instance.pose.header.stamp = received_timestamp_; fiducial_instance.pose.header.frame_id = received_frame_id_; detection_array.detections.push_back(fiducial_instance); ROS_INFO("[fiducials] Detected PI-Tag '%s' at x,y,z,rw,rx,ry,rz ( %f, %f, %f, %f, %f, %f, %f ) ", fiducial_instance.label.c_str(), vec7d[0], vec7d[1], vec7d[2], vec7d[3], vec7d[4], vec7d[5], vec7d[6]); } } else { pose_array_size = 0; } // Publish 2d image if (publish_2d_image_) { for (unsigned int i=0; i<pose_array_size; i++) { RenderPose(color_image, tags_vec[i].rot, tags_vec[i].trans); cv_bridge::CvImage cv_ptr; cv_ptr.image = color_mat_8U3_; cv_ptr.encoding = CobFiducialsNode::color_image_encoding_; img2D_pub_.publish(cv_ptr.toImageMsg()); } } // Publish tf if (publish_tf_) { for (unsigned int i=0; i<pose_array_size; i++) { // Broadcast transform of fiducial tf::Transform transform; std::stringstream tf_name; tf_name << "pi_tag" <<"_" << "0"; transform.setOrigin(tf::Vector3(vec_vec7d[i][0], vec_vec7d[i][1], vec_vec7d[i][2])); transform.setRotation(tf::Quaternion(vec_vec7d[i][4], vec_vec7d[i][5], vec_vec7d[i][6], vec_vec7d[i][3])); tf_broadcaster_.sendTransform(tf::StampedTransform(transform, ros::Time::now(), received_frame_id_, tf_name.str())); } } // Publish marker array if (publish_marker_array_) { // 3 arrows for each coordinate system of each detected fiducial marker_array_size = 3*pose_array_size; if (marker_array_size >= prev_marker_array_size_) { marker_array_msg_.markers.resize(marker_array_size); } // publish a coordinate system from arrow markers for each object for (unsigned int i=0; i<pose_array_size; i++) { for (unsigned int j=0; j<3; j++) { unsigned int idx = 3*i+j; marker_array_msg_.markers[idx].header.frame_id = received_frame_id_;// "/" + frame_id;//"tf_name.str()"; marker_array_msg_.markers[idx].header.stamp = received_timestamp_; marker_array_msg_.markers[idx].ns = "fiducials"; marker_array_msg_.markers[idx].id = id_start_idx + idx; marker_array_msg_.markers[idx].type = visualization_msgs::Marker::ARROW; marker_array_msg_.markers[idx].action = visualization_msgs::Marker::ADD; marker_array_msg_.markers[idx].color.a = 0.85; marker_array_msg_.markers[idx].color.r = 0; marker_array_msg_.markers[idx].color.g = 0; marker_array_msg_.markers[idx].color.b = 0; marker_array_msg_.markers[idx].points.resize(2); marker_array_msg_.markers[idx].points[0].x = 0.0; marker_array_msg_.markers[idx].points[0].y = 0.0; marker_array_msg_.markers[idx].points[0].z = 0.0; marker_array_msg_.markers[idx].points[1].x = 0.0; marker_array_msg_.markers[idx].points[1].y = 0.0; marker_array_msg_.markers[idx].points[1].z = 0.0; if (j==0) { marker_array_msg_.markers[idx].points[1].x = 0.2; marker_array_msg_.markers[idx].color.r = 255; } else if (j==1) { marker_array_msg_.markers[idx].points[1].y = 0.2; marker_array_msg_.markers[idx].color.g = 255; } else if (j==2) { marker_array_msg_.markers[idx].points[1].z = 0.2; marker_array_msg_.markers[idx].color.b = 255; } marker_array_msg_.markers[idx].pose.position.x = vec_vec7d[i][0]; marker_array_msg_.markers[idx].pose.position.y = vec_vec7d[i][1]; marker_array_msg_.markers[idx].pose.position.z = vec_vec7d[i][2]; marker_array_msg_.markers[idx].pose.orientation.x = vec_vec7d[i][4]; marker_array_msg_.markers[idx].pose.orientation.y = vec_vec7d[i][5]; marker_array_msg_.markers[idx].pose.orientation.z = vec_vec7d[i][6]; marker_array_msg_.markers[idx].pose.orientation.w = vec_vec7d[i][3]; ros::Duration one_hour = ros::Duration(1); // 1 second marker_array_msg_.markers[idx].lifetime = one_hour; marker_array_msg_.markers[idx].scale.x = 0.01; // shaft diameter marker_array_msg_.markers[idx].scale.y = 0.015; // head diameter marker_array_msg_.markers[idx].scale.z = 0; // head length 0=default } if (prev_marker_array_size_ > marker_array_size) { for (unsigned int i = marker_array_size; i < prev_marker_array_size_; ++i) { marker_array_msg_.markers[i].action = visualization_msgs::Marker::DELETE; } } prev_marker_array_size_ = marker_array_size; fiducials_marker_array_publisher_.publish(marker_array_msg_); } } // End: publish markers if (tags_vec.empty()) return false; return true; }
// Publish the current tracker pose void SystemFrontendBase::PublishPose() { ROS_ASSERT(mpTracker); if (mpTracker->GetTrackingQuality() == Tracker::NONE || mpTracker->IsLost()) return; geometry_msgs::PoseWithCovarianceStamped pose_cov_msg; pose_cov_msg.header.stamp = mpTracker->GetCurrentTimestamp(); pose_cov_msg.header.frame_id = "vision_world"; TooN::SE3<> pose = mpTracker->GetCurrentPose().inverse(); TooN::Matrix<3> rot = pose.get_rotation().get_matrix(); TooN::Matrix<6> cov = mpTracker->GetCurrentCovariance(); // clear cross correlation cov.slice<0, 3, 3, 3>() = TooN::Zeros; cov.slice<3, 0, 3, 3>() = TooN::Zeros; // Change covariance matrix frame from camera to world cov.slice<0, 0, 3, 3>() = rot * cov.slice<0, 0, 3, 3>() * rot.T(); cov.slice<3, 3, 3, 3>() = rot * cov.slice<3, 3, 3, 3>() * rot.T(); // Some hacky heuristics here if (mpTracker->GetTrackingQuality() == Tracker::GOOD) { cov = cov * 1e2; } else if (mpTracker->GetTrackingQuality() == Tracker::DODGY) { cov = cov * 1e5; } else if (mpTracker->GetTrackingQuality() == Tracker::BAD) { cov = cov * 1e8; } pose_cov_msg.pose.pose = util::SE3ToPoseMsg(pose); int numElems = 6; for (int i = 0; i < numElems; ++i) { for (int j = 0; j < numElems; ++j) { pose_cov_msg.pose.covariance[i * numElems + j] = cov(i, j); } } mPoseWithCovPub.publish(pose_cov_msg); static tf::TransformBroadcaster br; tf::Transform transform; tf::poseMsgToTF(pose_cov_msg.pose.pose, transform); br.sendTransform(tf::StampedTransform(transform, mpTracker->GetCurrentTimestamp(), "vision_world", "vision_pose")); SE3Map mPoses = mpTracker->GetCurrentCameraPoses(); geometry_msgs::PoseArray pose_array_msg; pose_array_msg.header.stamp = pose_cov_msg.header.stamp; pose_array_msg.header.frame_id = pose_cov_msg.header.frame_id; pose_array_msg.poses.resize(1 + mPoses.size()); pose_array_msg.poses[0] = pose_cov_msg.pose.pose; int i = 1; for (SE3Map::iterator it = mPoses.begin(); it != mPoses.end(); ++it, ++i) { pose_array_msg.poses[i] = util::SE3ToPoseMsg(it->second.inverse()); tf::poseMsgToTF(pose_array_msg.poses[i], transform); br.sendTransform(tf::StampedTransform(transform, mpTracker->GetCurrentTimestamp(), "vision_world", it->first)); } mPoseArrayPub.publish(pose_array_msg); }
void send(const string& name,const RigidTransform& T,const char* parent="world") { tf::Transform transform; KlamptToROS(T,transform); broadcaster.sendTransform(tf::StampedTransform(transform, ros::Time::now(), parent, name)); }
void Mapper::processCloud(unique_ptr<DP> newPointCloud, const std::string& scannerFrame, const ros::Time& stamp, uint32_t seq) { processingNewCloud = true; BoolSetter stopProcessingSetter(processingNewCloud, false); // if the future has completed, use the new map processNewMapIfAvailable(); // IMPORTANT: We need to receive the point clouds in local coordinates (scanner or robot) timer t; // Convert point cloud const size_t goodCount(newPointCloud->features.cols()); if (goodCount == 0) { ROS_ERROR("I found no good points in the cloud [at mapper.cpp]"); return; } // Dimension of the point cloud, important since we handle 2D and 3D const int dimp1(newPointCloud->features.rows()); ROS_INFO("Processing new point cloud"); { timer t; // Print how long take the algo // Apply filters to incoming cloud, in scanner coordinates inputFilters.apply(*newPointCloud); ROS_INFO_STREAM("Input filters took " << t.elapsed() << " [s]"); } string reason; // Initialize the transformation to identity if empty if(!icp.hasMap()) { // we need to know the dimensionality of the point cloud to initialize properly publishLock.lock(); TOdomToMap = PM::TransformationParameters::Identity(dimp1, dimp1); publishLock.unlock(); } // Fetch transformation from scanner to odom // Note: we don't need to wait for transform. It is already called in transformListenerToEigenMatrix() PM::TransformationParameters TOdomToScanner; try { TOdomToScanner = PointMatcher_ros::eigenMatrixToDim<float>( PointMatcher_ros::transformListenerToEigenMatrix<float>( tfListener, scannerFrame, odomFrame, stamp ), dimp1); } catch(tf::ExtrapolationException e) { ROS_ERROR_STREAM("Extrapolation Exception. stamp = "<< stamp << " now = " << ros::Time::now() << " delta = " << ros::Time::now() - stamp); return; } //Correct TOdomToMap to fit dimp1 (dimension of input data) //We need to do this because we could have called 'CorrectPose' which always returns a [4][4] matrix TOdomToMap = PointMatcher_ros::eigenMatrixToDim<float>(TOdomToMap, dimp1); ROS_DEBUG_STREAM("TOdomToScanner(" << odomFrame<< " to " << scannerFrame << "):\n" << TOdomToScanner); ROS_DEBUG_STREAM("TOdomToMap(" << odomFrame<< " to " << mapFrame << "):\n" << TOdomToMap); const PM::TransformationParameters TscannerToMap = transformation->correctParameters(TOdomToMap * TOdomToScanner.inverse()); ROS_DEBUG_STREAM("TscannerToMap (" << scannerFrame << " to " << mapFrame << "):\n" << TscannerToMap); // Ensure a minimum amount of point after filtering const int ptsCount = newPointCloud->features.cols(); if(ptsCount < minReadingPointCount) { ROS_ERROR_STREAM("Not enough points in newPointCloud: only " << ptsCount << " pts."); return; } // Initialize the map if empty if(!icp.hasMap()) { ROS_INFO_STREAM("Creating an initial map"); mapCreationTime = stamp; setMap(updateMap(newPointCloud.release(), TscannerToMap, false)); // we must not delete newPointCloud because we just stored it in the mapPointCloud return; } // Check dimension if (newPointCloud->features.rows() != icp.getInternalMap().features.rows()) { ROS_ERROR_STREAM("Dimensionality missmatch: incoming cloud is " << newPointCloud->features.rows()-1 << " while map is " << icp.getInternalMap().features.rows()-1); return; } // Call this function in order to verbose errors when matching the newPointCloud and the icp Internal Map debugFeaturesDescriptors(*newPointCloud); try { // Apply ICP PM::TransformationParameters Ticp; Ticp = icp(*newPointCloud, TscannerToMap); ROS_DEBUG_STREAM("Ticp:\n" << Ticp); // Ensure minimum overlap between scans const double estimatedOverlap = icp.errorMinimizer->getOverlap(); ROS_INFO_STREAM("Overlap: " << estimatedOverlap); if (estimatedOverlap < minOverlap) { ROS_ERROR_STREAM("Estimated overlap too small, ignoring ICP correction!"); return; } // Compute tf publishStamp = stamp; publishLock.lock(); TOdomToMap = Ticp * TOdomToScanner; // Publish tf tfBroadcaster.sendTransform(PointMatcher_ros::eigenMatrixToStampedTransform<float>(TOdomToMap, mapFrame, odomFrame, stamp)); publishLock.unlock(); processingNewCloud = false; ROS_DEBUG_STREAM("TOdomToMap:\n" << TOdomToMap); // Publish odometry if (odomPub.getNumSubscribers()) odomPub.publish(PointMatcher_ros::eigenMatrixToOdomMsg<float>(Ticp, mapFrame, stamp)); // Publish error on odometry if (odomErrorPub.getNumSubscribers()) odomErrorPub.publish(PointMatcher_ros::eigenMatrixToOdomMsg<float>(TOdomToMap, mapFrame, stamp)); // Publish outliers if (outlierPub.getNumSubscribers()) { //DP outliers = PM::extractOutliers(transformation->compute(*newPointCloud, Ticp), *mapPointCloud, 0.6); //outlierPub.publish(PointMatcher_ros::pointMatcherCloudToRosMsg<float>(outliers, mapFrame, mapCreationTime)); } // check if news points should be added to the map if ( mapping && ((estimatedOverlap < maxOverlapToMerge) || (icp.getInternalMap().features.cols() < minMapPointCount)) && #if BOOST_VERSION >= 104100 (!mapBuildingInProgress) #else // BOOST_VERSION >= 104100 true #endif // BOOST_VERSION >= 104100 ) { // make sure we process the last available map mapCreationTime = stamp; #if BOOST_VERSION >= 104100 ROS_INFO("Adding new points to the map in background"); mapBuildingTask = MapBuildingTask(boost::bind(&Mapper::updateMap, this, newPointCloud.release(), Ticp, true)); mapBuildingFuture = mapBuildingTask.get_future(); mapBuildingThread = boost::thread(boost::move(boost::ref(mapBuildingTask))); mapBuildingInProgress = true; #else // BOOST_VERSION >= 104100 ROS_INFO("Adding new points to the map"); setMap(updateMap( newPointCloud.release(), Ticp, true)); #endif // BOOST_VERSION >= 104100 } } catch (PM::ConvergenceError error) { ROS_ERROR_STREAM("ICP failed to converge: " << error.what()); return; } //Statistics about time and real-time capability int realTimeRatio = 100*t.elapsed() / (stamp.toSec()-lastPoinCloudTime.toSec()); ROS_INFO_STREAM("[TIME] Total ICP took: " << t.elapsed() << " [s]"); if(realTimeRatio < 80) ROS_INFO_STREAM("[TIME] Real-time capability: " << realTimeRatio << "%"); else ROS_WARN_STREAM("[TIME] Real-time capability: " << realTimeRatio << "%"); lastPoinCloudTime = stamp; }
void RosAriaNode::publish() { // Note, this is called via SensorInterpTask callback (myPublishCB, named "ROSPublishingTask"). ArRobot object 'robot' sholud not be locked or unlocked. pos = robot->getPose(); tf::poseTFToMsg(tf::Transform(tf::createQuaternionFromYaw(pos.getTh()*M_PI/180), tf::Vector3(pos.getX()/1000, pos.getY()/1000, 0)), position.pose.pose); //Aria returns pose in mm. position.twist.twist.linear.x = robot->getVel()/1000.0; //Aria returns velocity in mm/s. position.twist.twist.linear.y = robot->getLatVel()/1000.0; position.twist.twist.angular.z = robot->getRotVel()*M_PI/180; position.header.frame_id = frame_id_odom; position.child_frame_id = frame_id_base_link; position.header.stamp = ros::Time::now(); pose_pub.publish(position); ROS_DEBUG("RosAria: publish: (time %f) pose x: %f, y: %f, angle: %f; linear vel x: %f, y: %f; angular vel z: %f", position.header.stamp.toSec(), (double)position.pose.pose.position.x, (double)position.pose.pose.position.y, (double)position.pose.pose.orientation.w, (double) position.twist.twist.linear.x, (double) position.twist.twist.linear.y, (double) position.twist.twist.angular.z ); // publishing transform odom->base_link odom_trans.header.stamp = ros::Time::now(); odom_trans.header.frame_id = frame_id_odom; odom_trans.child_frame_id = frame_id_base_link; odom_trans.transform.translation.x = pos.getX()/1000; odom_trans.transform.translation.y = pos.getY()/1000; odom_trans.transform.translation.z = 0.0; odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(pos.getTh()*M_PI/180); odom_broadcaster.sendTransform(odom_trans); // getStallValue returns 2 bytes with stall bit and bumper bits, packed as (00 00 FrontBumpers RearBumpers) int stall = robot->getStallValue(); unsigned char front_bumpers = (unsigned char)(stall >> 8); unsigned char rear_bumpers = (unsigned char)(stall); bumpers.header.frame_id = frame_id_bumper; bumpers.header.stamp = ros::Time::now(); std::stringstream bumper_info(std::stringstream::out); // Bit 0 is for stall, next bits are for bumpers (leftmost is LSB) for (unsigned int i=0; i<robot->getNumFrontBumpers(); i++) { bumpers.front_bumpers[i] = (front_bumpers & (1 << (i+1))) == 0 ? 0 : 1; bumper_info << " " << (front_bumpers & (1 << (i+1))); } ROS_DEBUG("RosAria: Front bumpers:%s", bumper_info.str().c_str()); bumper_info.str(""); // Rear bumpers have reverse order (rightmost is LSB) unsigned int numRearBumpers = robot->getNumRearBumpers(); for (unsigned int i=0; i<numRearBumpers; i++) { bumpers.rear_bumpers[i] = (rear_bumpers & (1 << (numRearBumpers-i))) == 0 ? 0 : 1; bumper_info << " " << (rear_bumpers & (1 << (numRearBumpers-i))); } ROS_DEBUG("RosAria: Rear bumpers:%s", bumper_info.str().c_str()); bumpers_pub.publish(bumpers); //Publish battery information // TODO: Decide if BatteryVoltageNow (normalized to (0,12)V) is a better option std_msgs::Float64 batteryVoltage; batteryVoltage.data = robot->getRealBatteryVoltageNow(); voltage_pub.publish(batteryVoltage); if(robot->haveStateOfCharge()) { std_msgs::Float32 soc; soc.data = robot->getStateOfCharge()/100.0; state_of_charge_pub.publish(soc); } // publish recharge state if changed char s = robot->getChargeState(); if(s != recharge_state.data) { ROS_INFO("RosAria: publishing new recharge state %d.", s); recharge_state.data = s; recharge_state_pub.publish(recharge_state); } // publish motors state if changed bool e = robot->areMotorsEnabled(); if(e != motors_state.data || !published_motors_state) { ROS_INFO("RosAria: publishing new motors state %d.", e); motors_state.data = e; motors_state_pub.publish(motors_state); published_motors_state = true; } // Publish sonar information, if enabled. if (publish_sonar || publish_sonar_pointcloud2) { sensor_msgs::PointCloud cloud; //sonar readings. cloud.header.stamp = position.header.stamp; //copy time. // sonar sensors relative to base_link cloud.header.frame_id = frame_id_sonar; std::stringstream sonar_debug_info; // Log debugging info sonar_debug_info << "Sonar readings: "; for (int i = 0; i < robot->getNumSonar(); i++) { ArSensorReading* reading = NULL; reading = robot->getSonarReading(i); if(!reading) { ROS_WARN("RosAria: Did not receive a sonar reading."); continue; } // getRange() will return an integer between 0 and 5000 (5m) sonar_debug_info << reading->getRange() << " "; // local (x,y). Appears to be from the centre of the robot, since values may // exceed 5000. This is good, since it means we only need 1 transform. // x & y seem to be swapped though, i.e. if the robot is driving north // x is north/south and y is east/west. // //ArPose sensor = reading->getSensorPosition(); //position of sensor. // sonar_debug_info << "(" << reading->getLocalX() // << ", " << reading->getLocalY() // << ") from (" << sensor.getX() << ", " // << sensor.getY() << ") ;; " ; //add sonar readings (robot-local coordinate frame) to cloud geometry_msgs::Point32 p; p.x = reading->getLocalX() / 1000.0; p.y = reading->getLocalY() / 1000.0; p.z = 0.0; cloud.points.push_back(p); } ROS_DEBUG_STREAM(sonar_debug_info.str()); // publish topic(s) if(publish_sonar_pointcloud2) { sensor_msgs::PointCloud2 cloud2; if(!sensor_msgs::convertPointCloudToPointCloud2(cloud, cloud2)) { ROS_WARN("Error converting sonar point cloud message to point_cloud2 type before publishing! Not publishing this time."); } else { sonar_pointcloud2_pub.publish(cloud2); } } if(publish_sonar) { sonar_pub.publish(cloud); } } // end if sonar_enabled }
void* ControlThread(void *p) { COdometerCapture *pComm = (COdometerCapture*)p; /// Handling events const int EVENT_NUM = 4; HANDLE hWait[EVENT_NUM]; hWait[0] = pComm->m_hEventOdoCalTimer; hWait[1] = pComm->m_hEventOdoPubTimer; hWait[2] = pComm->m_hEventSpeedTimer; hWait[3] = pComm->m_hEventSpeedCheckTimer; /// Real Send Speed // double real_vx = 0.0; // double real_vy = 0.0; // double real_w = 0.0; // allowance change value for a single cycle // double allow_vx_change_per_cycle = 2.5; // cm/s // double allow_vx_decrease_per_cycle = 5.75; //cm/s // double allow_vy_change_per_cycle = 2.5; // cm/s // double allow_w_increase_per_cycle = 4.5; // deg/s // double allow_w_decrease_per_cycle = 4.5; // deg/s // // extern double g_closest_obstacle_x; //urg看到的最近障碍物点 x,y坐标 // extern double g_closest_obstacle_y; //urg看到的最近障碍物点 // extern double g_max_acc_inc_x; //最大加速度 // extern double g_max_acc_dec_x; //最减加速度 // extern double g_max_acc_w; // extern double g_max_vel_x; //最大速度 // extern double g_max_vel_w; //最大角速度 extern boost::mutex g_mutex_obstacle; // // double cur_ac_vx,cur_ac_vy,cur_ac_vw; // double old_x = 0,old_y = 0,old_w = 0; boost::posix_time::ptime prev_odom_time; boost::mutex vel_mutex; /// Handling loop fd_set fdset; //uint64_t exp; struct timeval timeout; timeout.tv_sec=5; timeout.tv_usec=0; while (pComm->m_initial) { FD_ZERO(&fdset); int maxfp=0; for(int k=0;k<EVENT_NUM;k++) { FD_SET(hWait[k],&fdset); if(maxfp<hWait[k]) { maxfp=hWait[k]; } } int dRes = select(maxfp+1,&fdset,NULL,NULL,&timeout); switch (dRes) { //*********************************// // 修改case后面的值 // //********************************// case -1 : cout<<"error of select"<<endl; case 0 : cout<<"No Data within five seconds"<<endl; default : if(FD_ISSET(hWait[0],&fdset)) // 里程计合成 { pComm->m_mutex_cap.Lock(); ODOCAL->onTimerCal2(); /// Timer on calculation - using position pComm->m_mutex_cap.Unlock(); } if(FD_ISSET(hWait[1],&fdset)) // 里程计发布 { pComm->m_mutex_cap.Lock(); nav_msgs::Odometry odometerData; pComm->GetCurOdometry(odometerData); odometerData.header.stamp = ros::Time::now(); odometerData.header.frame_id = "odom"; odometerData.child_frame_id = "base_link"; static tf::TransformBroadcaster odom_broadcaster; geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(odometerData.pose.pose.orientation.z); ///first, we'll publish odom the transform over tf geometry_msgs::TransformStamped odom_trans; odom_trans.header.stamp = ros::Time::now(); odom_trans.header.frame_id = "odom"; odom_trans.child_frame_id = "base_link"; odom_trans.transform.translation.x = odometerData.pose.pose.position.x; odom_trans.transform.translation.y = odometerData.pose.pose.position.y; odom_trans.transform.translation.z = 0.0; odom_trans.transform.rotation = odom_quat; ///send the transform odom_broadcaster.sendTransform(odom_trans); ///publish laser transform over tf ////sensor_msgs::LaserScan LaserData; // pComm->GetCurLaser(LaserData); static tf::TransformBroadcaster laser_broadcaster; tf::Transform laser_transform; laser_transform.setOrigin( tf::Vector3(0.3, 0, 0.05) ); tf::Quaternion q; q.setRPY(0, 0, 0); laser_transform.setRotation(q); laser_broadcaster.sendTransform(tf::StampedTransform(laser_transform, ros::Time::now(), "base_link", "laser")); // geometry_msgs::Quaternion laser_quat = tf::createQuaternionMsgFromYaw(0); // geometry_msgs::TransformStamped laser_trans; // LaserData.header.stamp = ros::Time::now(); // laser_trans.transform.translation.x = 0.3; // laser_trans.transform.translation.y = 0; // laser_trans.transform.translation.z = 0.05; // laser_trans.transform.rotation = laser_quat; //s_laser.publish(LaserData); m_odom.publish(odometerData); pComm->m_mutex_cap.Unlock(); } if(FD_ISSET(hWait[2],&fdset)) // 速度下发 { geometry_msgs::Twist sendSpeed; pComm->GetCurSpeed(sendSpeed); double send_vx = sendSpeed.linear.x; double send_vy = sendSpeed.linear.y; double send_w = sendSpeed.angular.z; //限速 send_vx = Utils::clip(send_vx,-1000.0,1000.0); send_vy = Utils::clip(send_vy,-1000.0,1000.0); send_w = Utils::clip(send_w,-50.0,50.0); MECANUM->SendVelocities(send_vx/100.0, send_vy/100.0, send_w*3.1415926/180.0); pComm->DoSafeSpeed(); } if(FD_ISSET(hWait[3],&fdset)) // 速度下发检查 { pComm->SpeedCheck(); } break; } } return 0; }