Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[2613] pkg/trunk
|
From: <tjh...@us...> - 2008-08-06 00:45:02
|
Revision: 2613
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2613&view=rev
Author: tjhunter
Date: 2008-08-06 00:45:10 +0000 (Wed, 06 Aug 2008)
Log Message:
-----------
Added a RosJoint class that exposes the joints as ROS nodes. See RosGazeboNode for enabling in
Modified Paths:
--------------
pkg/trunk/deprecated/rosControllers/CMakeLists.txt
pkg/trunk/deprecated/rosControllers/manifest.xml
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/joint.h
pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h
pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
pkg/trunk/visualization/wxpy_ros/src/wxpy_ros/wxplot.py
Modified: pkg/trunk/deprecated/rosControllers/CMakeLists.txt
===================================================================
--- pkg/trunk/deprecated/rosControllers/CMakeLists.txt 2008-08-06 00:36:17 UTC (rev 2612)
+++ pkg/trunk/deprecated/rosControllers/CMakeLists.txt 2008-08-06 00:45:10 UTC (rev 2613)
@@ -4,4 +4,4 @@
genmsg()
add_definitions(-Wall)
include_directories(msg/cpp)
-rospack_add_library(rosControllers src/RosJointController.cpp)
+rospack_add_library(rosControllers src/RosJointController.cpp src/RosJoint.cpp)
Modified: pkg/trunk/deprecated/rosControllers/manifest.xml
===================================================================
--- pkg/trunk/deprecated/rosControllers/manifest.xml 2008-08-06 00:36:17 UTC (rev 2612)
+++ pkg/trunk/deprecated/rosControllers/manifest.xml 2008-08-06 00:45:10 UTC (rev 2613)
@@ -6,6 +6,7 @@
<depend package="genericControllers" />
<depend package="pr2Controllers" />
<depend package="pr2Core" />
+ <depend package="libpr2HW" />
<depend package="std_msgs" />
<depend package="rosthread" />
<depend package="rosTF" />
Modified: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/joint.h
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/joint.h 2008-08-06 00:36:17 UTC (rev 2612)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/joint.h 2008-08-06 00:45:10 UTC (rev 2613)
@@ -33,6 +33,7 @@
*********************************************************************/
#ifndef JOINT_H
#define JOINT_H
+rewrewrewre
namespace mechanism {
Modified: pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h
===================================================================
--- pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h 2008-08-06 00:36:17 UTC (rev 2612)
+++ pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h 2008-08-06 00:45:10 UTC (rev 2613)
@@ -35,6 +35,9 @@
#include <pr2Controllers/LaserScannerController.h>
#include <pr2Controllers/GripperController.h>
#include <rosControllers/RosJointController.h>
+#include <rosControllers/RosJoint.h>
+//When defined, will expose all the joints of the robot through ros
+// #define EXPOSE_JOINTS
// for rosController
#include <list>
@@ -43,6 +46,12 @@
#include "mechanism_model/joint.h"
// roscpp
#include <ros/node.h>
+// roscpp - laser
+#include <std_msgs/LaserScan.h>
+// roscpp - laser image (point cloud)
+#include <std_msgs/PointCloudFloat32.h>
+#include <std_msgs/Point3DFloat32.h>
+#include <std_msgs/ChannelFloat32.h>
// roscpp - used for shutter message right now
#include <std_msgs/Empty.h>
// roscpp - used for broadcasting time over ros
@@ -55,6 +64,9 @@
#include <pr2_msgs/EndEffectorState.h>
+// roscpp - camera
+#include <std_msgs/Image.h>
+
// for frame transforms
#include <rosTF/rosTF.h>
@@ -72,10 +84,14 @@
private:
// Messages that we'll send or receive
std_msgs::BaseVel velMsg;
+ std_msgs::LaserScan laserMsg;
+ std_msgs::PointCloudFloat32 cloudMsg;
+ std_msgs::PointCloudFloat32 full_cloudMsg;
+ //std_msgs::PointCloudFloat32 transformed_cloudMsg;
+ std_msgs::Empty shutterMsg; // marks end of a cloud message
std_msgs::RobotBase2DOdom odomMsg;
rostools::Time timeMsg;
- std_msgs::Empty shutterMsg; // marks end of a cloud message
-
+
std_msgs::Point3DFloat32 objectPosMsg;
// A mutex to lock access to fields that are used in message callbacks
@@ -160,6 +176,20 @@
// July 24, 2008 - Advait - only right arm is supported
bool reset_IK_guess(rosgazebo::VoidVoid::request &req, rosgazebo::VoidVoid::response &res);
+ // laser range data
+ float ranges[GZ_LASER_MAX_RANGES];
+ uint8_t intensities[GZ_LASER_MAX_RANGES];
+ double cameraTime, baseLaserTime, tiltLaserTime;
+
+ // camera data
+ std_msgs::Image img;
+ std_msgs::Image img_ptz_right;
+ std_msgs::Image img_ptz_left;
+ std_msgs::Image img_wrist_right;
+ std_msgs::Image img_wrist_left;
+ std_msgs::Image img_forearm_right;
+ std_msgs::Image img_forearm_left;
+
// arm joint data
std_msgs::PR2Arm leftarm;
std_msgs::PR2Arm rightarm;
@@ -177,11 +207,26 @@
//Flag set to indicate that we should use new controls architecture
bool useControllerArray;
+ // for the point cloud data
+ ringBuffer<std_msgs::Point3DFloat32> *cloud_pts;
+ ringBuffer<float> *cloud_ch1;
+
+ vector<std_msgs::Point3DFloat32> *full_cloud_pts;
+ vector<float> *full_cloud_ch1;
+
+ // keep count for full cloud
+ int max_cloud_pts;
+ int max_full_cloud_pts;
+
//Keep track of controllers
controller::JointController** ControllerArray;
typedef list<controller::RosJointController *> RCList;
RCList RosControllers;
+
+ // A small API to expose the joints only even when no controller is present
+ typedef list<controller::RosJoint *> RJList;
+ RJList RosJoints;
};
Modified: pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
===================================================================
--- pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp 2008-08-06 00:36:17 UTC (rev 2612)
+++ pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp 2008-08-06 00:45:10 UTC (rev 2613)
@@ -19,6 +19,7 @@
#include <RosGazeboNode/RosGazeboNode.h>
#include <iostream>
+#include <sstream>
void
RosGazeboNode::cmd_rightarmconfigReceived()
@@ -209,6 +210,19 @@
// initialize random seed
srand(time(NULL));
+ // Initialize ring buffer for point cloud data
+ this->cloud_pts = new ringBuffer<std_msgs::Point3DFloat32>();
+ this->cloud_ch1 = new ringBuffer<float>();
+ this->full_cloud_pts = new vector<std_msgs::Point3DFloat32>();
+ this->full_cloud_ch1 = new vector<float>();
+
+ // FIXME: move this to Subscribe Models
+ param("tilting_laser/max_cloud_pts",max_cloud_pts, 683); // number of point in one scan line
+ this->cloud_pts->allocate(this->max_cloud_pts);
+ this->cloud_ch1->allocate(this->max_cloud_pts);
+ this->full_cloud_pts->clear();
+ this->full_cloud_ch1->clear();
+
// initialize times
this->PR2Copy->GetTime(&(this->lastTime));
this->PR2Copy->GetTime(&(this->simTime));
@@ -219,7 +233,7 @@
//Don't use new architecture
useControllerArray = false;
-}
+}
RosGazeboNode::RosGazeboNode(int argc, char** argv, const char* fname,
PR2::PR2Robot *myPR2,
@@ -237,6 +251,19 @@
// initialize random seed
srand(time(NULL));
+ // Initialize ring buffer for point cloud data
+ this->cloud_pts = new ringBuffer<std_msgs::Point3DFloat32>();
+ this->cloud_ch1 = new ringBuffer<float>();
+ this->full_cloud_pts = new vector<std_msgs::Point3DFloat32>();
+ this->full_cloud_ch1 = new vector<float>();
+
+ // FIXME: move this to Subscribe Models
+ param("tilting_laser/max_cloud_pts",max_cloud_pts, 683); // number of point in one scan line
+ this->cloud_pts->allocate(this->max_cloud_pts);
+ this->cloud_ch1->allocate(this->max_cloud_pts);
+ this->full_cloud_pts->clear();
+ this->full_cloud_ch1->clear();
+
// initialize times
this->PR2Copy->GetTime(&(this->lastTime));
this->PR2Copy->GetTime(&(this->simTime));
@@ -250,7 +277,26 @@
//TODO: if you want to advertise some information about joints, this is the place to do it:
// JointController * controller = ...
+#ifdef EXPOSE_JOINTS
+
+ for(int i=0;i<110-75;i++)
+ {
+ std::ostringstream ss;
+ ss<<"JOINT_"<<i;
+ controller::RosJoint * rjoint;
+ rjoint = new controller::RosJoint(&(PR2Copy->hw),PR2::ARM_L_PAN,ss.str());
+ RosJoints.push_back(rjoint);
+ }
+// RosJoints.push_back(rjoint);
+// rjoint = new controller::RosJoint(&(PR2Copy->hw),PR2::ARM_L_ELBOW_PITCH,"ARM_L_ELBOW_PITCH");
+
+#endif
+
// RosControllers.push_back(new RosJointController(controller))
+// for(mechanism::Robot::IndexMap::iterator it=PR2Copy->joints_lookup_.begin() ; it!=PR2Copy->joints_lookup_.begin() ; ++it)
+// {
+// std::cout<<(*it).first<<std::endl;
+// }
}
RosGazeboNode::RosGazeboNode(int argc, char** argv, const char* fname,
@@ -273,6 +319,19 @@
// initialize random seed
srand(time(NULL));
+ // Initialize ring buffer for point cloud data
+ this->cloud_pts = new ringBuffer<std_msgs::Point3DFloat32>();
+ this->cloud_ch1 = new ringBuffer<float>();
+ this->full_cloud_pts = new vector<std_msgs::Point3DFloat32>();
+ this->full_cloud_ch1 = new vector<float>();
+
+ // FIXME: move this to Subscribe Models
+ param("tilting_laser/max_cloud_pts",max_cloud_pts, 683); // number of point in one scan line
+ this->cloud_pts->allocate(this->max_cloud_pts);
+ this->cloud_ch1->allocate(this->max_cloud_pts);
+ this->full_cloud_pts->clear();
+ this->full_cloud_ch1->clear();
+
// initialize times
this->PR2Copy->GetTime(&(this->lastTime));
this->PR2Copy->GetTime(&(this->simTime));
@@ -311,9 +370,23 @@
int
RosGazeboNode::AdvertiseSubscribeMessages()
{
+ //advertise<std_msgs::LaserScan>("laser");
+ advertise<std_msgs::LaserScan>("scan");
+
+ //advertise<std_msgs::Image>("image");
+ advertise<std_msgs::Image>("image_ptz_right");
+ advertise<std_msgs::Image>("image_ptz_left");
+ advertise<std_msgs::Image>("image_wrist_right");
+ advertise<std_msgs::Image>("image_wrist_left");
+ advertise<std_msgs::Image>("image_forearm_right");
+ advertise<std_msgs::Image>("image_forearm_left");
+ advertise<std_msgs::PointCloudFloat32>("cloud");
+ advertise<std_msgs::PointCloudFloat32>("full_cloud");
+
advertise<std_msgs::PointCloudFloat32>("cloudStereo");
advertise<std_msgs::RobotBase2DOdom>("odom");
+ advertise<std_msgs::Empty>("shutter");
advertise<std_msgs::PR2Arm>("left_pr2arm_pos");
advertise<std_msgs::PR2Arm>("right_pr2arm_pos");
advertise<rostools::Time>("time");
@@ -331,7 +404,10 @@
(*it)->AdvertiseSubscribeMessages();
}
- //------ services ----------
+ for(RJList::iterator it = RosJoints.begin(); it != RosJoints.end(); ++it)
+ {
+ (*it)->AdvertiseSubscribeMessages();
+ } //------ services ----------
advertise_service("reset_IK_guess", &RosGazeboNode::reset_IK_guess);
return(0);
@@ -389,6 +465,18 @@
{
this->lock.lock();
+ float angle_min;
+ float angle_max;
+ float angle_increment;
+ float range_max;
+ uint32_t ranges_size;
+ uint32_t ranges_alloc_size;
+ uint32_t intensities_size;
+ uint32_t intensities_alloc_size;
+ std_msgs::Point3DFloat32 local_cloud_pt;
+ std_msgs::Point3DFloat32 global_cloud_pt;
+
+
/***************************************************************/
/* */
/* publish time */
@@ -415,14 +503,128 @@
/* laser - pitching */
/* */
/***************************************************************/
+ if (this->PR2Copy->hw.GetLaserRanges(PR2::LASER_HEAD,
+ &angle_min, &angle_max, &angle_increment,
+ &range_max, &ranges_size , &ranges_alloc_size,
+ &intensities_size, &intensities_alloc_size,
+ this->ranges , this->intensities, &tiltLaserTime) == PR2::PR2_ALL_OK)
+ {
+ for(unsigned int i=0;i<ranges_size;i++)
+ {
+ // get laser pitch angle
+ double laser_yaw, laser_pitch, laser_pitch_rate;
+ this->PR2Copy->hw.GetJointServoActual(PR2::HEAD_LASER_PITCH , &laser_pitch, &laser_pitch_rate);
+ // get laser yaw angle
+ laser_yaw = angle_min + (double)i * angle_increment;
+ //std::cout << " pit " << laser_pitch << "yaw " << laser_yaw
+ // << " amin " << angle_min << " inc " << angle_increment << std::endl;
+ // populating cloud data by range
+ double tmp_range = this->ranges[i];
+ // transform from range to x,y,z
+ local_cloud_pt.x = tmp_range * cos(laser_yaw) * cos(laser_pitch);
+ local_cloud_pt.y = tmp_range * sin(laser_yaw) ; //* cos(laser_pitch);
+ local_cloud_pt.z = tmp_range * cos(laser_yaw) * sin(laser_pitch);
+ // add gaussian noise
+ const double sigma = 0.002; // 2 millimeter sigma
+ local_cloud_pt.x = local_cloud_pt.x + GaussianKernel(0,sigma);
+ local_cloud_pt.y = local_cloud_pt.y + GaussianKernel(0,sigma);
+ local_cloud_pt.z = local_cloud_pt.z + GaussianKernel(0,sigma);
+
+ // get position cheats from simulator
+ //double cheat_x,cheat_y,cheat_z,cheat_roll,cheat_pitch,cheat_yaw;
+ //this->PR2Copy->GetBasePositionActual(&cheat_x,&cheat_y,&cheat_z,&cheat_roll,&cheat_pitch,&cheat_yaw);
+ //global_cloud_pt.x = local_cloud_pt.x + cheat_x;
+ //global_cloud_pt.y = local_cloud_pt.y + cheat_y;
+ //global_cloud_pt.z = local_cloud_pt.z + cheat_z;
+ // apply rotataions yaw, pitch, roll
+
+ // add mixed pixel noise
+ // if this point is some threshold away from last, add mixing model
+
+ // push pcd point into structure
+ this->cloud_pts->add((std_msgs::Point3DFloat32)local_cloud_pt);
+ this->cloud_ch1->add(this->intensities[i]);
+
+ this->full_cloud_pts->push_back((std_msgs::Point3DFloat32)local_cloud_pt);
+ this->full_cloud_ch1->push_back(this->intensities[i]);
+ }
+ /***************************************************************/
+ /* */
+ /* point cloud from laser image */
+ /* */
+ /***************************************************************/
+ //std::cout << " pcd num " << this->cloud_pts->length << std::endl;
+ //
+ this->cloudMsg.header.frame_id = tf.lookup("FRAMEID_TILT_LASER_BLOCK");
+ this->cloudMsg.header.stamp.sec = (unsigned long)floor(this->tiltLaserTime);
+ this->cloudMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->tiltLaserTime - this->cloudMsg.header.stamp.sec) );
+
+ int num_channels = 1;
+ this->cloudMsg.set_pts_size(this->cloud_pts->length);
+ this->cloudMsg.set_chan_size(num_channels);
+ this->cloudMsg.chan[0].name = "intensities";
+ this->cloudMsg.chan[0].set_vals_size(this->cloud_ch1->length);
+
+ for(int i=0;i< this->cloud_pts->length ;i++)
+ {
+ this->cloudMsg.pts[i].x = this->cloud_pts->buffer[i].x;
+ this->cloudMsg.pts[i].y = this->cloud_pts->buffer[i].y;
+ this->cloudMsg.pts[i].z = this->cloud_pts->buffer[i].z;
+ this->cloudMsg.chan[0].vals[i] = this->cloud_ch1->buffer[i];
+ }
+
+ // TODO: does anyone need transform to MAP FRAME directly here? answer: use 3d world model
+ // try
+ // {
+ // transformed_cloudMsg = tfc.transformPointCloud("FRAMEID_MAP", this->cloudMsg);
+ // }
+ // catch(libTF::TransformReference::LookupException& ex)
+ // {
+ // puts("no global->local Tx yet");
+ // return;
+ // }
+ // publish("cloud",transformed_cloudMsg);
+
+ publish("cloud",cloudMsg);
+ }
+
+
/***************************************************************/
/* */
/* laser - base */
/* */
/***************************************************************/
+ if (this->PR2Copy->hw.GetLaserRanges(PR2::LASER_BASE,
+ &angle_min, &angle_max, &angle_increment,
+ &range_max, &ranges_size , &ranges_alloc_size,
+ &intensities_size, &intensities_alloc_size,
+ this->ranges , this->intensities, &baseLaserTime) == PR2::PR2_ALL_OK)
+ {
+ // Get latest laser data
+ this->laserMsg.angle_min = angle_min;
+ this->laserMsg.angle_max = angle_max;
+ this->laserMsg.angle_increment = angle_increment;
+ this->laserMsg.range_max = range_max;
+ this->laserMsg.set_ranges_size(ranges_size);
+ this->laserMsg.set_intensities_size(intensities_size);
+ for(unsigned int i=0;i<ranges_size;i++)
+ {
+ double tmp_range = this->ranges[i];
+ this->laserMsg.ranges[i] =tmp_range;
+ this->laserMsg.intensities[i] = this->intensities[i];
+ }
+ this->laserMsg.header.frame_id = tf.lookup("FRAMEID_BASE_LASER_BLOCK");
+ this->laserMsg.header.stamp.sec = (unsigned long)floor(this->baseLaserTime);
+ this->laserMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->baseLaserTime - this->laserMsg.header.stamp.sec) );
+ //publish("laser",this->laserMsg); // for laser_view FIXME: can alias this at the commandline or launch script
+ publish("scan",this->laserMsg); // for rosstage
+ }
+
+
+
/***************************************************************/
/* */
/* odometry */
@@ -475,16 +677,16 @@
// those values are reused in the sendInverseEuler() call above.
publish("odom",this->odomMsg);
- /***************************************************************/
- /* */
- /* object position */
- /* */
- /***************************************************************/
+ /***************************************************************/
+ /* */
+ /* object position */
+ /* */
+ /***************************************************************/
this->PR2Copy->GetObjectPositionActual(&x,&y,&z,&roll,&pitch,&yaw);
this->objectPosMsg.x = x;
this->objectPosMsg.y = y;
this->objectPosMsg.z = z;
- publish("object_position", this->objectPosMsg);
+ publish("object_position", this->objectPosMsg);
/***************************************************************/
/* */
@@ -492,8 +694,175 @@
/* */
/***************************************************************/
// deprecated to using ros+gazebo plugins
+ uint32_t width, height, depth;
+ std::string compression, colorspace;
+ uint32_t buf_size;
+ static unsigned char buf_ptz_right[GAZEBO_CAMERA_MAX_IMAGE_SIZE];
+ static unsigned char buf_ptz_left[GAZEBO_CAMERA_MAX_IMAGE_SIZE];
+ static unsigned char buf_wrist_right[GAZEBO_CAMERA_MAX_IMAGE_SIZE];
+ static unsigned char buf_wrist_left[GAZEBO_CAMERA_MAX_IMAGE_SIZE];
+ static unsigned char buf_forearm_right[GAZEBO_CAMERA_MAX_IMAGE_SIZE];
+ static unsigned char buf_forearm_left[GAZEBO_CAMERA_MAX_IMAGE_SIZE];
+ //this->PR2Copy->hw.GetCameraImage(PR2::CAMERA_GLOBAL,
+ // ----------------------- get image ----------------------------
+ if (PR2::PR2_ALL_OK == this->PR2Copy->hw.GetCameraImage(PR2::CAMERA_HEAD_RIGHT,
+ &width , &height ,
+ &depth ,
+ &compression , &colorspace ,
+ &buf_size , buf_ptz_right , &cameraTime)) {
+ this->img_ptz_right.width = width;
+ this->img_ptz_right.height = height;
+ this->img_ptz_right.compression = compression;
+ this->img_ptz_right.colorspace = colorspace;
+ if(buf_size >0)
+ {
+ this->img_ptz_right.header.frame_id = tf.lookup("FRAMEID_PTZ_CAM_R");
+ this->img_ptz_right.header.stamp.sec = (unsigned long)floor(this->cameraTime);
+ this->img_ptz_right.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->cameraTime - this->laserMsg.header.stamp.sec) );
+
+ this->img_ptz_right.set_data_size(buf_size);
+
+ this->img_ptz_right.data = buf_ptz_right;
+ //memcpy(this->img_ptz_right.data,buf,data_size);
+
+ //publish("image",this->img_ptz_right);
+ publish("image_ptz_right",this->img_ptz_right);
+ }
+ }
+ // ----------------------- get image ----------------------------
+ if (PR2::PR2_ALL_OK == this->PR2Copy->hw.GetCameraImage(PR2::CAMERA_HEAD_LEFT,
+ &width , &height ,
+ &depth ,
+ &compression , &colorspace ,
+ &buf_size , buf_ptz_left , &cameraTime)) {
+ this->img_ptz_left .width = width;
+ this->img_ptz_left .height = height;
+ this->img_ptz_left .compression = compression;
+ this->img_ptz_left .colorspace = colorspace;
+
+ if(buf_size >0)
+ {
+ this->img_ptz_left.header.frame_id = tf.lookup("FRAMEID_PTZ_CAM_L");
+ this->img_ptz_left.header.stamp.sec = (unsigned long)floor(this->cameraTime);
+ this->img_ptz_left.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->cameraTime - this->laserMsg.header.stamp.sec) );
+
+ this->img_ptz_left .set_data_size(buf_size);
+
+ this->img_ptz_left .data = buf_ptz_left ;
+ //memcpy(this->img_ptz_left .data,buf,data_size);
+
+ publish("image_ptz_left",this->img_ptz_left);
+ }
+ }
+
+ // ----------------------- get image ----------------------------
+ if (PR2::PR2_ALL_OK == this->PR2Copy->hw.GetCameraImage(PR2::CAMERA_WRIST_RIGHT,
+ &width , &height ,
+ &depth ,
+ &compression , &colorspace ,
+ &buf_size , buf_wrist_right , &cameraTime )) {
+ this->img_wrist_right.width = width;
+ this->img_wrist_right.height = height;
+ this->img_wrist_right.compression = compression;
+ this->img_wrist_right.colorspace = colorspace;
+
+ if(buf_size >0)
+ {
+ this->img_wrist_right.header.frame_id = tf.lookup("FRAMEID_WRIST_CAM_R");
+ this->img_wrist_right.header.stamp.sec = (unsigned long)floor(this->cameraTime);
+ this->img_wrist_right.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->cameraTime - this->laserMsg.header.stamp.sec) );
+
+ this->img_wrist_right.set_data_size(buf_size);
+
+ this->img_wrist_right.data = buf_wrist_right;
+ //memcpy(this->img_wrist_right.data,buf,data_size);
+
+ publish("image_wrist_right",this->img_wrist_right);
+ }
+ }
+
+ // ----------------------- get image ----------------------------
+ if (PR2::PR2_ALL_OK == this->PR2Copy->hw.GetCameraImage(PR2::CAMERA_WRIST_LEFT,
+ &width , &height ,
+ &depth ,
+ &compression , &colorspace ,
+ &buf_size , buf_wrist_left , &cameraTime )) {
+ this->img_wrist_left .width = width;
+ this->img_wrist_left .height = height;
+ this->img_wrist_left .compression = compression;
+ this->img_wrist_left .colorspace = colorspace;
+
+ if(buf_size >0)
+ {
+ this->img_wrist_left.header.frame_id = tf.lookup("FRAMEID_WRIST_CAM_L");
+ this->img_wrist_left.header.stamp.sec = (unsigned long)floor(this->cameraTime);
+ this->img_wrist_left.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->cameraTime - this->laserMsg.header.stamp.sec) );
+
+ this->img_wrist_left .set_data_size(buf_size);
+
+ this->img_wrist_left .data = buf_wrist_left ;
+ //memcpy(this->img_wrist_left .data,buf,data_size);
+
+ publish("image_wrist_left",this->img_wrist_left);
+ }
+ }
+
+ // ----------------------- get image ----------------------------
+ if (PR2::PR2_ALL_OK == this->PR2Copy->hw.GetCameraImage(PR2::CAMERA_FOREARM_RIGHT,
+ &width , &height ,
+ &depth ,
+ &compression , &colorspace ,
+ &buf_size , buf_forearm_right , &cameraTime )){
+ this->img_forearm_right.width = width;
+ this->img_forearm_right.height = height;
+ this->img_forearm_right.compression = compression;
+ this->img_forearm_right.colorspace = colorspace;
+
+ if(buf_size >0)
+ {
+ this->img_forearm_right.header.frame_id = tf.lookup("FRAMEID_FOREARM_CAM_R");
+ this->img_forearm_right.header.stamp.sec = (unsigned long)floor(this->cameraTime);
+ this->img_forearm_right.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->cameraTime - this->laserMsg.header.stamp.sec) );
+
+ this->img_forearm_right.set_data_size(buf_size);
+
+ this->img_forearm_right.data = buf_forearm_right;
+ //memcpy(this->img_forearm_right.data,buf,data_size);
+
+ publish("image_forearm_right",this->img_forearm_right);
+ }
+ }
+
+ // ----------------------- get image ----------------------------
+ if (PR2::PR2_ALL_OK == this->PR2Copy->hw.GetCameraImage(PR2::CAMERA_FOREARM_LEFT,
+ &width , &height ,
+ &depth ,
+ &compression , &colorspace ,
+ &buf_size , buf_forearm_left , &cameraTime )) {
+ this->img_forearm_left .width = width;
+ this->img_forearm_left .height = height;
+ this->img_forearm_left .compression = compression;
+ this->img_forearm_left .colorspace = colorspace;
+
+ if(buf_size >0)
+ {
+ this->img_forearm_left.header.frame_id = tf.lookup("FRAMEID_FOREARM_CAM_L");
+ this->img_forearm_left.header.stamp.sec = (unsigned long)floor(this->cameraTime);
+ this->img_forearm_left.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->cameraTime - this->laserMsg.header.stamp.sec) );
+
+ this->img_forearm_left .set_data_size(buf_size);
+
+ this->img_forearm_left .data = buf_forearm_left ;
+ //memcpy(this->img_forearm_left .data,buf,data_size);
+
+ publish("image_forearm_left",this->img_forearm_left);
+ }
+ }
+
+
+
/***************************************************************/
/* */
/* pitching Hokuyo joint */
@@ -508,16 +877,52 @@
simPitchAngle = simPitchOffset + simPitchAmp * sin(this->simTime * simPitchTimeScale);
simPitchRate = simPitchAmp * simPitchTimeScale * cos(this->simTime * simPitchTimeScale); // TODO: check rate correctness
this->PR2Copy->GetTime(&this->simTime);
+ //std::cout << "sim time: " << this->simTime << std::endl;
+ //std::cout << "ang: " << simPitchAngle*180.0/M_PI << "rate: " << simPitchRate*180.0/M_PI << std::endl;
this->PR2Copy->hw.SetJointTorque(PR2::HEAD_LASER_PITCH , 200.0);
this->PR2Copy->hw.SetJointGains(PR2::HEAD_LASER_PITCH, 3.0, 1.0, 0.0);
- this->PR2Copy->hw.SetJointServoCmd(PR2::HEAD_LASER_PITCH , -simPitchAngle, simPitchRate);
+ this->PR2Copy->hw.SetJointServoCmd(PR2::HEAD_LASER_PITCH , simPitchAngle, simPitchRate);
if (dAngle * simPitchRate < 0.0)
{
+ // shutter in irrlicht viewer clears the cloud memory, goes before republish of full_cloud
+ publish("shutter",this->shutterMsg);
+
dAngle = -dAngle;
+
+ //if (false)
+ //{
+ this->full_cloudMsg.header.frame_id = tf.lookup("base");
+ this->full_cloudMsg.header.stamp.sec = (unsigned long)floor(this->tiltLaserTime);
+ this->full_cloudMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->tiltLaserTime - this->full_cloudMsg.header.stamp.sec) );
+
+ int num_channels = 1;
+ this->full_cloudMsg.set_pts_size(this->full_cloud_pts->size());
+ this->full_cloudMsg.set_chan_size(num_channels);
+ this->full_cloudMsg.chan[0].name = "intensities";
+ this->full_cloudMsg.chan[0].set_vals_size(this->full_cloud_ch1->size());
+ // TODO: make sure this is doing the right memcopy stuff
+ //memcpy(this->full_cloudMsg.pts , &(this->full_cloud_pts->front()), this->full_cloud_pts->size());
+ //memcpy(this->full_cloudMsg.chan[0].vals , &(this->full_cloud_ch1->front()), this->full_cloud_ch1->size());
+
+ for(unsigned int i=0;i< this->full_cloud_pts->size() ;i++)
+ {
+ this->full_cloudMsg.pts[i].x = (this->full_cloud_pts->at(i)).x;
+ this->full_cloudMsg.pts[i].y = (this->full_cloud_pts->at(i)).y;
+ this->full_cloudMsg.pts[i].z = (this->full_cloud_pts->at(i)).z;
+ this->full_cloudMsg.chan[0].vals[i] = (this->full_cloud_ch1->at(i));
+ }
+
+ publish("full_cloud",this->full_cloudMsg);
+ this->full_cloud_pts->clear();
+ this->full_cloud_ch1->clear();
+ //}
+
}
+ // should send shutter when changing direction, or wait for Tully to implement ring buffer in viewer
+
/***************************************************************/
/* */
/* arm */
@@ -907,69 +1312,17 @@
link->xyz[0],
link->xyz[1],
link->xyz[2],
+ 0.0, //FIXME: get angle of finger tip...
0.0,
0.0,
- 0.0,
odomMsg.header.stamp);
- // forearm camera left
- link = pr2Description.getLink("forearm_camera_left");
- if (link)
- tf.sendEuler("forearm_camera_left",
- "forearm_roll_left",
- link->xyz[0],
- link->xyz[1],
- link->xyz[2],
- 0.0,
- 0.0,
- 0.0,
- odomMsg.header.stamp);
- // forearm camera right
- link = pr2Description.getLink("forearm_camera_right");
- if (link)
- tf.sendEuler("forearm_camera_right",
- "forearm_roll_right",
- link->xyz[0],
- link->xyz[1],
- link->xyz[2],
- 0.0,
- 0.0,
- 0.0,
- odomMsg.header.stamp);
- // wrist camera left
- link = pr2Description.getLink("wrist_camera_left");
- if (link)
- tf.sendEuler("wrist_camera_left",
- "gripper_roll_left",
- link->xyz[0],
- link->xyz[1],
- link->xyz[2],
- 0.0,
- 0.0,
- 0.0,
- odomMsg.header.stamp);
- // wrist camera right
- link = pr2Description.getLink("wrist_camera_right");
- if (link)
- tf.sendEuler("wrist_camera_right",
- "gripper_roll_right",
- link->xyz[0],
- link->xyz[1],
- link->xyz[2],
- 0.0,
- 0.0,
- 0.0,
- odomMsg.header.stamp);
-
-
-
-
// head pan angle
link = pr2Description.getLink("head_pan");
if (link)
@@ -1022,8 +1375,8 @@
// tilt laser location
double tmpPitch, tmpPitchRate;
+ link = pr2Description.getLink("tilt_laser");
this->PR2Copy->hw.GetJointServoCmd(PR2::HEAD_LASER_PITCH, &tmpPitch, &tmpPitchRate );
- link = pr2Description.getLink("tilt_laser");
if (link)
tf.sendEuler("tilt_laser",
"torso",
@@ -1192,6 +1545,9 @@
for(RCList::iterator it = RosControllers.begin(); it != RosControllers.end(); ++it)
(*it)->Update();
+ for(RJList::iterator it = RosJoints.begin(); it != RosJoints.end(); ++it)
+ (*it)->Update();
+
this->lock.unlock();
}
Modified: pkg/trunk/visualization/wxpy_ros/src/wxpy_ros/wxplot.py
===================================================================
--- pkg/trunk/visualization/wxpy_ros/src/wxpy_ros/wxplot.py 2008-08-06 00:36:17 UTC (rev 2612)
+++ pkg/trunk/visualization/wxpy_ros/src/wxpy_ros/wxplot.py 2008-08-06 00:45:10 UTC (rev 2613)
@@ -507,7 +507,7 @@
app = wx.PySimpleApp(0)
frame = wx.Frame(None, -1,"")
- panel = WXPlot(frame)
+ panel = WXSlidingPlot(frame)
panel.setTimespan(10.0)
channel = MyChannel('r')
channel2 = MyChannel2('y+')
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
