Merge pull request #17 from Field-Robotics-Lab/uw_lidar_waves

UW lidar waves
Field-Robotics-Lab · Sep 26, 2020 · ae2e16b · ae2e16b
2 parents d345dcd + f19ef8c
commit ae2e16b
Show file tree

Hide file tree

Showing 6 changed files with 347 additions and 123 deletions.
diff --git a/launch/uw_lidar_standalone.launch b/launch/uw_lidar_standalone.launch
@@ -11,5 +11,18 @@
     <arg name="paused"      value="$(arg paused)"/>
   </include>
 
+  <!-- Load and spawn the model, starts the joint & state publishers, make the model move -->
+  <param name="nps_uwl" command="$(find xacro)/xacro $(find nps_uw_sensors_gazebo)/urdf/uw_lidar_pedestal_robot.xacro" />
+  <param name="model_name" type="str" value="nps_uwl"/>
+  <param name="base_link_name" type="str" value="robot_base_link"/>
+  <node name="spawn_uwl" pkg="gazebo_ros" type="spawn_model"
+        args="-x 2.1 -y 1.0 -z 0.26 -urdf -param nps_uwl -model nps_uwl" respawn="false" output="screen" />
+  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" respawn="false">
+    <remap from="robot_description" to="nps_uwl" />
+  </node>
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher">
+    <remap from="robot_description" to="nps_uwl" />
+  </node>
+
 </launch>
 
diff --git a/rviz/uw_lidar.rviz b/rviz/uw_lidar.rviz
@@ -8,7 +8,7 @@ Panels:
         - /Status1
         - /PointCloud21
       Splitter Ratio: 0.5
-    Tree Height: 585
+    Tree Height: 555
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -78,7 +78,7 @@ Visualization Manager:
       Size (Pixels): 3
       Size (m): 0.009999999776482582
       Style: Flat Squares
-      Topic: /nps_gazebo_ros_uw/pulse_lidar/points
+      Topic: /nps_uwl_points
       Unreliable: false
       Use Fixed Frame: true
       Use rainbow: true
@@ -87,7 +87,7 @@ Visualization Manager:
   Global Options:
     Background Color: 48; 48; 48
     Default Light: true
-    Fixed Frame: nps_gazebo_ros_uw/pulse_lidar_link
+    Fixed Frame: world
     Frame Rate: 30
   Name: root
   Tools:
@@ -111,41 +111,41 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 4.6233906745910645
+      Distance: 24.759632110595703
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 3.451838254928589
-        Y: -1.4690022468566895
-        Z: -0.4074924886226654
+        X: 0
+        Y: 0
+        Z: 0
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.1197977066040039
+      Pitch: 0.40039798617362976
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 2.795984983444214
+      Yaw: 1.6653974056243896
     Saved: ~
 Window Geometry:
   Displays:
-    collapsed: true
-  Height: 579
-  Hide Left Dock: true
-  Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd000000040000000000000156000002d4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d000002d4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000224fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000224000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000002eb0000003efc0100000002fb0000000800540069006d00650100000000000002eb000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002eb000001a500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+    collapsed: false
+  Height: 846
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000024400fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
     collapsed: false
   Tool Properties:
     collapsed: false
   Views:
-    collapsed: true
-  Width: 747
-  X: 1279
-  Y: 527
+    collapsed: false
+  Width: 1200
+  X: 3066
+  Y: 197
diff --git a/src/gazebo_ros_pulse_lidar.cpp b/src/gazebo_ros_pulse_lidar.cpp
@@ -56,8 +56,8 @@ namespace gazebo
      this->model = _model;
 
      // Get the joints
-     this->pan_joint = this->model->GetJoint("3dad_sl3::base_top_joint");
-     this->tilt_joint = this->model->GetJoint("3dad_sl3::top_tray_joint");
+     this->pan_joint = this->model->GetJoint("uwl/uwl_base_swivel_joint");
+     this->tilt_joint = this->model->GetJoint("uwl/uwl_swivel_tray_joint");
 
      // Setup a P-controller, with _imax = 1
      this->pan_pid = common::PID(1, 0, 2.5, 1);
@@ -74,21 +74,25 @@ namespace gazebo
      this->model->GetJointController()->SetPositionPID(
          this->tilt_joint->GetScopedName(), this->tilt_pid);
 
-
-     // Default to zero velocity
+     // Default to no pan or tilt
      double pan_position = 0;
      double tilt_position = 0;
 
      // Check that the velocity element exists, then read the value
      if (_sdf->HasElement("pan_position"))
+     {
        pan_position = _sdf->Get<double>("pan_position");
+       ROS_INFO_NAMED("pulse_lidar", "pan_position = %f", pan_position);
+     }
 
      // Check that the velocity element exists, then read the value
      if (_sdf->HasElement("tilt_position"))
+     {
        tilt_position = _sdf->Get<double>("tilt_position");
+       ROS_INFO_NAMED("pulse_lidar", "tilt_position = %f", tilt_position);
+     }
 
-     // Set the joint's target velocity. This target velocity is just
-     // for demonstration purposes.
+     // Set the joints' target positions.
      this->model->GetJointController()->SetPositionTarget(
          this->pan_joint->GetScopedName(), pan_position);
 
@@ -103,13 +107,6 @@ namespace gazebo
      this->node->Init(this->model->GetWorld()->Name());
      #endif
 
-     // Create a topic name
-     std::string topicName = "~/" + this->model->GetName() + "/lidar_cmd";
-
-     // Subscribe to the topic, and register a callback
-     this->sub = this->node->Subscribe(topicName,
-        &GazeboRosPulseLidar::OnMsg, this);
-
      // Initialize ros, if it has not already bee initialized.
      if (!ros::isInitialized())
      {
@@ -126,7 +123,7 @@ namespace gazebo
      // Create a named topic, and subscribe to it.
      ros::SubscribeOptions so_pan =
        ros::SubscribeOptions::create<std_msgs::Float32>(
-           "/" + this->model->GetName() + "/lidar_pan_cmd",
+           "/" + this->model->GetName() + "/uwl_cmd/pan",
            1,
            boost::bind(&GazeboRosPulseLidar::OnRosPanMsg, this, _1),
            ros::VoidPtr(), &this->rosQueue);
@@ -137,7 +134,7 @@ namespace gazebo
      // Create a named topic, and subscribe to it.
      ros::SubscribeOptions so_tilt =
        ros::SubscribeOptions::create<std_msgs::Float32>(
-           "/" + this->model->GetName() + "/lidar_tilt_cmd",
+           "/" + this->model->GetName() + "/uwl_cmd/tilt",
            1,
            boost::bind(&GazeboRosPulseLidar::OnRosTiltMsg, this, _1),
            ros::VoidPtr(), &this->rosQueue);