Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[1906] pkg/trunk
|
From: <MP...@us...> - 2008-07-22 19:44:49
|
Revision: 1906
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1906&view=rev
Author: MPPics
Date: 2008-07-22 19:44:56 +0000 (Tue, 22 Jul 2008)
Log Message:
-----------
Fixes to rosgazebo and image viewing.
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
pkg/trunk/drivers/robot/pr2/libpr2API/src/test/test_FK.cpp
pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp
pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h
pkg/trunk/simulators/rosgazebo/manifest.xml
pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
pkg/trunk/visualization/irrlicht_viewer/ILClient.cc
pkg/trunk/visualization/irrlicht_viewer/cloudGenerator.cc
pkg/trunk/visualization/pr2_gui/src/Vis3d.h
pkg/trunk/visualization/pr2_gui/src/launcherimpl.cpp
pkg/trunk/visualization/pr2_gui/src/launcherimpl.h
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-07-22 19:04:16 UTC (rev 1905)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-07-22 19:44:56 UTC (rev 1906)
@@ -334,9 +334,9 @@
{
NEWMAT::Matrix g(4,4);
g = myHead.ComputeFK(rS.headJntAngles,id-JointStart[HEAD]+1);
- g(1,4) = g(1,4) + BASE_HEAD_OFFSET.x;
- g(2,4) = g(2,4) + BASE_HEAD_OFFSET.y;
- g(3,4) = g(3,4) + BASE_HEAD_OFFSET.z;
+ g(1,4) = g(1,4) + BASE_TORSO_OFFSET.x + TORSO_HEAD_OFFSET.x;
+ g(2,4) = g(2,4) + BASE_TORSO_OFFSET.y + TORSO_HEAD_OFFSET.y;
+ g(3,4) = g(3,4) + BASE_TORSO_OFFSET.z + TORSO_HEAD_OFFSET.z;
return g;
};
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/src/test/test_FK.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/test/test_FK.cpp 2008-07-22 19:04:16 UTC (rev 1905)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/test/test_FK.cpp 2008-07-22 19:44:56 UTC (rev 1906)
@@ -62,11 +62,10 @@
unsigned long time_now_nsec;
// get time from robot
-/* myPR2.InitializeRobot();
- myPR2.GetTime(&time_now);
- time_now_sec = (unsigned long)floor(time_now);
- time_now_nsec = (unsigned long)floor( 1e9 * ( time_now - (double)time_now_sec) );
-*/
+ // myPR2.InitializeRobot();
+ // myPR2.GetTime(&time_now);
+ // time_now_sec = (unsigned long)floor(time_now);
+ // time_now_nsec = (unsigned long)floor( 1e9 * ( time_now - (double)time_now_sec) );
// unsigned long time_now_sec = 0;
// unsigned long time_now_nsec = 0;
@@ -81,24 +80,36 @@
for(ii = (int) PR2::CASTER_FL_STEER; ii <= (int) PR2::CASTER_RR_DRIVE_R; ii++)
{
myPR2.ComputeBodyPose((PR2::PR2_JOINT_ID)ii,rS,&x,&y,&z,&roll,&pitch,&yaw);
-// tfServer.sendEuler((unsigned int) ii,(unsigned int) PR2::PR2_WORLD,x,y,z,yaw,pitch,roll,time_now_sec,time_now_nsec);
- tfServer.sendEuler((unsigned int) ii,(unsigned int) PR2::PR2_WORLD,x,y,z,yaw,pitch,roll,0,0);
- }
+ // tfServer.sendEuler((unsigned int) ii,(unsigned int) PR2::PR2_WORLD,x,y,z,yaw,pitch,roll,time_now_sec,time_now_nsec);
+ //tfServer.sendEuler((unsigned int) ii+1,(unsigned int) PR2::PR2_WORLD+1,x,y,z,yaw,pitch,roll,0,0);
+ tfServer.sendEuler((unsigned int) ii+1,(unsigned int) PR2::PR2_WORLD+1,ii,0,0,0,0,0,0,0);
+ cout << "Base:: " << ii << endl;
+ cout << "pos:: (" << x << "," << y << "," << z << ")" << endl;
+ cout << "rot:: (" << roll << "," << pitch << "," << yaw << ")" << endl;
+ }
// Publish the arms
for(ii = (int) PR2::ARM_L_PAN; ii <= (int) PR2::ARM_R_WRIST_ROLL; ii++)
{
myPR2.ComputeBodyPose((PR2::PR2_JOINT_ID)ii,rS,&x,&y,&z,&roll,&pitch,&yaw);
-// tfServer.sendEuler((unsigned int) ii,(unsigned int) PR2::PR2_WORLD,x,y,z,yaw,pitch,roll,time_now_sec,time_now_nsec);
- tfServer.sendEuler((unsigned int) ii,(unsigned int) PR2::PR2_WORLD,x,y,z,yaw,pitch,roll,0,0);
- }
+ // tfServer.sendEuler((unsigned int) ii,(unsigned int) PR2::PR2_WORLD,x,y,z,yaw,pitch,roll,time_now_sec,time_now_nsec);
+ //tfServer.sendEuler((unsigned int) ii+1,(unsigned int) PR2::PR2_WORLD+1,x,y,z,yaw,pitch,roll,0,0);
+ tfServer.sendEuler((unsigned int) ii+1,(unsigned int) PR2::PR2_WORLD+1,0,0,0,0,0,0,0,0);
+ cout << "Arm:: " << ii << endl;
+ cout << "pos:: (" << x << "," << y << "," << z << ")" << endl;
+ cout << "rot:: (" << roll << "," << pitch << "," << yaw << ")" << endl;
+ }
// Publish the head
for(ii = (int) PR2::HEAD_YAW; ii <= (int) PR2::HEAD_PITCH; ii++)
{
myPR2.ComputeBodyPose((PR2::PR2_JOINT_ID)ii,rS,&x,&y,&z,&roll,&pitch,&yaw);
-// tfServer.sendEuler((unsigned int) ii,(unsigned int) PR2::PR2_WORLD,x,y,z,yaw,pitch,roll,time_now_sec,time_now_nsec);
- tfServer.sendEuler((unsigned int) ii,(unsigned int) PR2::PR2_WORLD,x,y,z,yaw,pitch,roll,0,0);
+ // tfServer.sendEuler((unsigned int) ii,(unsigned int) PR2::PR2_WORLD,x,y,z,yaw,pitch,roll,time_now_sec,time_now_nsec);
+ //tfServer.sendEuler((unsigned int) ii+1,(unsigned int) PR2::PR2_WORLD+1,x,y,z,yaw,pitch,roll,0,0);
+ tfServer.sendEuler((unsigned int) ii+1,(unsigned int) PR2::PR2_WORLD+1,0,0,0,0,0,0,0,0);
+ cout << "Head:: " << ii << endl;
+ cout << "pos:: (" << x << "," << y << "," << z << ")" << endl;
+ cout << "rot:: (" << roll << "," << pitch << "," << yaw << ")" << endl;
}
usleep(1000000);
cout << "Publishing:: " << endl;
Modified: pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp 2008-07-22 19:04:16 UTC (rev 1905)
+++ pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp 2008-07-22 19:44:56 UTC (rev 1906)
@@ -677,8 +677,8 @@
pr2CameraIface->Lock(1);
*width = (uint32_t)pr2CameraIface->data->width;
*height = (uint32_t)pr2CameraIface->data->height;
- *compression = "jpeg";
- *colorspace = "rgb"; //"mono";
+ *compression = "raw";
+ *colorspace = "rgb24"; //"mono";
*data_size = pr2CameraIface->data->image_size;
// on first pass, the sensor does not update after cameraIface is opened.
@@ -689,7 +689,7 @@
uint32_t buf_size = (*width) * (*height) * (*depth);
// copy the image into local buffer
-#if 0
+#if 1
//buf = (void*)(pr2CameraIface->data->image);
memcpy(buf,pr2CameraIface->data->image,buf_size);
#else
Modified: pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h 2008-07-22 19:04:16 UTC (rev 1905)
+++ pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h 2008-07-22 19:44:56 UTC (rev 1906)
@@ -331,9 +331,9 @@
const point3 TORSO_RIGHT_ARM_PAN_OFFSET = { 0 , -0.1329361, 0 };
const point3 ARM_PAN_SHOULDER_PITCH_OFFSET = {0.1 ,0 ,0.5 }; // FIXME: what is z?
const point3 ARM_SHOULDER_PITCH_ROLL_OFFSET = {0 ,0 ,0 };
- const point3 ARM_SHOULDER_ROLL_ELBOW_PITCH_OFFSET = {0.475 ,0 ,0 };
+ const point3 ARM_SHOULDER_ROLL_ELBOW_PITCH_OFFSET = {0.385 ,0 ,0 };//{0.475 ,0 ,0 };
const point3 ELBOW_PITCH_ELBOW_ROLL_OFFSET = {0 ,0 ,0 };//{0.09085 ,0 ,0 };
- const point3 ELBOW_ROLL_WRIST_PITCH_OFFSET = {0.2237 ,0 ,0 };
+ const point3 ELBOW_ROLL_WRIST_PITCH_OFFSET = {0.3137 ,0 ,0 };
const point3 WRIST_PITCH_WRIST_ROLL_OFFSET = {0 ,0 ,0 };
const point3 WRIST_ROLL_GRIPPER_OFFSET = {0 ,0 ,0 };
const point3 SPINE_RIGHT_ARM_OFFSET = {0.0 ,-0.15 ,0.68 };
@@ -344,6 +344,8 @@
const point3 HEAD_PAN_HEAD_PITCH_OFFSET = {0,0,0};
- const point3 BASE_HEAD_OFFSET = {-.1829361, 0, 0.80981};
+ const point3 TORSO_HEAD_OFFSET = {-.02, 0, 0.80981};
+ const point3 TORSO_TILT_LASER_OFFSET = {.07, 0, .68};
+ const point3 BASE_BASE_LASER_OFFSET = {.26, 0,.28};
}
#endif
Modified: pkg/trunk/simulators/rosgazebo/manifest.xml
===================================================================
--- pkg/trunk/simulators/rosgazebo/manifest.xml 2008-07-22 19:04:16 UTC (rev 1905)
+++ pkg/trunk/simulators/rosgazebo/manifest.xml 2008-07-22 19:44:56 UTC (rev 1906)
@@ -13,6 +13,7 @@
<depend package="std_msgs" />
<depend package="rosthread" />
<depend package="rosTF" />
+ <depend package="gazebo_plugin" />
<depend package="math_utils" />
<depend package="string_utils" />
<depend package="libKDL" />
Modified: pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
===================================================================
--- pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp 2008-07-22 19:04:16 UTC (rev 1905)
+++ pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp 2008-07-22 19:44:56 UTC (rev 1906)
@@ -288,10 +288,12 @@
advertise<std_msgs::Image>("image");
advertise<std_msgs::PointCloudFloat32>("cloud");
advertise<std_msgs::PointCloudFloat32>("full_cloud");
+ advertise<std_msgs::PointCloudFloat32>("cloudStereo");
advertise<std_msgs::Empty>("shutter");
advertise<std_msgs::PR2Arm>("left_pr2arm_pos");
advertise<std_msgs::PR2Arm>("right_pr2arm_pos");
advertise<rostools::Time>("time");
+ advertise<std_msgs::Empty>("transform");
subscribe("cmd_vel", velMsg, &RosGazeboNode::cmdvelReceived);
subscribe("cmd_leftarmconfig", leftarm, &RosGazeboNode::cmd_leftarmconfigReceived);
@@ -402,7 +404,7 @@
tmp_cloud_pt.z = tmp_range * cos(laser_yaw) * sin(laser_pitch);
// add gaussian noise
- const double sigma = 0.002; // 2 millimeters sigma
+ const double sigma = 0.002; // 2 millimeter sigma
tmp_cloud_pt.x = tmp_cloud_pt.x + GaussianKernel(0,sigma);
tmp_cloud_pt.y = tmp_cloud_pt.y + GaussianKernel(0,sigma);
tmp_cloud_pt.z = tmp_cloud_pt.z + GaussianKernel(0,sigma);
@@ -695,31 +697,34 @@
roll,
odomMsg.header.stamp.sec,
odomMsg.header.stamp.nsec);
+ //std::cout << "base y p r " << yaw << " " << pitch << " " << roll << std::endl;
// base = center of the bottom of the base box
// torso = midpoint of bottom of turrets
tf.sendEuler(PR2::FRAMEID_TORSO,
PR2::FRAMEID_BASE,
- PR2::BASE_LEFT_ARM_OFFSET.x,
+ PR2::BASE_TORSO_OFFSET.x,
+ PR2::BASE_TORSO_OFFSET.y,
+ PR2::BASE_TORSO_OFFSET.z, /* FIXME: spine elevator not accounted for */
0.0,
- PR2::BASE_LEFT_ARM_OFFSET.z, /* FIXME: spine elevator not accounted for */
0.0,
0.0,
- 0.0,
odomMsg.header.stamp.sec,
odomMsg.header.stamp.nsec);
// arm_l_turret = bottom of left turret
tf.sendEuler(PR2::FRAMEID_ARM_L_TURRET,
PR2::FRAMEID_TORSO,
- 0.0,
- PR2::BASE_LEFT_ARM_OFFSET.y,
- 0.0,
+ PR2::TORSO_LEFT_ARM_PAN_OFFSET.x,
+ PR2::TORSO_LEFT_ARM_PAN_OFFSET.y,
+ PR2::TORSO_LEFT_ARM_PAN_OFFSET.z,
larm.turretAngle,
+ //0.0,
0.0,
0.0,
odomMsg.header.stamp.sec,
odomMsg.header.stamp.nsec);
+ //std::cout << "left pan angle " << larm.turretAngle << std::endl;
// arm_l_shoulder = center of left shoulder pitch bracket
tf.sendEuler(PR2::FRAMEID_ARM_L_SHOULDER,
@@ -821,9 +826,9 @@
// arm_r_turret = bottom of right turret
tf.sendEuler(PR2::FRAMEID_ARM_R_TURRET,
PR2::FRAMEID_TORSO,
- 0.0,
- PR2::BASE_RIGHT_ARM_OFFSET.y,
- 0.0,
+ PR2::TORSO_RIGHT_ARM_PAN_OFFSET.x,
+ PR2::TORSO_RIGHT_ARM_PAN_OFFSET.y,
+ PR2::TORSO_RIGHT_ARM_PAN_OFFSET.z,
rarm.turretAngle,
0.0,
0.0,
@@ -929,12 +934,12 @@
// FIXME: not implemented
tf.sendEuler(PR2::FRAMEID_HEAD_PAN_BASE,
PR2::FRAMEID_TORSO,
+ PR2::TORSO_HEAD_OFFSET.x,
0.0,
+ PR2::TORSO_HEAD_OFFSET.z,
0.0,
- 1.0,
0.0,
0.0,
- 0.0,
odomMsg.header.stamp.sec,
odomMsg.header.stamp.nsec);
@@ -964,10 +969,10 @@
// FIXME: not implemented
tf.sendEuler(PR2::FRAMEID_TILT_LASER_BLOCK,
- PR2::FRAMEID_BASE,
- 0.035,
+ PR2::FRAMEID_TORSO,
+ PR2::TORSO_TILT_LASER_OFFSET.x,
0.0,
- 0.26,
+ PR2::TORSO_TILT_LASER_OFFSET.z,
0.0,
0.0,
0.0,
@@ -977,9 +982,9 @@
// FIXME: not implemented
tf.sendEuler(PR2::FRAMEID_BASE_LASER_BLOCK,
PR2::FRAMEID_BASE,
- 0.035,
+ PR2::BASE_BASE_LASER_OFFSET.x,
0.0,
- 0.26,
+ PR2::BASE_BASE_LASER_OFFSET.z,
0.0,
0.0,
0.0,
@@ -1001,9 +1006,9 @@
PR2::BASE_BODY_OFFSETS[0].x,
PR2::BASE_BODY_OFFSETS[0].y,
PR2::BASE_BODY_OFFSETS[0].z,
+ tmpSteerFL,
0.0,
0.0,
- tmpSteerFL,
odomMsg.header.stamp.sec,
odomMsg.header.stamp.nsec);
tf.sendEuler(PR2::FRAMEID_CASTER_FL_WHEEL_L,
@@ -1032,9 +1037,9 @@
PR2::BASE_BODY_OFFSETS[3].x,
PR2::BASE_BODY_OFFSETS[3].y,
PR2::BASE_BODY_OFFSETS[3].z,
+ tmpSteerFR,
0.0,
0.0,
- tmpSteerFR,
odomMsg.header.stamp.sec,
odomMsg.header.stamp.nsec);
tf.sendEuler(PR2::FRAMEID_CASTER_FR_WHEEL_L,
@@ -1063,9 +1068,9 @@
PR2::BASE_BODY_OFFSETS[6].x,
PR2::BASE_BODY_OFFSETS[6].y,
PR2::BASE_BODY_OFFSETS[6].z,
+ tmpSteerRL,
0.0,
0.0,
- tmpSteerRL,
odomMsg.header.stamp.sec,
odomMsg.header.stamp.nsec);
tf.sendEuler(PR2::FRAMEID_CASTER_RL_WHEEL_L,
@@ -1094,9 +1099,9 @@
PR2::BASE_BODY_OFFSETS[9].x,
PR2::BASE_BODY_OFFSETS[9].y,
PR2::BASE_BODY_OFFSETS[9].z,
+ tmpSteerRR,
0.0,
0.0,
- tmpSteerRR,
odomMsg.header.stamp.sec,
odomMsg.header.stamp.nsec);
tf.sendEuler(PR2::FRAMEID_CASTER_RR_WHEEL_L,
@@ -1121,7 +1126,7 @@
odomMsg.header.stamp.nsec);
-
+ publish("transform",this->shutterMsg);
this->lock.unlock();
}
Modified: pkg/trunk/visualization/irrlicht_viewer/ILClient.cc
===================================================================
--- pkg/trunk/visualization/irrlicht_viewer/ILClient.cc 2008-07-22 19:04:16 UTC (rev 1905)
+++ pkg/trunk/visualization/irrlicht_viewer/ILClient.cc 2008-07-22 19:44:56 UTC (rev 1906)
@@ -62,7 +62,8 @@
s_pRenderer = NULL;
pthread_mutex_destroy(s_pSingletonMutex);
delete s_pSingletonMutex;
- } else {
+ }
+ else {
// Release the lock
pthread_mutex_unlock(s_pSingletonMutex);
}
Modified: pkg/trunk/visualization/irrlicht_viewer/cloudGenerator.cc
===================================================================
--- pkg/trunk/visualization/irrlicht_viewer/cloudGenerator.cc 2008-07-22 19:04:16 UTC (rev 1905)
+++ pkg/trunk/visualization/irrlicht_viewer/cloudGenerator.cc 2008-07-22 19:44:56 UTC (rev 1906)
@@ -1,5 +1,6 @@
#include <ros/node.h>
#include <rostime/clock.h>
+//#include <rostools/Time.h>
#include <std_msgs/PointCloudFloat32.h>
#include <std_msgs/Empty.h>
Modified: pkg/trunk/visualization/pr2_gui/src/Vis3d.h
===================================================================
--- pkg/trunk/visualization/pr2_gui/src/Vis3d.h 2008-07-22 19:04:16 UTC (rev 1905)
+++ pkg/trunk/visualization/pr2_gui/src/Vis3d.h 2008-07-22 19:44:56 UTC (rev 1906)
@@ -31,8 +31,6 @@
/**
@mainpage
-@htmlinclude manifest.html
-
@b Vis3d is a 3D visualization of the robot's current state and sensor feedback using the Irrlicht rendering engine.
<hr>
Modified: pkg/trunk/visualization/pr2_gui/src/launcherimpl.cpp
===================================================================
--- pkg/trunk/visualization/pr2_gui/src/launcherimpl.cpp 2008-07-22 19:04:16 UTC (rev 1905)
+++ pkg/trunk/visualization/pr2_gui/src/launcherimpl.cpp 2008-07-22 19:44:56 UTC (rev 1906)
@@ -4,7 +4,8 @@
:
launcher( parent )
{
-
+ PTZLCodec = new ImageCodec<std_msgs::Image>(&PTZLImage);
+
wxInitAllImageHandlers();
LeftDock_FGS->Hide(HeadLaser_RB,true);
LeftDock_FGS->Hide(Visualization_SBS,true);
@@ -259,7 +260,7 @@
tiltPTZL_S->Enable(true);
zoomPTZL_S->Enable(true);
PTZL_B->Enable(true);
- myNode->subscribe("PTZL_image", PTZLImage, &LauncherImpl::incomingPTZLImageConn,this);
+ myNode->subscribe("image", PTZLImage, &LauncherImpl::incomingPTZLImageConn,this);
myNode->subscribe("PTZL_state", PTZL_state, &LauncherImpl::incomingPTZLState,this);
myNode->advertise<std_msgs::PTZActuatorCmd>("PTZL_cmd");
@@ -297,19 +298,22 @@
{
if(PTZL_GET_NEW_IMAGE)
{
- PTZL_GET_NEW_IMAGE = false;
- const uint32_t count = PTZLImage.get_data_size();
- delete PTZLImageData;
+ PTZL_GET_NEW_IMAGE = false;
+ if(PTZLImage.compression == string("raw"))
+ {
+ PTZLCodec->inflate();
+ PTZLImage.compression = string("jpeg");
+ if(!PTZLCodec->deflate(100))
+ return;
+ }
+ const uint32_t count = PTZLImage.get_data_size();
+ delete PTZLImageData;
PTZLImageData = new uint8_t[count];
memcpy(PTZLImageData, PTZLImage.data, sizeof(uint8_t) * count);
wxMemoryInputStream mis(PTZLImageData,PTZLImage.get_data_size());
delete PTZL_im;
- PTZL_im = new wxImage(mis,wxBITMAP_TYPE_JPEG,-1);
-
- //Event stuff
- wxCommandEvent PTZL_Event(wxEVT_COMMAND_BUTTON_CLICKED, PTZL_B->GetId());
- PTZL_Event.SetEventObject(this);
- this->AddPendingEvent(PTZL_Event);
+ PTZL_im = new wxImage(mis,wxBITMAP_TYPE_ANY,-1);
+
if(PTZL_bmp == NULL){
std::cout << "Layout\n";
wxSize size(PTZLImage.width,PTZLImage.height);
@@ -318,6 +322,11 @@
Layout();
Fit();
}
+
+ //Event stuff
+ wxCommandEvent PTZL_Event(wxEVT_COMMAND_BUTTON_CLICKED, PTZL_B->GetId());
+ PTZL_Event.SetEventObject(this);
+ this->AddPendingEvent(PTZL_Event);
}
//else
//std::cout << "!\n";
@@ -383,19 +392,21 @@
if(PTZR_GET_NEW_IMAGE)
{
PTZR_GET_NEW_IMAGE = false;
+ if(PTZRImage.compression == string("raw"))
+ {
+ PTZRCodec->inflate();
+ PTZRImage.compression = string("jpeg");
+ if(!PTZRCodec->deflate(100))
+ return;
+ }
const uint32_t count = PTZRImage.get_data_size();
delete PTZRImageData;
PTZRImageData = new uint8_t[count];
memcpy(PTZRImageData, PTZRImage.data, sizeof(uint8_t) * count);
wxMemoryInputStream mis(PTZRImageData,PTZRImage.get_data_size());
delete PTZR_im;
- PTZR_im = new wxImage(mis,wxBITMAP_TYPE_JPEG,-1);
-
- //Event stuff
- wxCommandEvent PTZR_Event(wxEVT_COMMAND_BUTTON_CLICKED, PTZR_B->GetId());
- PTZR_Event.SetEventObject(this);
- this->AddPendingEvent(PTZR_Event);
- if(PTZR_bmp == NULL){
+ PTZR_im = new wxImage(mis,wxBITMAP_TYPE_ANY,-1);
+ if(PTZR_bmp == NULL){
std::cout << "Layout\n";
wxSize size(PTZRImage.width,PTZRImage.height);
PTZR_B->SetMinSize(size);
@@ -403,6 +414,10 @@
Layout();
Fit();
}
+ //Event stuff
+ wxCommandEvent PTZR_Event(wxEVT_COMMAND_BUTTON_CLICKED, PTZR_B->GetId());
+ PTZR_Event.SetEventObject(this);
+ this->AddPendingEvent(PTZR_Event);
}
//else
//std::cout << "!\n";
@@ -446,19 +461,21 @@
if(WristL_GET_NEW_IMAGE)
{
WristL_GET_NEW_IMAGE = false;
+ if(WristLImage.compression == string("raw"))
+ {
+ WristLCodec->inflate();
+ WristLImage.compression = string("jpeg");
+ if(!WristLCodec->deflate(100))
+ return;
+ }
const uint32_t count = WristLImage.get_data_size();
delete WristLImageData;
WristLImageData = new uint8_t[count];
memcpy(WristLImageData, WristLImage.data, sizeof(uint8_t) * count);
wxMemoryInputStream mis(WristLImageData,WristLImage.get_data_size());
delete WristL_im;
- WristL_im = new wxImage(mis,wxBITMAP_TYPE_JPEG,-1);
-
- //Event stuff
- wxCommandEvent WristL_Event(wxEVT_COMMAND_BUTTON_CLICKED, WristL_B->GetId());
- WristL_Event.SetEventObject(this);
- this->AddPendingEvent(WristL_Event);
- if(WristL_bmp == NULL){
+ WristL_im = new wxImage(mis,wxBITMAP_TYPE_ANY,-1);
+ if(WristL_bmp == NULL){
std::cout << "Layout\n";
wxSize size(WristLImage.width,WristLImage.height);
WristL_B->SetMinSize(size);
@@ -466,6 +483,11 @@
Layout();
Fit();
}
+ //Event stuff
+ wxCommandEvent WristL_Event(wxEVT_COMMAND_BUTTON_CLICKED, WristL_B->GetId());
+ WristL_Event.SetEventObject(this);
+ this->AddPendingEvent(WristL_Event);
+
}
//else
//std::cout << "!\n";
@@ -510,19 +532,21 @@
if(WristR_GET_NEW_IMAGE)
{
WristR_GET_NEW_IMAGE = false;
+ if(WristRImage.compression == string("raw"))
+ {
+ WristRCodec->inflate();
+ WristRImage.compression = string("jpeg");
+ if(!WristRCodec->deflate(100))
+ return;
+ }
const uint32_t count = WristRImage.get_data_size();
delete WristRImageData;
WristRImageData = new uint8_t[count];
memcpy(WristRImageData, WristRImage.data, sizeof(uint8_t) * count);
wxMemoryInputStream mis(WristRImageData,WristRImage.get_data_size());
delete WristR_im;
- WristR_im = new wxImage(mis,wxBITMAP_TYPE_JPEG,-1);
-
- //Event stuff
- wxCommandEvent WristR_Event(wxEVT_COMMAND_BUTTON_CLICKED, WristR_B->GetId());
- WristR_Event.SetEventObject(this);
- this->AddPendingEvent(WristR_Event);
- if(WristR_bmp == NULL){
+ WristR_im = new wxImage(mis,wxBITMAP_TYPE_ANY,-1);
+ if(WristR_bmp == NULL){
std::cout << "Layout\n";
wxSize size(WristRImage.width,WristRImage.height);
WristR_B->SetMinSize(size);
@@ -530,6 +554,11 @@
Layout();
Fit();
}
+ //Event stuff
+ wxCommandEvent WristR_Event(wxEVT_COMMAND_BUTTON_CLICKED, WristR_B->GetId());
+ WristR_Event.SetEventObject(this);
+ this->AddPendingEvent(WristR_Event);
+
}
//else
//std::cout << "!\n";
Modified: pkg/trunk/visualization/pr2_gui/src/launcherimpl.h
===================================================================
--- pkg/trunk/visualization/pr2_gui/src/launcherimpl.h 2008-07-22 19:04:16 UTC (rev 1905)
+++ pkg/trunk/visualization/pr2_gui/src/launcherimpl.h 2008-07-22 19:44:56 UTC (rev 1906)
@@ -1,7 +1,62 @@
#ifndef __launcherimpl__
#define __launcherimpl__
+///////////////////////////////////////////////////////////////////////////////
+//
+// Copyright (C) 2008, Willow Garage Inc.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of Stanford University nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
/**
+@mainpage
+
+@htmlinclude manifest.html
+
+@b Subclass of the Launcher window
+
+<hr>
+
+@section topic ROS topics
+
+Subscribes to (name/type):
+- @b "PTZL_image"/std_msgs::Image : Image received from the left PTZ
+- @b "PTZR_image"/std_msgs::Image : Image received from the right PTZ
+- @b "WristL_image"/std_msgs::Image : Image received from the left wrist camera
+- @b "WristR_image"/std_msgs::Image : Image received from the right wrist camera
+- @b "PTZL_state"/std_msgs::PTZActuatorState : Receives state from the left PTZ
+- @b "PTZR_state"/std_msgs::PTZActuatorState : Receives state from the right PTZ
+
+Publishes to (name/type):
+- @b "PTZR_cmd"/std_msgs::std_msgs::PTZActuatorCmd : Moves the right PTZ to the given position
+- @b "PTZL_cmd"/std_msgs::std_msgs::PTZActuatorCmd : Moves the left PTZ to the given position
+
+
+@todo
+- Turn me into subclasses and change widgets to the ros panels
+**/
+
+/**
@file
Subclass of launcher, which is generated by wxFormBuilder.
*/
@@ -24,67 +79,126 @@
{
protected:
// Handlers for launcher events.
+ ///Enables and disables the head hokuyo point cloud
void startStopHeadPtCld( wxCommandEvent& event );
+ ///Enables and disables the base mounted hokuyo laser scan
void startStopFloorPtCld( wxCommandEvent& event );
+ ///Enables and disables the stereo vision point cloud
void startStopStereoPtCld( wxCommandEvent& event );
+ ///Enables and disables the 3d model
void startStopModel( wxCommandEvent& event );
+ ///Enables and disalbes the UCS
void startStopUCS( wxCommandEvent& event );
+ ///Enables and disables the grid
void startStopGrid( wxCommandEvent& event );
+ ///Enables and disables the user inserted objects
void startStopObjects( wxCommandEvent& event );
+ ///Changes to the selected view
void viewChanged( wxCommandEvent& event );
+ ///Changes the head hokuyo scan type
void HeadLaserChanged( wxCommandEvent& event );
+ ///Enables or disables the 3d visualization
void startStop_Visualization( wxCommandEvent& event );
+ ///Enables or disables the topdown 2d visualization
void startStop_Topdown( wxCommandEvent& event );
+ ///Enables or disables the left PTZ camera
void startStop_PTZL( wxCommandEvent& event );
+ ///Enables or disables the right PTZ camera
void startStop_PTZR( wxCommandEvent& event );
+ ///Enables or disables the left wrist camera
void startStop_WristL( wxCommandEvent& event );
+ ///Enables or disables the right wrist camera
void startStop_WristR( wxCommandEvent& event );
+ //Stops the robot from moving more (not implemented)
void EmergencyStop( wxCommandEvent& event );
+ ///Draws the left PTZ image
void PTZLDrawPic( wxCommandEvent& event );
+ ///Draws the right PTZ image
void PTZRDrawPic( wxCommandEvent& event );
+ ///Draws the right wrist image
void WristRDrawPic( wxCommandEvent& event );
+ ///Draws the left wrist image
void WristLDrawPic( wxCommandEvent& event );
+ ///Writes to the gui's console (instead of the command line)
void consoleOut(wxString Line);
+ ///(Callback) Gets an image for the left PTZ
void incomingPTZLImageConn();
+ ///(Callback) Gets an image for the right PTZ
void incomingPTZRImageConn();
+ ///(Callback) Gets an image for the right wrist
void incomingWristRImageConn();
+ ///(Callback) Gets an image for the left wrist
void incomingWristLImageConn();
+ ///(Callback) Gets an state for the left PTZ
void incomingPTZLState();
+ ///(Callback) Gets an state for the right PTZ
void incomingPTZRState();
+ ///(Publisher) Sends a position command to the right PTZ
void PTZR_ptzChanged(wxScrollEvent& event);
+ ///(Publisher) Sends a position command to the left PTZ
void PTZL_ptzChanged(wxScrollEvent& event);
public:
//Variables
+ ///ROS node
ros::node *myNode;
+ ///Image for the left PTZ
std_msgs::Image PTZLImage;
+ ///Image for the right PTZ
std_msgs::Image PTZRImage;
+ ///Image for the left wrist
std_msgs::Image WristLImage;
+ ///Image for the right wrist
std_msgs::Image WristRImage;
+ ///Command message shared between the PTZs
std_msgs::PTZActuatorCmd ptz_cmd;
+ ///State of the left PTZ
std_msgs::PTZActuatorState PTZL_state;
+ ///State of the right PTZ
std_msgs::PTZActuatorState PTZR_state;
+ ///3d visualization object (irrlicht window)
Vis3d *vis3d_Window;
+ ///Copy of the left PTZ image data
uint8_t *PTZLImageData;
+ ///Copy of the right PTZ image data
uint8_t *PTZRImageData;
+ ///Copy of the left wrist image data
uint8_t *WristLImageData;
+ ///Copy of the right wrist image data
uint8_t *WristRImageData;
+ ///Flag stating a new frame can be displayed for the left PTZ
bool PTZL_GET_NEW_IMAGE;
+ ///Flag stating a new frame can be displayed for the right PTZ
bool PTZR_GET_NEW_IMAGE;
+ ///Flag stating a new frame can be displayed for the right wrist
bool WristR_GET_NEW_IMAGE;
+ ///Flag stating a new frame can be displayed for the left wrist
bool WristL_GET_NEW_IMAGE;
+ ///wx bitmap object necessary for displaying camera/pictures
wxBitmap *PTZL_bmp;
+ ///wx bitmap object necessary for displaying camera/pictures
wxBitmap *PTZR_bmp;
+ ///wx bitmap object necessary for displaying camera/pictures
wxBitmap *WristR_bmp;
+ ///wx bitmap object necessary for displaying camera/pictures
wxBitmap *WristL_bmp;
+ ///wx image object necessary for displaying camera/pictures
wxImage *PTZL_im;
+ ///wx image object necessary for displaying camera/pictures
wxImage *PTZR_im;
+ ///wx image object necessary for displaying camera/pictures
wxImage *WristR_im;
+ ///wx image object necessary for displaying camera/pictures
wxImage *WristL_im;
+
+ ImageCodec<std_msgs::Image> *PTZLCodec;
+ ImageCodec<std_msgs::Image> *PTZRCodec;
+ ImageCodec<std_msgs::Image> *WristLCodec;
+ ImageCodec<std_msgs::Image> *WristRCodec;
/** Constructor */
LauncherImpl( wxWindow* parent );
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
