資源簡介
LS01C_ROS,鐳神,激光雷達,在ros下的完整開發包,大家可以下載討論

代碼片段和文件信息
#include?
#include?
#include?
#include?“ros/ros.h“
#include?“std_msgs/String.h“
#include?“sensor_msgs/LaserScan.h“
#include?“uart_driver.h“
using?namespace?std;
int?startOrStop?=?1; //?1是start,0是stop
pthread_mutex_t?mutex?=?PTHREAD_MUTEX_INITIALIZER;
void?publish_scan(ros::Publisher?*pub?double?*dist?int?count?ros::Time?start?double?scan_time)
{
static?int?scan_count?=?0;
sensor_msgs::LaserScan?scan_msg;
scan_msg.header.stamp?=?start;
scan_msg.header.frame_id?=?“laser“;
scan_count++;
scan_msg.angle_min?=?3.1415926;
scan_msg.angle_max?=?-3.1415926;
scan_msg.angle_increment?=?(scan_msg.angle_max?-?scan_msg.angle_min)?/?(double)(count?-?1);
scan_msg.scan_time?=?scan_time;
scan_msg.time_increment?=?scan_time?/?(double)(count?-?1);
scan_msg.range_min?=?0.15;
scan_msg.range_max?=?6.0;
scan_msg.intensities.resize(count);
scan_msg.ranges.resize(count);
for?(int?i?=?0;?i? {
if?(dist[i]?==?0.0)
scan_msg.ranges[i]?=?std::numeric_limits::infinity();
else
scan_msg.ranges[i]?=?dist[i]?/?1000.0;
scan_msg.intensities[i]?=?0;
}
pub->publish(scan_msg);
}
void?startStopCB(const?std_msgs::Int32ConstPtr?msg)
{
pthread_mutex_lock(&mutex);
startOrStop?=?msg->data;
pthread_mutex_unlock(&mutex);
}
int?main(int?argv?char?**argc)
{
ros::init(argv?argc?“talker“);
ros::NodeHandle?n;
ros::Publisher?scan_pub?=?n.advertise(“scan“?1000);
ros::Subscriber?stop_sub?=?n.subscribe(“startOrStop“?10?startStopCB);
io_driver?driver;
?? int?ret?=?driver.OpenSerial(B230400);
????????driver.StartScan();
bool?isStarted?=?true;
double?angle[360?*?2];
double?distance[360?*?2];
double?data[360?*?2];
double?speed;
int?count?=?0;
ros::Time?starts?=?ros::Time::now();
ros::Time?ends?=?ros::Time::now();
ROS_INFO(“talker....“);
while(ros::ok())
{
ros::spinOnce();
pthread_mutex_lock(&mutex);
if(isStarted?&&?0?==?startOrStop)?//?當前正在掃描且要求停止
{
ROS_INFO(“stop“);
driver.StopScan();
isStarted?=?false;
}
else?if(!isStarted?&&?1?==?startOrStop) //?當前未掃描且要求開始掃描
{
ROS_INFO(“start“);
driver.StartScan();
isStarted?=?true;
}
pthread_mutex_unlock(&mutex);
ROS_INFO(“%s“?isStarted???“Started“:“Stopped“);
if(!isStarted)
continue;
memset(data?0?sizeof(data));
int?ret?=?driver.GetScanData(angle?distance?PACKLEN?&speed);
for?(int?i?=?0;?i? {
data[(int)(angle[i]+0.5)]?=?distance[i];
}
ends?=?ros::Time::now();
float?scan_duration?=?(ends?-?starts).toSec()?*?1e-3;
publish_scan(&scan_pub?data?360?starts?scan_duration);
starts?=?ends;
}
return?0;
}
?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????文件???????3469??2016-11-30?20:27??LS01C_ROS\talker\.cproject
?????文件????????807??2016-11-30?20:27??LS01C_ROS\talker\.project
?????文件????????321??2016-11-30?20:27??LS01C_ROS\talker\CMakeLists.txt
?????文件?????????72??2016-11-30?20:27??LS01C_ROS\talker\launch\talker.launch
?????文件???????2098??2016-11-30?20:27??LS01C_ROS\talker\package.xm
?????文件???????2734??2016-11-30?20:27??LS01C_ROS\talker\src\talker.cpp
?????文件???????2744??2016-11-30?20:27??LS01C_ROS\talker\src\talker.cpp~
?????文件??????19179??2016-11-30?20:27??LS01C_ROS\talker\src\uart_driver.cpp
?????文件???????1974??2016-11-30?20:27??LS01C_ROS\talker\src\uart_driver.h
????..AD...?????????0??2016-11-30?20:27??LS01C_ROS\talker\include\talker
????..AD...?????????0??2016-11-30?20:27??LS01C_ROS\talker\include
????..AD...?????????0??2016-11-30?20:27??LS01C_ROS\talker\launch
????..AD...?????????0??2016-11-30?20:27??LS01C_ROS\talker\src
????..AD...?????????0??2016-11-30?20:27??LS01C_ROS\talker
????..AD...?????????0??2016-11-30?20:29??LS01C_ROS
-----------?---------??----------?-----??----
????????????????33398????????????????????15
- 上一篇:ZedGraph .net4.0版本
- 下一篇:脈沖多普勒雷達測速
評論
共有 條評論