Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[1996] pkg/trunk
|
From: <hsu...@us...> - 2008-07-23 21:35:17
|
Revision: 1996
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1996&view=rev
Author: hsujohnhsu
Date: 2008-07-23 21:35:25 +0000 (Wed, 23 Jul 2008)
Log Message:
-----------
added forearm cameras.
broadcast all cameras on ros.
added time detect and repeat prevention for camera data sending.
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h
pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp
pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2-test.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_torque.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_torque_control.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_torque_position.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_tray1.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_trimesh.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_wg.world
pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h
pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
Modified: pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h 2008-07-23 21:21:46 UTC (rev 1995)
+++ pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h 2008-07-23 21:35:25 UTC (rev 1996)
@@ -362,6 +362,13 @@
private: double lastTiltLaserTime;
private: double lastBaseLaserTime;
+ private: double lastCameraGlobalTime;
+ private: double lastPTZCameraLeftTime;
+ private: double lastPTZCameraRightTime;
+ private: double lastWristCameraLeftTime;
+ private: double lastWristCameraRightTime;
+ private: double lastForearmCameraLeftTime;
+ private: double lastForearmCameraRightTime;
};
Modified: pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp 2008-07-23 21:21:46 UTC (rev 1995)
+++ pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp 2008-07-23 21:35:25 UTC (rev 1996)
@@ -38,7 +38,6 @@
gazebo::PR2GripperIface *pr2GripperRightIface;
gazebo::LaserIface *pr2LaserIface;
gazebo::LaserIface *pr2BaseLaserIface;
-gazebo::CameraIface *pr2CameraIface;
gazebo::CameraIface *pr2CameraGlobalIface;
gazebo::CameraIface *pr2PTZCameraLeftIface;
gazebo::CameraIface *pr2PTZCameraRightIface;
@@ -47,6 +46,10 @@
gazebo::PositionIface *pr2LeftWristIface;
gazebo::PositionIface *pr2RightWristIface;
gazebo::PositionIface *pr2BaseIface;
+gazebo::CameraIface *pr2WristCameraLeftIface;
+gazebo::CameraIface *pr2WristCameraRightIface;
+gazebo::CameraIface *pr2ForearmCameraLeftIface;
+gazebo::CameraIface *pr2ForearmCameraRightIface;
////////////////////////////////////////////////////////////////////
// //
@@ -84,6 +87,10 @@
pr2PTZCameraRightIface = new gazebo::CameraIface();
pr2PTZLeftIface = new gazebo::PTZIface();
pr2PTZRightIface = new gazebo::PTZIface();
+ pr2WristCameraLeftIface = new gazebo::CameraIface();
+ pr2WristCameraRightIface = new gazebo::CameraIface();
+ pr2ForearmCameraLeftIface = new gazebo::CameraIface();
+ pr2ForearmCameraRightIface = new gazebo::CameraIface();
pr2LeftWristIface = new gazebo::PositionIface();
pr2RightWristIface = new gazebo::PositionIface();
@@ -144,6 +151,47 @@
<< e << "\n";
}
+ /// Open the wrist and forearm cameras
+ try
+ {
+ pr2WristCameraLeftIface->Open(client, "wrist_cam_left_iface");
+ }
+ catch (std::string e)
+ {
+ std::cout << "Gazebo error: Unable to connect to the pr2 Left wrist camera interface\n"
+ << e << "\n";
+ }
+
+ try
+ {
+ pr2WristCameraRightIface->Open(client, "wrist_cam_right_iface");
+ }
+ catch (std::string e)
+ {
+ std::cout << "Gazebo error: Unable to connect to the pr2 Right wrist camera interface\n"
+ << e << "\n";
+ }
+
+ try
+ {
+ pr2ForearmCameraLeftIface->Open(client, "forearm_cam_left_iface");
+ }
+ catch (std::string e)
+ {
+ std::cout << "Gazebo error: Unable to connect to the pr2 Left forearm camera interface\n"
+ << e << "\n";
+ }
+
+ try
+ {
+ pr2ForearmCameraRightIface->Open(client, "forearm_cam_right_iface");
+ }
+ catch (std::string e)
+ {
+ std::cout << "Gazebo error: Unable to connect to the pr2 Right forearm camera interface\n"
+ << e << "\n";
+ }
+
/// Open the Position interface for gripper left
try
{
@@ -307,6 +355,13 @@
GetSimTime(&(this->lastTiltLaserTime));
GetSimTime(&(this->lastBaseLaserTime));
+ GetSimTime(&(this->lastCameraGlobalTime ));
+ GetSimTime(&(this->lastPTZCameraLeftTime ));
+ GetSimTime(&(this->lastPTZCameraRightTime ));
+ GetSimTime(&(this->lastWristCameraLeftTime ));
+ GetSimTime(&(this->lastWristCameraRightTime ));
+ GetSimTime(&(this->lastForearmCameraLeftTime ));
+ GetSimTime(&(this->lastForearmCameraRightTime ));
return PR2_ALL_OK;
};
@@ -671,28 +726,153 @@
uint32_t* data_size ,void* buf )
{
+ gazebo::CameraIface *tmpCameraIface;
+
switch(id)
{
case CAMERA_GLOBAL:
- pr2CameraIface = pr2CameraGlobalIface;
+ if (pr2CameraGlobalIface && pr2CameraGlobalIface->Lock(1))
+ {
+ if (pr2CameraGlobalIface->data->head.time == this->lastCameraGlobalTime)
+ {
+ tmpCameraIface = NULL;
+ }
+ else
+ {
+ tmpCameraIface = pr2CameraGlobalIface;
+ this->lastCameraGlobalTime = pr2CameraGlobalIface->data->head.time;
+ }
+ pr2CameraGlobalIface->Unlock();
+ }
+ else
+ tmpCameraIface = NULL;
+
break;
case CAMERA_HEAD_LEFT:
- pr2CameraIface = pr2PTZCameraLeftIface;
+ if (pr2PTZCameraLeftIface && pr2PTZCameraLeftIface->Lock(1))
+ {
+ if (pr2PTZCameraLeftIface->data->head.time == this->lastPTZCameraLeftTime)
+ {
+ tmpCameraIface = NULL;
+ }
+ else
+ {
+ tmpCameraIface = pr2PTZCameraLeftIface;
+ this->lastPTZCameraLeftTime = pr2PTZCameraLeftIface->data->head.time;
+ }
+ pr2PTZCameraLeftIface->Unlock();
+ }
+ else
+ tmpCameraIface = NULL;
+
break;
case CAMERA_HEAD_RIGHT:
- pr2CameraIface = pr2PTZCameraRightIface;
+ if (pr2PTZCameraRightIface && pr2PTZCameraRightIface->Lock(1))
+ {
+ if (pr2PTZCameraRightIface->data->head.time == this->lastPTZCameraRightTime)
+ {
+ tmpCameraIface = NULL;
+ }
+ else
+ {
+ tmpCameraIface = pr2PTZCameraRightIface;
+ this->lastPTZCameraRightTime = pr2PTZCameraRightIface->data->head.time;
+ }
+ pr2PTZCameraRightIface->Unlock();
+ }
+ else
+ tmpCameraIface = NULL;
+
break;
+ case CAMERA_WRIST_LEFT:
+ if (pr2WristCameraLeftIface && pr2WristCameraLeftIface->Lock(1))
+ {
+ if (pr2WristCameraLeftIface->data->head.time == this->lastWristCameraLeftTime)
+ {
+ tmpCameraIface = NULL;
+ }
+ else
+ {
+ tmpCameraIface = pr2WristCameraLeftIface;
+ this->lastWristCameraLeftTime = pr2WristCameraLeftIface->data->head.time;
+ }
+ pr2WristCameraLeftIface->Unlock();
+ }
+ else
+ tmpCameraIface = NULL;
+
+ break;
+ case CAMERA_WRIST_RIGHT:
+ if (pr2WristCameraRightIface && pr2WristCameraRightIface->Lock(1))
+ {
+ if (pr2WristCameraRightIface->data->head.time == this->lastWristCameraRightTime)
+ {
+ tmpCameraIface = NULL;
+ }
+ else
+ {
+ tmpCameraIface = pr2WristCameraRightIface;
+ this->lastWristCameraRightTime = pr2WristCameraRightIface->data->head.time;
+ }
+ pr2WristCameraRightIface->Unlock();
+ }
+ else
+ tmpCameraIface = NULL;
+
+ break;
+ case CAMERA_FOREARM_LEFT:
+ if (pr2ForearmCameraLeftIface && pr2ForearmCameraLeftIface->Lock(1))
+ {
+ if (pr2ForearmCameraLeftIface->data->head.time == this->lastForearmCameraLeftTime)
+ {
+ tmpCameraIface = NULL;
+ }
+ else
+ {
+ tmpCameraIface = pr2ForearmCameraLeftIface;
+ this->lastForearmCameraLeftTime = pr2ForearmCameraLeftIface->data->head.time;
+ }
+ pr2ForearmCameraLeftIface->Unlock();
+ }
+ else
+ tmpCameraIface = NULL;
+
+ break;
+ case CAMERA_FOREARM_RIGHT:
+ if (pr2ForearmCameraRightIface && pr2ForearmCameraRightIface->Lock(1))
+ {
+ if (pr2ForearmCameraRightIface->data->head.time == this->lastForearmCameraRightTime)
+ {
+ tmpCameraIface = NULL;
+ }
+ else
+ {
+ tmpCameraIface = pr2ForearmCameraRightIface;
+ this->lastForearmCameraRightTime = pr2ForearmCameraRightIface->data->head.time;
+ }
+ pr2ForearmCameraRightIface->Unlock();
+ }
+ else
+ tmpCameraIface = NULL;
+
+ break;
default:
- pr2CameraIface = pr2CameraGlobalIface;
+ tmpCameraIface = NULL;
break;
}
- pr2CameraIface->Lock(1);
- *width = (uint32_t)pr2CameraIface->data->width;
- *height = (uint32_t)pr2CameraIface->data->height;
+ if (tmpCameraIface == NULL)
+ {
+ return PR2_ERROR;
+ }
+ else
+ {
+ tmpCameraIface->Lock(1);
+ *width = (uint32_t)tmpCameraIface->data->width;
+ *height = (uint32_t)tmpCameraIface->data->height;
*compression = "raw";
*colorspace = "rgb24"; //"mono";
- *data_size = pr2CameraIface->data->image_size;
+ *data_size = tmpCameraIface->data->image_size;
// on first pass, the sensor does not update after cameraIface is opened.
if (*data_size > 0)
@@ -703,22 +883,23 @@
// copy the image into local buffer
#if 1
- //buf = (void*)(pr2CameraIface->data->image);
- memcpy(buf,pr2CameraIface->data->image,buf_size);
+ //buf = (void*)(tmpCameraIface->data->image);
+ memcpy(buf,tmpCameraIface->data->image,buf_size);
#else
for (uint32_t i = 0; i < buf_size ; i=i+3)
{
// flip red and blue
- ((unsigned char*)buf)[i ] = pr2CameraIface->data->image[i+2];
- ((unsigned char*)buf)[i+1] = pr2CameraIface->data->image[i+1];
- ((unsigned char*)buf)[i+2] = pr2CameraIface->data->image[i ];
- //printf("%d %d\n",i,pr2CameraIface->data->image[i]);
+ ((unsigned char*)buf)[i ] = tmpCameraIface->data->image[i+2];
+ ((unsigned char*)buf)[i+1] = tmpCameraIface->data->image[i+1];
+ ((unsigned char*)buf)[i+2] = tmpCameraIface->data->image[i ];
+ //printf("%d %d\n",i,tmpCameraIface->data->image[i]);
}
#endif
}
- pr2CameraIface->Unlock();
+ tmpCameraIface->Unlock();
return PR2_ALL_OK;
+ }
};
PR2_ERROR_CODE PR2HW::GetWristPoseGroundTruth(PR2_MODEL_ID id, double *x, double *y, double *z, double *roll, double *pitch, double *yaw)
Modified: pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h 2008-07-23 21:21:46 UTC (rev 1995)
+++ pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h 2008-07-23 21:35:25 UTC (rev 1996)
@@ -111,8 +111,10 @@
CAMERA_STEREO_RIGHT ,
CAMERA_HEAD_LEFT ,
CAMERA_HEAD_RIGHT ,
- CAMERA_ARM_LEFT ,
- CAMERA_ARM_RIGHT ,
+ CAMERA_FOREARM_LEFT ,
+ CAMERA_FOREARM_RIGHT ,
+ CAMERA_WRIST_LEFT ,
+ CAMERA_WRIST_RIGHT ,
LASER_HEAD ,
LASER_BASE ,
TACTILE_FINGER_LEFT ,
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2-test.model
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2-test.model 2008-07-23 21:21:46 UTC (rev 1995)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2-test.model 2008-07-23 21:35:25 UTC (rev 1996)
@@ -54,116 +54,118 @@
<!-- Hokuyo laser body -->
- <model:physical name="base_laser_model">
- <body:box name="base_laser_body">
- <xyz>0.45 0.0 0.27</xyz> <!-- from the robot coordinate system (center bottom of base box)-->
- <rpy>0.0 0.0 0.0</rpy>
-
- <geom:box name="base_laser_box">
- <xyz>0.0 0.0 0.02</xyz>
- <rpy>0 0 0</rpy>
- <size>0.05 0.05 0.041</size>
- <mass>0.12</mass>
+ <body:box name="base_laser_body">
+ <xyz>0.45 0.0 0.27</xyz> <!-- from the robot coordinate system (center bottom of base box)-->
+ <rpy>0.0 0.0 0.0</rpy>
+
+ <geom:box name="base_laser_box">
+ <xyz>0.0 0.0 0.02</xyz>
+ <rpy>0 0 0</rpy>
+ <size>0.05 0.05 0.041</size>
+ <mass>0.12</mass>
- <visual>
- <scale>0.05 0.05 0.041</scale>
- <mesh>unit_box</mesh>
- <material>Gazebo/Blue</material>
- </visual>
+ <visual>
+ <scale>0.05 0.05 0.041</scale>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Blue</material>
+ </visual>
- </geom:box>
- <geom:cylinder name="base_laser_cylinder1">
- <xyz>0.0 0.0 0.041</xyz>
+ </geom:box>
+ <geom:cylinder name="base_laser_cylinder1">
+ <xyz>0.0 0.0 0.041</xyz>
+ <rpy>0 0 0</rpy>
+ <size>0.02 0.013</size>
+ <mass>0.02</mass>
+
+ <visual>
<rpy>0 0 0</rpy>
- <size>0.02 0.013</size>
- <mass>0.02</mass>
-
- <visual>
- <rpy>0 0 0</rpy>
- <scale>0.04 0.04 0.013</scale>
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Black</material>
- </visual>
+ <scale>0.04 0.04 0.013</scale>
+ <mesh>unit_cylinder</mesh>
+ <material>Gazebo/Black</material>
+ </visual>
- </geom:cylinder>
+ </geom:cylinder>
- <geom:cylinder name="base_laser_cylinder2">
- <xyz>0.0 0.0 0.054</xyz>
+ <geom:cylinder name="base_laser_cylinder2">
+ <xyz>0.0 0.0 0.054</xyz>
+ <rpy>0 0 0</rpy>
+ <size>0.018 0.009</size>
+ <mass>0.02</mass>
+
+ <visual>
<rpy>0 0 0</rpy>
- <size>0.018 0.009</size>
- <mass>0.02</mass>
-
- <visual>
- <rpy>0 0 0</rpy>
- <size>0.036 0.036 0.009</size>
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Black</material>
- </visual>
+ <size>0.036 0.036 0.009</size>
+ <mesh>unit_cylinder</mesh>
+ <material>Gazebo/Black</material>
+ </visual>
- </geom:cylinder>
+ </geom:cylinder>
- <geom:cylinder name="base_laser_cylinder3">
- <xyz>0.0 0.0 0.063</xyz>
+ <geom:cylinder name="base_laser_cylinder3">
+ <xyz>0.0 0.0 0.063</xyz>
+ <rpy>0 0 0</rpy>
+ <size>0.0175 0.008</size>
+ <mass>0.02</mass>
+
+ <visual>
<rpy>0 0 0</rpy>
- <size>0.0175 0.008</size>
- <mass>0.02</mass>
-
- <visual>
- <rpy>0 0 0</rpy>
- <size>0.035 0.035 0.008</size>
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Shell</material>
- </visual>
+ <size>0.035 0.035 0.008</size>
+ <mesh>unit_cylinder</mesh>
+ <material>Gazebo/Shell</material>
+ </visual>
- </geom:cylinder>
+ </geom:cylinder>
- <sensor:ray name="base_laser_1">
- <rayCount>68</rayCount>
- <rangeCount>683</rangeCount>
- <laserCount>1</laserCount>
- <origin>0.0 0.0 0.05</origin>
+ <sensor:ray name="base_laser_1">
+ <rayCount>68</rayCount>
+ <rangeCount>683</rangeCount>
+ <laserCount>1</laserCount>
+ <origin>0.0 0.0 0.05</origin>
- <minAngle>-135</minAngle>
- <maxAngle>135</maxAngle>
+ <minAngle>-135</minAngle>
+ <maxAngle>135</maxAngle>
- <minRange>0.05</minRange>
- <maxRange>10.0</maxRange>
- <updateRate>10.0</updateRate>
- <controller:sicklms200_laser name="base_laser_controller_1">
- <interface:laser name="base_laser_iface_1"/>
- <updateRate>10</updateRate>
- </controller:sicklms200_laser>
- </sensor:ray>
- <!--
- -->
- <!--
- <sensor:ray2 name="laser_1">
- <rayCount>683</rayCount>
- <rangeCount>683</rangeCount>
- <laserCount>1</laserCount>
- <origin>0.0 0.0 0.05</origin>
+ <minRange>0.05</minRange>
+ <maxRange>10.0</maxRange>
+ <updateRate>10.0</updateRate>
+ <controller:sicklms200_laser name="base_laser_controller_1">
+ <interface:laser name="base_laser_iface_1"/>
+ <updateRate>10</updateRate>
+ </controller:sicklms200_laser>
+ </sensor:ray>
+ <!--
+ -->
+ <!--
+ <sensor:ray2 name="laser_1">
+ <rayCount>683</rayCount>
+ <rangeCount>683</rangeCount>
+ <laserCount>1</laserCount>
+ <origin>0.0 0.0 0.05</origin>
- <minAngle>-135</minAngle>
- <maxAngle>135</maxAngle>
+ <minAngle>-135</minAngle>
+ <maxAngle>135</maxAngle>
- <minRange>0.05</minRange>
- <maxRange>10.0</maxRange>
- <controller:sick2lms200_laser name="laser_controller_1">
- <interface:laser name="laser_iface_1"/>
- <updateRate>10</updateRate>
- </controller:sick2lms200_laser>
- </sensor:ray2>
- -->
+ <minRange>0.05</minRange>
+ <maxRange>10.0</maxRange>
+ <controller:sick2lms200_laser name="laser_controller_1">
+ <interface:laser name="laser_iface_1"/>
+ <updateRate>10</updateRate>
+ </controller:sick2lms200_laser>
+ </sensor:ray2>
+ -->
+ </body:box>
- </body:box>
+ <!-- attach hokuyo to torso -->
+ <joint:hinge name="tiltlaser_torso_attach_joint">
+ <body1>base_body</body1>
+ <body2>base_laser_body</body2>
+ <anchor>base_laser_body</anchor>
+ <axis> 1.0 0.0 0.0 </axis>
+ <lowStop> 0.0 </lowStop>
+ <highStop> 0.0 </highStop>
+ </joint:hinge>
- <!-- attach hokuyo to torso -->
- <attach>
- <parentBody>base_body</parentBody>
- <myBody>base_laser_body</myBody>
- </attach>
- </model:physical>
@@ -847,461 +849,392 @@
<!-- neck body -->
- <model:physical name="neck_model">
- <body:box name="neck_body">
- <xyz>0.10 0.0 1.03</xyz>
- <rpy>0.0 0.0 0.0</rpy>
- <!--xyz>0.0 0.0 0.0</xyz>
- <rpy>0.0 0.0 0.0</rpy-->
- <geom:box name="neck_geom">
+ <body:box name="neck_body">
+ <xyz>0.10 0.0 1.03</xyz>
+ <rpy>0.0 0.0 0.0</rpy>
+ <!--xyz>0.0 0.0 0.0</xyz>
+ <rpy>0.0 0.0 0.0</rpy-->
+ <geom:box name="neck_geom">
+ <!--size>.20 .38 .14</size-->
+ <size>0.01 0.01 0.01</size>
+ <mass>0.01</mass>
+ <visual>
+ <mesh>unit_box</mesh>
<!--size>.20 .38 .14</size-->
<size>0.01 0.01 0.01</size>
- <mass>0.01</mass>
- <visual>
- <mesh>unit_box</mesh>
- <!--size>.20 .38 .14</size-->
- <size>0.01 0.01 0.01</size>
- <material>Gazebo/Black</material>
- </visual>
- </geom:box>
- </body:box>
+ <material>Gazebo/Black</material>
+ </visual>
+ </geom:box>
+ </body:box>
- <!-- attach neck to torso -->
- <attach>
- <parentBody>torso_body</parentBody>
- <myBody>neck_body</myBody>
- </attach>
+ <!-- attach neck to torso -->
+ <joint:hinge name="neck_torso_attach_joint">
+ <body1>torso_body</body1>
+ <body2>neck_body</body2>
+ <anchor>neck_body</anchor>
+ <axis> 1.0 0.0 0.0 </axis>
+ <lowStop> 0.0 </lowStop>
+ <highStop> 0.0 </highStop>
+ </joint:hinge>
- <body:box name="head_base_body">
- <xyz>0.10 0.0 1.13 </xyz>
- <rpy>0.0 0.0 0.0 </rpy>
- <!-- report when this body collides with static bodies -->
- <reportStaticCollision>true</reportStaticCollision>
- <geom:box name="head_base_geom">
- <xyz> 0.0 0.00 0.000</xyz>
+ <body:box name="head_base_body">
+ <xyz>0.10 0.0 1.13 </xyz>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <!-- report when this body collides with static bodies -->
+ <reportStaticCollision>true</reportStaticCollision>
+ <geom:box name="head_base_geom">
+ <xyz> 0.0 0.00 0.000</xyz>
+ <size>0.165 0.21 0.003</size>
+ <mass>0.1</mass>
+ <mul>1</mul>
+ <!--color> 0.0 1.0 0.0</color-->
+ <visual>
<size>0.165 0.21 0.003</size>
- <mass>0.1</mass>
- <mul>1</mul>
- <!--color> 0.0 1.0 0.0</color-->
- <visual>
- <size>0.165 0.21 0.003</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/PioneerBody</material>
- </visual>
- </geom:box>
- </body:box>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/PioneerBody</material>
+ </visual>
+ </geom:box>
+ </body:box>
- <body:box name="head_tilt_body">
- <xyz>0.10 0.0 1.07 </xyz>
- <rpy>0.0 0.0 0.0 </rpy>
- <!-- report when this body collides with static bodies -->
- <reportStaticCollision>true</reportStaticCollision>
- <geom:box name="head_tilt_geom">
- <xyz> 0.0 0.00 0.150</xyz>
+ <body:box name="head_tilt_body">
+ <xyz>0.10 0.0 1.07 </xyz>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <!-- report when this body collides with static bodies -->
+ <reportStaticCollision>true</reportStaticCollision>
+ <geom:box name="head_tilt_geom">
+ <xyz> 0.0 0.00 0.150</xyz>
+ <size>0.165 0.21 0.003</size>
+ <mass>0.1</mass>
+ <mul>1</mul>
+ <!--color> 0.0 1.0 0.0</color-->
+ <visual>
<size>0.165 0.21 0.003</size>
- <mass>0.1</mass>
- <mul>1</mul>
- <!--color> 0.0 1.0 0.0</color-->
- <visual>
- <size>0.165 0.21 0.003</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/Black</material>
- </visual>
- </geom:box>
- <geom:box name="head_tilt_left_geom">
- <xyz> 0.0 -0.105 0.10</xyz>
- <size>0.165 0.003 0.10</size>
- <mass>0.1</mass>
- <mul>1</mul>
- <!--color> 0.0 1.0 0.0</color-->
- <visual>
- <size>0.165 0.003 0.10</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/Shell</material>
- </visual>
- </geom:box>
- <geom:box name="head_tilt_right_geom">
- <xyz> 0.0 0.105 0.10</xyz>
- <size>0.165 0.003 0.10</size>
- <mass>0.1</mass>
- <mul>1</mul>
- <!--color> 0.0 1.0 0.0</color-->
- <visual>
- <size>0.165 0.003 0.10</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/Shell</material>
- </visual>
- </geom:box>
- </body:box>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Black</material>
+ </visual>
+ </geom:box>
+ <geom:box name="head_tilt_left_geom">
+ <xyz> 0.0 -0.105 0.10</xyz>
+ <size>0.165 0.003 0.10</size>
+ <mass>0.1</mass>
+ <mul>1</mul>
+ <!--color> 0.0 1.0 0.0</color-->
+ <visual>
+ <size>0.165 0.003 0.10</size>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Shell</material>
+ </visual>
+ </geom:box>
+ <geom:box name="head_tilt_right_geom">
+ <xyz> 0.0 0.105 0.10</xyz>
+ <size>0.165 0.003 0.10</size>
+ <mass>0.1</mass>
+ <mul>1</mul>
+ <!--color> 0.0 1.0 0.0</color-->
+ <visual>
+ <size>0.165 0.003 0.10</size>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Shell</material>
+ </visual>
+ </geom:box>
+ </body:box>
- <!-- attach head_base to head_tilt -->
- <joint:hinge name="head_tilt_joint">
- <body2>head_tilt_body</body2>
- <body1>head_base_body</body1>
- <anchor>head_tilt_body</anchor>
- <axis> 0.0 1.0 0.0 </axis>
- </joint:hinge>
+ <!-- attach head_base to head_tilt -->
+ <joint:hinge name="head_tilt_joint">
+ <body2>head_tilt_body</body2>
+ <body1>head_base_body</body1>
+ <anchor>head_tilt_body</anchor>
+ <axis> 0.0 1.0 0.0 </axis>
+ </joint:hinge>
- <!-- attach head to neck -->
- <joint:hinge name="neck_yaw_joint">
- <body1>neck_body</body1>
- <body2>head_base_body</body2>
- <anchor>head_base_body</anchor>
- <axis> 0.0 0.0 1.0 </axis>
- </joint:hinge>
+ <!-- attach head to neck -->
+ <joint:hinge name="neck_yaw_joint">
+ <body1>neck_body</body1>
+ <body2>head_base_body</body2>
+ <anchor>head_base_body</anchor>
+ <axis> 0.0 0.0 1.0 </axis>
+ </joint:hinge>
- <!-- left camera -->
- <body:box name="head_cam_left_body">
- <xyz> 0.15 0.23 1.07 </xyz>
- <rpy> 0.00 0.0 0.00 </rpy>
- <sensor:camera name="head_cam_left_sensor">
- <imageSize>640 480</imageSize>
- <hfov>60</hfov>
- <nearClip>0.1</nearClip>
- <farClip>100</farClip>
+ <!-- left camera -->
+ <body:box name="ptz_cam_left_body">
+ <xyz> 0.15 0.23 1.07 </xyz>
+ <rpy> 0.00 0.0 0.00 </rpy>
+ <sensor:camera name="ptz_cam_left_sensor">
+ <imageSize>640 480</imageSize>
+ <hfov>60</hfov>
+ <nearClip>0.1</nearClip>
+ <farClip>100</farClip>
+ <updateRate>15.0</updateRate>
+ <controller:generic_camera name="ptz_cam_left_controller">
<updateRate>15.0</updateRate>
- <controller:generic_camera name="head_cam_left_controller">
- <updateRate>15.0</updateRate>
- <interface:camera name="head_cam_left_iface_0" />
- </controller:generic_camera>
- </sensor:camera>
- <geom:box name="head_cam_left_geom">
- <xyz> 0.00 0.00 0.00</xyz>
+ <interface:camera name="ptz_cam_left_iface" />
+ </controller:generic_camera>
+ </sensor:camera>
+ <geom:box name="ptz_cam_left_geom">
+ <xyz> 0.00 0.00 0.00</xyz>
+ <size>0.05 0.03 0.04</size>
+ <mass>0.1</mass>
+ <!--color> 0.0 1.0 0.0</color-->
+ <visual>
<size>0.05 0.03 0.04</size>
- <mass>0.1</mass>
- <!--color> 0.0 1.0 0.0</color-->
- <visual>
- <size>0.05 0.03 0.04</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/PioneerBody</material>
- </visual>
- </geom:box>
- </body:box>
- <body:box name="head_cam_base_left_body">
- <xyz> 0.15 0.20 1.07 </xyz>
- <rpy> 0.00 0.0 0.00 </rpy>
- <geom:box name="head_cam_base_left_geom">
- <xyz> 0.00 0.00 0.00</xyz>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/PioneerBody</material>
+ </visual>
+ </geom:box>
+ </body:box>
+ <body:box name="ptz_cam_base_left_body">
+ <xyz> 0.15 0.20 1.07 </xyz>
+ <rpy> 0.00 0.0 0.00 </rpy>
+ <geom:box name="ptz_cam_base_left_geom">
+ <xyz> 0.00 0.00 0.00</xyz>
+ <size>0.05 0.02 0.02</size>
+ <mass>0.1</mass>
+ <!--color> 0.0 1.0 0.0</color-->
+ <visual>
<size>0.05 0.02 0.02</size>
- <mass>0.1</mass>
- <!--color> 0.0 1.0 0.0</color-->
- <visual>
- <size>0.05 0.02 0.02</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/Shell</material>
- </visual>
- </geom:box>
- </body:box>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Shell</material>
+ </visual>
+ </geom:box>
+ </body:box>
- <!-- right camera -->
- <body:empty name="head_cam_right_body">
- <xyz> 0.15 -0.23 1.07 </xyz>
- <rpy> 0.00 0.0 0.00 </rpy>
- <sensor:camera name="head_cam_right_sensor">
- <imageSize>640 480</imageSize>
- <hfov>60</hfov>
- <nearClip>0.1</nearClip>
- <farClip>100</farClip>
+ <!-- right camera -->
+ <body:empty name="ptz_cam_right_body">
+ <xyz> 0.15 -0.23 1.07 </xyz>
+ <rpy> 0.00 0.0 0.00 </rpy>
+ <sensor:camera name="ptz_cam_right_sensor">
+ <imageSize>640 480</imageSize>
+ <hfov>60</hfov>
+ <nearClip>0.1</nearClip>
+ <farClip>100</farClip>
+ <updateRate>15.0</updateRate>
+ <controller:generic_camera name="ptz_cam_right_controller">
<updateRate>15.0</updateRate>
- <controller:generic_camera name="head_cam_right_controller">
- <updateRate>15.0</updateRate>
- <interface:camera name="head_cam_right_iface_0" />
- </controller:generic_camera>
- </sensor:camera>
- <geom:box name="head_cam_right_geom">
- <xyz> 0.00 0.00 0.00</xyz>
+ <interface:camera name="ptz_cam_right_iface" />
+ </controller:generic_camera>
+ </sensor:camera>
+ <geom:box name="ptz_cam_right_geom">
+ <xyz> 0.00 0.00 0.00</xyz>
+ <size>0.05 0.03 0.04</size>
+ <mass>0.1</mass>
+ <!--color> 0.0 1.0 0.0</color-->
+ <visual>
<size>0.05 0.03 0.04</size>
- <mass>0.1</mass>
- <!--color> 0.0 1.0 0.0</color-->
- <visual>
- <size>0.05 0.03 0.04</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/PioneerBody</material>
- </visual>
- </geom:box>
- </body:empty>
- <body:box name="head_cam_base_right_body">
- <xyz> 0.15 -0.20 1.07 </xyz>
- <rpy> 0.00 0.0 0.00 </rpy>
- <geom:box name="head_cam_base_right_geom">
- <xyz> 0.00 0.00 0.00</xyz>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/PioneerBody</material>
+ </visual>
+ </geom:box>
+ </body:empty>
+ <body:box name="ptz_cam_base_right_body">
+ <xyz> 0.15 -0.20 1.07 </xyz>
+ <rpy> 0.00 0.0 0.00 </rpy>
+ <geom:box name="ptz_cam_base_right_geom">
+ <xyz> 0.00 0.00 0.00</xyz>
+ <size>0.05 0.02 0.02</size>
+ <mass>0.1</mass>
+ <!--color> 0.0 1.0 0.0</color-->
+ <visual>
<size>0.05 0.02 0.02</size>
- <mass>0.1</mass>
- <!--color> 0.0 1.0 0.0</color-->
- <visual>
- <size>0.05 0.02 0.02</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/Shell</material>
- </visual>
- </geom:box>
- </body:box>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Shell</material>
+ </visual>
+ </geom:box>
+ </body:box>
- <!-- attach neck_body to ptz cameras -->
- <joint:hinge name="head_cam_left_yaw_joint">
- <body2>head_cam_left_body</body2>
- <body1>head_cam_base_left_body</body1>
- <anchor>head_cam_left_body</anchor>
- <axis> 0.0 0.0 1.0 </axis>
- </joint:hinge>
- <joint:hinge name="head_cam_left_pitch_joint">
- <body2>head_cam_base_left_body</body2>
- <body1>neck_body</body1>
- <anchor>head_cam_base_left_body</anchor>
- <axis> 0.0 1.0 0.0 </axis>
- </joint:hinge>
- <joint:hinge name="head_cam_right_yaw_joint">
- <body2>head_cam_right_body</body2>
- <body1>head_cam_base_right_body</body1>
- <anchor>head_cam_right_body</anchor>
- <axis> 0.0 0.0 1.0 </axis>
- </joint:hinge>
- <joint:hinge name="head_cam_right_pitch_joint">
- <body2>head_cam_base_right_body</body2>
- <body1>neck_body</body1>
- <anchor>head_cam_base_right_body</anchor>
- <axis> 0.0 1.0 0.0 </axis>
- </joint:hinge>
+ <!-- attach neck_body to ptz cameras -->
+ <joint:hinge name="ptz_cam_left_yaw_joint">
+ <body2>ptz_cam_left_body</body2>
+ <body1>ptz_cam_base_left_body</body1>
+ <anchor>ptz_cam_left_body</anchor>
+ <axis> 0.0 0.0 1.0 </axis>
+ </joint:hinge>
+ <joint:hinge name="ptz_cam_left_pitch_joint">
+ <body2>ptz_cam_base_left_body</body2>
+ <body1>neck_body</body1>
+ <anchor>ptz_cam_base_left_body</anchor>
+ <axis> 0.0 1.0 0.0 </axis>
+ </joint:hinge>
+ <joint:hinge name="ptz_cam_right_yaw_joint">
+ <body2>ptz_cam_right_body</body2>
+ <body1>ptz_cam_base_right_body</body1>
+ <anchor>ptz_cam_right_body</anchor>
+ <axis> 0.0 0.0 1.0 </axis>
+ </joint:hinge>
+ <joint:hinge name="ptz_cam_right_pitch_joint">
+ <body2>ptz_cam_base_right_body</body2>
+ <body1>neck_body</body1>
+ <anchor>ptz_cam_base_right_body</anchor>
+ <axis> 0.0 1.0 0.0 </axis>
+ </joint:hinge>
- <!-- stereo camera -->
- <body:empty name="stereo_camera_body">
- <xyz> 0.10 0.0 1.25 </xyz>
- <rpy> 0.0 0.0 0.0 </rpy>
- <sensor:stereocamera name="stereo_camera_sensor">
- <imageSize>640 480</imageSize>
- <hfov>60</hfov>
- <nearClip>0.1</nearClip>
- <farClip>100</farClip>
- <saveFrames>false</saveFrames>
- <saveFramePath>frames</saveFramePath>
- <baseline>0.2</baseline>
+ <!-- stereo camera -->
+ <body:empty name="stereo_camera_body">
+ <xyz> 0.10 0.0 1.25 </xyz>
+ <rpy> 0.0 0.0 0.0 </rpy>
+ <sensor:stereocamera name="stereo_camera_sensor">
+ <imageSize>640 480</imageSize>
+ <hfov>60</hfov>
+ <nearClip>0.1</nearClip>
+ <farClip>100</farClip>
+ <saveFrames>false</saveFrames>
+ <saveFramePath>frames</saveFramePath>
+ <baseline>0.2</baseline>
+ <updateRate>15.0</updateRate>
+ <controller:stereocamera name="stereo_camera_controller">
<updateRate>15.0</updateRate>
- <controller:stereocamera name="stereo_camera_controller">
- <updateRate>15.0</updateRate>
- <interface:stereocamera name="stereo_iface_0" />
- <interface:camera name="camera_iface_0" />
- <interface:camera name="camera_iface_1" />
- <leftcamera>camera_iface_0</leftcamera>
- <rightcamera>camera_iface_1</rightcamera>
- </controller:stereocamera>
- </sensor:stereocamera>
- <geom:box name="stereo_camera_geom">
- <xyz> 0.00 0.00 0.00</xyz>
+ <interface:stereocamera name="stereo_iface_0" />
+ <interface:camera name="camera_iface_0" />
+ <interface:camera name="camera_iface_1" />
+ <leftcamera>camera_iface_0</leftcamera>
+ <rightcamera>camera_iface_1</rightcamera>
+ </controller:stereocamera>
+ </sensor:stereocamera>
+ <geom:box name="stereo_camera_geom">
+ <xyz> 0.00 0.00 0.00</xyz>
+ <size>0.10 0.15 0.10</size>
+ <mass>0.1</mass>
+ <visual>
<size>0.10 0.15 0.10</size>
- <mass>0.1</mass>
- <visual>
- <size>0.10 0.15 0.10</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/PioneerBody</material>
- </visual>
- </geom:box>
- </body:empty>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/PioneerBody</material>
+ </visual>
+ </geom:box>
+ </body:empty>
- <!-- attach stereo_camera to head_tilt -->
- <joint:hinge name="stereo_camera_roll_joint">
- <body2>head_tilt_body</body2>
- <body1>stereo_camera_body</body1>
- <anchor>head_tilt_body</anchor>
- <axis> 1.0 0.0 0.0 </axis>
- </joint:hinge>
- <joint:hinge name="stereo_camera_pitch_joint">
- <body2>head_tilt_body</body2>
- <body1>stereo_camera_body</body1>
- <anchor>head_tilt_body</anchor>
- <axis> 0.0 1.0 0.0 </axis>
- </joint:hinge>
- <joint:hinge name="stereo_camera_yaw_joint">
- <body2>head_tilt_body</body2>
- <body1>stereo_camera_body</body1>
- <anchor>head_tilt_body</anchor>
- <axis> 0.0 0.0 010 </axis>
- </joint:hinge>
+ <!-- attach stereo_camera to head_tilt -->
+ <joint:hinge name="stereo_camera_attach_joint">
+ <body2>head_tilt_body</body2>
+ <body1>stereo_camera_body</body1>
+ <anchor>head_tilt_body</anchor>
+ <axis> 1.0 0.0 0.0 </axis>
+ <lowStop> 0.0 </lowStop>
+ <highStop> 0.0 </highStop>
+ </joint:hinge>
- <!-- Hokuyo laser body -->
- <body:box name="laser_body">
- <xyz>0.23 0.0 1.0</xyz>
- <rpy>0.0 0.0 0.0</rpy>
-
- <geom:box name="laser_box">
- <xyz>0.0 0.0 0.02</xyz>
- <rpy>0 0 0</rpy>
- <size>0.05 0.05 0.041</size>
- <mass>0.12</mass>
+ <!-- Hokuyo laser body -->
+ <body:box name="laser_body">
+ <xyz>0.23 0.0 1.0</xyz>
+ <rpy>0.0 0.0 0.0</rpy>
+
+ <geom:box name="laser_box">
+ <xyz>0.0 0.0 0.02</xyz>
+ <rpy>0 0 0</rpy>
+ <size>0.05 0.05 0.041</size>
+ <mass>0.12</mass>
- <visual>
- <scale>0.05 0.05 0.041</scale>
- <mesh>unit_box</mesh>
- <material>Gazebo/Blue</material>
- </visual>
+ <visual>
+ <scale>0.05 0.05 0.041</scale>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Blue</material>
+ </visual>
- </geom:box>
- <geom:cylinder name="laser_cylinder1">
- <xyz>0.0 0.0 0.041</xyz>
+ </geom:box>
+ <geom:cylinder name="laser_cylinder1">
+ <xyz>0.0 0.0 0.041</xyz>
+ <rpy>0 0 0</rpy>
+ <size>0.02 0.013</size>
+ <mass>0.02</mass>
+
+ <visual>
<rpy>0 0 0</rpy>
- <size>0.02 0.013</size>
- <mass>0.02</mass>
-
- <visual>
- <rpy>0 0 0</rpy>
- <scale>0.04 0.04 0.013</scale>
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Black</material>
- </visual>
+ <scale>0.04 0.04 0.013</scale>
+ <mesh>unit_cylinder</mesh>
+ <material>Gazebo/Black</material>
+ </visual>
- </geom:cylinder>
+ </geom:cylinder>
- <geom:cylinder name="laser_cylinder2">
- <xyz>0.0 0.0 0.054</xyz>
+ <geom:cylinder name="laser_cylinder2">
+ <xyz>0.0 0.0 0.054</xyz>
+ <rpy>0 0 0</rpy>
+ <size>0.018 0.009</size>
+ <mass>0.02</mass>
+
+ <visual>
<rpy>0 0 0</rpy>
- <size>0.018 0.009</size>
- <mass>0.02</mass>
-
- <visual>
- <rpy>0 0 0</rpy>
- <size>0.036 0.036 0.009</size>
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Black</material>
- </visual>
+ <size>0.036 0.036 0.009</size>
+ <mesh>unit_cylinder</mesh>
+ <material>Gazebo/Black</material>
+ </visual>
- </geom:cylinder>
+ </geom:cylinder>
- <geom:cylinder name="laser_cylinder3">
- <xyz>0.0 0.0 0.063</xyz>
+ <geom:cylinder name="laser_cylinder3">
+ <xyz>0.0 0.0 0.063</xyz>
+ <rpy>0 0 0</rpy>
+ <size>0.0175 0.008</size>
+ <mass>0.02</mass>
+
+ <visual>
<rpy>0 0 0</rpy>
- <size>0.0175 0.008</size>
- <mass>0.02</mass>
-
- <visual>
- <rpy>0 0 0</rpy>
- <size>0.035 0.035 0.008</size>
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Shell</material>
- </visual>
+ <size>0.035 0.035 0.008</size>
+ <mesh>unit_cylinder</mesh>
+ <material>Gazebo/Shell</material>
+ </visual>
- </geom:cylinder>
+ </geom:cylinder>
- <sensor:ray name="laser_1">
- <rayCount>68</rayCount>
- <rangeCount>683</rangeCount>
- <laserCount>1</laserCount>
- <origin>0.0 0.0 0.05</origin>
+ <sensor:ray name="laser_1">
+ <rayCount>68</rayCount>
+ <rangeCount>683</rangeCount>
+ <laserCount>1</laserCount>
+ <origin>0.0 0.0 0.05</origin>
- <minAngle>-45</minAngle>
- <maxAngle>45</maxAngle>
+ <minAngle>-45</minAngle>
+ <maxAngle>45</maxAngle>
- <minRange>0.05</minRange>
- <maxRange>10.0</maxRange>
+ <minRange>0.05</minRange>
+ <maxRange>10.0</maxRange>
+ <updateRate>10</updateRate>
+ <controller:sicklms200_laser name="laser_controller_1">
+ <interface:laser name="laser_iface_1"/>
<updateRate>10</updateRate>
- <controller:sicklms200_laser name="laser_controller_1">
- <interface:laser name="laser_iface_1"/>
- <updateRate>10</updateRate>
- </controller:sicklms200_laser>
- </sensor:ray>
- <!--
- -->
- <!--
- <sensor:ray2 name="laser_1">
- <rayCount>683</rayCount>
- <rangeCount>683</rangeCount>
- <laserCount>1</laserCount>
- <origin>0.0 0.0 0.05</origin>
+ </controller:sicklms200_laser>
+ </sensor:ray>
+ <!--
+ -->
+ <!--
+ <sensor:ray2 name="laser_1">
+ <rayCount>683</rayCount>
+ <rangeCount>683</rangeCount>
+ <laserCount>1</laserCount>
+ <origin>0.0 0.0 0.05</origin>
- <minAngle>-135</minAngle>
- <maxAngle>135</maxAngle>
+ <minAngle>-135</minAngle>
+ <maxAngle>135</maxAngle>
- <minRange>0.05</minRange>
- <maxRange>10.0</maxRange>
- <controller:sick2lms200_laser name="laser_controller_1">
- <interface:laser name="laser_iface_1"/>
- <updateRate>10</updateRate>
- </controller:sick2lms200_laser>
- </sensor:ray2>
- -->
+ <minRange>0.05</minRange>
+ <maxRange>10.0</maxRange>
+ <controller:sick2lms200_laser name="laser_controller_1">
+ <interface:laser name="laser_iface_1"/>
+ <updateRate>10</updateRate>
+ </controller:sick2lms200_laser>
+ </sensor:ray2>
+ -->
- </body:box>
+ </body:box>
- <!-- attach hokuyo to torso -->
- <joint:hinge name="hokuyo_pitch_joint">
- <body1>neck_body</body1>
- <body2>laser_body</body2>
- <anchor>laser_body</anchor>
- <axis> 0.0 1.0 0.0 </axis>
- </joint:hinge>
+ <!-- attach hokuyo to torso -->
+ <joint:hinge name="hokuyo_pitch_joint">
+ <body1>neck_body</body1>
+ <body2>laser_body</body2>
+ <anchor>laser_body</anchor>
+ <axis> 0.0 1.0 0.0 </axis>
+ </joint:hinge>
- <controller:pr2_actarray name="pr2_head_controller" plugin="libPr2_Actarray.so">
- <updateRate>100.0</updateRate>
- <joint name="neck_yaw_joint">
- <saturationTorque>100</saturationTorque>
- <pGain>1</pGain>
- <dGain>0</dGain>
- <iGain>0</iGain>
- <iClamp>0</iClamp>
- </joint>
- <joint name="head_tilt_joint">
- <saturationTorque>10</saturationTorque>
- <pGain>1</pGain>
- <dGain>0</dGain>
- <iGain>0</iGain>
- <iClamp>0</iClamp>
- </joint>
- <joint name="hokuyo_pitch_joint">
- <saturationTorque>1000</saturationTorque>
- <pGain>1</pGain>
- <dGain>0</dGain>
- <iGain>0</iGain>
- <iClamp>0</iClamp>
- </joint>
- <joint name="head_cam_left_yaw_joint">
- <saturationTorque>100</saturationTorque>
- <pGain>1</pGain>
- <dGain>0</dGain>
- <iGain>0</iGain>
- <iClamp>0</iClamp>
- </joint>
- <joint name="head_cam_left_pitch_joint">
- <saturationTorque>100</saturationTorque>
- <pGain>1</pGain>
- <dGain>0</dGain>
- <iGain>0</iGain>
- <iClamp>0</iClamp>
- </joint>
- <joint name="head_cam_right_yaw_joint">
- <saturationTorque>100</saturationTorque>
- <pGain>1</pGain>
- <dGain>0</dGain>
- <iGain>0</iGain>
- <iClamp>0</iClamp>
- </joint>
- <joint name="head_cam_right_pitch_joint">
- <saturationTorque>100</saturationTorque>
- <pGain>1</pGain>
- <dGain>0</dGain>
- <iGain>0</iGain>
- <iClamp>0</iClamp>
- </joint>
- <interface:pr2array name="pr2_head_iface"/>
- </controller:pr2_actarray>
-
-
- </model:physical>
-
-
-
-
<!--=========================== caster ===========================-->
-
-
<!--=========================== caster body ===========================-->
<body:box name="front_left_caster_body">
<xyz>0.425 0.25 0.16069 </xyz> <!-- caster height + height off ground of base bottom -->
@@ -1787,6 +1720,29 @@
<!-- PR2_ACTARRAY -->
<controller:pr2_actarray name="pr2_controller" plugin="libPr2_Actarray.so">
<updateRate>100.0</updateRate>
+ <!-- head and above array -->
+ <joint name="neck_yaw_joint">
+ <saturationTorque>100</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>0</iClamp>
+ </joint>
+ <joint name="head_tilt_joint">
+ <saturationTorque>10</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>0</iClamp>
+ </joint>
+ <joint name="hokuyo_pitch_joint">
+ <saturationTorque>500</saturationTorque>
+ <pGain>2</pGain>
+ <dGain>0</dGain>
+ <iGain>1</iGain>
+ <iClamp>20</iClamp>
+ </joint>
+ <!-- caster array -->
<joint name="front_left_caster_steer">
<saturationTorque>1000</saturationTorque>
<pGain>2</pGain>
@@ -2024,6 +1980,22 @@
<interface:position name="p3d_base_position"/>
</controller:P3D>
+ <!-- ptz camera controls -->
+ <controller:generic_ptz name="ptz_cam_left_controller">
+ <updateRate>15.0</updateRate> <!-- rate not implemented in gazebo yet-->
+ <panJoint>ptz_cam_left_pitch_joint</panJoint>
+ <tiltJoint>ptz_cam_left_yaw_joint</tiltJoint>
+ <interface:ptz name="ptz_left_iface" />
+ </controller:generic_ptz>
+ <controller:generic_ptz name="ptz_cam_right_controller">
+ <updateRate>15.0</updateRate> <!-- rate not implemented in gazebo yet-->
+ <panJoint>ptz_cam_right_pitch_joint</panJoint>
+ <tiltJoint>ptz_cam_right_yaw_joint</tiltJoint>
+ <interface:ptz name="ptz_right_iface" />
+ </controller:generic_ptz>
+
+
+
</model:physical>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model 2008-07-23 21:21:46 UTC (rev 1995)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model 2008-07-23 21:35:25 UTC (rev 1996)
@@ -326,6 +326,17 @@
<material>Gazebo/Shell</material>
</visual>
</geom:box>
+ <sensor:camera name="forearm_cam_right_sensor">
+ <imageSize>640 480</imageSize>
+ <hfov>60</hfov>
+ <nearClip>0.1</nearClip>
+ <farClip>100</farClip>
+ <updateRate>15.0</updateRate>
+ <controller:generic_camera name="forearm_cam_right_controller">
+ <updateRate>15.0</updateRate>
+ <interface:camera name="forearm_cam_right_iface" />
+ </controller:generic_camera>
+ </sensor:camera>
</body:box>
<body:cylinder name="wrist_right">
@@ -356,6 +367,17 @@
<material>Gazebo/PioneerBody</material>
</visual>
</geom:cylinder>
+ <sensor:camera name="wrist_cam_right_sensor">
+ <imageSize>640 480</imageSize>
+ <hfov>60</hfov>
+ <nearClip>0.1</nearClip>
+ <farClip>100</farClip>
+ <updateRate>15.0</updateRate>
+ <controller:generic_camera name="wrist_cam_right_controller">
+ <updateRate>15.0</updateRate>
+ <interface:camera name="wrist_cam_right_iface" />
+ </controller:generic_camera>
+ </sensor:camera>
</body:cylinder>
<body:box name="palm_right">
@@ -643,6 +665,17 @@
<material>Gazebo/Shell</material>
</visual>
</geom:box>
+ <sensor:camera name="forearm_cam_left_sensor">
+ <imageSize>640 480</imageSize>
+ <hfov>60</hfov>
+ <nearClip>0.1</nearClip>
+ <farClip>100</farClip>
+ <updateRate>15.0</updateRate>
+ <controller:generic_camera name="forearm_cam_left_controller">
+ <updateRate>15.0</updateRate>
+ <interface:camera name="forearm_cam_left_iface" />
+ </controller:generic_camera>
+ </sensor:camera>
</body:box>
<body:cylinder name="wrist_left">
@@ -673,6 +706,17 @@
<material>Gazebo/PioneerBody</material>
</visual>
</geom:cylinder>
+ <sensor:camera name="wrist_cam_left_sensor">
+ <imageSize>640 480</imageSize>
+ <hfov>60</hfov>
+ <nearClip>0.1</nearClip>
+ <farClip>100</farClip>
+ <updateRate>15.0</updateRate>
+ <controller:generic_camera name="wrist_cam_left_controller">
+ <updateRate>15.0</updateRate>
+ <interface:camera name="wrist_cam_left_iface" />
+ </controller:generic_camera>
+ </sensor:camera>
</body:cylinder>
<body:box name="palm_left">
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_torque.model
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_torque.model 2008-07-23 21:21:46 UTC (rev 1995)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_torque.model 2008-07-23 21:35:25 UTC (rev 1996)
@@ -15,8 +15,6 @@
<xyz>0.0 0.0 0.02</xyz>
<rpy>0.0 0.0 0.0 </rpy>
- <canonicalBody>base_body</canonicalBody>
-
<body:box name="base_body">
<xyz>0.175 0.0 0.15 </xyz> <!-- base bottom is h/2 + some height for wheel clearance-->
<rpy>0.0 0.0 0.0 </rpy>
@@ -57,122 +55,123 @@
<!-- Hokuyo laser body -->
- <model:physical name="base_laser_model">
- <canonicalBody>base_laser_body</canonicalBody>
- <body:box name="base_laser_body">
- <xyz>0.45 0.0 0.27</xyz> <!-- from the robot coordinate system (center bottom of base box)-->
- <rpy>0.0 0.0 0.0</rpy>
-
- <geom:box name="base_laser_box">
- <xyz>0.0 0.0 0.02</xyz>
- <rpy>0 0 0</rpy>
- <size>0.05 0.05 0.041</size>
- <mass>0.12</mass>
+ <body:box name="base_laser_body">
+ <xyz>0.45 0.0 0.27</xyz> <!-- from the robot coordinate system (center bottom of base box)-->
+ <rpy>0.0 0.0 0.0</rpy>
+
+ <geom:box name="base_laser_box">
+ <xyz>0.0 0.0 0.02</xyz>
+ <rpy>0 0 0</rpy>
+ <size>0.05 0.05 0.041</size>
+ <mass>0.12</mass>
- <visual>
- <scale>0.05 0.05 0.041</scale>
- <mesh>unit_box</mesh>
- <material>Gazebo/Blue</material>
- </visual>
+ <visual>
+ <scale>0.05 0.05 0.041</scale>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Blue</material>
+ </visual>
- </geom:box>
- <geom:cylinder name="base_laser_cylinder1">
- <xyz>0.0 0.0 0.041</xyz>
+ </geom:box>
+ <geom:cylinder name="base_laser_cylinder1">
+ <xyz>0.0 0.0 0.041</xyz>
+ <rpy>0 0 0</rpy>
+ <size>0.02 0.013</size>
+ <mass>0.02</mass>
+
+ <visual>
<rpy>0 0 0</rpy>
- <size>0.02 0.013</size>
- <mass>0.02</mass>
-
- <visual>
- <rpy>0 0 0</rpy>
- <scale>0.04 0.04 0.013</scale>
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Black</material>
- </visual>
+ <scale>0.04 0.04 0.013</scale>
+ <mesh>unit_cylinder</mesh>
+ <material>Gazebo/Black</material>
+ </visual>
- </geom:cylinder>
+ </geom:cylinder>
- <geom:cylinder name="base_laser_cylinder2">
- <xyz>0.0 0.0 0.054</xyz>
+ <geom:cylinder name="base_laser_cylinder2">
+ <xyz>0.0 0.0 0.054</xyz>
+ <rpy>0 0 0</rpy>
+ <size>0.018 0.009</size>
+ <mass>0.02</mass>
+
+ <visual>
<rpy>0 0 0</rpy>
- <size>0.018 0.009</size>
- <mass>0.02</mass>
-
- <visual>
- <rpy>0 0 0</rpy>
- <size>0.036 0.036 0.009</size>
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Black</material>
- </visual>
+ <size>0.036 0.036 0.009</size>
+ <mesh>unit_cylinder</mesh>
+ <material>Gazebo/Black</material>
+ </visual>
- </geom:cylinder>
+ </geom:cylinder>
- <geom:cylinder name="base_laser_cylinder3">
- <xyz>0.0 0.0 0.063</xyz>
+ <geom:cylinder name="base_laser_cylinder3">
+ <xyz>0.0 0.0 0.063</xyz>
+ <rpy>0 0 0</rpy>
+ <size>0.0175 0.008</size>
+ <mass>0.02</mass>
+
+ <visual>
<rpy>0 0 0</rpy>
- <size>0.0175 0.008</size>
- <mass>0.02</mass>
-
- <visual>
- <rpy>0 0 0</rpy>
- <size>0.035 0.035 0.008</size>
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Shell</material>
- </visual>
+ <size>0.035 0.035 0.008</size>
+ <mesh>unit_cylinder</mesh>
+ <material>Gazebo/Shell</material>
+ </visual>
- </geom:cylinder>
+ </geom:cylinder>
- <sensor:ray name="base_laser_1">
- <rayCount>68</rayCount>
- <rangeCount>683</rangeCount>
- <laserCount>1</laserCount>
- <origin>0.0 0.0 0.05</origin>
+ <sensor:ray name="base_laser_1">
+ <rayCount>68</rayCount>
+ <rangeCount>683</rangeCount>
+ <laserCount>1</laserCount>
+ <origin>0.0 0.0 0.05</origin>
- <minAngle>-135</minAngle>
- <maxAngle>135</maxAngle>
+ <minAngle>-135</minAngle>
+ <maxAngle>135</maxAngle>
- <minRange>0.05</minRange>
- <maxRange>10.0</maxRange>
- <updateRate>10.0</updateRate>
- <controller:sicklms200_laser name="base_laser_controller_1">
- <interface:laser name="base_laser_iface_1"/>
- <updateRate>10</updateRate>
- </controller:sicklms200_laser>
- </sensor:ray>
- <!--
- -->
- <!--
- <sensor:ray2 name="laser_1">
- <rayCount>683</rayCount>
- <rangeCount>683</rangeCount>
- <laserCount>1</laserCount>
- <origin>0.0 0.0 0.05</origin>
+ <minRange>0.05</minRange>
+ <maxRange>10.0</maxRange>
+ <updateRate>10.0</updateRate>
+ <controller:sicklms200_laser name="base_laser_controller_1">
+ <interface:laser name="base_laser_iface_1"/>
+ <updateRate>10</updateRate>
+ </controller:sicklms200_laser>
+ </sensor:ray>
+ <!--
+ -->
+ <!--
+ <sensor:ray2 name="laser_1">
+ <rayCount>683</rayCount>
+ <rangeCount>683</rangeCount>
+ <laserCount>1</laserCount>
+ <origin>0.0 0.0 0.05</origin>
- <minAngle>-135</minAngle>
- <maxAngle>135</maxAngle>
+ <minAngle>-135</minAngle>
+ <maxAngle>135</maxAngle>
- <minRange>0.05</minRange>
- <maxRange>10.0</maxRange>
- <controller:sick2lms200_laser name="laser_controller_1">
- <interface:laser name="laser_iface_1"/>
- <updateRate>10</updateRate>
- </controller:sick2lms200_laser>
- </sensor:ray2>
- -->
+ <minRange>0.05</minRange>
+ <maxRange>10.0</maxRange>
+ <controller:sick2lms200_laser name="laser_controller_1">
+ <interface:laser name="laser_iface_1"/>
+ <updateRate>10</updateRate>
+ </controller:sick2lms200_laser>
+ </sensor:ray2>
+ -->
- </body:box>
+ </body:box>
- <!-- attach hokuyo to torso -->
- <attach>
- <parentBody>base_body</parentBody>
- <myBody>base_laser_body</myBody>
- </attach>
- </model:physical>
+ <!-- attach hokuyo to torso -->
+ <joint:hinge name="tiltlaser_torso_attach_joint">
+ <body1>base_body</body1>
+ <body2>base_laser_body</body2>
+ <anchor>base_laser_body</anchor>
+ <axis> 1.0 0.0 0.0 </axis>
+ <lowStop> 0.0 </lowStop>
+ <highStop> 0.0 </highStop>
+ </joint:hinge>
+
<!-- ========== spine and shoulder =========== -->
- <canonicalBody>torso_body</canonicalBody>
<body:box name="torso_body">
<xyz>0.056 0.0 0.45</xyz>
@@ -852,475 +851,392 @@
<!-- neck body -->
- <model:physical name="neck_model">
- <body:box name="neck_body">
- <xyz>0.10 0.0 1.03</xyz>
- <rpy>0.0 0.0 0.0</rpy>
- <!--xyz>0.0 0.0 0.0</xyz>
- <rpy>0.0 0.0 0.0</rpy-->
- <geom:box name="neck_geom">
+ <body:box name="neck_body">
+ <xyz>0.10 0.0 1.03</xyz>
+ <rpy>0.0 0.0 0.0</rpy>
+ <!--xyz>0.0 0.0 0.0</xyz>
+ <rpy>0.0 0.0 0.0</rpy-->
+ <geom:box name="neck_geom">
+ <!--size>.20 .38 .14</size-->
+ <size>0.01 0.01 0.01</size>
+ <mass>0.01</mass>
+ <visual>
+ <mesh>unit_box</mesh>
<!--size>.20 .38 .14</size-->
<size>0.01 0.01 0.01</size>
- <mass>0.01</mass>
- <visual>
- <mesh>unit_box</mesh>
- <!--size>.20 .38 .14</size-->
- <size>0.01 0.01 0.01</size>
- <material>Gazebo/Black</material>
- </visual>
- </geom:box>
- </body:box>
+ <material>Gazebo/Black</material>
+ </visual>
+ </geom:box>
+ </body:box>
- <!-- attach neck to torso -->
- <attach>
- <parentBody>torso_body</parentBody>
- <myBody>neck_body</myBody>
- </attach>
+ <!-- attach neck to torso -->
+ <joint:hinge name="neck_torso_attach_joint">
+ <body1>torso_body</body1>
+ <body2>neck_body</body2>
+ <anchor>neck_body</anchor>
+ <axis> 1.0 0.0 0.0 </axis>
+ <lowStop> 0.0 </lowStop>
+ <highStop> 0.0 </highStop>
+ </joint:hinge>
- <canonicalBody>head_base_body</canonicalBody>
- <body:box name="head_base_body">
- <xyz>0.10 0.0 1.13 </xyz>
- <rpy>0.0 0.0 0.0 </rpy>
- <!-- report when this body collides with static bodies -->
- <reportStaticCollision>true</reportStaticCollision>
- <geom:box name="head_base_geom">
- <xyz> 0.0 0.00 0.000</xyz>
+ <body:box name="head_base_body">
+ <xyz>0.10 0.0 1.13 </xyz>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <!-- report when this body collides with static bodies -->
+ <reportStaticCollision>true</reportStaticCollision>
+ <geom:box name="head_base_geom">
+ <xyz> 0.0 0.00 0.000</xyz>
+ <size>0.165 0.21 0.003</size>
+ <mass>0.1</mass>
+ <mul>1</mul>
+ <!--color> 0.0 1.0 0.0</color-->
+ <visual>
<size>0.165 0.21 0.003</size>
- <mass>0.1</mass>
- <mul>1</mul>
- <!--color> 0.0 1.0 0.0</color-->
- <visual>
- <size>0.165 0.21 0.003</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/PioneerBody</material>
- </visual>
- </geom:box>
- </body:box>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/PioneerBody</material>
+ </visual>
+ </geom:box>
+ </body:box>
- <canonicalBody>head_tilt_body</canonicalBody>
- <body:box name="head_tilt_body">
- <xyz>0.10 0.0 1.07 </xyz>
- <rpy>0.0 0.0 0.0 </rpy>
- <!-- report when this body collides with static bodies -->
- <reportStaticCollision>true</reportStaticCollision>
- <geom:box name="head_tilt_geom">
- <xyz> 0.0 0.00 0.150</xyz>
+ <body:box name="head_tilt_body">
+ <xyz>0.10 0.0 1.07 </xyz>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <!-- report when this body collides with static bodies -->
+ <reportStaticCollision>true</reportStaticCollision>
+ <geom:box name="head_tilt_geom">
+ <xyz> 0.0 0.00 0.150</xyz>
+ <size>0.165 0.21 0.003</size>
+ <mass>0.1</mass>
+ <mul>1</mul>
+ <!--color> 0.0 1.0 0.0</color-->
+ <visual>
<size>0.165 0.21 0.003</size>
- <mass>0.1<...
[truncated message content] |
