ROS_Robot/jie_ware
1
0
Fork 0
mirror of https://github.com/6-robot/jie_ware.git synced 2025-09-15 12:59:05 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

add laser_topic

Browse Source
This commit is contained in:
Robot 2024-10-13 22:57:32 +08:00
parent 2b8ecba033
commit 17277b245f
3 changed files with 14 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
README.md
View File

@ -17,11 +17,12 @@ git clone https://github.com/6-robot/jie_ware.git
cd ~/catkin_ws
catkin_make
```
3. 修改Launch文件，用下面的内容替换AMCL节点
3. 修改Launch文件，用如下内容替换AMCL节点
```
<node pkg="jie_ware" type="lidar_loc" name="lidar_loc" >
<param name="base_frame" value="base_footprint" />
<param name="laser_frame" value="laser" />
<param name="laser_topic" value="scan" />
</node>
```
4. 运行修改后的Launch文件

3
launch/lidar_loc_test.launch
View File

@ -32,7 +32,7 @@
<node name="chair_3" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/chair.model -x 2.7 -y 2.3 -z 0 -Y -1.57 -urdf -model chair_3" />
<!-- Spawn a robot into Gazebo -->
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/wpb_home.model -urdf -x -0.0 -y -0.0 -model wpb_home" />
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-file $(find wpr_simulation)/models/wpb_home.model -urdf -x 0.0 -y 0.0 -model wpb_home" />
<!-- Run the map server -->
<node name="map_server" pkg="map_server" type="map_server" args="$(find wpr_simulation)/maps/map.yaml"/>
@ -41,6 +41,7 @@
<node pkg="jie_ware" type="lidar_loc" name="lidar_loc" >
<param name="base_frame" value="base_footprint" />
<param name="laser_frame" value="laser" />
<param name="laser_topic" value="scan" />
</node>
<!--- Run move base -->

14
src/lidar_loc.cpp
View File

@ -27,11 +27,13 @@ std::vector<cv::Point2f> transform_points;
ros::ServiceClient clear_costmaps_client;
std::string base_frame;
std::string laser_frame;
std::string laser_topic;
float lidar_x = 250, lidar_y = 250, lidar_yaw = 0;
float deg_to_rad = M_PI / 180.0;
int cur_sum = 0;
int clear_countdown = -1;
int scan_count = 0;
// 初始姿态回调函数
void initialPoseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
@ -182,6 +184,8 @@ void scanCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
}
angle += msg->angle_increment;
}
if(scan_count == 0)
scan_count ++;
while (ros::ok())
{
@ -356,6 +360,7 @@ void processMap()
void pose_tf()
{
if (scan_count == 0) return;
if (map_cropped.empty() || map_msg.data.empty()) return;
static tf2_ros::Buffer tfBuffer;
@ -429,12 +434,13 @@ int main(int argc, char** argv)
ros::NodeHandle private_nh("~");
private_nh.param<std::string>("base_frame", base_frame, "base_footprint");
private_nh.param<std::string>("laser_frame", laser_frame, "laser");
private_nh.param<std::string>("laser_topic", laser_topic, "scan");
ros::NodeHandle nh;
ros::Subscriber map_sub = nh.subscribe("/map", 1, mapCallback);
ros::Subscriber scan_sub = nh.subscribe("/scan", 1, scanCallback);
ros::Subscriber initial_pose_sub = nh.subscribe("/initialpose", 1, initialPoseCallback);
clear_costmaps_client = nh.serviceClient<std_srvs::Empty>("/move_base/clear_costmaps");
ros::Subscriber map_sub = nh.subscribe("map", 1, mapCallback);
ros::Subscriber scan_sub = nh.subscribe(laser_topic, 1, scanCallback);
ros::Subscriber initial_pose_sub = nh.subscribe("initialpose", 1, initialPoseCallback);
clear_costmaps_client = nh.serviceClient<std_srvs::Empty>("move_base/clear_costmaps");
ros::Rate rate(30); // tf发送频率