diff --git a/launch/uw_lidar_standalone.launch b/launch/uw_lidar_standalone.launch
index 3afb9d0..b72c092 100644
--- a/launch/uw_lidar_standalone.launch
+++ b/launch/uw_lidar_standalone.launch
@@ -11,5 +11,18 @@
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/rviz/uw_lidar.rviz b/rviz/uw_lidar.rviz
index 2821ef1..eaaeedb 100644
--- 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,33 +111,33 @@ 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:
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:
@@ -145,7 +145,7 @@ Window Geometry:
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
index 169a886..ca38fcc 100644
--- 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("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("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(
- "/" + 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(
- "/" + this->model->GetName() + "/lidar_tilt_cmd",
+ "/" + this->model->GetName() + "/uwl_cmd/tilt",
1,
boost::bind(&GazeboRosPulseLidar::OnRosTiltMsg, this, _1),
ros::VoidPtr(), &this->rosQueue);
diff --git a/urdf/uw_lidar.xacro b/urdf/uw_lidar.xacro
new file mode 100644
index 0000000..db2d468
--- /dev/null
+++ b/urdf/uw_lidar.xacro
@@ -0,0 +1,255 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ 0 0 0 0 0 0
+ ${update_rate}
+ true
+ true
+
+
+ nps_uwl_points
+ ${reference_frame}
+ 0
+ 1
+ 20
+ 0.05
+
+
+
+
+
+
+
+ 1450
+
+ 0.1
+
+ ${-pi/12}
+
+ ${pi/12}
+
+
+
+ 1450
+ 0.1
+ ${-pi/12}
+ ${pi/12}
+
+
+
+
+
+
+ 1
+
+ 20
+
+ 0.1
+
+
+
+ gaussian
+ 0.0
+ 0.05
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ ${namespace}
+ ${pan}
+ ${tilt}
+
+
+
+
diff --git a/urdf/uw_lidar_pedestal_robot.xacro b/urdf/uw_lidar_pedestal_robot.xacro
new file mode 100644
index 0000000..09aa5c7
--- /dev/null
+++ b/urdf/uw_lidar_pedestal_robot.xacro
@@ -0,0 +1,49 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/worlds/uw_lidar_basic.world b/worlds/uw_lidar_basic.world
index bec0bca..3c729b6 100644
--- a/worlds/uw_lidar_basic.world
+++ b/worlds/uw_lidar_basic.world
@@ -28,95 +28,5 @@
true
-
-
- false
- 2.1 1.0 0.26 0 0 0
-
-
- 40
-
- 1.458333
- 0
- 0
- 1.458333
- 0
- 1.25
-
- 0 0 0 0 -0 0
-
-
- 255
-
-
- 0.25
- 0.5
-
-
- 10
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- 0.25
- 0.5
-
-
-
-
-
-
- 0
- 0
- 0
-
-
-
- model://3dad_sl3
- 0 0 0.25 0 0 -1.57079632679
-
-
- -1.57079632679
- 0
-
-
-
-
- 0 0 0 0 0 0
-
- pedestal_link
-
- 3dad_sl3::base
-
-
-
-
- 0 0 -0.25 0 0 0
-
- world
-
- pedestal_link
-
-
-
-
-
-
-