From 784fdc0a8da775660ec51c78908feb26fe08b3d9 Mon Sep 17 00:00:00 2001 From: caroline Date: Tue, 3 May 2022 18:15:01 +0200 Subject: [PATCH] lines from clusters presentation --- cfg/config.cfg | 6 ++--- src/initial_processing.cpp | 46 +++++++++----------------------------- 2 files changed, 14 insertions(+), 38 deletions(-) diff --git a/cfg/config.cfg b/cfg/config.cfg index 372d14b..aa5ba68 100755 --- a/cfg/config.cfg +++ b/cfg/config.cfg @@ -15,18 +15,18 @@ gen.add("zbpos", double_t, 0, "Max bound of Z - Axis cropping", 5, -5, 5) gen.add("zbneg", double_t, 0, "Min bound of Z - Axis cropping", 0, -5, 5) gen.add("rotx", double_t, 0, "Rotation of cloud around X - Axis", 0, 0, 180) -gen.add("roty", double_t, 0, "Rotation of cloud around Y - Axis", 0, 0, 180) +gen.add("roty", double_t, 0, "Rotation of cloud around Y - Axis", 30, 0, 180) gen.add("rotz", double_t, 0, "Rotation of cloud around Z - Axis", 0, 0, 180) gen.add("movex", double_t, 0, "Move along X - Axis", 0, -10, 10) gen.add("movey", double_t, 0, "Move along Y - Axis", 0, -10, 10) gen.add("movez", double_t, 0, "Move along Z - Axis", 0, -10, 10) -gen.add("passx_min", double_t, 0, "Start value for Passthrough Filtering for X - Axis", 0.0, -5.0 , 5.0) +gen.add("passx_min", double_t, 0, "Start value for Passthrough Filtering for X - Axis", -5.0, -5.0 , 5.0) gen.add("passx_max", double_t, 0, "End value for Passthrough Filtering for X - Axis", 5.0, -5.0 , 5.0) gen.add("passy_min", double_t, 0, "Start value for Passthrough Filtering for Y - Axis", -5.0, -5.0 , 5.0) gen.add("passy_max", double_t, 0, "End value for Passthrough Filtering for Y - Axis", 5.0, -5.0 , 5.0) -gen.add("passz_min", double_t, 0, "Start value for Passthrough Filtering for Z - Axis", 0.0, -5.0 , 5.0) +gen.add("passz_min", double_t, 0, "Start value for Passthrough Filtering for Z - Axis", -5.0, -5.0 , 5.0) gen.add("passz_max", double_t, 0, "End value for Passthrough Filtering for Z - Axis", 5.0, -5.0 , 5.0) gen.add("voxel_size", double_t, 0, "Size of the Leafs for the Voxel Filter", 0.02, 0.0 , 0.1) diff --git a/src/initial_processing.cpp b/src/initial_processing.cpp index 2379b4e..e177402 100755 --- a/src/initial_processing.cpp +++ b/src/initial_processing.cpp @@ -57,10 +57,7 @@ ros::Publisher visual; ros::Publisher steer; ros::Publisher image_pub; ros::Publisher line_pub; -ros::Publisher pub4_1; -ros::Publisher pub4_2; -ros::Publisher pub4_3; -ros::Publisher pub4_4; +ros::Publisher pub_lines; float xbpos = 5.0; float xbneg = 0.0; @@ -69,16 +66,16 @@ float ybneg = -5.0; float zbpos = 5.0; float zbneg = 0.0; float rotx = 0; -float roty = 0; +float roty = 30; float rotz = 0; float movex = 0.0; float movey = 0.0; float movez = 0.0; -float passx_min = 0.0; +float passx_min = -5.0; float passx_max = 5.0; float passy_min = -5.0; float passy_max = 5.0; -float passz_min = 0.0; +float passz_min = -5.0; float passz_max = 5.0; float voxel_size = 0.02; float Pseg_dist = 0.025; @@ -503,9 +500,9 @@ void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg){ point.y = pt_line_y; point.z = pt_line_z; middle_line.points.push_back(point); - point.x = pt_line_x+pt_direction_x; - point.y = pt_line_y+pt_direction_y; - point.z = pt_line_z+pt_direction_z; + point.x = pt_line_x+3*pt_direction_x; + point.y = pt_line_y+3*pt_direction_y; + point.z = pt_line_z+3*pt_direction_z; middle_line.points.push_back(point); } @@ -524,29 +521,11 @@ void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg){ middle_line.header.frame_id = "base_link"; line_pub.publish (middle_line); - //sensor_msgs::PointCloud2 line_cloud_1,line_cloud_2,line_cloud_3,line_cloud_4; - - /*pcl::toROSMsg(*final_1, line_cloud_1); - pcl::toROSMsg(*final_2, line_cloud_2); - pcl::toROSMsg(*final_3, line_cloud_3); - pcl::toROSMsg(*final_4, line_cloud_4); - - line_cloud_1.header.frame_id = "base_link"; - line_cloud_2.header.frame_id = "base_link"; - line_cloud_3.header.frame_id = "base_link"; - line_cloud_4.header.frame_id = "base_link"; - - //pub4_2.publish(line_cloud_2); - //pub4_3.publish(line_cloud_3); - //pub4_4.publish(line_cloud_4);*/ sensor_msgs::PointCloud2 line_cloud; pcl::toROSMsg(cluster_lines, line_cloud); line_cloud.header.frame_id = "base_link"; - pub4_1.publish(line_cloud); + pub_lines.publish(line_cloud); } - - - } // convert to ROS data type @@ -576,16 +555,13 @@ int main(int argc, char **argv){ pub2 = n.advertise("processed_cloud", 1); pub3 = n.advertise("clustered_cloud", 1); pub4 = n.advertise("line_cloud", 1); - marker_pub = n.advertise("visualization_marker", 10); - marker_pub2 = n.advertise("visualization_marker2", 10); + marker_pub = n.advertise("bounding_box", 10); + marker_pub2 = n.advertise("passthrough_box", 10); steer = n.advertise("centroid_marker", 1); image_pub = n.advertise("pcl_image", 30); line_pub = n.advertise("center_line", 10); - pub4_1 = n.advertise("line_cloud_1", 1); - pub4_2 = n.advertise("line_cloud_2", 1); - pub4_3 = n.advertise("line_cloud_3", 1); - pub4_4 = n.advertise("line_cloud_4", 1); + pub_lines = n.advertise("line_clouds", 1); // create publisher for visualising marker visual = n.advertise ("marker", 1);