usless message

command/sub8_launch/launch/tf.launch

@@ -2,8 +2,15 @@
 <launch>
   <include file="$(find sub8_launch)/launch/upload_urdf.launch" />
   <node pkg="robot_state_publisher" name="robot_state_publisher" type="robot_state_publisher">
-    <param name="use_tf_static" value="false" />
+    <param name="use_tf_static" value="true" />
   </node>
+
+  <node pkg="joint_state_publisher" name="joint_state_publisher" type="joint_state_publisher">
+  	<param name="robot_description" textfile="$(find sub8_gazebo)/urdf/sub8.urdf" />
+  	<param name="publish_default_velocities" value="True"/>
+  	<param name="publish_default_efforts" value="True" />
+  </node>
+
   <node pkg="sub8_launch" type="tf_republisher.py" name="tf_replublisher" />
   <node pkg="nodelet" type="nodelet" args="standalone odometry_utils/odometry_to_tf" name="odometry_to_tf"/>
 </launch>

command/sub8_launch/launch/upload_urdf.launch

@@ -12,4 +12,6 @@
     air_density: 1.225
     G: 9.81
   </rosparam>
+
+
 </launch>

command/sub8_launch/package.xml

@@ -8,6 +8,7 @@
   <build_depend>roslaunch</build_depend>
   <run_depend>roslaunch</run_depend>
   <run_depend>robot_state_publisher</run_depend>
+  <run_depend>joint_state_publisher</run_depend>
   <run_depend>nodelet</run_depend>
   <run_depend>c3_trajectory_generator</run_depend>
   <run_depend>rise_6dof</run_depend>

perception/sub8_pointcloud/include/OGridGen.hpp

@@ -30,6 +30,7 @@
 
 #include <Classification.hpp>
 
+#include <sub8_gazebo/simulated_sonar_ping.h>
 #include <mil_msgs/ObjectDBQuery.h>
 #include <mil_msgs/PerceptionObject.h>
 #include <mil_msgs/PerceptionObjectArray.h>
@@ -62,7 +63,9 @@ class OGridGen
   OGridGen();
   void publish_big_pointcloud(const ros::TimerEvent &);
 
-  void callback(const mil_blueview_driver::BlueViewPingPtr &ping_msg);
+  //void callback(const mil_blueview_driver::BlueViewPingPtr &ping_msg);
+
+  void callback(const sub8_gazebo::simulated_sonar_pingPtr &ping_msg);
   void dvl_callback(const mil_msgs::RangeStampedConstPtr &dvl);
 
   bool clear_ogrid_callback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res);

perception/sub8_pointcloud/src/OGridGen.cpp

@@ -49,8 +49,9 @@ OGridGen::OGridGen()
   // Run the publisher
   timer_ =
       nh_.createTimer(ros::Duration(0.3), std::bind(&OGridGen::publish_big_pointcloud, this, std::placeholders::_1));
-  sub_to_imaging_sonar_ = nh_.subscribe("/blueview_driver/ranges", 1, &OGridGen::callback, this);
+ // sub_to_imaging_sonar_ = nh_.subscribe("/blueview_driver/ranges", 1, &OGridGen::callback, this);
   sub_to_dvl_ = nh_.subscribe("/dvl/range", 1, &OGridGen::dvl_callback, this);
+  sub_to_imaging_sonar_ = nh_.subscribe("/simRanges", 1, &OGridGen::callback, this);
 
   mat_ogrid_ = cv::Mat::zeros(int(ogrid_size_ / resolution_), int(ogrid_size_ / resolution_), CV_8U);
   persistant_ogrid_ = cv::Mat(int(ogrid_size_) / resolution_, int(ogrid_size_ / resolution_), CV_32FC1);
@@ -116,14 +117,14 @@ void OGridGen::publish_big_pointcloud(const ros::TimerEvent &)
   }
 }
 
-/*
-  Subscribes to pingmsgs from blueview sonar and saves a plane of pings into a buffer based on sub pose
-*/
-void OGridGen::callback(const mil_blueview_driver::BlueViewPingPtr &ping_msg)
+
+  //Subscribes to pingmsgs from blueview sonar and saves a plane of pings into a buffer based on sub pose
+//void OGridGen::callback(const mil_blueview_driver::BlueViewPingPtr &ping_msg)
+void OGridGen::callback(const sub8_gazebo::simulated_sonar_pingPtr &ping_msg)
 {
   try  // TODO: Switch to TF2
   {
-    listener_.lookupTransform("/map", "/blueview", ros::Time(0), transform_);
+    listener_.lookupTransform("/map", "/blueview/sonar_link", ros::Time(0), transform_);
   }
   catch (tf::TransformException ex)
   {
@@ -141,8 +142,11 @@ void OGridGen::callback(const mil_blueview_driver::BlueViewPingPtr &ping_msg)
   pcl::PointCloud<pcl::PointXYZI>::Ptr point_cloud_plane(new pcl::PointCloud<pcl::PointXYZI>());
   for (size_t i = 0; i < ping_msg->ranges.size(); ++i)
   {
-    if (ping_msg->intensities.at(i) > min_intensity_)
+      //ROS_INFO("Bearing: %f \n", ping_msg->bearings.at(i));
+
+    if (ping_msg->intensities.at(i) > 5)
     {  // TODO: Better thresholding
+      //ROS_INFO("Intensity: %u, Bearing: %f , Ranges: %f \n", ping_msg->intensities.at(i), ping_msg->bearings.at(i), ping_msg->ranges.at(i));
 
       // Get x and y of a ping. RIGHT TRIANGLES
       double x_d = ping_msg->ranges.at(i) * cos(ping_msg->bearings.at(i));
@@ -178,6 +182,9 @@ void OGridGen::callback(const mil_blueview_driver::BlueViewPingPtr &ping_msg)
     publish_ogrid();
   }
 }
+
+
+
 void OGridGen::populate_mat_ogrid()
 {
   classification_.zonify(persistant_ogrid_, resolution_, transform_, mat_origin_);

simulation/sub8_gazebo/CMakeLists.txt

@@ -17,6 +17,11 @@ find_package(catkin
     tf
     tf2_ros
     tf2
+    geometry_msgs
+    gazebo_plugins
+    message_filters
+    pcl_conversions
+    pcl_ros
 )
 
 catkin_python_setup()
@@ -88,6 +93,12 @@ add_service_files(
   SetTurbulence.srv
 )
 
+add_message_files(
+FILES
+simulated_sonar_ping.msg
+
+)
+
 generate_messages(DEPENDENCIES std_msgs)
 
 add_dependencies(sub8_thrusters