Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[4795] pkg/trunk
|
From: <hsu...@us...> - 2008-09-30 00:17:29
|
Revision: 4795
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4795&view=rev
Author: hsujohnhsu
Date: 2008-09-30 00:17:02 +0000 (Tue, 30 Sep 2008)
Log Message:
-----------
stripped ifaces from ros plugins for gazebo.
udpate 2dnav gazebo to non-respawn dead pr2_guis.
Modified Paths:
--------------
pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-simple-headless-fake_localization.xml
pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-simple-headless.xml
pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg-fake_localization.xml
pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg-headless-fake_localization.xml
pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg-headless.xml
pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg.xml
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Block_Laser.hh
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Camera.hh
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Laser.hh
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_PTZ.hh
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Time.hh
pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc
pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Block_Laser.cc
pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Camera.cc
pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Laser.cc
pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_PTZ.cc
Modified: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-simple-headless-fake_localization.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-simple-headless-fake_localization.xml 2008-09-29 23:27:11 UTC (rev 4794)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-simple-headless-fake_localization.xml 2008-09-30 00:17:02 UTC (rev 4795)
@@ -8,7 +8,7 @@
<node pkg="nav_view" type="nav_view" respawn="false" output="screen" />
<!-- for visualization -->
- <node pkg="pr2_gui" type="pr2_gui" respawn="true" output="screen" />
+ <node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" />
<!-- for manual control -->
<!--node pkg="teleop_base_keyboard" type="teleop_base_keyboard" respawn="false" output="screen" /-->
Modified: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-simple-headless.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-simple-headless.xml 2008-09-29 23:27:11 UTC (rev 4794)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-simple-headless.xml 2008-09-30 00:17:02 UTC (rev 4795)
@@ -8,7 +8,7 @@
<node pkg="nav_view" type="nav_view" respawn="false" output="screen" />
<!-- for visualization -->
- <node pkg="pr2_gui" type="pr2_gui" respawn="true" output="screen" />
+ <node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" />
<!-- for manual control -->
<!--node pkg="teleop_base_keyboard" type="teleop_base_keyboard" respawn="false" output="screen" /-->
Modified: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg-fake_localization.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg-fake_localization.xml 2008-09-29 23:27:11 UTC (rev 4794)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg-fake_localization.xml 2008-09-30 00:17:02 UTC (rev 4795)
@@ -8,7 +8,7 @@
<node pkg="nav_view" type="nav_view" respawn="false" output="screen" />
<!-- for visualization -->
- <node pkg="pr2_gui" type="pr2_gui" respawn="true" output="screen" />
+ <node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" />
<!-- for manual control -->
<!--node pkg="teleop_base_keyboard" type="teleop_base_keyboard" respawn="false" output="screen" /-->
Modified: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg-headless-fake_localization.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg-headless-fake_localization.xml 2008-09-29 23:27:11 UTC (rev 4794)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg-headless-fake_localization.xml 2008-09-30 00:17:02 UTC (rev 4795)
@@ -8,7 +8,7 @@
<node pkg="nav_view" type="nav_view" respawn="false" output="screen" />
<!-- for visualization -->
- <node pkg="pr2_gui" type="pr2_gui" respawn="true" output="screen" />
+ <node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" />
<!-- for manual control -->
<!--node pkg="teleop_base_keyboard" type="teleop_base_keyboard" respawn="false" output="screen" /-->
Modified: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg-headless.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg-headless.xml 2008-09-29 23:27:11 UTC (rev 4794)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg-headless.xml 2008-09-30 00:17:02 UTC (rev 4795)
@@ -8,7 +8,7 @@
<node pkg="nav_view" type="nav_view" respawn="false" output="screen" />
<!-- for visualization -->
- <node pkg="pr2_gui" type="pr2_gui" respawn="true" output="screen" />
+ <node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" />
<!-- for manual control -->
<!--node pkg="teleop_base_keyboard" type="teleop_base_keyboard" respawn="false" output="screen" /-->
Modified: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg.xml 2008-09-29 23:27:11 UTC (rev 4794)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg.xml 2008-09-30 00:17:02 UTC (rev 4795)
@@ -5,7 +5,7 @@
<node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/willowMap.png 0.1" respawn="false" />
<node pkg="amcl_player" type="amcl_player" args="scan:=base_scan" respawn="false" />
<node pkg="wavefront_player" type="wavefront_player" args="scan:=base_scan" respawn="false" />
- <node pkg="nav_view" type="nav_view" respawn="true" />
+ <node pkg="nav_view" type="nav_view" respawn="false" />
<!--node pkg="teleop_base_keyboard" type="teleop_base_keyboard" respawn="false" /-->
</group>
</launch>
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh 2008-09-29 23:27:11 UTC (rev 4794)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh 2008-09-30 00:17:02 UTC (rev 4795)
@@ -66,7 +66,7 @@
/// \brief P3D controller
/// \li Starts a ROS node if none exists.
- /// \li This controller simulates a 6 dof position and rate sensor.
+ /// \li This controller simulates a 6 dof position and rate sensor, publishes std_msgs::TransformWithRateStamped.msg ROS topic.
class P3D : public Controller
{
/// \brief Constructor
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Block_Laser.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Block_Laser.hh 2008-09-29 23:27:11 UTC (rev 4794)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Block_Laser.hh 2008-09-30 00:17:02 UTC (rev 4795)
@@ -36,8 +36,6 @@
namespace gazebo
{
- class LaserIface;
- class FiducialIface;
class RaySensor;
/// @addtogroup gazebo_dynamic_plugins Gazebo ROS Dynamic Plugins
@@ -46,7 +44,7 @@
\brief ROS Block Laser Scanner Controller Plugin
- This is a controller that gathers range data from a ray sensor, and returns results via publishing ROS topic for point clouds and Iface.
+ This is a controller that gathers range data from a ray sensor, and returns results via publishing ROS topic for point clouds.
\verbatim
<model:physical name="ray_model">
@@ -90,6 +88,7 @@
/// \brief ROS laser block simulation.
/// \li Starts a ROS node if none exists.
/// \li This controller simulates a block of laser range detections.
+/// Resulting point cloud (std_msgs::PointCloudFloat32.msg are published as a ROS topic.
class Ros_Block_Laser : public Controller
{
/// \brief Constructor
@@ -112,17 +111,9 @@
/// \brief Finalize the controller
protected: virtual void FiniChild();
- /// \brief Put laser data to the iface
+ /// \brief Put laser data to the ROS topic
private: void PutLaserData();
- /// \brief Put fiducial data to the iface
- private: void PutFiducialData();
-
- /// \brief The laser interface
- private: LaserIface *laserIface;
-
- private: FiducialIface *fiducialIface;
-
/// \brief The parent sensor
private: RaySensor *myParent;
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Camera.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Camera.hh 2008-09-29 23:27:11 UTC (rev 4794)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Camera.hh 2008-09-30 00:17:02 UTC (rev 4795)
@@ -33,7 +33,6 @@
namespace gazebo
{
- class CameraIface;
class MonoCameraSensor;
/// @addtogroup gazebo_dynamic_plugins Gazebo ROS Dynamic Plugins
@@ -53,7 +52,6 @@
<updateRate>15.0</updateRate>
<topicName>camera_name/image</topicName>
<frameName>camera_body_name</frameName>
- <interface:camera name="ros_camera_iface" />
</controller:ros_camera>
</sensor:camera>
</body:empty>
@@ -88,12 +86,9 @@
/// \brief Finalize the controller, unadvertise topics
protected: virtual void FiniChild();
- /// \brief Put camera data to the iface
+ /// \brief Put camera data to the ROS topic
private: void PutCameraData();
- /// \brief The camera Iface
- private: CameraIface *cameraIface;
-
/// \brief A pointer to the parent camera sensor
private: MonoCameraSensor *myParent;
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Laser.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Laser.hh 2008-09-29 23:27:11 UTC (rev 4794)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Laser.hh 2008-09-30 00:17:02 UTC (rev 4795)
@@ -35,8 +35,6 @@
namespace gazebo
{
- class LaserIface;
- class FiducialIface;
class RaySensor;
/// @addtogroup gazebo_dynamic_plugins Gazebo ROS Dynamic Plugins
@@ -46,7 +44,7 @@
\brief ROS Laser Scanner Controller Plugin
This controller gathers range data from a simulated ray sensor, publishes range data through
- std_msgs::LaserScan ROS topic and Gazebo Iface.
+ std_msgs::LaserScan ROS topic.
\verbatim
<model:physical name="ray_model">
@@ -80,7 +78,7 @@
/// \brief ROS laser scan controller.
/// \li Starts a ROS node if none exists.
-/// \li Simulates a laser range sensor.
+/// \li Simulates a laser range sensor and publish results over ROS on std_msgs::LaserScan.msg
class Ros_Laser : public Controller
{
/// \brief Constructor
@@ -103,17 +101,9 @@
/// \brief Finalize the controller
protected: virtual void FiniChild();
- /// \brief Put laser data to the iface
+ /// \brief Put laser data to the ROS topic
private: void PutLaserData();
- /// \brief Put fiducial data to the iface
- private: void PutFiducialData();
-
- /// \brief The laser interface
- private: LaserIface *laserIface;
-
- private: FiducialIface *fiducialIface;
-
/// \brief The parent sensor
private: RaySensor *myParent;
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_PTZ.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_PTZ.hh 2008-09-29 23:27:11 UTC (rev 4794)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_PTZ.hh 2008-09-30 00:17:02 UTC (rev 4795)
@@ -41,7 +41,6 @@
namespace gazebo
{
class HingeJoint;
- class PTZIface;
/// @addtogroup gazebo_dynamic_plugins Gazebo ROS Dynamic Plugins
/// @{
@@ -68,9 +67,12 @@
\{
*/
- /// \brief Ros ptz controller.
+ /// \brief ROS Pan/Tilt/Zoom Camera Controller
+ /// \li Starts a ROS node if none exists.
+ /// \li Simulates PTZ camera actuators.
+ /// - publish state information (PT angles) to ROS topic: \e camera_name/ptz_state
+ /// - subscribe to ROS topic: \e camera_name/ptz_cmd
///
- /// This is a controller for a ros PTZ
class Ros_PTZ : public Controller
{
/// \brief Constructor
@@ -100,12 +102,9 @@
/// \brief Reset the controller
protected: virtual void ResetChild();
- /// \brief Put camera data to the iface
+ /// \brief Put camera data to the ROS topic
private: void PutPTZData();
- /// \brief The camera interface
- private: PTZIface *ptzIface;
-
/// \brief The parent sensor
private: Model *myParent;
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Time.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Time.hh 2008-09-29 23:27:11 UTC (rev 4794)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Time.hh 2008-09-30 00:17:02 UTC (rev 4795)
@@ -69,8 +69,10 @@
\{
*/
-/// \brief Starts a ROS node if none exists and broadcast simulator time
-/// over rostools::Time.
+/// \brief ROS Time Controller
+/// \li Starts a ROS node if none exists
+/// \li broadcast simulator time over rostools::Time.
+/// .
class Ros_Time : public Controller
{
/// \brief Constructor
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc 2008-09-29 23:27:11 UTC (rev 4794)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc 2008-09-30 00:17:02 UTC (rev 4795)
@@ -77,7 +77,7 @@
this->myIface = dynamic_cast<PositionIface*>(this->ifaces[0]);
if (!this->myIface)
- gzthrow("P3D controller requires a Actarray Iface");
+ gzthrow("P3D controller requires a Actarray Iface, though not used.");
std::string bodyName = node->GetString("bodyName", "", 1);
this->myBody = dynamic_cast<Body*>(this->myParent->GetBody(bodyName));
@@ -116,17 +116,8 @@
rpyTotal.z = this->rpyOffsets.z + rot.GetYaw();
rot.SetFromEuler(rpyTotal);
- this->myIface->Lock(1);
- this->myIface->data->head.time = Simulator::Instance()->GetSimTime();
+ double cur_time = Simulator::Instance()->GetSimTime();
- this->myIface->data->pose.pos.x = pos.x;
- this->myIface->data->pose.pos.y = pos.y;
- this->myIface->data->pose.pos.z = pos.z;
-
- this->myIface->data->pose.roll = rot.GetRoll();
- this->myIface->data->pose.pitch = rot.GetPitch();
- this->myIface->data->pose.yaw = rot.GetYaw();
-
// get Rates
Vector3 vpos = this->myBody->GetPositionRate(); // get velocity in gazebo frame
Quatern vrot = this->myBody->GetRotationRate(); // get velocity in gazebo frame
@@ -135,8 +126,8 @@
this->lock.lock();
// copy data into pose message
this->transformMsg.header.frame_id = this->frameName;
- this->transformMsg.header.stamp.sec = (unsigned long)floor(this->myIface->data->head.time);
- this->transformMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->myIface->data->head.time - this->transformMsg.header.stamp.sec) );
+ this->transformMsg.header.stamp.sec = (unsigned long)floor(cur_time);
+ this->transformMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( cur_time - this->transformMsg.header.stamp.sec) );
this->transformMsg.transform.translation.x = pos.x;
this->transformMsg.transform.translation.y = pos.y;
@@ -167,7 +158,6 @@
rosnode->publish(this->topicName,this->transformMsg);
this->lock.unlock();
- this->myIface->Unlock();
}
////////////////////////////////////////////////////////////////////////////////
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Block_Laser.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Block_Laser.cc 2008-09-29 23:27:11 UTC (rev 4794)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Block_Laser.cc 2008-09-30 00:17:02 UTC (rev 4795)
@@ -57,9 +57,6 @@
// set parent sensor to active automatically
this->myParent->SetActive(true);
- this->laserIface = NULL;
- this->fiducialIface = NULL;
-
rosnode = ros::g_node; // comes from where? common.h exports as global variable
int argc = 0;
char** argv = NULL;
@@ -82,18 +79,6 @@
// Load the controller
void Ros_Block_Laser::LoadChild(XMLConfigNode *node)
{
- std::vector<Iface*>::iterator iter;
-
- for (iter = this->ifaces.begin(); iter != this->ifaces.end(); iter++)
- {
- if ((*iter)->GetType() == "laser")
- this->laserIface = dynamic_cast<LaserIface*>(*iter);
- else if ((*iter)->GetType() == "fiducial")
- this->fiducialIface = dynamic_cast<FiducialIface*>(*iter);
- }
-
- if (!this->laserIface) gzthrow("Ros_Block_Laser controller requires a LaserIface");
-
this->topicName = node->GetString("topicName","default_ros_laser",0); //read from xml file
std::cout << "================= " << this->topicName << std::endl;
rosnode->advertise<std_msgs::PointCloudFloat32>(this->topicName,10);
@@ -128,40 +113,7 @@
// Update the controller
void Ros_Block_Laser::UpdateChild()
{
-#if 0
- bool laserOpened = false;
- bool fidOpened = false;
-
- if (this->laserIface->Lock(1))
- {
- laserOpened = this->laserIface->GetOpenCount() > 0;
- this->laserIface->Unlock();
- }
-
- if (this->fiducialIface && this->fiducialIface->Lock(1))
- {
- fidOpened = this->fiducialIface->GetOpenCount() > 0;
- this->fiducialIface->Unlock();
- }
-
- if (laserOpened)
- {
- this->myParent->SetActive(true);
this->PutLaserData();
- }
-
- if (fidOpened)
- {
- this->myParent->SetActive(true);
- this->PutFiducialData();
- }
-
- if (!laserOpened && !fidOpened)
- {
- this->myParent->SetActive(false);
- }
-#endif
- this->PutLaserData();
}
////////////////////////////////////////////////////////////////////////////////
@@ -206,228 +158,103 @@
for (int i=0; i< r_size; i++)
this->cloudMsg.chan[i].set_vals_size(1);
+ /***************************************************************/
+ /* */
+ /* point scan from laser */
+ /* */
+ /***************************************************************/
+ this->lock.lock();
+ // Add Frame Name
+ this->cloudMsg.header.frame_id = this->frameName;
+ this->cloudMsg.header.stamp.sec = (unsigned long)floor(Simulator::Instance()->GetSimTime());
+ this->cloudMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( Simulator::Instance()->GetSimTime() - this->cloudMsg.header.stamp.sec) );
- if (this->laserIface->Lock(1))
+ for (j = 0; j<verticalRangeCount; j++)
{
- // Add Frame Name
+ // interpolating in vertical direction
+ vb = (verticalRangeCount == 1) ? 0 : (double) j * (verticalRayCount - 1) / (verticalRangeCount - 1);
+ vja = (int) floor(vb);
+ vjb = std::min(vja + 1, verticalRayCount - 1);
+ vb = vb - floor(vb); // fraction from min
+ assert(vja >= 0 && vja < verticalRayCount);
+ assert(vjb >= 0 && vjb < verticalRayCount);
- // Data timestamp
- this->laserIface->data->head.time = Simulator::Instance()->GetSimTime();
-
- // Read out the laser range data
- this->laserIface->data->min_angle = minAngle.GetAsRadian();
- this->laserIface->data->max_angle = maxAngle.GetAsRadian();
- this->laserIface->data->res_angle = (maxAngle.GetAsRadian() - minAngle.GetAsRadian()) / (rangeCount - 1);
- this->laserIface->data->res_range = 0.1;
- this->laserIface->data->max_range = maxRange;
- this->laserIface->data->range_count = rangeCount;
-
- assert(this->laserIface->data->range_count < GZ_LASER_MAX_RANGES );
-
- /***************************************************************/
- /* */
- /* point scan from laser */
- /* */
- /***************************************************************/
- this->lock.lock();
- this->cloudMsg.header.frame_id = this->frameName;
- this->cloudMsg.header.stamp.sec = (unsigned long)floor(this->laserIface->data->head.time);
- this->cloudMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->laserIface->data->head.time - this->cloudMsg.header.stamp.sec) );
-
- for (j = 0; j<verticalRangeCount; j++)
+ for (i = 0; i<rangeCount; i++)
{
- // interpolating in vertical direction
- vb = (verticalRangeCount == 1) ? 0 : (double) j * (verticalRayCount - 1) / (verticalRangeCount - 1);
- vja = (int) floor(vb);
- vjb = std::min(vja + 1, verticalRayCount - 1);
- vb = vb - floor(vb); // fraction from min
+ // Interpolate the range readings from the rays in horizontal direction
+ hb = (rangeCount == 1)? 0 : (double) i * (rayCount - 1) / (rangeCount - 1);
+ hja = (int) floor(hb);
+ hjb = std::min(hja + 1, rayCount - 1);
+ hb = hb - floor(hb); // fraction from min
- assert(vja >= 0 && vja < verticalRayCount);
- assert(vjb >= 0 && vjb < verticalRayCount);
+ assert(hja >= 0 && hja < rayCount);
+ assert(hjb >= 0 && hjb < rayCount);
- for (i = 0; i<rangeCount; i++)
- {
- // Interpolate the range readings from the rays in horizontal direction
- hb = (rangeCount == 1)? 0 : (double) i * (rayCount - 1) / (rangeCount - 1);
- hja = (int) floor(hb);
- hjb = std::min(hja + 1, rayCount - 1);
- hb = hb - floor(hb); // fraction from min
+ // indices of 4 corners
+ j1 = hja + vja * rayCount;
+ j2 = hjb + vja * rayCount;
+ j3 = hja + vjb * rayCount;
+ j4 = hjb + vjb * rayCount;
+ // range readings of 4 corners
+ r1 = std::min(this->myParent->GetRange(j1) , maxRange);
+ r2 = std::min(this->myParent->GetRange(j2) , maxRange);
+ r3 = std::min(this->myParent->GetRange(j3) , maxRange);
+ r4 = std::min(this->myParent->GetRange(j4) , maxRange);
- assert(hja >= 0 && hja < rayCount);
- assert(hjb >= 0 && hjb < rayCount);
+ // Range is linear interpolation if values are close,
+ // and min if they are very different
+ r = (1-vb)*((1 - hb) * r1 + hb * r2)
+ + vb *((1 - hb) * r3 + hb * r4);
- // indices of 4 corners
- j1 = hja + vja * rayCount;
- j2 = hjb + vja * rayCount;
- j3 = hja + vjb * rayCount;
- j4 = hjb + vjb * rayCount;
- // range readings of 4 corners
- r1 = std::min(this->myParent->GetRange(j1) , maxRange);
- r2 = std::min(this->myParent->GetRange(j2) , maxRange);
- r3 = std::min(this->myParent->GetRange(j3) , maxRange);
- r4 = std::min(this->myParent->GetRange(j4) , maxRange);
+ // Intensity is either-or
+ v = (int) this->myParent->GetRetro(j1) || (int) this->myParent->GetRetro(j2) ||
+ (int) this->myParent->GetRetro(j3) || (int) this->myParent->GetRetro(j4);
- // Range is linear interpolation if values are close,
- // and min if they are very different
- r = (1-vb)*((1 - hb) * r1 + hb * r2)
- + vb *((1 - hb) * r3 + hb * r4);
+ // std::cout << " block debug "
+ // << " ij("<<i<<","<<j<<")"
+ // << " j1234("<<j1<<","<<j2<<","<<j3<<","<<j4<<")"
+ // << " r1234("<<r1<<","<<r2<<","<<r3<<","<<r4<<")"
+ // << std::endl;
- // Intensity is either-or
- v = (int) this->myParent->GetRetro(j1) || (int) this->myParent->GetRetro(j2) ||
- (int) this->myParent->GetRetro(j3) || (int) this->myParent->GetRetro(j4);
+ // get angles of ray to get xyz for point
+ double yAngle = 0.5*(hja+hjb) * yDiff / (rayCount -1) + minAngle.GetAsRadian();
+ double pAngle = 0.5*(vja+vjb) * pDiff / (verticalRayCount -1) + verticalMinAngle.GetAsRadian();
- // std::cout << " block debug "
- // << " ij("<<i<<","<<j<<")"
- // << " j1234("<<j1<<","<<j2<<","<<j3<<","<<j4<<")"
- // << " r1234("<<r1<<","<<r2<<","<<r3<<","<<r4<<")"
- // << std::endl;
-
- this->laserIface->data->ranges[i] = r + minRange;
- this->laserIface->data->intensity[i] = v;
-
- // get angles of ray to get xyz for point
- double yAngle = 0.5*(hja+hjb) * yDiff / (rayCount -1) + minAngle.GetAsRadian();
- double pAngle = 0.5*(vja+vjb) * pDiff / (verticalRayCount -1) + verticalMinAngle.GetAsRadian();
-
- int n = i + j*rayCount;
- /***************************************************************/
- /* */
- /* point scan from laser */
- /* */
- /***************************************************************/
- if (r == maxRange)
- {
- // no noise if at max range
- this->cloudMsg.pts[n].x = (r+minRange) * cos(pAngle)*cos(yAngle);
- this->cloudMsg.pts[n].y = (r+minRange) * sin(yAngle);
- this->cloudMsg.pts[n].z = (r+minRange) * sin(pAngle)*cos(yAngle);
- }
- else
- {
- this->cloudMsg.pts[n].x = (r+minRange) * cos(pAngle)*cos(yAngle) + this->GaussianKernel(0,this->gaussianNoise) ;
- this->cloudMsg.pts[n].y = (r+minRange) * sin(yAngle) + this->GaussianKernel(0,this->gaussianNoise) ;
- this->cloudMsg.pts[n].z = (r+minRange) * sin(pAngle)*cos(yAngle) + this->GaussianKernel(0,this->gaussianNoise) ;
- }
- this->cloudMsg.chan[n].vals[0] = v + this->GaussianKernel(0,this->gaussianNoise) ;
- }
- }
-
- // iface writing can be skipped if iface is not used.
- // send data out via ros message
- rosnode->publish(this->topicName,this->cloudMsg);
- this->lock.unlock();
-
- this->laserIface->Unlock();
-
- // New data is available
- this->laserIface->Post();
- }
-
-
-}
-
-//////////////////////////////////////////////////////////////////////////////
-// Update the data in the interface
-void Ros_Block_Laser::PutFiducialData()
-{
- int i, j, count;
- FiducialFid *fid;
- double r, b;
- double ax, ay, bx, by, cx, cy;
-
- Angle maxAngle = this->myParent->GetMaxAngle();
- Angle minAngle = this->myParent->GetMinAngle();
-
- double maxRange = this->myParent->GetMaxRange();
- double minRange = this->myParent->GetMinRange();
- int rayCount = this->myParent->GetRayCount();
- int rangeCount = this->myParent->GetRangeCount();
-
- if (this->fiducialIface->Lock(1))
- {
- // Data timestamp
- this->fiducialIface->data->head.time = Simulator::Instance()->GetSimTime();
- this->fiducialIface->data->count = 0;
-
- // TODO: clean this up
- count = 0;
- for (i = 0; i < rayCount; i++)
- {
- if (this->myParent->GetFiducial(i) < 0)
- continue;
-
- // Find the end of the fiducial
- for (j = i + 1; j < rayCount; j++)
+ int n = i + j*rayCount;
+ /***************************************************************/
+ /* */
+ /* point scan from laser */
+ /* */
+ /***************************************************************/
+ if (r == maxRange)
{
- if (this->myParent->GetFiducial(j) != this->myParent->GetFiducial(i))
- break;
+ // no noise if at max range
+ this->cloudMsg.pts[n].x = (r+minRange) * cos(pAngle)*cos(yAngle);
+ this->cloudMsg.pts[n].y = (r+minRange) * sin(yAngle);
+ this->cloudMsg.pts[n].z = (r+minRange) * sin(pAngle)*cos(yAngle);
}
- j--;
-
- // Need at least three points to get orientation
- if (j - i + 1 >= 3)
- {
- r = minRange + this->myParent->GetRange(i);
- b = minAngle.GetAsRadian() + i * ((maxAngle.GetAsRadian()-minAngle.GetAsRadian()) / (rayCount - 1));
- ax = r * cos(b);
- ay = r * sin(b);
-
- r = minRange + this->myParent->GetRange(j);
- b = minAngle.GetAsRadian() + j * ((maxAngle.GetAsRadian()-minAngle.GetAsRadian()) / (rayCount - 1));
- bx = r * cos(b);
- by = r * sin(b);
-
- cx = (ax + bx) / 2;
- cy = (ay + by) / 2;
-
- assert(count < GZ_FIDUCIAL_MAX_FIDS);
- fid = this->fiducialIface->data->fids + count++;
-
- fid->id = this->myParent->GetFiducial(j);
- fid->pose.pos.x = cx;
- fid->pose.pos.y = cy;
- fid->pose.yaw = atan2(by - ay, bx - ax) + M_PI / 2;
- }
-
- // Fewer points get no orientation
else
{
- r = minRange + this->myParent->GetRange(i);
- b = minAngle.GetAsRadian() + i * ((maxAngle.GetAsRadian()-minAngle.GetAsRadian()) / (rayCount - 1));
- ax = r * cos(b);
- ay = r * sin(b);
-
- r = minRange + this->myParent->GetRange(j);
- b = minAngle.GetAsRadian() + j * ((maxAngle.GetAsRadian()-minAngle.GetAsRadian()) / (rayCount - 1));
- bx = r * cos(b);
- by = r * sin(b);
-
- cx = (ax + bx) / 2;
- cy = (ay + by) / 2;
-
- assert(count < GZ_FIDUCIAL_MAX_FIDS);
- fid = this->fiducialIface->data->fids + count++;
-
- fid->id = this->myParent->GetFiducial(j);
- fid->pose.pos.x = cx;
- fid->pose.pos.y = cy;
- fid->pose.yaw = atan2(cy, cx) + M_PI;
+ this->cloudMsg.pts[n].x = (r+minRange) * cos(pAngle)*cos(yAngle) + this->GaussianKernel(0,this->gaussianNoise) ;
+ this->cloudMsg.pts[n].y = (r+minRange) * sin(yAngle) + this->GaussianKernel(0,this->gaussianNoise) ;
+ this->cloudMsg.pts[n].z = (r+minRange) * sin(pAngle)*cos(yAngle) + this->GaussianKernel(0,this->gaussianNoise) ;
}
-
- /*printf("fiducial %d i[%d] j[%d] %.2f %.2f %.2f\n",
- fid->id, i,j,fid->pos[0], fid->pos[1], fid->rot[2]);
- */
- i = j;
+ this->cloudMsg.chan[n].vals[0] = v + this->GaussianKernel(0,this->gaussianNoise) ;
+ this->cloudMsg.chan[n].vals[0] = 4000; //v + this->GaussianKernel(0,this->gaussianNoise) ;
+ this->cloudMsg.chan[0].vals[0] = 0; //v + this->GaussianKernel(0,this->gaussianNoise) ;
}
+ }
- this->fiducialIface->data->count = count;
+ // send data out via ros message
+ rosnode->publish(this->topicName,this->cloudMsg);
+ this->lock.unlock();
- this->fiducialIface->Unlock();
- this->fiducialIface->Post();
- }
+
+
}
+
//////////////////////////////////////////////////////////////////////////////
// Utility for adding noise
double Ros_Block_Laser::GaussianKernel(double mu,double sigma)
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Camera.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Camera.cc 2008-09-29 23:27:11 UTC (rev 4794)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Camera.cc 2008-09-30 00:17:02 UTC (rev 4795)
@@ -25,7 +25,7 @@
Date: 24 Sept 2008
SVN info: $Id$
@htmlinclude manifest.html
- @b Ros_Camera plugin broadcasts ROS Image messages as well as fills in Iface.
+ @b Ros_Camera plugin broadcasts ROS Image messages
*/
#include <algorithm>
@@ -85,11 +85,6 @@
// Load the controller
void Ros_Camera::LoadChild(XMLConfigNode *node)
{
- this->cameraIface = dynamic_cast<CameraIface*>(this->ifaces[0]);
-
- if (!this->cameraIface)
- gzthrow("Ros_Camera controller requires a CameraIface");
-
this->topicName = node->GetString("topicName","default_ros_camera",0); //read from xml file
this->frameName = node->GetString("frameName","default_ros_camera",0); //read from xml file
@@ -125,51 +120,19 @@
// Put laser data to the interface
void Ros_Camera::PutCameraData()
{
- CameraData *data = this->cameraIface->data;
const unsigned char *src;
- unsigned char *dst;
- Pose3d cameraPose;
- this->cameraIface->Lock(1);
-
- // Data timestamp
- data->head.time = Simulator::Instance()->GetSimTime();
-
- data->width = this->myParent->GetImageWidth();
- data->height = this->myParent->GetImageHeight();
- data->image_size = data->width * data->height * 3;
-
- // GetFOV() returns radians
- data->hfov = *(this->myParent->GetHFOV());
- data->vfov = *(this->myParent->GetVFOV());
-
- // Set the pose of the camera
- cameraPose = this->myParent->GetWorldPose();
- data->camera_pose.pos.x = cameraPose.pos.x;
- data->camera_pose.pos.y = cameraPose.pos.y;
- data->camera_pose.pos.z = cameraPose.pos.z;
- data->camera_pose.roll = cameraPose.rot.GetRoll();
- data->camera_pose.pitch = cameraPose.rot.GetPitch();
- data->camera_pose.yaw = cameraPose.rot.GetYaw();
-
- // Make sure there is room to store the image
- assert (data->image_size <= sizeof(data->image));
-
- // Copy the pixel data to the interface
+ // Get a pointer to image data
src = this->myParent->GetImageData(0);
- dst = data->image;
//std::cout << " updating camera " << this->topicName << " " << data->image_size << std::endl;
- // TODO: can skip copy to Iface if Iface is not used
if (src)
{
- //memcpy(dst, src, data->image_size); // FIXME remove all Iface components
-
this->lock.lock();
// copy data into image
this->imageMsg.header.frame_id = this->frameName;
- this->imageMsg.header.stamp.sec = (unsigned long)floor(this->cameraIface->data->head.time);
- this->imageMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->cameraIface->data->head.time - this->imageMsg.header.stamp.sec) );
+ this->imageMsg.header.stamp.sec = (unsigned long)floor(Simulator::Instance()->GetSimTime());
+ this->imageMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( Simulator::Instance()->GetSimTime() - this->imageMsg.header.stamp.sec) );
int width = this->myParent->GetImageWidth();
int height = this->myParent->GetImageHeight();
@@ -180,21 +143,16 @@
this->imageMsg.compression = "raw";
this->imageMsg.colorspace = "rgb24";
- // on first pass, the sensor does not update after cameraIface is opened.
+ // set buffer size
uint32_t buf_size = (width) * (height) * (depth);
this->imageMsg.set_data_size(buf_size);
- memcpy(&(this->imageMsg.data[0]), src, data->image_size);
+ memcpy(&(this->imageMsg.data[0]), src, buf_size);
// publish to ros
rosnode->publish(this->topicName,this->imageMsg);
this->lock.unlock();
}
- this->cameraIface->Unlock();
-
- // New data is available
- this->cameraIface->Post();
-
}
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Laser.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Laser.cc 2008-09-29 23:27:11 UTC (rev 4794)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Laser.cc 2008-09-30 00:17:02 UTC (rev 4795)
@@ -57,9 +57,6 @@
// set parent sensor to active automatically
this->myParent->SetActive(true);
- this->laserIface = NULL;
- this->fiducialIface = NULL;
-
rosnode = ros::g_node; // comes from where? common.h exports as global variable
int argc = 0;
char** argv = NULL;
@@ -82,18 +79,6 @@
// Load the controller
void Ros_Laser::LoadChild(XMLConfigNode *node)
{
- std::vector<Iface*>::iterator iter;
-
- for (iter = this->ifaces.begin(); iter != this->ifaces.end(); iter++)
- {
- if ((*iter)->GetType() == "laser")
- this->laserIface = dynamic_cast<LaserIface*>(*iter);
- else if ((*iter)->GetType() == "fiducial")
- this->fiducialIface = dynamic_cast<FiducialIface*>(*iter);
- }
-
- if (!this->laserIface) gzthrow("Ros_Laser controller requires a LaserIface");
-
this->topicName = node->GetString("topicName","default_ros_laser",0); //read from xml file
std::cout << "================= " << this->topicName << std::endl;
rosnode->advertise<std_msgs::LaserScan>(this->topicName,10);
@@ -111,40 +96,7 @@
// Update the controller
void Ros_Laser::UpdateChild()
{
-#if 0
- bool laserOpened = false;
- bool fidOpened = false;
-
- if (this->laserIface->Lock(1))
- {
- laserOpened = this->laserIface->GetOpenCount() > 0;
- this->laserIface->Unlock();
- }
-
- if (this->fiducialIface && this->fiducialIface->Lock(1))
- {
- fidOpened = this->fiducialIface->GetOpenCount() > 0;
- this->fiducialIface->Unlock();
- }
-
- if (laserOpened)
- {
- this->myParent->SetActive(true);
this->PutLaserData();
- }
-
- if (fidOpened)
- {
- this->myParent->SetActive(true);
- this->PutFiducialData();
- }
-
- if (!laserOpened && !fidOpened)
- {
- this->myParent->SetActive(false);
- }
-#endif
- this->PutLaserData();
}
////////////////////////////////////////////////////////////////////////////////
@@ -170,201 +122,72 @@
int rayCount = this->myParent->GetRayCount();
int rangeCount = this->myParent->GetRangeCount();
- if (this->laserIface->Lock(1))
+ /***************************************************************/
+ /* */
+ /* point scan from laser */
+ /* */
+ /***************************************************************/
+ this->lock.lock();
+ // Add Frame Name
+ this->laserMsg.header.frame_id = this->frameName;
+ this->laserMsg.header.stamp.sec = (unsigned long)floor(Simulator::Instance()->GetSimTime());
+ this->laserMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( Simulator::Instance()->GetSimTime() - this->laserMsg.header.stamp.sec) );
+
+
+ double tmp_res_angle = (maxAngle.GetAsRadian() - minAngle.GetAsRadian())/((double)(rangeCount -1)); // for computing yaw
+ int num_channels = 1;
+ this->laserMsg.angle_min = minAngle.GetAsRadian();
+ this->laserMsg.angle_max = maxAngle.GetAsRadian();
+ this->laserMsg.angle_increment = tmp_res_angle;
+ this->laserMsg.time_increment = 0; // instantaneous simulator scan
+ this->laserMsg.scan_time = 0; // FIXME: what's this?
+ this->laserMsg.range_min = minRange;
+ this->laserMsg.range_max = maxRange;
+ this->laserMsg.set_ranges_size(rangeCount);
+ this->laserMsg.set_intensities_size(rangeCount);
+
+ // Interpolate the range readings from the rays
+ for (i = 0; i<rangeCount; i++)
{
- // Add Frame Name
+ b = (double) i * (rayCount - 1) / (rangeCount - 1);
+ ja = (int) floor(b);
+ jb = std::min(ja + 1, rayCount - 1);
+ b = b - floor(b);
+ assert(ja >= 0 && ja < rayCount);
+ assert(jb >= 0 && jb < rayCount);
- // Data timestamp
- this->laserIface->data->head.time = Simulator::Instance()->GetSimTime();
+ ra = std::min(this->myParent->GetRange(ja) , maxRange);
+ rb = std::min(this->myParent->GetRange(jb) , maxRange);
- // Read out the laser range data
- this->laserIface->data->min_angle = minAngle.GetAsRadian();
- this->laserIface->data->max_angle = maxAngle.GetAsRadian();
- this->laserIface->data->res_angle = (maxAngle.GetAsRadian() - minAngle.GetAsRadian()) / (rangeCount - 1);
- this->laserIface->data->res_range = 0.1;
- this->laserIface->data->max_range = maxRange;
- this->laserIface->data->range_count = rangeCount;
+ // Range is linear interpolation if values are close,
+ // and min if they are very different
+ //if (fabs(ra - rb) < 0.10)
+ r = (1 - b) * ra + b * rb;
+ //else r = std::min(ra, rb);
- assert(this->laserIface->data->range_count < GZ_LASER_MAX_RANGES );
+ // Intensity is either-or
+ v = (int) this->myParent->GetRetro(ja) || (int) this->myParent->GetRetro(jb);
/***************************************************************/
/* */
/* point scan from laser */
/* */
/***************************************************************/
- this->lock.lock();
- this->laserMsg.header.frame_id = this->frameName;
- this->laserMsg.header.stamp.sec = (unsigned long)floor(this->laserIface->data->head.time);
- this->laserMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->laserIface->data->head.time - this->laserMsg.header.stamp.sec) );
-
-
- double tmp_res_angle = (maxAngle.GetAsRadian() - minAngle.GetAsRadian())/((double)(rangeCount -1)); // for computing yaw
- int num_channels = 1;
- this->laserMsg.angle_min = minAngle.GetAsRadian();
- this->laserMsg.angle_max = maxAngle.GetAsRadian();
- this->laserMsg.angle_increment = tmp_res_angle;
- this->laserMsg.time_increment = 0; // instantaneous simulator scan
- this->laserMsg.scan_time = 0; // FIXME: what's this?
- this->laserMsg.range_min = minRange;
- this->laserMsg.range_max = maxRange;
- this->laserMsg.set_ranges_size(rangeCount);
- this->laserMsg.set_intensities_size(rangeCount);
-
- // Interpolate the range readings from the rays
- for (i = 0; i<rangeCount; i++)
- {
- b = (double) i * (rayCount - 1) / (rangeCount - 1);
- ja = (int) floor(b);
- jb = std::min(ja + 1, rayCount - 1);
- b = b - floor(b);
-
- assert(ja >= 0 && ja < rayCount);
- assert(jb >= 0 && jb < rayCount);
-
- ra = std::min(this->myParent->GetRange(ja) , maxRange);
- rb = std::min(this->myParent->GetRange(jb) , maxRange);
-
- // Range is linear interpolation if values are close,
- // and min if they are very different
- //if (fabs(ra - rb) < 0.10)
- r = (1 - b) * ra + b * rb;
- //else r = std::min(ra, rb);
-
- // Intensity is either-or
- v = (int) this->myParent->GetRetro(ja) || (int) this->myParent->GetRetro(jb);
-
- this->laserIface->data->ranges[i] = r + minRange;
- this->laserIface->data->intensity[i] = v;
-
- /***************************************************************/
- /* */
- /* point scan from laser */
- /* */
- /***************************************************************/
- if (r == maxRange)
- this->laserMsg.ranges[i] = r + minRange; // no noise if at max range
- else
- this->laserMsg.ranges[i] = r + minRange + this->GaussianKernel(0,this->gaussianNoise) ;
- this->laserMsg.intensities[i] = v + this->GaussianKernel(0,this->gaussianNoise) ;
- }
-
- // iface writing can be skipped if iface is not used.
- // send data out via ros message
- rosnode->publish(this->topicName,this->laserMsg);
- this->lock.unlock();
-
- this->laserIface->Unlock();
-
- // New data is available
- this->laserIface->Post();
+ if (r == maxRange)
+ this->laserMsg.ranges[i] = r + minRange; // no noise if at max range
+ else
+ this->laserMsg.ranges[i] = r + minRange + this->GaussianKernel(0,this->gaussianNoise) ;
+ this->laserMsg.intensities[i] = v + this->GaussianKernel(0,this->gaussianNoise) ;
}
+ // send data out via ros message
+ rosnode->publish(this->topicName,this->laserMsg);
+ this->lock.unlock();
}
//////////////////////////////////////////////////////////////////////////////
-// Update the data in the interface
-void Ros_Laser::PutFiducialData()
-{
- int i, j, count;
- FiducialFid *fid;
- double r, b;
- double ax, ay, bx, by, cx, cy;
-
- Angle maxAngle = this->myParent->GetMaxAngle();
- Angle minAngle = this->myParent->GetMinAngle();
-
- double maxRange = this->myParent->GetMaxRange();
- double minRange = this->myParent->GetMinRange();
- int rayCount = this->myParent->GetRayCount();
- int rangeCount = this->myParent->GetRangeCount();
-
- if (this->fiducialIface->Lock(1))
- {
- // Data timestamp
- this->fiducialIface->data->head.time = Simulator::Instance()->GetSimTime();
- this->fiducialIface->data->count = 0;
-
- // TODO: clean this up
- count = 0;
- for (i = 0; i < rayCount; i++)
- {
- if (this->myParent->GetFiducial(i) < 0)
- continue;
-
- // Find the end of the fiducial
- for (j = i + 1; j < rayCount; j++)
- {
- if (this->myParent->GetFiducial(j) != this->myParent->GetFiducial(i))
- break;
- }
- j--;
-
- // Need at least three points to get orientation
- if (j - i + 1 >= 3)
- {
- r = minRange + this->myParent->GetRange(i);
- b = minAngle.GetAsRadian() + i * ((maxAngle.GetAsRadian()-minAngle.GetAsRadian()) / (rayCount - 1));
- ax = r * cos(b);
- ay = r * sin(b);
-
- r = minRange + this->myParent->GetRange(j);
- b = minAngle.GetAsRadian() + j * ((maxAngle.GetAsRadian()-minAngle.GetAsRadian()) / (rayCount - 1));
- bx = r * cos(b);
- by = r * sin(b);
-
- cx = (ax + bx) / 2;
- cy = (ay + by) / 2;
-
- assert(count < GZ_FIDUCIAL_MAX_FIDS);
- fid = this->fiducialIface->data->fids + count++;
-
- fid->id = this->myParent->GetFiducial(j);
- fid->pose.pos.x = cx;
- fid->pose.pos.y = cy;
- fid->pose.yaw = atan2(by - ay, bx - ax) + M_PI / 2;
- }
-
- // Fewer points get no orientation
- else
- {
- r = minRange + this->myParent->GetRange(i);
- b = minAngle.GetAsRadian() + i * ((maxAngle.GetAsRadian()-minAngle.GetAsRadian()) / (rayCount - 1));
- ax = r * cos(b);
- ay = r * sin(b);
-
- r = minRange + this->myParent->GetRange(j);
- b = minAngle.GetAsRadian() + j * ((maxAngle.GetAsRadian()-minAngle.GetAsRadian()) / (rayCount - 1));
- bx = r * cos(b);
- by = r * sin(b);
-
- cx = (ax + bx) / 2;
- cy = (ay + by) / 2;
-
- assert(count < GZ_FIDUCIAL_MAX_FIDS);
- fid = this->fiducialIface->data->fids + count++;
-
- fid->id = this->myParent->GetFiducial(j);
- fid->pose.pos.x = cx;
- fid->pose.pos.y = cy;
- fid->pose.yaw = atan2(cy, cx) + M_PI;
- }
-
- /*printf("fiducial %d i[%d] j[%d] %.2f %.2f %.2f\n",
- fid->id, i,j,fid->pos[0], fid->pos[1], fid->rot[2]);
- */
- i = j;
- }
-
- this->fiducialIface->data->count = count;
-
- this->fiducialIface->Unlock();
- this->fiducialIface->Post();
- }
-}
-
-//////////////////////////////////////////////////////////////////////////////
// Utility for adding noise
double Ros_Laser::GaussianKernel(double mu,double sigma)
{
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_PTZ.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_PTZ.cc 2008-09-29 23:27:11 UTC (rev 4794)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_PTZ.cc 2008-09-30 00:17:02 UTC (rev 4795)
@@ -105,11 +105,7 @@
// Load the controller
void Ros_PTZ::LoadChild(XMLConfigNode *node)
{
- this->ptzIface = dynamic_cast<PTZIface*>(this->ifaces[0]);
- if (!this->ptzIface)
- gzthrow("Ros_PTZ controller requires a PTZIface");
-
this->panJointNameP->Load(node);
this->tiltJointNameP->Load(node);
this->motionGainP->Load(node);
@@ -178,15 +174,6 @@
// Update the controller
void Ros_PTZ::UpdateChild()
{
- //this->ptzIface->Lock(1);
-
- // pan tilt command set in void Ros_PTZ::PTZCommandReceived() rather than from Iface
- //this->cmdPan = this->ptzIface->data->cmd_pan;
- //this->cmdTilt = this->ptzIface->data->cmd_tilt;
- //this->cmdZoom = this->hfov / this->ptzIface->data->cmd_zoom;
-
- //this->ptzIface->Unlock();
-
// Apply joint limits to commanded pan/tilt angles
if (this->cmdTilt > M_PI*0.3)
this->cmdTilt = M_PI*0.3;
@@ -233,22 +220,9 @@
// Put laser data to the interface
void Ros_PTZ::PutPTZData()
{
- PTZData *data = this->ptzIface->data;
+ // Data timestamp, not used
+ //double cur_time = Simulator::Instance()->GetSimTime();
- this->ptzIface->Lock(1);
-
- // Data timestamp
- data->head.time = Simulator::Instance()->GetSimTime();
-
- data->pan = this->panJoint->GetAngle();
- data->tilt = this->tiltJoint->GetAngle();
- //data->zoom = this->camera->GetFOV();
-
- this->ptzIface->Unlock();
-
- // New data is available
- this->ptzIface->Post();
-
this->lock.lock();
// also put data into ros message
PTZStateMessage.pan.pos_valid =1;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
