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;
}