|
| 1 | +#include <ros/ros.h> |
| 2 | +#include <laser_geometry/laser_geometry.h> |
| 3 | +#include <sensor_msgs/PointCloud.h> |
| 4 | +#include <sensor_msgs/LaserScan.h> |
| 5 | +#include <geometry_msgs/Point32.h> |
| 6 | +#include <geometry_msgs/Twist.h> |
| 7 | +#include <nav_msgs/Odometry.h> |
| 8 | +#include <std_msgs/Int8.h> |
| 9 | +#include <float.h> |
| 10 | + |
| 11 | +#define SEARCH 0 |
| 12 | +#define GET_GOAL 1 |
| 13 | +#define FIND 1 |
| 14 | + |
| 15 | +ros::Publisher goal_poisition_pub; |
| 16 | +ros::Publisher get_goal_state_pub; |
| 17 | +ros::Subscriber turtlebot_position_sub; |
| 18 | +ros::Subscriber scan_filtered_sub; |
| 19 | +ros::Subscriber searching_dock_state_sub; |
| 20 | + |
| 21 | +sensor_msgs::PointCloud cloud; |
| 22 | +geometry_msgs::Point32 final_goal_point; |
| 23 | +geometry_msgs::Point32 turtlebot_point; |
| 24 | +std_msgs::Int8 searching_dock_state_msg; |
| 25 | +std_msgs::Int8 get_goal_state_msg; |
| 26 | +laser_geometry::LaserProjection projector; |
| 27 | + |
| 28 | +float material_intensities_array[360] = {0}; |
| 29 | +float point_x_sum = 0.0; |
| 30 | +float point_y_sum = 0.0; |
| 31 | +float point_z_sum = 0.0; |
| 32 | +float average_intensity = 0.0; |
| 33 | +float material_intensity_sum = 0.0; |
| 34 | +int j = 0; |
| 35 | +int k = 0; |
| 36 | +int count = 0; |
| 37 | + |
| 38 | +void get_goal_position(void); |
| 39 | + |
| 40 | +void searching_dock_state_Callback(const std_msgs::Int8::ConstPtr &searching_dock_state_sub_msg) |
| 41 | +{ |
| 42 | + searching_dock_state_msg.data = searching_dock_state_sub_msg->data; |
| 43 | +} |
| 44 | + |
| 45 | +void odom_Callback(const nav_msgs::Odometry &odom) |
| 46 | +{ |
| 47 | + turtlebot_point.x = odom.pose.pose.position.x; |
| 48 | + turtlebot_point.y = odom.pose.pose.position.y; |
| 49 | +} |
| 50 | + |
| 51 | +void scan_point_Callback(const sensor_msgs::LaserScan &scan_filtered) |
| 52 | +{ |
| 53 | + if(searching_dock_state_msg.data == FIND) |
| 54 | + { |
| 55 | + // put a condition statement with count == 0 for do it once. |
| 56 | + if(count == 0) |
| 57 | + { |
| 58 | + // Replace LaserScan values with x, y, z coordinates. |
| 59 | + projector.projectLaser(scan_filtered, cloud); //index = cloud.channels[1].values //indensity = cloud.channels[0].values |
| 60 | + for(int i=0; i<360; i++) |
| 61 | + { |
| 62 | + // Scan_filtered.ranges [i] and scan_filtered.intensities [i] may have nan. It makes conditional statements when not nan. |
| 63 | + if(isnan(scan_filtered.ranges[i]) == 0.0 && isnan(scan_filtered.intensities[i]) == 0.0) |
| 64 | + { |
| 65 | + // Gets the average of material_intensities. Distinguishes the intensities of the safety reflector |
| 66 | + material_intensities_array[j] = scan_filtered.intensities[i] * scan_filtered.ranges[i]; |
| 67 | + material_intensity_sum += material_intensities_array[j]; |
| 68 | + j += 1; |
| 69 | + } |
| 70 | + } |
| 71 | + //Gets the average of material_intensity |
| 72 | + average_intensity = material_intensity_sum / j; |
| 73 | + |
| 74 | + for(int i=0; i<j; i++) |
| 75 | + { |
| 76 | + if(material_intensities_array[i] > average_intensity) |
| 77 | + { |
| 78 | + // Count the number of coordinates. |
| 79 | + k += 1; |
| 80 | + // Add x and y of scanned laser values. |
| 81 | + // Get the average of x and y coordinates to get the goal coordinates |
| 82 | + point_x_sum += cloud.points[i].x; |
| 83 | + point_y_sum += cloud.points[i].y; |
| 84 | + //ROS_INFO("%d goal_x %f, goal_y %f ",i, cloud.points[i].x, cloud.points[i].y); |
| 85 | + } |
| 86 | + } |
| 87 | + // Add the turtlebot's current position and 1/2 the size of the goal coordinates to get the final coordinates. |
| 88 | + final_goal_point.x = turtlebot_point.x + (point_x_sum / (2 * k)); |
| 89 | + final_goal_point.y = turtlebot_point.y + (point_y_sum / (2 * k)); |
| 90 | + |
| 91 | + ROS_INFO("goal_x %f, goal_y %f ", final_goal_point.x, final_goal_point.y); |
| 92 | + ROS_INFO("average_intensity %f", average_intensity); |
| 93 | + count = 1; |
| 94 | + } |
| 95 | + get_goal_state_msg.data = GET_GOAL; |
| 96 | + goal_poisition_pub.publish(final_goal_point); |
| 97 | + get_goal_state_pub.publish(get_goal_state_msg); |
| 98 | + } |
| 99 | +} |
| 100 | + |
| 101 | +int main(int argc, char** argv) |
| 102 | +{ |
| 103 | + ros::init(argc, argv, "get_goal_position"); |
| 104 | + ros::NodeHandle node; |
| 105 | + |
| 106 | + goal_poisition_pub = node.advertise<geometry_msgs::Point32>("/goal_poisition", 10); |
| 107 | + get_goal_state_pub = node.advertise<std_msgs::Int8>("/get_goal_state", 10); |
| 108 | + searching_dock_state_sub = node.subscribe("/searching_dock_state", 10, &searching_dock_state_Callback); |
| 109 | + turtlebot_position_sub = node.subscribe("/odom", 10, &odom_Callback); |
| 110 | + scan_filtered_sub = node.subscribe("/scan_filtered", 10, &scan_point_Callback); |
| 111 | + |
| 112 | + ros::spin(); |
| 113 | + |
| 114 | + return 0; |
| 115 | +} |
0 commit comments