Menu
▾
▴
personalrobots-commit
|
From: <hsu...@us...> - 2008-08-05 23:13:35
|
Revision: 2600
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2600&view=rev
Author: hsujohnhsu
Date: 2008-08-05 23:13:43 +0000 (Tue, 05 Aug 2008)
Log Message:
-----------
laser and cameras using ros+gazebo plugins.
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Laser.cc
pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h
pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Laser.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Laser.cc 2008-08-05 23:08:05 UTC (rev 2599)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Laser.cc 2008-08-05 23:13:43 UTC (rev 2600)
@@ -220,21 +220,10 @@
this->cloudMsg.pts[i].y =r * sin(laser_yaw) + this->GaussianKernel(0,sigma) ;
this->cloudMsg.pts[i].z =r * cos(laser_yaw) * sin(laser_pitch) + this->GaussianKernel(0,sigma) ;
this->cloudMsg.chan[0].vals[i] = v;
- std::cout << " i " << i
- << " x " << this->cloudMsg.pts[i].x
- << " y " << this->cloudMsg.pts[i].y
- << " z " << this->cloudMsg.pts[i].z
- << " " << this->cloudMsg.header.frame_id
- << " " << this->cloudMsg.header.stamp.sec
- << " " << this->cloudMsg.header.stamp.nsec
- << " " << Simulator::Instance()->GetSimTime()
- << std::endl;
-
}
// iface writing can be skipped if iface is not used.
// send data out via ros message
- std::cout<< "putting message " << this->topicName << " " << rangeCount << std::endl;
rosnode->publish(this->topicName,this->cloudMsg);
this->lock.unlock();
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml 2008-08-05 23:08:05 UTC (rev 2599)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml 2008-08-05 23:13:43 UTC (rev 2600)
@@ -1273,18 +1273,12 @@
<minRange>0.05</minRange>
<maxRange>10.0</maxRange>
<updateRate>10.0</updateRate>
- <!--
<controller:ros_laser name="ros_tilt_laser_controller" plugin="libRos_Laser.so">
<updateRate>15.0</updateRate>
<topicName>cloud</topicName>
<frameName>tilt_laser</frameName>
<interface:laser name="ros_tilt_laser_iface" />
</controller:ros_laser>
- -->
- <controller:sicklms200_laser name="tilt_laser_controller_1">
- <interface:laser name="tilt_laser_iface_1"/>
- <updateRate>10</updateRate>
- </controller:sicklms200_laser>
</sensor:ray>
</verbatim>
</data>
@@ -1337,7 +1331,7 @@
<rangeCount>683</rangeCount>
<laserCount>1</laserCount>
<origin>0.0 0.0 0.05</origin>
- <displayRays>true</displayRays>
+ <displayRays>false</displayRays>
<minAngle>-90</minAngle>
<maxAngle>90</maxAngle>
@@ -1345,18 +1339,12 @@
<minRange>0.05</minRange>
<maxRange>10.0</maxRange>
<updateRate>10.0</updateRate>
- <!--
<controller:ros_laser name="ros_base_laser_controller" plugin="libRos_Laser.so">
<updateRate>10</updateRate>
<topicName>scan</topicName>
<frameName>base_laser</frameName>
<interface:laser name="ros_base_laser_iface" />
</controller:ros_laser>
- -->
- <controller:sicklms200_laser name="base_laser_controller_1">
- <interface:laser name="base_laser_iface_1"/>
- <updateRate>10</updateRate>
- </controller:sicklms200_laser>
</sensor:ray>
</verbatim>
</data>
Modified: pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h
===================================================================
--- pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h 2008-08-05 23:08:05 UTC (rev 2599)
+++ pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h 2008-08-05 23:13:43 UTC (rev 2600)
@@ -43,12 +43,6 @@
#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
@@ -61,9 +55,6 @@
#include <pr2_msgs/EndEffectorState.h>
-// roscpp - camera
-#include <std_msgs/Image.h>
-
// for frame transforms
#include <rosTF/rosTF.h>
@@ -81,14 +72,10 @@
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
@@ -173,20 +160,6 @@
// 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;
@@ -204,17 +177,6 @@
//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;
Modified: pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
===================================================================
--- pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp 2008-08-05 23:08:05 UTC (rev 2599)
+++ pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp 2008-08-05 23:13:43 UTC (rev 2600)
@@ -209,19 +209,6 @@
// 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,19 +237,6 @@
// 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));
@@ -299,19 +273,6 @@
// 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));
@@ -350,23 +311,9 @@
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");
@@ -442,18 +389,6 @@
{
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 */
@@ -480,128 +415,14 @@
/* 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("tilt_laser");
- 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 */
@@ -654,16 +475,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);
/***************************************************************/
/* */
@@ -671,180 +492,8 @@
/* */
/***************************************************************/
// 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];
- // ----------------------- get image ----------------------------
- if (false)
- 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 (false)
- 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 (false)
- 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("wrist_camera_right");
- 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 (false)
- 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("wrist_camera_left");
- 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 (false)
- 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("forearm_camera_right");
- 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 (false)
- 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("forearm_camera_left");
- 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 */
@@ -859,52 +508,16 @@
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 */
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-08-05 23:38:49
|
Revision: 2603
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2603&view=rev
Author: isucan
Date: 2008-08-05 23:38:59 +0000 (Tue, 05 Aug 2008)
Log Message:
-----------
more careful loading of robot models
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/world_models/world_3d_map/params.xml
pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-08-05 23:30:34 UTC (rev 2602)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-08-05 23:38:59 UTC (rev 2603)
@@ -154,15 +154,17 @@
printf("Loading robot descriptions...\n\n");
std::string description_files;
- get_param("robotdesc_list", description_files);
- std::stringstream sdf(description_files);
- while (sdf.good())
+ if (get_param("robotdesc_list", description_files))
{
- std::string file;
- std::string content;
- sdf >> file;
- get_param(file, content);
- addRobotDescriptionFromData(content.c_str());
+ std::stringstream sdf(description_files);
+ while (sdf.good())
+ {
+ std::string file;
+ std::string content;
+ sdf >> file;
+ if (get_param(file, content))
+ addRobotDescriptionFromData(content.c_str());
+ }
}
printf("\n\n");
}
Modified: pkg/trunk/world_models/world_3d_map/params.xml
===================================================================
--- pkg/trunk/world_models/world_3d_map/params.xml 2008-08-05 23:30:34 UTC (rev 2602)
+++ pkg/trunk/world_models/world_3d_map/params.xml 2008-08-05 23:38:59 UTC (rev 2603)
@@ -2,5 +2,6 @@
<double name="world_3d_map/max_publish_frequency">0.3</double> <!-- Hz -->
<double name="world_3d_map/retain_pointcloud_duration">60</double> <!-- seconds -->
<double name="world_3d_map/retain_pointcloud_fraction">0.02</double> <!-- percentage (between 0 and 1) -->
- <int name="world_3d_map/verbosity_level">1</int> <!-- integer value -->
+ <int name="world_3d_map/verbosity_level">1</int> <!-- integer value -->
+ <string name="world_3d_map/robot_model">robotdesc/pr2</string> <!-- string value -->
</params>
Modified: pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
===================================================================
--- pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-08-05 23:30:34 UTC (rev 2602)
+++ pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-08-05 23:38:59 UTC (rev 2603)
@@ -153,21 +153,13 @@
void loadRobotDescriptions(void)
{
- printf("Loading robot descriptions...\n\n");
-
- std::string description_files;
- get_param("robotdesc_list", description_files);
- std::stringstream sdf(description_files);
- while (sdf.good())
+ std::string model;
+ if (get_param("world_3d_map/robot_model", model))
{
- std::string file;
std::string content;
- sdf >> file;
- printf("Loading '%s'\n", file.c_str());
- get_param(file, content);
- addRobotDescriptionFromData(content.c_str());
+ if (get_param(model, content))
+ addRobotDescriptionFromData(content.c_str());
}
- printf("\n\n");
}
void addRobotDescriptionFromData(const char *data)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
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");
- ...
[truncated message content] |
|
From: <ge...@us...> - 2008-08-06 02:36:22
|
Revision: 2622
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2622&view=rev
Author: gerkey
Date: 2008-08-06 02:36:30 +0000 (Wed, 06 Aug 2008)
Log Message:
-----------
Added optional range_cutoff argument to projectLaser, and added usage of it
to wavefront_player.
Modified Paths:
--------------
pkg/trunk/nav/wavefront_player/wavefront_player.cc
pkg/trunk/util/laser_scan_utils/include/laser_scan_utils/laser_scan.h
pkg/trunk/util/laser_scan_utils/src/laser_scan.cc
Modified: pkg/trunk/nav/wavefront_player/wavefront_player.cc
===================================================================
--- pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-08-06 02:14:47 UTC (rev 2621)
+++ pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-08-06 02:36:30 UTC (rev 2622)
@@ -280,9 +280,9 @@
dist_penalty(2.0), // overridden by param retrieval below!
plan_halfwidth(5.0),
dist_eps(1.0), // overridden by param retrieval below!
- ang_eps(DTOR(10.0)),
+ ang_eps(DTOR(4.0)),
cycletime(0.1),
- laser_maxrange(4.0),
+ laser_maxrange(10.0),
laser_buffer_time(3.0),
lookahead_maxdist(2.0),
lookahead_distweight(5.0),
@@ -433,7 +433,7 @@
// Assemble a point cloud, in the laser's frame
std_msgs::PointCloudFloat32 local_cloud;
- projector_.projectLaser(*it, local_cloud);
+ projector_.projectLaser(*it, local_cloud, laser_maxrange);
// Convert to a point cloud in the map frame
std_msgs::PointCloudFloat32 global_cloud;
Modified: pkg/trunk/util/laser_scan_utils/include/laser_scan_utils/laser_scan.h
===================================================================
--- pkg/trunk/util/laser_scan_utils/include/laser_scan_utils/laser_scan.h 2008-08-06 02:14:47 UTC (rev 2621)
+++ pkg/trunk/util/laser_scan_utils/include/laser_scan_utils/laser_scan.h 2008-08-06 02:36:30 UTC (rev 2622)
@@ -62,7 +62,7 @@
/** \brief Project Laser Scan
* This will project a laser scan from a linear array into a 3D point cloud
*/
- void projectLaser(const std_msgs::LaserScan& scan_in, std_msgs::PointCloudFloat32 & cloud_out);
+ void projectLaser(const std_msgs::LaserScan& scan_in, std_msgs::PointCloudFloat32 & cloud_out, double range_cutoff=-1.0);
/** \brief Return the unit vectors for this configuration
* Return the unit vectors for this configuration.
Modified: pkg/trunk/util/laser_scan_utils/src/laser_scan.cc
===================================================================
--- pkg/trunk/util/laser_scan_utils/src/laser_scan.cc 2008-08-06 02:14:47 UTC (rev 2621)
+++ pkg/trunk/util/laser_scan_utils/src/laser_scan.cc 2008-08-06 02:36:30 UTC (rev 2622)
@@ -33,7 +33,7 @@
namespace laser_scan{
- void LaserProjection::projectLaser(const std_msgs::LaserScan& scan_in, std_msgs::PointCloudFloat32 & cloud_out)
+ void LaserProjection::projectLaser(const std_msgs::LaserScan& scan_in, std_msgs::PointCloudFloat32 & cloud_out, double range_cutoff)
{
NEWMAT::Matrix ranges(2, scan_in.ranges_size);
double * matPointer = ranges.Store();
@@ -64,8 +64,9 @@
unsigned int count = 0;
for (unsigned int index = 0; index< scan_in.ranges_size; index++)
{
- if (matPointer[index] <= scan_in.range_max
- && matPointer[index] >= scan_in.range_min) //only valid
+ if ((matPointer[index] < scan_in.range_max) &&
+ ((range_cutoff < 0.0) || (matPointer[index] < range_cutoff)) &&
+ (matPointer[index] > scan_in.range_min)) //only valid
{
cloud_out.pts[count].x = outputMat[index];
cloud_out.pts[count].y = outputMat[index + scan_in.ranges_size];
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-08-06 19:43:02
|
Revision: 2647
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2647&view=rev
Author: hsujohnhsu
Date: 2008-08-06 19:43:10 +0000 (Wed, 06 Aug 2008)
Log Message:
-----------
more clean up of pr2.xml sensors. ptz cameras and stereo camera moved from controllers_gazebo.xml to pr2.xml.
update 2dnav-gazebo xml to use new scan names (base_scan and tilt_scan).
RosGazeboNode FRAMEID_ODOM now publishing 3D data.
Modified Paths:
--------------
pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg.xml
pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
Modified: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg.xml 2008-08-06 19:40:21 UTC (rev 2646)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg.xml 2008-08-06 19:43:10 UTC (rev 2647)
@@ -6,7 +6,7 @@
<node pkg="pr2_gazebo" type="run-gazebo.sh" args="$(find gazebo_robot_description)/world/robot_wg.world" respawn="true" />
<node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/willowMap.png 0.1" respawn="false" />
<node pkg="pr2_gazebo" type="run-pr2_gazebo.sh" args="" respawn="true" />
- <node pkg="amcl_player" type="amcl_player" respawn="false" />
+ <node pkg="amcl_player" type="amcl_player" args="scan:=base_scan" respawn="false" />
<node pkg="wavefront_player" type="wavefront_player" respawn="false" />
<node pkg="nav_view" type="nav_view" respawn="true" />
<node pkg="teleop_base_keyboard" type="teleop_base_keyboard" respawn="false" />
Modified: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo.xml 2008-08-06 19:40:21 UTC (rev 2646)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo.xml 2008-08-06 19:43:10 UTC (rev 2647)
@@ -6,7 +6,7 @@
<node pkg="pr2_gazebo" type="run-gazebo.sh" args="$(find gazebo_robot_description)/world/robot.world" respawn="true" />
<node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/map3.png 0.1" respawn="false" />
<node pkg="pr2_gazebo" type="run-pr2_gazebo.sh" args="" respawn="true" />
- <node pkg="amcl_player" type="amcl_player" respawn="false" />
+ <node pkg="amcl_player" type="amcl_player" args="scan:=base_scan" respawn="false" />
<node pkg="wavefront_player" type="wavefront_player" respawn="false" />
<node pkg="nav_view" type="nav_view" respawn="true" />
<node pkg="teleop_base_keyboard" type="teleop_base_keyboard" respawn="false" />
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo.xml 2008-08-06 19:40:21 UTC (rev 2646)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo.xml 2008-08-06 19:43:10 UTC (rev 2647)
@@ -27,51 +27,6 @@
- <!-- stereo camera -->
- <body:empty name="stereo_camera_body">
- <xyz> 0.03 0.0 1.30 </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>
- <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.05 0.10 0.05</size>
- <mass>0.1</mass>
- <visual>
- <size>0.05 0.10 0.05</size>
- <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_attach_joint">
- <body2>head_tilt</body2>
- <body1>stereo_camera_body</body1>
- <anchor>head_tilt</anchor>
- <axis> 1.0 0.0 0.0 </axis>
- <lowStop> 0.0 </lowStop>
- <highStop> 0.0 </highStop>
- </joint:hinge>
-
<!-- PR2_ACTARRAY -->
<controller:pr2_actarray name="pr2_controller" plugin="libPr2_Actarray.so">
<updateRate>100.0</updateRate>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml 2008-08-06 19:40:21 UTC (rev 2646)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml 2008-08-06 19:43:10 UTC (rev 2647)
@@ -32,7 +32,7 @@
<const name="torso_size_z" value=".823 " /> <!-- for defining collision geometry -->
<const name="base_torso_offset_x" value="-0.05 " /> <!-- mp 20080801 -->
- <const name="base_torso_offset_z" value=" 0.739675" /> <!-- mp 20080801 -->
+ <const name="base_torso_offset_z" value=" 0.739675" /> <!-- mp 20080801 this is the offset for home position: lowest spine setting -->
<const name="torso_center_box_center_offset_x" value="-.10" /> <!-- FIXME -->
<const name="torso_center_box_center_offset_z" value="-.50" /> <!-- FIXME -->
@@ -115,6 +115,11 @@
<!-- stereo camera -->
<const name="head_tilt_stereo_offset_x" value="0.0232" /> <!-- mp 20080801 -->
<const name="head_tilt_stereo_offset_z" value="0.0645" /> <!-- mp 20080801 -->
+ <const name="stereo_size_x" value=" 0.05 " /> <!-- for definint visual and collision geometry -->
+ <const name="stereo_size_y" value=" 0.10 " /> <!-- for definint visual and collision geometry -->
+ <const name="stereo_size_z" value=" 0.05 " /> <!-- for definint visual and collision geometry -->
+ <const name="stereo_center_box_center_offset_x" value=" 0.00 " /> <!-- from center of stereo to stereo mount point, which is provided above -->
+ <const name="stereo_center_box_center_offset_z" value=" 0.05 " /> <!-- from center of stereo to stereo mount point, which is provided above -->
<!-- ptz cameras -->
<const name="torso_ptz_pan_left_offset_x" value=" 0.0000" /> <!-- mp 20080801 -->
@@ -136,10 +141,20 @@
<const name="ptz_tilt_min_limit" value=" -M_PI/2 " /> <!-- FIXME -->
<const name="ptz_tilt_max_limit" value=" M_PI/2 " /> <!-- FIXME -->
+ <const name="ptz_pan_radius" value=" 0.05 " /> <!-- for definint visual and collision geometry -->
+ <const name="ptz_pan_length" value=" 0.05 " /> <!-- for definint visual and collision geometry -->
<const name="ptz_pan_size_x" value=" 0.05 " /> <!-- for definint visual and collision geometry -->
<const name="ptz_pan_size_y" value=" 0.05 " /> <!-- for definint visual and collision geometry -->
<const name="ptz_pan_size_z" value=" 0.05 " /> <!-- for definint visual and collision geometry -->
+ <const name="ptz_pan_left_center_box_center_offset_x" value=" 0.00 " /> <!-- for definint visual and collision geometry -->
+ <const name="ptz_pan_left_center_box_center_offset_y" value=" 0.025 " /> <!-- for definint visual and collision geometry -->
+ <const name="ptz_pan_left_center_box_center_offset_z" value=" 0.00 " /> <!-- for definint visual and collision geometry -->
+ <const name="ptz_pan_right_center_box_center_offset_x" value=" 0.00 " /> <!-- for definint visual and collision geometry -->
+ <const name="ptz_pan_right_center_box_center_offset_y" value="-0.025 " /> <!-- for definint visual and collision geometry -->
+ <const name="ptz_pan_right_center_box_center_offset_z" value=" 0.00 " /> <!-- for definint visual and collision geometry -->
+ <const name="ptz_tilt_radius" value=" 0.03 " /> <!-- for definint visual and collision geometry -->
+ <const name="ptz_tilt_length" value=" 0.03 " /> <!-- for definint visual and collision geometry -->
<const name="ptz_tilt_size_x" value=" 0.03 " /> <!-- for definint visual and collision geometry -->
<const name="ptz_tilt_size_y" value=" 0.03 " /> <!-- for definint visual and collision geometry -->
<const name="ptz_tilt_size_z" value=" 0.03 " /> <!-- for definint visual and collision geometry -->
@@ -196,7 +211,7 @@
<const name="torso_tilt_laser_offset_z" value="0.19525" /> <!-- mp 20080801 -->
<const name="base_base_laser_offset_x" value="0.275" /> <!-- mp 20080801 -->
- <const name="base_base_laser_offset_z" value="0.182" /> <!-- mp 20080801 -->
+ <const name="base_base_laser_offset_z" value="0.182+base_size_z/2" /> <!-- FIXME: seems low. mp 20080801 -->
<const name="gripper_roll_camera_offset_x" value="0.05" /> <!-- this is a guess, please change me -->
<const name="gripper_roll_camera_offset_y" value="0 " /> <!-- this is a guess, please change me -->
@@ -1627,19 +1642,19 @@
</inertial>
<visual>
- <xyz>0 0 0.0</xyz> <!-- location defined with respect to the link origin in a local coordinate frame -->
+ <xyz>ptz_pan_left_center_box_center_offset_x ptz_pan_left_center_box_center_offset_y ptz_pan_left_center_box_center_offset_z</xyz> <!-- location defined with respect to the link origin in a local coordinate frame -->
<rpy>M_PI/2 0 0 </rpy> <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
<material>Gazebo/Blue</material>
+ <scale>ptz_pan_size_x ptz_pan_size_y ptz_pan_size_z</scale>
<geometry type="cylinder" name="pr2_ptz_pan_left_visual">
- <size>ptz_pan_size_x ptz_pan_size_y ptz_pan_size_z</size>
</geometry>
</visual>
<collision>
- <xyz>0 0 0</xyz> <!-- location defined with respect to the link origin in a local coordinate frame -->
+ <xyz>ptz_pan_left_center_box_center_offset_x ptz_pan_left_center_box_center_offset_y ptz_pan_left_center_box_center_offset_z</xyz> <!-- location defined with respect to the link origin in a local coordinate frame -->
<rpy>0.0 0.0 0.0 </rpy> <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
<geometry type="cylinder">
- <size>ptz_pan_size_x ptz_pan_size_y ptz_pan_size_z </size>
+ <size>ptz_pan_radius ptz_pan_length</size>
</geometry>
</collision>
@@ -1665,10 +1680,10 @@
<visual>
<xyz>0 0 0.0</xyz> <!-- location defined with respect to the link origin in a local coordinate frame -->
- <rpy>M_PI/2 0 0 </rpy> <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
+ <rpy>0 M_PI/2 0 </rpy> <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
<material>Gazebo/Blue</material>
+ <scale>ptz_tilt_size_x ptz_tilt_size_y ptz_tilt_size_z</scale>
<geometry type="cylinder" name="pr2_ptz_tilt_left_visual">
- <size>ptz_tilt_size_x ptz_tilt_size_y ptz_tilt_size_z</size>
</geometry>
</visual>
@@ -1676,7 +1691,7 @@
<xyz>0 0 0</xyz> <!-- location defined with respect to the link origin in a local coordinate frame -->
<rpy>0.0 0.0 0.0 </rpy> <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
<geometry type="cylinder">
- <size>ptz_tilt_size_x ptz_tilt_size_y ptz_tilt_size_z </size>
+ <size>ptz_tilt_radius ptz_tilt_length</size>
</geometry>
</collision>
@@ -1719,19 +1734,19 @@
</inertial>
<visual>
- <xyz>0 0 0.0</xyz> <!-- location defined with respect to the link origin in a local coordinate frame -->
+ <xyz>ptz_pan_right_center_box_center_offset_x ptz_pan_right_center_box_center_offset_y ptz_pan_right_center_box_center_offset_z</xyz> <!-- location defined with respect to the link origin in a local coordinate frame -->
<rpy>M_PI/2 0 0 </rpy> <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
<material>Gazebo/Blue</material>
+ <scale>ptz_pan_size_x ptz_pan_size_y ptz_pan_size_z</scale>
<geometry type="cylinder" name="pr2_ptz_pan_right_visual">
- <size>ptz_pan_size_x ptz_pan_size_y ptz_pan_size_z</size>
</geometry>
</visual>
<collision>
- <xyz>0 0 0</xyz> <!-- location defined with respect to the link origin in a local coordinate frame -->
+ <xyz>ptz_pan_right_center_box_center_offset_x ptz_pan_right_center_box_center_offset_y ptz_pan_right_center_box_center_offset_z</xyz> <!-- location defined with respect to the link origin in a local coordinate frame -->
<rpy>0.0 0.0 0.0 </rpy> <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
<geometry type="cylinder">
- <size>ptz_pan_size_x ptz_pan_size_y ptz_pan_size_z </size>
+ <size>ptz_pan_radius ptz_pan_length</size>
</geometry>
</collision>
@@ -1757,10 +1772,10 @@
<visual>
<xyz>0 0 0.0</xyz> <!-- location defined with respect to the link origin in a local coordinate frame -->
- <rpy>M_PI/2 0 0 </rpy> <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
+ <rpy>0 M_PI/2 0 </rpy> <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
<material>Gazebo/Blue</material>
+ <scale>ptz_tilt_size_x ptz_tilt_size_y ptz_tilt_size_z</scale>
<geometry type="cylinder" name="pr2_ptz_tilt_right_visual">
- <size>ptz_tilt_size_x ptz_tilt_size_y ptz_tilt_size_z</size>
</geometry>
</visual>
@@ -1768,7 +1783,7 @@
<xyz>0 0 0</xyz> <!-- location defined with respect to the link origin in a local coordinate frame -->
<rpy>0.0 0.0 0.0 </rpy> <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
<geometry type="cylinder">
- <size>ptz_tilt_size_x ptz_tilt_size_y ptz_tilt_size_z </size>
+ <size>ptz_tilt_radius ptz_tilt_length</size>
</geometry>
</collision>
@@ -1793,6 +1808,74 @@
</sensor>
+
+
+
+
+
+
+
+ <!-- stereo camera -->
+ <sensor name="stereo" type="camera"><!-- link specifying the shoulder of the robot -->
+ <parent>head_tilt</parent>
+ <rpy> 0 0 0 </rpy> <!-- rotation of a local coordinate frame attached to the link with respect to the parent's frame -->
+ <xyz> head_tilt_stereo_offset_x 0 head_tilt_stereo_offset_z</xyz> <!-- position of a local coordinate frame attached to the link with respect to parent's frame-->
+
+
+ <joint type="fixed">
+ <axis> 0 0 1 </axis> <!-- direction of the joint in a local coordinate frame -->
+ <anchor>0 0 0 </anchor> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
+ <limit> 0 0 </limit>
+ </joint>
+
+ <inertial>
+ <mass> 0.1 </mass>
+ <com> 0 0 0 </com> <!-- position of the center of mass with respect to the link's own anchor in a local coordinate frame -->
+ <inertia>0.00482611007 -0.000144683999 0.000110076136 0.005218991412 -0.000314239509 0.008618784925 </inertia>
+ </inertial>
+
+ <visual>
+ <xyz>stereo_center_box_center_offset_x 0 stereo_center_box_center_offset_z </xyz>
+ <rpy>0 0 0 </rpy> <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
+ <material>Gazebo/Blue</material>
+ <geometry type="box" name="pr2_stereo_visual">
+ <size>stereo_size_x stereo_size_y stereo_size_z</size>
+ </geometry>
+ </visual>
+
+ <collision>
+ <xyz>stereo_center_box_center_offset_x 0 stereo_center_box_center_offset_z </xyz>
+ <rpy>0.0 0.0 0.0 </rpy> <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
+ <geometry type="box">
+ <size>stereo_size_x stereo_size_y stereo_size_z</size>
+ </geometry>
+ </collision>
+
+ <data name="sensor" type="gazebo">
+ <verbatim key="sensor_camera">
+ <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>
+ <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>
+ </verbatim>
+ </data>
+
+ </sensor>
+
<!-- End sensor definitions -->
<frame name="test">
Modified: pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
===================================================================
--- pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp 2008-08-06 19:40:21 UTC (rev 2646)
+++ pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp 2008-08-06 19:43:10 UTC (rev 2647)
@@ -474,24 +474,6 @@
this->odomMsg.header.stamp.sec = (unsigned long)floor(this->simTime);
this->odomMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->simTime - this->odomMsg.header.stamp.sec) );
- /***************************************************************/
- /* */
- /* frame transforms */
- /* */
- /* TODO: should we send z, roll, pitch, yaw? seems to confuse */
- /* localization */
- /* */
- /***************************************************************/
- tf.sendInverseEuler("FRAMEID_ODOM",
- "FRAMEID_ROBOT",
- odomMsg.pos.x,
- odomMsg.pos.y,
- 0.0,
- odomMsg.pos.th,
- 0.0,
- 0.0,
- odomMsg.header.stamp);
-
// This publish call resets odomMsg.header.stamp.sec and
// odomMsg.header.stamp.nsec to zero. Thus, it must be called *after*
// those values are reused in the sendInverseEuler() call above.
@@ -586,7 +568,29 @@
rarm.gripperGapCmd = position;
publish("right_pr2arm_pos", rarm);
+ /***************************************************************/
+ /* */
+ /* frame transforms */
+ /* */
+ /* TODO: should we send z, roll, pitch, yaw? seems to confuse */
+ /* localization */
+ /* */
+ /***************************************************************/
+ robot_desc::URDF::Link* link;
+ link = pr2Description.getLink("base");
+ if (link)
+ this->PR2Copy->GetBasePositionActual(&x,&y,&z,&roll,&pitch,&yaw); // actual CoM of base
+ tf.sendInverseEuler("FRAMEID_ODOM",
+ "base",
+ x,
+ y,
+ z-link->collision->xyz[3], /* get infor from xml: half height of base box */
+ yaw,
+ pitch,
+ roll,
+ odomMsg.header.stamp);
+
/***************************************************************/
/* */
/* frame transforms */
@@ -594,7 +598,6 @@
/* x,y,z,yaw,pitch,roll */
/* */
/***************************************************************/
- //this->PR2Copy->GetBasePositionActual(&x,&y,&z,&roll,&pitch,&yaw); // actual CoM of base
tf.sendEuler("base",
"FRAMEID_ROBOT",
0,
@@ -605,24 +608,11 @@
0,
odomMsg.header.stamp);
- //this->PR2Copy->GetBasePositionActual(&x,&y,&z,&roll,&pitch,&yaw); // actual CoM of base
- tf.sendInverseEuler("FRAMEID_ODOM",
- "base",
- x,
- y,
- z-0.13, /* half height of base box */
- yaw,
- pitch,
- roll,
- odomMsg.header.stamp);
-
//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
- robot_desc::URDF::Link* link;
-
link = pr2Description.getLink("torso");
if (link)
tf.sendEuler("torso",
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-08-07 01:03:07
|
Revision: 2692
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2692&view=rev
Author: isucan
Date: 2008-08-07 01:03:15 +0000 (Thu, 07 Aug 2008)
Log Message:
-----------
moved and renamed robot_motion_planning_models -> planning_models
Modified Paths:
--------------
pkg/trunk/util/collision_space/manifest.xml
pkg/trunk/world_models/world_3d_map/manifest.xml
Added Paths:
-----------
pkg/trunk/motion_planning/planning_models/
pkg/trunk/motion_planning/planning_models/CMakeLists.txt
pkg/trunk/motion_planning/planning_models/Makefile
pkg/trunk/motion_planning/planning_models/include/
pkg/trunk/motion_planning/planning_models/manifest.xml
pkg/trunk/motion_planning/planning_models/src/
Removed Paths:
-------------
pkg/trunk/motion_planning/planning_models/CMakeLists.txt
pkg/trunk/motion_planning/planning_models/Makefile
pkg/trunk/motion_planning/planning_models/include/
pkg/trunk/motion_planning/planning_models/manifest.xml
pkg/trunk/motion_planning/planning_models/src/
pkg/trunk/robot_models/robot_motion_planning_models/
Deleted: pkg/trunk/motion_planning/planning_models/CMakeLists.txt
===================================================================
--- pkg/trunk/robot_models/robot_motion_planning_models/CMakeLists.txt 2008-08-06 20:41:02 UTC (rev 2654)
+++ pkg/trunk/motion_planning/planning_models/CMakeLists.txt 2008-08-07 01:03:15 UTC (rev 2692)
@@ -1,4 +0,0 @@
-cmake_minimum_required(VERSION 2.6)
-include(rosbuild)
-rospack(robot_motion_planning_models)
-rospack_add_library(PlanningRobotModels src/planning_models/kinematic.cpp)
Copied: pkg/trunk/motion_planning/planning_models/CMakeLists.txt (from rev 2691, pkg/trunk/robot_models/robot_motion_planning_models/CMakeLists.txt)
===================================================================
--- pkg/trunk/motion_planning/planning_models/CMakeLists.txt (rev 0)
+++ pkg/trunk/motion_planning/planning_models/CMakeLists.txt 2008-08-07 01:03:15 UTC (rev 2692)
@@ -0,0 +1,4 @@
+cmake_minimum_required(VERSION 2.6)
+include(rosbuild)
+rospack(planning_models)
+rospack_add_library(planning_models src/planning_models/kinematic.cpp)
Deleted: pkg/trunk/motion_planning/planning_models/Makefile
===================================================================
--- pkg/trunk/robot_models/robot_motion_planning_models/Makefile 2008-08-06 20:41:02 UTC (rev 2654)
+++ pkg/trunk/motion_planning/planning_models/Makefile 2008-08-07 01:03:15 UTC (rev 2692)
@@ -1 +0,0 @@
-include $(shell rospack find mk)/cmake.mk
Copied: pkg/trunk/motion_planning/planning_models/Makefile (from rev 2691, pkg/trunk/robot_models/robot_motion_planning_models/Makefile)
===================================================================
--- pkg/trunk/motion_planning/planning_models/Makefile (rev 0)
+++ pkg/trunk/motion_planning/planning_models/Makefile 2008-08-07 01:03:15 UTC (rev 2692)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Deleted: pkg/trunk/motion_planning/planning_models/manifest.xml
===================================================================
--- pkg/trunk/robot_models/robot_motion_planning_models/manifest.xml 2008-08-06 20:41:02 UTC (rev 2654)
+++ pkg/trunk/motion_planning/planning_models/manifest.xml 2008-08-07 01:03:15 UTC (rev 2692)
@@ -1,17 +0,0 @@
-<package>
- <description brief="A set of robot models">
- A set of robot models that can be instantiated from a parsed URDF file.
- See: http://pr.willowgarage.com/wiki/RobotDescriptionFormat
- </description>
- <author>Ioan Sucan/is...@wi...</author>
- <license>BSD</license>
- <url>http://pr.willowgarage.com/wiki/RobotDescriptionFormat</url>
-
- <depend package="wg_robot_description_parser"/>
- <depend package="libTF"/>
-
- <export>
- <cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lPlanningRobotModels"/>
- </export>
-
-</package>
Copied: pkg/trunk/motion_planning/planning_models/manifest.xml (from rev 2691, pkg/trunk/robot_models/robot_motion_planning_models/manifest.xml)
===================================================================
--- pkg/trunk/motion_planning/planning_models/manifest.xml (rev 0)
+++ pkg/trunk/motion_planning/planning_models/manifest.xml 2008-08-07 01:03:15 UTC (rev 2692)
@@ -0,0 +1,17 @@
+<package>
+ <description brief="A set of robot models">
+ A set of robot models that can be instantiated from a parsed URDF file.
+ See: http://pr.willowgarage.com/wiki/RobotDescriptionFormat
+ </description>
+ <author>Ioan Sucan/is...@wi...</author>
+ <license>BSD</license>
+ <url>http://pr.willowgarage.com/wiki/RobotDescriptionFormat</url>
+
+ <depend package="wg_robot_description_parser"/>
+ <depend package="libTF"/>
+
+ <export>
+ <cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lplanning_models"/>
+ </export>
+
+</package>
Modified: pkg/trunk/util/collision_space/manifest.xml
===================================================================
--- pkg/trunk/util/collision_space/manifest.xml 2008-08-07 01:00:28 UTC (rev 2691)
+++ pkg/trunk/util/collision_space/manifest.xml 2008-08-07 01:03:15 UTC (rev 2692)
@@ -7,7 +7,7 @@
<license>BSD</license>
<depend package="rosthread"/>
- <depend package="robot_motion_planning_models"/>
+ <depend package="planning_models"/>
<depend package="opende"/>
<depend package="gtest"/>
Modified: pkg/trunk/world_models/world_3d_map/manifest.xml
===================================================================
--- pkg/trunk/world_models/world_3d_map/manifest.xml 2008-08-07 01:00:28 UTC (rev 2691)
+++ pkg/trunk/world_models/world_3d_map/manifest.xml 2008-08-07 01:03:15 UTC (rev 2692)
@@ -9,6 +9,6 @@
<depend package="rosTF" />
<depend package="random_utils" />
<depend package="wg_robot_description_parser" />
-<depend package="robot_motion_planning_models" />
+<depend package="planning_models" />
<depend package="collision_space" />
</package>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2008-08-07 01:40:56
|
Revision: 2699
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2699&view=rev
Author: sfkwc
Date: 2008-08-07 01:41:05 +0000 (Thu, 07 Aug 2008)
Log Message:
-----------
moving up to top level
Added Paths:
-----------
pkg/trunk/robot_msgs/
Removed Paths:
-------------
pkg/trunk/messages/robot_msgs/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2008-08-07 01:41:19
|
Revision: 2700
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2700&view=rev
Author: sfkwc
Date: 2008-08-07 01:41:28 +0000 (Thu, 07 Aug 2008)
Log Message:
-----------
moving up to top level
Added Paths:
-----------
pkg/trunk/pr2_msgs/
Removed Paths:
-------------
pkg/trunk/messages/pr2_msgs/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-08-07 01:44:44
|
Revision: 2702
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2702&view=rev
Author: isucan
Date: 2008-08-07 01:44:53 +0000 (Thu, 07 Aug 2008)
Log Message:
-----------
moved collision_space and test_collision_space
Added Paths:
-----------
pkg/trunk/motion_planning/collision_space/
pkg/trunk/motion_planning/collision_space/CMakeLists.txt
pkg/trunk/motion_planning/collision_space/Makefile
pkg/trunk/motion_planning/collision_space/include/
pkg/trunk/motion_planning/collision_space/manifest.xml
pkg/trunk/motion_planning/collision_space/src/
pkg/trunk/motion_planning/collision_space/test/
pkg/trunk/motion_planning/test_collision_space/
Removed Paths:
-------------
pkg/trunk/motion_planning/collision_space/CMakeLists.txt
pkg/trunk/motion_planning/collision_space/Makefile
pkg/trunk/motion_planning/collision_space/include/
pkg/trunk/motion_planning/collision_space/lib/
pkg/trunk/motion_planning/collision_space/manifest.xml
pkg/trunk/motion_planning/collision_space/src/
pkg/trunk/motion_planning/collision_space/test/
pkg/trunk/util/collision_space/
pkg/trunk/util/test_collision_space/
Deleted: pkg/trunk/motion_planning/collision_space/CMakeLists.txt
===================================================================
--- pkg/trunk/util/collision_space/CMakeLists.txt 2008-08-07 01:41:05 UTC (rev 2699)
+++ pkg/trunk/motion_planning/collision_space/CMakeLists.txt 2008-08-07 01:44:53 UTC (rev 2702)
@@ -1,8 +0,0 @@
-cmake_minimum_required(VERSION 2.6)
-include(rosbuild)
-rospack(collision_space)
-rospack_add_library(collision_space src/collision_space/environment.cpp src/collision_space/environmentODE.cpp)
-
-# Unit tests
-rospack_add_gtest(test/test_util test/test_util.cpp)
-target_link_libraries(test/test_util collision_space)
Copied: pkg/trunk/motion_planning/collision_space/CMakeLists.txt (from rev 2701, pkg/trunk/util/collision_space/CMakeLists.txt)
===================================================================
--- pkg/trunk/motion_planning/collision_space/CMakeLists.txt (rev 0)
+++ pkg/trunk/motion_planning/collision_space/CMakeLists.txt 2008-08-07 01:44:53 UTC (rev 2702)
@@ -0,0 +1,8 @@
+cmake_minimum_required(VERSION 2.6)
+include(rosbuild)
+rospack(collision_space)
+rospack_add_library(collision_space src/collision_space/environment.cpp src/collision_space/environmentODE.cpp)
+
+# Unit tests
+rospack_add_gtest(test/test_util test/test_util.cpp)
+target_link_libraries(test/test_util collision_space)
Deleted: pkg/trunk/motion_planning/collision_space/Makefile
===================================================================
--- pkg/trunk/util/collision_space/Makefile 2008-08-07 01:41:05 UTC (rev 2699)
+++ pkg/trunk/motion_planning/collision_space/Makefile 2008-08-07 01:44:53 UTC (rev 2702)
@@ -1 +0,0 @@
-include $(shell rospack find mk)/cmake.mk
Copied: pkg/trunk/motion_planning/collision_space/Makefile (from rev 2701, pkg/trunk/util/collision_space/Makefile)
===================================================================
--- pkg/trunk/motion_planning/collision_space/Makefile (rev 0)
+++ pkg/trunk/motion_planning/collision_space/Makefile 2008-08-07 01:44:53 UTC (rev 2702)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Deleted: pkg/trunk/motion_planning/collision_space/manifest.xml
===================================================================
--- pkg/trunk/util/collision_space/manifest.xml 2008-08-07 01:41:05 UTC (rev 2699)
+++ pkg/trunk/motion_planning/collision_space/manifest.xml 2008-08-07 01:44:53 UTC (rev 2702)
@@ -1,18 +0,0 @@
-<package>
- <description brief="A set of collision spaces">
- A set of robot models that can do collision checking
- </description>
-
- <author>Ioan Sucan/is...@wi...</author>
- <license>BSD</license>
-
- <depend package="rosthread"/>
- <depend package="planning_models"/>
- <depend package="opende"/>
- <depend package="gtest"/>
-
- <export>
- <cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lcollision_space"/>
- </export>
-
-</package>
Copied: pkg/trunk/motion_planning/collision_space/manifest.xml (from rev 2701, pkg/trunk/util/collision_space/manifest.xml)
===================================================================
--- pkg/trunk/motion_planning/collision_space/manifest.xml (rev 0)
+++ pkg/trunk/motion_planning/collision_space/manifest.xml 2008-08-07 01:44:53 UTC (rev 2702)
@@ -0,0 +1,18 @@
+<package>
+ <description brief="A set of collision spaces">
+ A set of robot models that can do collision checking
+ </description>
+
+ <author>Ioan Sucan/is...@wi...</author>
+ <license>BSD</license>
+
+ <depend package="rosthread"/>
+ <depend package="planning_models"/>
+ <depend package="opende"/>
+ <depend package="gtest"/>
+
+ <export>
+ <cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lcollision_space"/>
+ </export>
+
+</package>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2008-08-07 01:48:58
|
Revision: 2705
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2705&view=rev
Author: sfkwc
Date: 2008-08-07 01:49:07 +0000 (Thu, 07 Aug 2008)
Log Message:
-----------
moving up to top level
Added Paths:
-----------
pkg/trunk/pr2_srvs/
Removed Paths:
-------------
pkg/trunk/services/pr2_srvs/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2008-08-07 01:49:22
|
Revision: 2706
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2706&view=rev
Author: sfkwc
Date: 2008-08-07 01:49:31 +0000 (Thu, 07 Aug 2008)
Log Message:
-----------
moving up to top level
Added Paths:
-----------
pkg/trunk/robot_srvs/
Removed Paths:
-------------
pkg/trunk/services/robot_srvs/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2008-08-07 18:57:33
|
Revision: 2725
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2725&view=rev
Author: stuglaser
Date: 2008-08-07 18:57:41 +0000 (Thu, 07 Aug 2008)
Log Message:
-----------
Added a factory for registering and instantiating controllers.
Modified Paths:
--------------
pkg/trunk/controllers/generic_controllers/include/generic_controllers/controller.h
pkg/trunk/controllers/generic_controllers/manifest.xml
pkg/trunk/controllers/generic_controllers/src/joint_controller.cpp
pkg/trunk/mechanism/mechanism_control/manifest.xml
Modified: pkg/trunk/controllers/generic_controllers/include/generic_controllers/controller.h
===================================================================
--- pkg/trunk/controllers/generic_controllers/include/generic_controllers/controller.h 2008-08-07 18:55:46 UTC (rev 2724)
+++ pkg/trunk/controllers/generic_controllers/include/generic_controllers/controller.h 2008-08-07 18:57:41 UTC (rev 2725)
@@ -42,6 +42,7 @@
*/
/***************************************************/
+#include <misc_utils/factory.h>
#include <mechanism_model/robot.h>
class TiXmlElement;
@@ -63,6 +64,14 @@
CONTROLLER_VELOCITY, CONTROLLER_AUTOMATIC, ETHERDRIVE_SPEED
};
+class Controller;
+typedef Factory<Controller> ControllerFactory;
+
+#define ROS_REGISTER_CONTROLLER(c) \
+ controller::Controller *ROS_New_##c() { return new c(); } \
+ bool ROS_CONTROLLER_##c = \
+ controller::ControllerFactory::instance().registerType(#c, ROS_New_##c);
+
class Controller
{
public:
Modified: pkg/trunk/controllers/generic_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/generic_controllers/manifest.xml 2008-08-07 18:55:46 UTC (rev 2724)
+++ pkg/trunk/controllers/generic_controllers/manifest.xml 2008-08-07 18:57:41 UTC (rev 2725)
@@ -7,6 +7,7 @@
<depend package="mechanism_model" />
<depend package="wg_robot_description_parser" />
<depend package="stl_utils" />
+ <depend package="misc_utils" />
<url>http://pr.willowgarage.com</url>
<repository>http://pr.willowgarage.com/repos</repository>
<export>
Modified: pkg/trunk/controllers/generic_controllers/src/joint_controller.cpp
===================================================================
--- pkg/trunk/controllers/generic_controllers/src/joint_controller.cpp 2008-08-07 18:55:46 UTC (rev 2724)
+++ pkg/trunk/controllers/generic_controllers/src/joint_controller.cpp 2008-08-07 18:57:41 UTC (rev 2725)
@@ -41,6 +41,8 @@
using namespace controller;
+ROS_REGISTER_CONTROLLER(JointController)
+
//-------------------------------------------------------------------------//
//CONSTRUCTION/DESTRUCTION CALLS
//-------------------------------------------------------------------------//
Modified: pkg/trunk/mechanism/mechanism_control/manifest.xml
===================================================================
--- pkg/trunk/mechanism/mechanism_control/manifest.xml 2008-08-07 18:55:46 UTC (rev 2724)
+++ pkg/trunk/mechanism/mechanism_control/manifest.xml 2008-08-07 18:57:41 UTC (rev 2725)
@@ -9,6 +9,7 @@
<depend package="tinyxml"/>
<depend package="mechanism_model"/>
<depend package="generic_controllers"/>
+<depend package="misc_utils" />
<export>
<cpp cflags='-I${prefix}/include' lflags='-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lmechanism_control'/>
</export>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-08-07 20:34:17
|
Revision: 2741
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2741&view=rev
Author: isucan
Date: 2008-08-07 20:34:25 +0000 (Thu, 07 Aug 2008)
Log Message:
-----------
moved dependency on freeglut in a more appropriate place
Modified Paths:
--------------
pkg/trunk/3rdparty/drawstuff/manifest.xml
pkg/trunk/motion_planning/kinematic_planning/manifest.xml
Modified: pkg/trunk/3rdparty/drawstuff/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/drawstuff/manifest.xml 2008-08-07 20:29:48 UTC (rev 2740)
+++ pkg/trunk/3rdparty/drawstuff/manifest.xml 2008-08-07 20:34:25 UTC (rev 2741)
@@ -9,8 +9,11 @@
<license>LGPL</license>
<url>http://opende.sf.net</url>
<depend package="opende" />
+<sysdepend os="ubuntu" version="8.04-hardy" package="freeglut3-dev"/>
+
<export>
<cpp lflags="-Xlinker -rpath ${prefix}/lib -L${prefix}/lib -ldrawstuff -lGL -lglut" cflags="-I${prefix}/include"/>
<doxymaker external="http://opende.sourceforge.net/docs/"/>
</export>
+
</package>
Modified: pkg/trunk/motion_planning/kinematic_planning/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/manifest.xml 2008-08-07 20:29:48 UTC (rev 2740)
+++ pkg/trunk/motion_planning/kinematic_planning/manifest.xml 2008-08-07 20:34:25 UTC (rev 2741)
@@ -8,5 +8,4 @@
<depend package="xmlparam" />
<depend package="ompl" />
<depend package="collision_space" />
-<sysdepend os="ubuntu" version="8.04-hardy" package="freeglut3-dev"/>
</package>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-08-08 00:14:36
|
Revision: 2787
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2787&view=rev
Author: isucan
Date: 2008-08-08 00:14:46 +0000 (Fri, 08 Aug 2008)
Log Message:
-----------
removing undeground points from the scans
Modified Paths:
--------------
pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
Modified: pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-08-08 00:08:14 UTC (rev 2786)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-08-08 00:14:46 UTC (rev 2787)
@@ -33,7 +33,9 @@
*********************************************************************/
#include <ros/node.h>
+#include <ros/time.h>
#include <robot_srvs/KinematicMotionPlan.h>
+#include <std_msgs/RobotBase2DOdom.h>
class PlanKinematicPath : public ros::node
{
@@ -41,14 +43,22 @@
PlanKinematicPath(void) : ros::node("plan_kinematic_path")
{
+ m_basePos[0] = m_basePos[1] = m_basePos[2] = 0.0;
+ m_haveBasePos = false;
+ subscribe("localizedpose", m_localizedPose, &PlanKinematicPath::localizedPoseCallback);
}
+
+ bool haveBasePos(void) const
+ {
+ return m_haveBasePos;
+ }
void runTest1(void)
{
robot_srvs::KinematicMotionPlan::request req;
robot_srvs::KinematicMotionPlan::response res;
- req.model_id = "pr2::base+arms";
+ req.model_id = "pr2::base";
req.threshold = 1e-6;
req.distance_metric = "L2Square";
@@ -56,19 +66,17 @@
for (unsigned int i = 0 ; i < req.start_state.vals_size ; ++i)
req.start_state.vals[i] = 0.0;
- req.goal_state.set_vals_size(18);
+ req.goal_state.set_vals_size(3);
for (unsigned int i = 0 ; i < req.goal_state.vals_size ; ++i)
- req.goal_state.vals[i] = 0.0;
- for (unsigned int i = req.goal_state.vals_size-7 ; i < req.goal_state.vals_size ; ++i)
- req.goal_state.vals[i] = 0.2;
- req.goal_state.vals[0] = 4.0;
- req.goal_state.vals[1] = 0.0;
- req.goal_state.vals[2] = M_PI;
-
+ {
+ req.goal_state.vals[i] = m_basePos[i] + 1.0;
+ req.start_state.vals[i] = m_basePos[i];
+ }
+
req.allowed_time = 10.0;
- req.volumeMin.x = -5.0; req.volumeMin.y = -5.0; req.volumeMin.z = -5.0;
- req.volumeMax.x = 5.0; req.volumeMax.y = 5.0; req.volumeMax.z = 5.0;
+ req.volumeMin.x = -5.0 + m_basePos[0]; req.volumeMin.y = -5.0 + m_basePos[1]; req.volumeMin.z = 0.0;
+ req.volumeMax.x = 5.0 + m_basePos[0]; req.volumeMax.y = 5.0 + m_basePos[1]; req.volumeMax.z = 0.0;
if (ros::service::call("plan_kinematic_path", req, res))
{
@@ -85,6 +93,20 @@
fprintf(stderr, "Service 'plan_kinematic_path' failed\n");
}
+private:
+
+ void localizedPoseCallback(void)
+ {
+ m_basePos[0] = m_localizedPose.pos.x;
+ m_basePos[1] = m_localizedPose.pos.y;
+ m_basePos[2] = m_localizedPose.pos.th;
+ m_haveBasePos = true;
+ }
+
+ std_msgs::RobotBase2DOdom m_localizedPose;
+ double m_basePos[3];
+ bool m_haveBasePos;
+
};
@@ -93,7 +115,13 @@
ros::init(argc, argv);
PlanKinematicPath plan;
+
+ ros::Duration dur(0.1);
+ while (!plan.haveBasePos())
+ dur.sleep();
+
plan.runTest1();
+
plan.shutdown();
return 0;
Modified: pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
===================================================================
--- pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-08-08 00:08:14 UTC (rev 2786)
+++ pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-08-08 00:14:46 UTC (rev 2787)
@@ -378,7 +378,7 @@
}
rp.body->setDimensions(link->geom->size);
- rp.body->setScale(1.25);
+ rp.body->setScale(1.4);
m_selfSeeParts.push_back(rp);
}
@@ -469,12 +469,15 @@
double y = cloud.pts[k].y;
double z = cloud.pts[k].z;
- bool keep = true;
- for (int i = m_selfSeeParts.size() - 1 ; keep && i >= 0 ; --i)
- keep = !m_selfSeeParts[i].body->containsPoint(x, y, z);
-
- if (keep)
- copy->pts[j++] = cloud.pts[k];
+ if (z > 1e-6)
+ {
+ bool keep = true;
+ for (int i = m_selfSeeParts.size() - 1 ; keep && i >= 0 ; --i)
+ keep = !m_selfSeeParts[i].body->containsPoint(x, y, z);
+
+ if (keep)
+ copy->pts[j++] = cloud.pts[k];
+ }
}
printf("Discarded %d points\n", n - j);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rob...@us...> - 2008-08-08 15:32:31
|
Revision: 2798
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2798&view=rev
Author: rob_wheeler
Date: 2008-08-08 15:32:40 +0000 (Fri, 08 Aug 2008)
Log Message:
-----------
Deprecate etherdrive_old
Added Paths:
-----------
pkg/trunk/deprecated/etherdrive_old/
Removed Paths:
-------------
pkg/trunk/drivers/motor/etherdrive_old/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-08-08 20:46:08
|
Revision: 2818
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2818&view=rev
Author: isucan
Date: 2008-08-08 20:46:14 +0000 (Fri, 08 Aug 2008)
Log Message:
-----------
motion planning in an automatically built environment works :)
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp
pkg/trunk/robot_srvs/srv/KinematicMotionPlan.srv
pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
Added Paths:
-----------
pkg/trunk/world_models/world_3d_map/run.xml
Removed Paths:
-------------
pkg/trunk/world_models/world_3d_map/params.xml
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-08-08 20:44:59 UTC (rev 2817)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-08-08 20:46:14 UTC (rev 2818)
@@ -277,9 +277,13 @@
for (int j = 0 ; j < dim ; ++j)
res.path.states[i].vals[j] = path->states[i]->values[j];
}
+ res.distance = goal->getDifference();
}
else
+ {
res.path.set_states_size(0);
+ res.distance = -1.0;
+ }
/* cleanup */
p.si->clearGoal();
Modified: pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-08-08 20:44:59 UTC (rev 2817)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-08-08 20:46:14 UTC (rev 2818)
@@ -35,6 +35,7 @@
#include <ros/node.h>
#include <ros/time.h>
#include <robot_srvs/KinematicMotionPlan.h>
+#include <robot_msgs/NamedKinematicPath.h>
#include <std_msgs/RobotBase2DOdom.h>
class PlanKinematicPath : public ros::node
@@ -43,6 +44,8 @@
PlanKinematicPath(void) : ros::node("plan_kinematic_path")
{
+ advertise<robot_msgs::NamedKinematicPath>("display_kinematic_path");
+
m_basePos[0] = m_basePos[1] = m_basePos[2] = 0.0;
m_haveBasePos = false;
subscribe("localizedpose", m_localizedPose, &PlanKinematicPath::localizedPoseCallback);
@@ -53,13 +56,12 @@
return m_haveBasePos;
}
- void runTest1(void)
+ void runTestBase(void)
{
robot_srvs::KinematicMotionPlan::request req;
- robot_srvs::KinematicMotionPlan::response res;
req.model_id = "pr2::base";
- req.threshold = 1e-6;
+ req.threshold = 0.01;
req.distance_metric = "L2Square";
req.start_state.set_vals_size(50);
@@ -69,28 +71,66 @@
req.goal_state.set_vals_size(3);
for (unsigned int i = 0 ; i < req.goal_state.vals_size ; ++i)
{
- req.goal_state.vals[i] = m_basePos[i] + 1.0;
+ req.goal_state.vals[i] = m_basePos[i];
req.start_state.vals[i] = m_basePos[i];
}
+ req.goal_state.vals[0] += 4.5;
+
+ req.allowed_time = 5.0;
+
+ req.volumeMin.x = -5.0 + m_basePos[0]; req.volumeMin.y = -5.0 + m_basePos[1]; req.volumeMin.z = 0.0;
+ req.volumeMax.x = 5.0 + m_basePos[0]; req.volumeMax.y = 5.0 + m_basePos[1]; req.volumeMax.z = 0.0;
+
+ performCall(req);
+ }
+
+ void runTestLeftArm(void)
+ {
+ robot_srvs::KinematicMotionPlan::request req;
+
+ req.model_id = "pr2::leftArm";
+ req.threshold = 0.01;
+ req.distance_metric = "L2Square";
+
+ req.start_state.set_vals_size(50);
+ for (unsigned int i = 0 ; i < req.start_state.vals_size ; ++i)
+ req.start_state.vals[i] = 0.0;
+
+ req.goal_state.set_vals_size(7);
+ for (unsigned int i = 0 ; i < req.goal_state.vals_size ; ++i)
+ req.goal_state.vals[i] = 0.1;
+
req.allowed_time = 10.0;
req.volumeMin.x = -5.0 + m_basePos[0]; req.volumeMin.y = -5.0 + m_basePos[1]; req.volumeMin.z = 0.0;
req.volumeMax.x = 5.0 + m_basePos[0]; req.volumeMax.y = 5.0 + m_basePos[1]; req.volumeMax.z = 0.0;
+ performCall(req);
+ }
+
+ void performCall(robot_srvs::KinematicMotionPlan::request &req)
+ {
+ robot_srvs::KinematicMotionPlan::response res;
+ robot_msgs::NamedKinematicPath dpath;
+
if (ros::service::call("plan_kinematic_path", req, res))
{
unsigned int nstates = res.path.get_states_size();
- printf("Obtained solution path with %u states\n", nstates);
+ printf("Obtained %ssolution path with %u states, distance to goal = %f\n",
+ res.distance > req.threshold ? "approximate " : "", nstates, res.distance);
for (unsigned int i = 0 ; i < nstates ; ++i)
{
for (unsigned int j = 0 ; j < res.path.states[i].get_vals_size() ; ++j)
printf("%f ", res.path.states[i].vals[j]);
printf("\n");
- }
+ }
+ dpath.name = req.model_id;
+ dpath.path = res.path;
+ publish("display_kinematic_path", dpath);
}
else
- fprintf(stderr, "Service 'plan_kinematic_path' failed\n");
+ fprintf(stderr, "Service 'plan_kinematic_path' failed\n");
}
private:
@@ -119,9 +159,10 @@
ros::Duration dur(0.1);
while (!plan.haveBasePos())
dur.sleep();
+
+ // plan.runTestLeftArm();
+ plan.runTestBase();
- plan.runTest1();
-
plan.shutdown();
return 0;
Modified: pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp 2008-08-08 20:44:59 UTC (rev 2817)
+++ pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp 2008-08-08 20:46:14 UTC (rev 2818)
@@ -108,10 +108,12 @@
subscribe("display_kinematic_path", m_displayPath, &PlanningWorldViewer::displayPathCallback);
m_collisionSpace = new collision_space::EnvironmentModelODE();
+ m_collisionSpace->setSelfCollision(false);
m_basePos[0] = m_basePos[1] = m_basePos[2] = 0.0;
m_follow = true;
m_displayRobot = true;
m_displayObstacles = true;
+ m_checkCollision = false;
m_collisionSpace->lock();
loadRobotDescriptions();
@@ -172,6 +174,11 @@
m_collisionSpace->lock();
m_collisionSpace->getModel(0)->computeTransforms(m_basePos, group);
m_collisionSpace->updateRobotModel(0);
+ if (m_checkCollision)
+ {
+ ros::Time startTime = ros::Time::now();
+ printf("Collision: %d [%f s]\n", m_collisionSpace->isCollision(0), (ros::Time::now() - startTime).to_double());
+ }
m_collisionSpace->unlock();
}
}
@@ -257,13 +264,25 @@
/* add the model to the collision space */
unsigned int cid = m_collisionSpace->addRobotModel(kmodel);
+ defaultPosition(kmodel, cid);
+ }
+
+ void defaultPosition(planning_models::KinematicModel *kmodel = NULL, unsigned int cid = 0)
+ {
+ if (!kmodel)
+ {
+ if (m_collisionSpace->getModelCount() == 1)
+ kmodel = m_collisionSpace->getModel(0);
+ else
+ return;
+ }
+
double defaultPose[kmodel->stateDimension];
for (unsigned int i = 0 ; i < kmodel->stateDimension ; ++i)
defaultPose[i] = 0.0;
kmodel->computeTransforms(defaultPose);
- m_collisionSpace->updateRobotModel(cid);
-
+ m_collisionSpace->updateRobotModel(cid);
}
unsigned int getRobotCount(void) const
@@ -281,6 +300,16 @@
m_follow = follow;
}
+ bool getCheckCollision(void) const
+ {
+ return m_checkCollision;
+ }
+
+ void setCheckCollision(bool collision)
+ {
+ m_checkCollision = collision;
+ }
+
bool getDisplayRobot(void) const
{
return m_displayRobot;
@@ -334,7 +363,8 @@
bool m_follow;
bool m_displayRobot;
bool m_displayObstacles;
-
+ bool m_checkCollision;
+
};
static PlanningWorldViewer *viewer = NULL;
@@ -365,6 +395,10 @@
case 'F':
viewer->setFollow(!viewer->getFollow());
break;
+ case 'c':
+ case 'C':
+ viewer->setCheckCollision(!viewer->getCheckCollision());
+ break;
case 'R':
viewer->setDisplayRobot(!viewer->getDisplayRobot());
@@ -372,6 +406,9 @@
case 'O':
viewer->setDisplayObstacles(!viewer->getDisplayObstacles());
break;
+ case 'D':
+ viewer->defaultPosition();
+ break;
default:
break;
}
Modified: pkg/trunk/robot_srvs/srv/KinematicMotionPlan.srv
===================================================================
--- pkg/trunk/robot_srvs/srv/KinematicMotionPlan.srv 2008-08-08 20:44:59 UTC (rev 2817)
+++ pkg/trunk/robot_srvs/srv/KinematicMotionPlan.srv 2008-08-08 20:46:14 UTC (rev 2818)
@@ -59,5 +59,10 @@
# includes start and goal states) to define the motions for the
# robot. If more intermediate states are needed, linear interpolation
# is to be assumed.
+robot_msgs/KinematicPath path
-robot_msgs/KinematicPath path
+
+# The threshold that was actually achieved. If the planner did not have enough time,
+# it may return a distance larger than the desired threshold. The user may choose to
+# discard the path
+float64 distance
Deleted: pkg/trunk/world_models/world_3d_map/params.xml
===================================================================
--- pkg/trunk/world_models/world_3d_map/params.xml 2008-08-08 20:44:59 UTC (rev 2817)
+++ pkg/trunk/world_models/world_3d_map/params.xml 2008-08-08 20:46:14 UTC (rev 2818)
@@ -1,7 +0,0 @@
-<params>
- <double name="world_3d_map/max_publish_frequency">0.3</double> <!-- Hz -->
- <double name="world_3d_map/retain_pointcloud_duration">60</double> <!-- seconds -->
- <double name="world_3d_map/retain_pointcloud_fraction">0.02</double> <!-- percentage (between 0 and 1) -->
- <int name="world_3d_map/verbosity_level">1</int> <!-- integer value -->
- <string name="world_3d_map/robot_model">robotdesc/pr2</string> <!-- string value -->
-</params>
Added: pkg/trunk/world_models/world_3d_map/run.xml
===================================================================
--- pkg/trunk/world_models/world_3d_map/run.xml (rev 0)
+++ pkg/trunk/world_models/world_3d_map/run.xml 2008-08-08 20:46:14 UTC (rev 2818)
@@ -0,0 +1,11 @@
+<launch>
+
+ <param name="world_3d_map/max_publish_frequency" type="double" value="0.3" /> <!-- Hz -->
+ <param name="world_3d_map/retain_pointcloud_duration" type="double" value="60.0" /> <!-- seconds -->
+ <param name="world_3d_map/retain_pointcloud_fraction" type="double" value="0.02" /> <!-- percentage (between 0 and 1) -->
+ <param name="world_3d_map/verbosity_level" type="int" value="1" /> <!-- integer value -->
+ <param name="world_3d_map/robot_model" type="str" value="robotdesc/pr2" /> <!-- string value -->
+
+ <node pkg="world_3d_map" type="world_3d_map" respawn="false" />
+
+</launch>
Modified: pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
===================================================================
--- pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-08-08 20:44:59 UTC (rev 2817)
+++ pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-08-08 20:46:14 UTC (rev 2818)
@@ -310,7 +310,7 @@
if (m_shouldPublish)
{
m_worldDataMutex.lock();
- if (m_active)
+ if (m_active && m_currentWorld.size() > 0)
{
PointCloudFloat32 toPublish;
toPublish.header = m_currentWorld.back()->header;
@@ -378,7 +378,7 @@
}
rp.body->setDimensions(link->geom->size);
- rp.body->setScale(1.4);
+ rp.body->setScale(1.5);
m_selfSeeParts.push_back(rp);
}
@@ -469,7 +469,7 @@
double y = cloud.pts[k].y;
double z = cloud.pts[k].z;
- if (z > 1e-6)
+ if (z > 0.01)
{
bool keep = true;
for (int i = m_selfSeeParts.size() - 1 ; keep && i >= 0 ; --i)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-08-08 21:32:32
|
Revision: 2822
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2822&view=rev
Author: tfoote
Date: 2008-08-08 21:32:36 +0000 (Fri, 08 Aug 2008)
Log Message:
-----------
upgrading libTF exceptions to use std::runtime_error as a base and now there is a frame id associated with extrapolation exceptions
Modified Paths:
--------------
pkg/trunk/exec_trex/ROSNode.cc
pkg/trunk/nav/wavefront_player/wavefront_player.cc
pkg/trunk/simulators/rosgazebo/test/groundtruthtransform.cpp
pkg/trunk/util/libTF/include/libTF/Pose3DCache.h
pkg/trunk/util/libTF/include/libTF/libTF.h
pkg/trunk/util/libTF/src/Pose3DCache.cpp
pkg/trunk/util/libTF/src/libTF.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/laser_scan_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/marker_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/point_cloud_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_model_visualizer.cpp
pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp
pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
Modified: pkg/trunk/exec_trex/ROSNode.cc
===================================================================
--- pkg/trunk/exec_trex/ROSNode.cc 2008-08-08 21:10:11 UTC (rev 2821)
+++ pkg/trunk/exec_trex/ROSNode.cc 2008-08-08 21:32:36 UTC (rev 2822)
@@ -408,7 +408,7 @@
} catch(libTF::TransformReference::LookupException& ex) {
debugMsg("ROSNode::VS", "no global->local Tx yet");
return NULL;
- } catch(libTF::Pose3DCache::ExtrapolateException& ex) {
+ } catch(libTF::TransformReference::ExtrapolateException& ex) {
debugMsg("ROSNode::VS",
"libTF::Quaternion3D::ExtrapolateException occured");
}
@@ -751,7 +751,7 @@
delete[] pts.pts;
return;
}
- catch(libTF::Pose3DCache::ExtrapolateException& ex)
+ catch(libTF::TransformReference::ExtrapolateException& ex)
{
puts("extrapolation required");
delete[] pts.pts;
Modified: pkg/trunk/nav/wavefront_player/wavefront_player.cc
===================================================================
--- pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-08-08 21:10:11 UTC (rev 2821)
+++ pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-08-08 21:32:36 UTC (rev 2822)
@@ -447,7 +447,7 @@
puts("no global->local Tx yet");
return;
}
- catch(libTF::Pose3DCache::ExtrapolateException& ex)
+ catch(libTF::TransformReference::ExtrapolateException& ex)
{
// puts("extrapolation required");
continue;
@@ -587,7 +587,7 @@
this->stopRobot();
return;
}
- catch(libTF::Pose3DCache::ExtrapolateException& ex)
+ catch(libTF::TransformReference::ExtrapolateException& ex)
{
// this should never happen
puts("WARNING: extrapolation failed!");
Modified: pkg/trunk/simulators/rosgazebo/test/groundtruthtransform.cpp
===================================================================
--- pkg/trunk/simulators/rosgazebo/test/groundtruthtransform.cpp 2008-08-08 21:10:11 UTC (rev 2821)
+++ pkg/trunk/simulators/rosgazebo/test/groundtruthtransform.cpp 2008-08-08 21:32:36 UTC (rev 2822)
@@ -32,7 +32,7 @@
std::cerr << "LookupException in lookup(\"FRAMEID_ROBOT\"): " << ex.what() << "\n";
std::cout << "LookupException in lookup(\"FRAMEID_ROBOT\"): " << ex.what();
return;
- } catch(libTF::Pose3DCache::ExtrapolateException& ex) {
+ } catch(libTF::TransformReference::ExtrapolateException& ex) {
std::cerr << "ExtrapolateException in lookup(\"FRAMEID_ROBOT\"): " << ex.what() << "\n";
std::cout << "ExtrapolateException in lookup(\"FRAMEID_ROBOT\"): " << ex.what();
return;
@@ -50,7 +50,7 @@
std::cerr << "LookupException in transformPose(\"FRAMEID_ODOM\", robotPose): " << ex.what() << "\n";
std::cout << "LookupException in transformPose(\"FRAMEID_ODOM\", robotPose): " << ex.what();
return;
- } catch(libTF::Pose3DCache::ExtrapolateException& ex) {
+ } catch(libTF::TransformReference::ExtrapolateException& ex) {
std::cerr << "ExtrapolateException in transformPose(\"FRAMEID_ODOM\", robotPose): " << ex.what() << "\n";
std::cout << "ExtrapolateException in transformPose(\"FRAMEID_ODOM\", robotPose): " << ex.what();
return;
Modified: pkg/trunk/util/libTF/include/libTF/Pose3DCache.h
===================================================================
--- pkg/trunk/util/libTF/include/libTF/Pose3DCache.h 2008-08-08 21:10:11 UTC (rev 2821)
+++ pkg/trunk/util/libTF/include/libTF/Pose3DCache.h 2008-08-08 21:32:36 UTC (rev 2822)
@@ -36,6 +36,7 @@
#include <iostream>
#include <sstream>
+#include <stdexcept>
#include <newmat10/newmat.h>
#include <newmat10/newmatio.h>
#include <cmath>
@@ -79,15 +80,10 @@
/** \brief An exception class to notify that the requested value would have required extrapolation, and extrapolation is not allowed.
*
*/
- class ExtrapolateException : public std::exception
+ class ExtrapolationException : public std::runtime_error
{
public:
- ExtrapolateException(const std::string errorDescription) {
- errorDescription_ = new std::string(errorDescription);
- };
- std::string * errorDescription_;
- ~ExtrapolateException() throw() { delete errorDescription_; };
- virtual const char* what() const throw() { return errorDescription_->c_str(); };
+ ExtrapolationException(const std::string errorDescription) : std::runtime_error(errorDescription) { ; };
};
/** \brief The constructor
Modified: pkg/trunk/util/libTF/include/libTF/libTF.h
===================================================================
--- pkg/trunk/util/libTF/include/libTF/libTF.h 2008-08-08 21:10:11 UTC (rev 2821)
+++ pkg/trunk/util/libTF/include/libTF/libTF.h 2008-08-08 21:32:36 UTC (rev 2822)
@@ -32,6 +32,7 @@
#ifndef LIBTF_HH
#define LIBTF_HH
+#include <stdexcept>
#include <iostream>
#include <iomanip>
#include <newmat10/newmat.h>
@@ -291,18 +292,12 @@
* This is an exception class to be thrown in the case
* that the Reference Frame tree is not connected between
* the frames requested. */
- class ConnectivityException : public std::exception
+ class ConnectivityException : public std::runtime_error
{
public:
- ConnectivityException(const std::string errorDescription) {
- errorDescription_ = new std::string(errorDescription);
- };
- std::string * errorDescription_;
- ~ConnectivityException() throw() { delete errorDescription_; };
- virtual const char* what() const throw() { return errorDescription_->c_str(); };
- private:
+ ConnectivityException(const std::string errorDescription) : std::runtime_error(errorDescription) { ; };
};
-
+
/** \brief An exception class to notify that the search for connectivity descended too deep.
*
* This is an exception class which will be thrown if the tree search
@@ -310,19 +305,21 @@
* infinitely looping in the case that a tree was malformed and
* became cyclic.
*/
- class MaxDepthException : public std::exception
+ class MaxDepthException : public std::runtime_error
{
public:
- MaxDepthException(const std::string errorDescription) {
- errorDescription_ = new std::string(errorDescription);
- };
- std::string * errorDescription_;
- ~MaxDepthException() throw() { delete errorDescription_; };
- virtual const char* what() const throw() { return errorDescription_->c_str(); };
- private:
+ MaxDepthException(const std::string errorDescription): std::runtime_error(errorDescription) { ; };
};
+
+ /** \brief An exception class to notify that the requested value would have required extrapolation, and extrapolation is not allowed.
+ *
+ */
+ class ExtrapolateException : public std::runtime_error
+ {
+ public:
+ ExtrapolateException(const std::string &errorDescription): std::runtime_error(errorDescription) { ; };
+ };
-
private:
/** \brief The internal storage class for ReferenceTransform.
Modified: pkg/trunk/util/libTF/src/Pose3DCache.cpp
===================================================================
--- pkg/trunk/util/libTF/src/Pose3DCache.cpp 2008-08-08 21:10:11 UTC (rev 2821)
+++ pkg/trunk/util/libTF/src/Pose3DCache.cpp 2008-08-08 21:32:36 UTC (rev 2822)
@@ -159,16 +159,17 @@
bool retval = false;
pthread_mutex_lock(&linked_list_mutex);
- num_nodes = findClosest(p_temp_1,p_temp_2, time, time_diff);
-
- if (num_nodes == 0)
- retval= false;
- else if (num_nodes == 1)
+ try
+ {
+ num_nodes = findClosest(p_temp_1,p_temp_2, time, time_diff);
+ if (num_nodes == 0)
+ retval= false;
+ else if (num_nodes == 1)
{
memcpy(&buff, &p_temp_1, sizeof(Pose3DStorage));
retval = true;
}
- else
+ else
{
if(interpolating)
interpolate(p_temp_1, p_temp_2, time, buff);
@@ -176,7 +177,13 @@
buff = p_temp_1;
retval = true;
}
-
+ }
+ catch (ExtrapolationException &ex)
+ {
+ pthread_mutex_unlock(&linked_list_mutex);
+ throw ex;
+ }
+
pthread_mutex_unlock(&linked_list_mutex);
@@ -377,8 +384,8 @@
std::stringstream ss;
ss << "Extrapolation Too Far: target_time = "<< target_time <<", closest data at "
<< two.time << " and " << one.time <<" which are farther away than max_extrapolation_time "
- << max_extrapolation_time <<".";
- throw ExtrapolateException(ss.str());
+ << max_extrapolation_time <<" at "<< target_time - two.time<< " and " << target_time - one.time <<" respectively.";
+ throw ExtrapolationException(ss.str());
}
return 2;
Modified: pkg/trunk/util/libTF/src/libTF.cpp
===================================================================
--- pkg/trunk/util/libTF/src/libTF.cpp 2008-08-08 21:10:11 UTC (rev 2821)
+++ pkg/trunk/util/libTF/src/libTF.cpp 2008-08-08 21:32:36 UTC (rev 2822)
@@ -394,11 +394,27 @@
for (unsigned int i = 0; i < lists.inverseTransforms.size(); i++)
{
- retMat *= getFrame(lists.inverseTransforms[lists.inverseTransforms.size() -1 - i])->getMatrix(time); //Reverse to get left multiply
+ try {
+ retMat *= getFrame(lists.inverseTransforms[lists.inverseTransforms.size() -1 - i])->getMatrix(time); //Reverse to get left multiply
+ }
+ catch (libTF::Pose3DCache::ExtrapolationException &ex)
+ {
+ std::stringstream ss;
+ ss << "Frame "<< lists.inverseTransforms[lists.inverseTransforms.size() -1 - i] << " is out of date. " << ex.what();
+ throw ExtrapolateException(ss.str());
+ }
}
for (unsigned int i = 0; i < lists.forwardTransforms.size(); i++)
{
- retMat *= getFrame(lists.forwardTransforms[i])->getInverseMatrix(time); //Do this list backwards(from backwards) for it was generated traveling the wrong way
+ try {
+ retMat *= getFrame(lists.forwardTransforms[i])->getInverseMatrix(time); //Do this list backwards(from backwards) for it was generated traveling the wrong way
+ }
+ catch (libTF::Pose3DCache::ExtrapolationException &ex)
+ {
+ std::stringstream ss;
+ ss << "Frame "<< lists.forwardTransforms[i] << " is out of date. " << ex.what();
+ throw ExtrapolateException(ss.str());
+ }
}
return retMat;
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/laser_scan_visualizer.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/laser_scan_visualizer.cpp 2008-08-08 21:10:11 UTC (rev 2821)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/laser_scan_visualizer.cpp 2008-08-08 21:32:36 UTC (rev 2822)
@@ -199,7 +199,7 @@
{
printf( "Error transforming laser scan '%s': %s\n", m_Name.c_str(), e.what() );
}
- catch(libTF::Pose3DCache::ExtrapolateException& e)
+ catch(libTF::TransformReference::ExtrapolateException& e)
{
printf( "Error transforming laser scan '%s': %s\n", m_Name.c_str(), e.what() );
}
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/marker_visualizer.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/marker_visualizer.cpp 2008-08-08 21:10:11 UTC (rev 2821)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/marker_visualizer.cpp 2008-08-08 21:32:36 UTC (rev 2822)
@@ -233,7 +233,7 @@
{
printf( "Error transforming marker '%d': %s\n", message.id, e.what() );
}
- catch(libTF::Pose3DCache::ExtrapolateException& e)
+ catch(libTF::TransformReference::ExtrapolateException& e)
{
printf( "Error transforming marker '%d': %s\n", message.id, e.what() );
}
@@ -256,7 +256,7 @@
{
printf( "Error transforming marker '%d': %s\n", message.id, e.what() );
}
- catch(libTF::Pose3DCache::ExtrapolateException& e)
+ catch(libTF::TransformReference::ExtrapolateException& e)
{
printf( "Error transforming marker '%d': %s\n", message.id, e.what() );
}
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/point_cloud_visualizer.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/point_cloud_visualizer.cpp 2008-08-08 21:10:11 UTC (rev 2821)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/point_cloud_visualizer.cpp 2008-08-08 21:32:36 UTC (rev 2822)
@@ -162,7 +162,7 @@
{
printf( "Error transforming point cloud '%s': %s\n", m_Name.c_str(), e.what() );
}
- catch(libTF::Pose3DCache::ExtrapolateException& e)
+ catch(libTF::TransformReference::ExtrapolateException& e)
{
printf( "Error transforming point cloud '%s': %s\n", m_Name.c_str(), e.what() );
}
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_model_visualizer.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_model_visualizer.cpp 2008-08-08 21:10:11 UTC (rev 2821)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_model_visualizer.cpp 2008-08-08 21:32:36 UTC (rev 2822)
@@ -212,7 +212,7 @@
{
printf( "Error transforming from frame '%s' to frame '%s': %s\n", name.c_str(), m_TargetFrame.c_str(), e.what() );
}
- catch(libTF::Pose3DCache::ExtrapolateException& e)
+ catch(libTF::TransformReference::ExtrapolateException& e)
{
printf( "Error transforming from frame '%s' to frame '%s': %s\n", name.c_str(), m_TargetFrame.c_str(), e.what() );
}
Modified: pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp 2008-08-08 21:10:11 UTC (rev 2821)
+++ pkg/trunk/world_models/costmap_2d/src/costmap_2d_ros.cpp 2008-08-08 21:32:36 UTC (rev 2822)
@@ -157,7 +157,7 @@
puts("no global->local Tx yet");
return;
}
- catch(libTF::Pose3DCache::ExtrapolateException& ex)
+ catch(libTF::TransformReference::ExtrapolateException& ex)
{
puts("extrapolation required");
return;
Modified: pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
===================================================================
--- pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-08-08 21:10:11 UTC (rev 2821)
+++ pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-08-08 21:32:36 UTC (rev 2822)
@@ -253,7 +253,7 @@
fprintf(stderr, "Discarding pointcloud: Transform reference lookup exception\n");
success = false;
}
- catch(libTF::Pose3DCache::ExtrapolateException& ex)
+ catch(libTF::TransformReference::ExtrapolateException& ex)
{
fprintf(stderr, "Discarding pointcloud: Extrapolation exception: %s\n", ex.what());
success = false;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2008-08-08 21:55:17
|
Revision: 2828
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2828&view=rev
Author: stuglaser
Date: 2008-08-08 21:55:23 +0000 (Fri, 08 Aug 2008)
Log Message:
-----------
MechanismControlNode is MechanismControl on ROS. It's used by the gazebo_actuators gazebo plugin.
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_actuators.h
pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp
pkg/trunk/mechanism/mechanism_control/CMakeLists.txt
pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
pkg/trunk/mechanism/mechanism_control/manifest.xml
pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
pkg/trunk/util/misc_utils/include/misc_utils/factory.h
Added Paths:
-----------
pkg/trunk/mechanism/mechanism_control/srv/
pkg/trunk/mechanism/mechanism_control/srv/ListControllerTypes.srv
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_actuators.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_actuators.h 2008-08-08 21:54:14 UTC (rev 2827)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_actuators.h 2008-08-08 21:55:23 UTC (rev 2828)
@@ -60,7 +60,8 @@
Model *parent_model_;
HardwareInterface hw_;
- MechanismControl mc_;
+ MechanismControlNode mc_;
+ ros::node *rosnode_;
// Each joint in joints_ corresponds to the joint with the same
// index in mech_joints_. The mech_joints_ vector exists so that
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp 2008-08-08 21:54:14 UTC (rev 2827)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp 2008-08-08 21:55:23 UTC (rev 2828)
@@ -50,10 +50,21 @@
GazeboActuators::GazeboActuators(Entity *parent)
: Controller(parent), hw_(0), mc_(&hw_)
{
- this->parent_model_ = dynamic_cast<Model*>(this->parent);
+ this->parent_model_ = dynamic_cast<Model*>(this->parent);
- if (!this->parent_model_)
- gzthrow("GazeboActuators controller requires a Model as its parent");
+ if (!this->parent_model_)
+ gzthrow("GazeboActuators controller requires a Model as its parent");
+
+ rosnode_ = ros::g_node;
+ int argc = 0;
+ char** argv = NULL;
+ if (rosnode_ == NULL)
+ {
+ // start a ros node if none exist
+ ros::init(argc,argv);
+ rosnode_ = new ros::node("ros_gazebo",ros::node::DONT_HANDLE_SIGINT);
+ printf("-------------------- starting node in GazeboActuators\n");
+ }
}
GazeboActuators::~GazeboActuators()
Modified: pkg/trunk/mechanism/mechanism_control/CMakeLists.txt
===================================================================
--- pkg/trunk/mechanism/mechanism_control/CMakeLists.txt 2008-08-08 21:54:14 UTC (rev 2827)
+++ pkg/trunk/mechanism/mechanism_control/CMakeLists.txt 2008-08-08 21:55:23 UTC (rev 2828)
@@ -1,5 +1,6 @@
cmake_minimum_required(VERSION 2.6)
include(rosbuild)
rospack(mechanism_control)
+genmsg()
+gensrv()
rospack_add_library(mechanism_control src/mechanism_control.cpp)
-
Modified: pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
===================================================================
--- pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2008-08-08 21:54:14 UTC (rev 2827)
+++ pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2008-08-08 21:55:23 UTC (rev 2828)
@@ -41,18 +41,21 @@
#include <map>
#include <string>
#include <vector>
+#include "ros/node.h"
#include <tinyxml/tinyxml.h>
#include <hardware_interface/hardware_interface.h>
#include <mechanism_model/robot.h>
#include <rosthread/mutex.h>
#include <generic_controllers/controller.h>
+#include "mechanism_control/ListControllerTypes.h"
+
typedef controller::Controller* (*ControllerAllocator)();
class MechanismControl {
public:
MechanismControl(HardwareInterface *hw);
- ~MechanismControl();
+ virtual ~MechanismControl();
// Real-time functions
void update();
@@ -77,4 +80,19 @@
controller::Controller* controllers_[MAX_NUM_CONTROLLERS];
};
+/*
+ * Exposes MechanismControl's interface over ROS
+ */
+class MechanismControlNode : public MechanismControl, public ros::node
+{
+public:
+ MechanismControlNode(HardwareInterface *hw);
+ virtual ~MechanismControlNode() {}
+
+ bool listControllerTypes(mechanism_control::ListControllerTypes::request &req,
+ mechanism_control::ListControllerTypes::response &resp);
+
+private:
+};
+
#endif /* MECHANISM_CONTROL_H */
Modified: pkg/trunk/mechanism/mechanism_control/manifest.xml
===================================================================
--- pkg/trunk/mechanism/mechanism_control/manifest.xml 2008-08-08 21:54:14 UTC (rev 2827)
+++ pkg/trunk/mechanism/mechanism_control/manifest.xml 2008-08-08 21:55:23 UTC (rev 2828)
@@ -5,13 +5,16 @@
<license>BSD</license>
<url>http://pr.willowgarage.com</url>
<depend package="rosthread"/>
+<depend package="roscpp" />
<depend package="hardware_interface"/>
<depend package="tinyxml"/>
<depend package="mechanism_model"/>
<depend package="generic_controllers"/>
<depend package="misc_utils" />
+<depend package="rospy" />
<export>
- <cpp cflags='-I${prefix}/include' lflags='-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lmechanism_control'/>
+ <cpp cflags='-I${prefix}/include -I${prefix}/msg/cpp -I${prefix}/srv/cpp'
+ lflags='-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lmechanism_control'/>
</export>
</package>
Modified: pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-08-08 21:54:14 UTC (rev 2827)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-08-08 21:55:23 UTC (rev 2828)
@@ -175,3 +175,23 @@
return addController(c);
}
+
+
+
+MechanismControlNode::MechanismControlNode(HardwareInterface *hw)
+ : MechanismControl(hw), ros::node("MechanismControl")
+{
+ advertise_service("list_controller_types", &MechanismControlNode::listControllerTypes);
+}
+
+
+bool MechanismControlNode::listControllerTypes(
+ mechanism_control::ListControllerTypes::request &req,
+ mechanism_control::ListControllerTypes::response &resp)
+{
+ std::vector<std::string> types;
+ controller::ControllerFactory::instance().getTypes(&types);
+ resp.set_types_vec(types);
+ return true;
+}
+
Added: pkg/trunk/mechanism/mechanism_control/srv/ListControllerTypes.srv
===================================================================
--- pkg/trunk/mechanism/mechanism_control/srv/ListControllerTypes.srv (rev 0)
+++ pkg/trunk/mechanism/mechanism_control/srv/ListControllerTypes.srv 2008-08-08 21:55:23 UTC (rev 2828)
@@ -0,0 +1,2 @@
+---
+string[] types
\ No newline at end of file
Modified: pkg/trunk/util/misc_utils/include/misc_utils/factory.h
===================================================================
--- pkg/trunk/util/misc_utils/include/misc_utils/factory.h 2008-08-08 21:54:14 UTC (rev 2827)
+++ pkg/trunk/util/misc_utils/include/misc_utils/factory.h 2008-08-08 21:55:23 UTC (rev 2828)
@@ -31,6 +31,7 @@
#include <string>
#include <map>
+#include <vector>
template <class BaseResult,
class Constructor = BaseResult* (*)()>
@@ -60,6 +61,15 @@
return true;
}
+ void getTypes(std::vector<std::string> *result)
+ {
+ result->resize(types_.size());
+ int i = 0;
+ typename ConstructorMap::const_iterator it;
+ for (it = types_.begin(); it != types_.end(); ++it)
+ (*result)[i++] = it->first;
+ }
+
private:
Factory() {}
~Factory() {}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <adv...@us...> - 2008-08-08 22:35:57
|
Revision: 2837
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2837&view=rev
Author: advaitjain
Date: 2008-08-08 22:35:59 +0000 (Fri, 08 Aug 2008)
Log Message:
-----------
* kinematics now works from robot_kinematics. will soon move libKDL to deprecated.
* working object grasping demo in simulation.
* changed manifest.xml of rosgazebo, pr2_gazebo to depend on robot_kinematics instead of libKDL.
* pr2_gazebo - contoller initialisation has been commented.
* added pr2.xml.nolimits -- for use with object grasping demo.
Modified Paths:
--------------
pkg/trunk/clean_rosgazebo.scp
pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
pkg/trunk/drivers/robot/pr2/libpr2API/manifest.xml
pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
pkg/trunk/manip/overhead_grasp_behavior/manifest.xml
pkg/trunk/manip/overhead_grasp_behavior/src/overhead_grasp.cc
pkg/trunk/manip/pr2_kinematic_controllers/manifest.xml
pkg/trunk/manip/pr2_kinematic_controllers/src/pr2_kinematic_controllers.cc
pkg/trunk/manip/pr2_kinematic_controllers/test/test_pr2_kin_con.cc
pkg/trunk/manip/teleop_arm_keyboard/Makefile
pkg/trunk/robot_control_loops/pr2_gazebo/manifest.xml
pkg/trunk/robot_control_loops/pr2_gazebo/src/pr2_gazebo.cpp
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_tablegrasp.world
pkg/trunk/simulators/rosgazebo/CMakeLists.txt
pkg/trunk/simulators/rosgazebo/manifest.xml
pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
pkg/trunk/util/kinematics/libKDL/src/kdl_kinematics.cpp
pkg/trunk/util/kinematics/libKDL/test/pr2_kin_test.cpp
Added Paths:
-----------
pkg/trunk/drivers/robot/pr2/libpr2API/src/old/kinematic_controllers.h
pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml.nolimits
Removed Paths:
-------------
pkg/trunk/drivers/robot/pr2/libpr2API/include/kinematic_controllers.h
Modified: pkg/trunk/clean_rosgazebo.scp
===================================================================
--- pkg/trunk/clean_rosgazebo.scp 2008-08-08 22:27:44 UTC (rev 2836)
+++ pkg/trunk/clean_rosgazebo.scp 2008-08-08 22:35:59 UTC (rev 2837)
@@ -29,6 +29,7 @@
# clean kinematics
(cd util/kinematics/libKinematics ; rm -f lib/* ;make clean)
(cd util/kinematics/libKDL ; rm -f lib/* ;make clean)
+(cd util/kinematics/robot_kinematics ; rm -f lib/* ;make clean)
# clean visualization
(cd visualization/irrlicht_viewer ; rm -f lib/* ;make clean)
Deleted: pkg/trunk/drivers/robot/pr2/libpr2API/include/kinematic_controllers.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/include/kinematic_controllers.h 2008-08-08 22:27:44 UTC (rev 2836)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/include/kinematic_controllers.h 2008-08-08 22:35:59 UTC (rev 2837)
@@ -1,25 +0,0 @@
-#ifndef KINEMATIC_CONTROLLERS_H
-#define KINEMATIC_CONTROLLERS_H
-
-#include <libpr2API/pr2API.h>
-
-class kinematic_controllers
-{
- public:
- PR2::PR2Robot* myPR2;
-
- public:
- kinematic_controllers();
-
- // linear interpolation between current and desired position.
- // p,r -- desired pose, class stores current joint angles.
- void go(KDL::Vector p, KDL::Rotation r, double step_size);
- void init();
-
-
-};
-
-
-
-#endif
-
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h 2008-08-08 22:27:44 UTC (rev 2836)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h 2008-08-08 22:35:59 UTC (rev 2837)
@@ -41,7 +41,9 @@
#include <libpr2HW/pr2HW.h>
-#include <libKDL/kdl_kinematics.h> // for kinematics using KDL -- util/kinematics/libKDL
+//#include <libKDL/kdl_kinematics.h> // for kinematics using KDL -- util/kinematics/libKDL
+#include <robot_kinematics/serial_chain.h>
+#include <robot_kinematics/robot_kinematics.h>
namespace PR2
{
@@ -618,7 +620,8 @@
protected: PR2Arm myArm;
protected: PR2Head myHead;
- public: PR2_kinematics pr2_kin; // for kinematics using KDL.
+ public: robot_kinematics::SerialChain *right_arm_chain_;
+ public: robot_kinematics::RobotKinematics pr2_kin;
};
}
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/manifest.xml
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/manifest.xml 2008-08-08 22:27:44 UTC (rev 2836)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/manifest.xml 2008-08-08 22:35:59 UTC (rev 2837)
@@ -11,7 +11,8 @@
<depend package="pr2Core"/>
<depend package="libpr2HW"/>
<depend package="libKinematics"/>
-<depend package="libKDL"/>
+<!-- <depend package="libKDL"/> -->
+<depend package="robot_kinematics"/>
<depend package="rosTF"/>
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lpr2API"/>
Copied: pkg/trunk/drivers/robot/pr2/libpr2API/src/old/kinematic_controllers.h (from rev 2663, pkg/trunk/drivers/robot/pr2/libpr2API/include/kinematic_controllers.h)
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/old/kinematic_controllers.h (rev 0)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/old/kinematic_controllers.h 2008-08-08 22:35:59 UTC (rev 2837)
@@ -0,0 +1,25 @@
+#ifndef KINEMATIC_CONTROLLERS_H
+#define KINEMATIC_CONTROLLERS_H
+
+#include <libpr2API/pr2API.h>
+
+class kinematic_controllers
+{
+ public:
+ PR2::PR2Robot* myPR2;
+
+ public:
+ kinematic_controllers();
+
+ // linear interpolation between current and desired position.
+ // p,r -- desired pose, class stores current joint angles.
+ void go(KDL::Vector p, KDL::Rotation r, double step_size);
+ void init();
+
+
+};
+
+
+
+#endif
+
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-08-08 22:27:44 UTC (rev 2836)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-08-08 22:35:59 UTC (rev 2837)
@@ -3,7 +3,10 @@
#include <pr2Core/pr2Misc.h>
#include <math.h>
+#include <robot_kinematics/robot_kinematics.h>
+
using namespace PR2;
+using namespace robot_kinematics;
PR2Robot::PR2Robot()
{
@@ -15,8 +18,16 @@
PR2_ERROR_CODE PR2Robot::InitializeRobot()
{
// initialize robot
- hw.Init();
- return PR2_ALL_OK;
+ hw.Init();
+
+ //------- what is the best way to initialise the kinematics? -------
+ char *c_filename = getenv("ROS_PACKAGE_PATH");
+ std::stringstream filename;
+ filename << c_filename << "/robot_descriptions/wg_robot_description/pr2/pr2.xml" ;
+
+ pr2_kin.loadXML(filename.str());
+ right_arm_chain_ = pr2_kin.getSerialChain("rightArm");
+ return PR2_ALL_OK;
}
PR2_ERROR_CODE PR2Robot::CalibrateRobot()
@@ -99,7 +110,7 @@
*/
PR2_ERROR_CODE PR2Robot::SetArmCartesianPosition(PR2_MODEL_ID id, const KDL::Frame &f, const KDL::JntArray &q_init, KDL::JntArray &q_out)
{
- if (this->pr2_kin.IK(f) == false)
+ if (this->right_arm_chain_->computeIK(f) == false)
{
printf("[libpr2API]<pr2API.cpp> Could not compute Inv Kin.\n");
return PR2_ALL_OK;
@@ -113,10 +124,11 @@
/*
* This is the function which should be used -- Advait
+ * only right_chain is currently handled.
*/
PR2_ERROR_CODE PR2Robot::SetArmCartesianPosition(PR2_MODEL_ID id, const KDL::Frame &f, bool &reachable)
{
- reachable = this->pr2_kin.IK(f);
+ reachable = this->right_arm_chain_->computeIK(f);
if (reachable == false)
{
printf("[libpr2API]<pr2API.cpp> Could not compute Inv Kin.\n");
@@ -124,11 +136,11 @@
}
for(int ii = 0; ii < 7; ii++)
- hw.SetJointServoCmd((PR2::PR2_JOINT_ID) (JointStart[id]+ii),(*this->pr2_kin.q_IK_result)(ii),0);
+ hw.SetJointServoCmd((PR2::PR2_JOINT_ID) (JointStart[id]+ii),(*this->right_arm_chain_->q_IK_result)(ii),0);
- KDL::JntArray *t = this->pr2_kin.q_IK_result;
- this->pr2_kin.q_IK_result = this->pr2_kin.q_IK_guess;
- this->pr2_kin.q_IK_guess = t; // update guess with the computed IK result.
+ KDL::JntArray *t = this->right_arm_chain_->q_IK_result;
+ this->right_arm_chain_->q_IK_result = this->right_arm_chain_->q_IK_guess;
+ this->right_arm_chain_->q_IK_guess = t; // update guess with the computed IK result.
return PR2_ALL_OK;
@@ -544,7 +556,7 @@
return PR2_ERROR;
double pos,vel;
- for(int ii = JointStart[id]; ii <= JointEnd[id]; ii++)
+ for(int ii = JointStart[id]; ii < JointEnd[id]; ii++) // KDL does not use the gripper and doesn't need the gripper joint angle.
{
hw.GetJointServoCmd((PR2_JOINT_ID)ii,&pos,&vel);
q(ii-JointStart[id]) = pos;
Modified: pkg/trunk/manip/overhead_grasp_behavior/manifest.xml
===================================================================
--- pkg/trunk/manip/overhead_grasp_behavior/manifest.xml 2008-08-08 22:27:44 UTC (rev 2836)
+++ pkg/trunk/manip/overhead_grasp_behavior/manifest.xml 2008-08-08 22:35:59 UTC (rev 2837)
@@ -8,7 +8,6 @@
<depend package="std_msgs"/>
<depend package="pr2Core"/>
<depend package="libpr2API"/>
-<depend package="libKDL"/>
<depend package="pr2_gazebo"/>
-<depend package="interpolated_kinematic_controller"/>
+<depend package="pr2_kinematic_controllers"/>
</package>
Modified: pkg/trunk/manip/overhead_grasp_behavior/src/overhead_grasp.cc
===================================================================
--- pkg/trunk/manip/overhead_grasp_behavior/src/overhead_grasp.cc 2008-08-08 22:27:44 UTC (rev 2836)
+++ pkg/trunk/manip/overhead_grasp_behavior/src/overhead_grasp.cc 2008-08-08 22:35:59 UTC (rev 2837)
@@ -6,14 +6,18 @@
#include <libpr2API/pr2API.h>
#include <pr2_msgs/EndEffectorState.h>
+#include <pr2_kinematic_controllers/Float64Int32.h>
+
#include <std_msgs/Point3DFloat32.h>
#include <std_msgs/Float64.h>
#include <std_msgs/PR2Arm.h>
#include <unistd.h>
-using namespace KDL;
+#include <rosgazebo/GripperCmd.h>
+#include <std_msgs/PR2Arm.h>
+using namespace KDL;
class OverheadGrasper : public ros::node
{
@@ -21,38 +25,68 @@
// coordinates of arm's origin in gazebo/arm coordinate frame (the axes of both frames are parallel)
Vector gazebo_to_arm_vector;
std_msgs::Point3DFloat32 objectPosMsg;
+ pr2_msgs::EndEffectorState rightEndEffectorMsg;
+ Frame right_tooltip_frame;
+ Vector objectPosition;
+ const static double PR2_GRIPPER_LENGTH = 0.155;
+ Vector CAMERA_ENDEFFECTOR; // position of camera relative to end effector in end effector frame.
public:
OverheadGrasper(void) : ros::node("overhead_grasper")
{
advertise<std_msgs::PR2Arm>("cmd_rightarmconfig");
- advertise<pr2_msgs::EndEffectorState>("right_pr2arm_set_end_effector");
+ advertise<pr2_msgs::EndEffectorState>("right_pr2arm_set_end_effector"); // this should actually be a service.
advertise<std_msgs::Float64>("interpolate_step_size");
advertise<std_msgs::Float64>("interpolate_wait_time");
subscribe("object_position", objectPosMsg, &OverheadGrasper::objectPosition_cb);
+ subscribe("rightarm_tooltip_cartesian", rightEndEffectorMsg, &OverheadGrasper::currentRightArmPosCartesian_cb);
- gazebo_to_arm_vector = Vector(1.040-0.7987,-0.15,0.8269);
+// gazebo_to_arm_vector = Vector(1.020-0.7987,-0.15,0.8269);
+ gazebo_to_arm_vector = Vector(0.81-0.82025,-0.20,0.739675);
+
+ CAMERA_ENDEFFECTOR = Vector(0.0, 0.,0.05); // position of camera relative to end effector in base frame.
}
- void positionArmCartesian(Vector v, Rotation r)
+ void KDL_to_EndEffectorStateMsg(const Frame& f, pr2_msgs::EndEffectorState &efs)
{
- pr2_msgs::EndEffectorState efs;
efs.set_rot_size(9);
efs.set_trans_size(3);
- for(int i = 0; i < 9; i++) {
- efs.rot[i] = r.data[i];
- }
- for(int i = 0; i < 3; i++) {
- efs.trans[i] = v.data[i];
- }
+ for(int i = 0; i < 9; i++)
+ efs.rot[i] = f.M.data[i];
+ for(int i = 0; i < 3; i++)
+ efs.trans[i] = f.p.data[i];
+ }
+ void EndEffectorStateMsg_to_KDL(const pr2_msgs::EndEffectorState &efs, Frame& f)
+ {
+ for(int i = 0; i < 9; i++)
+ f.M.data[i] = efs.rot[i];
+ for(int i = 0; i < 3; i++)
+ f.p.data[i] = efs.trans[i];
+ }
+
+ void positionArmCartesian(const Vector& v, const Rotation& r)
+ {
+ pr2_msgs::EndEffectorState efs;
+ Frame f(r,v);
+ KDL_to_EndEffectorStateMsg(f,efs);
+
publish("right_pr2arm_set_end_effector",efs);
}
-
void overheadGraspPosture()
{
std_msgs::PR2Arm rightarm;
+// rightarm.turretAngle = deg2rad*0;
+// rightarm.shoulderLiftAngle = deg2rad*0;
+// rightarm.upperarmRollAngle = deg2rad*0;
+// rightarm.elbowAngle = deg2rad*0;
+// rightarm.forearmRollAngle = 0;
+// rightarm.wristPitchAngle = deg2rad*90;
+// rightarm.wristRollAngle = 0;
+// rightarm.gripperForceCmd = 50;
+// rightarm.gripperGapCmd = 0.;
+
rightarm.turretAngle = deg2rad*-20;
rightarm.shoulderLiftAngle = deg2rad*-20;
rightarm.upperarmRollAngle = deg2rad*180;
@@ -60,27 +94,103 @@
rightarm.forearmRollAngle = 0;
rightarm.wristPitchAngle = 0;
rightarm.wristRollAngle = 0;
- rightarm.gripperForceCmd = 0;
- rightarm.gripperGapCmd = 0.2;
+ rightarm.gripperForceCmd = 50;
+ rightarm.gripperGapCmd = 0.;
publish("cmd_rightarmconfig",rightarm);
}
-
- // Vector v is in gazebo coordinate frame.
- void positionEyecamOverObject(Vector v)
+ // Vector v is shoulder coordinate frame.
+// void positionEyecamOverObject(Vector v)
+ void positionEyecamOverObject()
{
- Vector v_arm = v - gazebo_to_arm_vector;
- v_arm.data[2] += 0.4; // I want end effector to be 0.5m above object.
- Rotation r = Rotation::RotY(DTOR(90)); // look down vertically
+ Vector v_arm = objectPosition;
+ v_arm.data[2] += 0.3; // I want end effector to be 0.4m above object.
+ Rotation r = Rotation::RotY(deg2rad*90) * Rotation::RotY(deg2rad*90); // look down vertically
cout<<"Going to: "<<v_arm<<endl;
positionArmCartesian(v_arm, r);
}
+ void moveArmSegmentation()
+ {
+// gmmseg::hrl_grasp:request req;
+// gmmseg::hrl_grasp:response res;
+// req.height = right_tooltip_frame.p[2] - objectPosition[2] - CAMERA_ENDEFFECTOR[0];
+// if (ros::service::call("hrl_grasp", req, res)==false)
+// {
+// printf("[overhead_grasp_behavior] <overhead_grasp.cpp> {segmentObject} hrl_grasp service failed.\nExiting..\n");
+// exit(0);
+// }
+// Vector move(-1*res.y, -1*res.x, 0);
+// Rotation r = Rotation::RotY(deg2rad*90)*Rotation::RotX(res.theta); // look down vertically, with correct wrist roll
+
+ Vector move(0,0,0);
+ move += Rotation::RotY(deg2rad*90)*CAMERA_ENDEFFECTOR;
+ Rotation r = Rotation::RotY(deg2rad*90)*Rotation::RotY(deg2rad*90); // look down vertically, with correct wrist roll
+ Vector goto_point = right_tooltip_frame.p+move;
+ cout<<"Going to: "<<goto_point<<endl;
+ positionArmCartesian(goto_point, r);
+ }
+
void objectPosition_cb(void)
{
+// cout<<"object's z coord: "<<objectPosMsg.z<<"\n";
+ objectPosition = Vector(objectPosMsg.x,objectPosMsg.y,objectPosMsg.z) - gazebo_to_arm_vector;
}
+ void currentRightArmPosCartesian_cb(void)
+ {
+ EndEffectorStateMsg_to_KDL(this->rightEndEffectorMsg, this->right_tooltip_frame);
+ }
+
+ void OpenGripper(void)
+ {
+ rosgazebo::GripperCmd::request req;
+ rosgazebo::GripperCmd::response res;
+ req.gap=0.3;
+ req.force=50;
+ if (ros::service::call("operate_right_gripper", req, res)==false)
+ {
+ printf("[overhead_grasp_behavior] <overhead_grasp.cpp> {OpenGripper} operate_right_gripper service failed.\nExiting..\n");
+ exit(0);
+ }
+ }
+
+ void CloseGripper(void)
+ {
+ rosgazebo::GripperCmd::request req;
+ rosgazebo::GripperCmd::response res;
+ req.gap=0.0;
+ req.force=200;
+ if (ros::service::call("operate_right_gripper", req, res)==false)
+ {
+ printf("[overhead_grasp_behavior] <overhead_grasp.cpp> {CloseGripper} operate_right_gripper service failed.\nExiting..\n");
+ exit(0);
+ }
+ }
+
+ void pickUpObject(void)
+ {
+ pr2_kinematic_controllers::Float64Int32::request req;
+ pr2_kinematic_controllers::Float64Int32::response res;
+ req.f = -1*PR2_GRIPPER_LENGTH;
+ ros::service::call("move_along_gripper", req, res);
+ }
+
+ void printTooltipTransformation(void)
+ {
+ cout<<"End effector transformation: "<<this->right_tooltip_frame<<"\n";
+ }
+
+ void graspFromAbove(void)
+ {
+ pr2_kinematic_controllers::Float64Int32::request req;
+ pr2_kinematic_controllers::Float64Int32::response res;
+ printf("arm tip z: %.4f\nobjectPosition.z: %.4f\n", right_tooltip_frame.p.data[2],objectPosition.data[2]);
+ req.f = right_tooltip_frame.p.data[2] - objectPosition.data[2] - PR2_GRIPPER_LENGTH;
+ printf("amount to move by: %.4f\n", req.f);
+ ros::service::call("move_along_gripper", req, res);
+ }
};
@@ -96,11 +206,11 @@
std_msgs::Float64 float64_msg;
sleep(1);
- double interpolate_step_size = 0.05;
+ double interpolate_step_size = 0.01;
float64_msg.data = interpolate_step_size;
ohGrasper.publish("interpolate_step_size", float64_msg);
- double interpolate_wait_time = 1.0;
+ double interpolate_wait_time = .3;
float64_msg.data = interpolate_wait_time;
ohGrasper.publish("interpolate_wait_time", float64_msg);
@@ -162,13 +272,42 @@
break;
case '2':
{
- Vector v(n.objectPosMsg.x,n.objectPosMsg.y,n.objectPosMsg.z);
- n.positionEyecamOverObject(v);
+ n.positionEyecamOverObject();
}
break;
case '3':
- printf("x: %.4f y: %.4f z: %.4f\n", n.objectPosMsg.x, n.objectPosMsg.y, n.objectPosMsg.z);
+ n.graspFromAbove();
break;
+ case '4':
+ n.pickUpObject();
+ break;
+
+ case 't':
+ n.moveArmSegmentation();
+ break;
+
+ case 'e':
+ n.printTooltipTransformation();
+ break;
+
+ case 's':
+ {
+ pr2_kinematic_controllers::Float64Int32::request req;
+ pr2_kinematic_controllers::Float64Int32::response res;
+ req.f=0.01;
+ ros::service::call("move_along_gripper", req, res);
+ }
+
+ // printf("x: %.4f y: %.4f z: %.4f\n", n.objectPosMsg.x, n.objectPosMsg.y, n.objectPosMsg.z);
+ break;
+ case 'o':
+ case 'O':
+ n.OpenGripper();
+ break;
+ case 'c':
+ case 'C':
+ n.CloseGripper();
+ break;
default:
break;
}
Modified: pkg/trunk/manip/pr2_kinematic_controllers/manifest.xml
===================================================================
--- pkg/trunk/manip/pr2_kinematic_controllers/manifest.xml 2008-08-08 22:27:44 UTC (rev 2836)
+++ pkg/trunk/manip/pr2_kinematic_controllers/manifest.xml 2008-08-08 22:35:59 UTC (rev 2837)
@@ -9,7 +9,8 @@
<depend package="pr2_msgs"/>
<depend package="pr2Core"/>
<depend package="libpr2API"/>
- <depend package="libKDL"/>
+ <!-- <depend package="libKDL"/> -->
+ <depend package="robot_kinematics"/>
<depend package="pr2_gazebo"/>
<export>
<cpp cflags="-I${prefix}/srv/cpp" />
Modified: pkg/trunk/manip/pr2_kinematic_controllers/src/pr2_kinematic_controllers.cc
===================================================================
--- pkg/trunk/manip/pr2_kinematic_controllers/src/pr2_kinematic_controllers.cc 2008-08-08 22:27:44 UTC (rev 2836)
+++ pkg/trunk/manip/pr2_kinematic_controllers/src/pr2_kinematic_controllers.cc 2008-08-08 22:35:59 UTC (rev 2837)
@@ -1,40 +1,52 @@
#include <ros/node.h>
-#include <rosthread/mutex.h>
-#include <std_msgs/Empty.h>
#include <std_msgs/PR2Arm.h>
#include <std_msgs/Float64.h>
#include <pr2_msgs/EndEffectorState.h>
+#include <pr2_kinematic_controllers/Float64Int32.h>
+#include <rosgazebo/VoidVoid.h>
+#include <rosgazebo/MoveCartesian.h>
-#include <pr2_kinematic_controllers/Float32Int32.h>
-
#include <libpr2API/pr2API.h>
+//#include <libKDL/kdl_kinematics.h>
+#include <robot_kinematics/robot_kinematics.h>
+#include <robot_kinematics/serial_chain.h>
+
#include <kdl/rotational_interpolation_sa.hpp>
-#include <rosgazebo/VoidVoid.h>
+#include <unistd.h>
using namespace std_msgs;
using namespace PR2;
using namespace KDL;
+using namespace robot_kinematics;
+
class Pr2KinematicControllers : public ros::node
{
public:
Pr2KinematicControllers(void) : ros::node("pr2_kinematic_controller")
{
+ char *c_filename = getenv("ROS_PACKAGE_PATH");
+ std::stringstream filename;
+ filename << c_filename << "/robot_descriptions/wg_robot_description/pr2/pr2.xml" ;
+ pr2_kin.loadXML(filename.str());
+ right_arm = pr2_kin.getSerialChain("rightArm");
+
step_size = 0.05;
- wait_time = 1.;
+ wait_time = 1;
advertise<pr2_msgs::EndEffectorState>("cmd_leftarm_cartesian");
- advertise<pr2_msgs::EndEffectorState>("cmd_rightarm_cartesian");
-// advertise<std_msgs::Empty>("reset_IK_guess"); // to tell RosGazeboNode to make q_IK_guess = current manipulator config.
+ advertise<pr2_msgs::EndEffectorState>("rightarm_tooltip_cartesian"); // position of tip of right arm in cartesian coord.
+ advertise_service("move_along_gripper", &Pr2KinematicControllers::moveAlongGripper);
+
subscribe("right_pr2arm_set_end_effector", _rightEndEffectorGoal, &Pr2KinematicControllers::setRightEndEffector);
subscribe("left_pr2arm_set_end_effector", _leftEndEffectorGoal, &Pr2KinematicControllers::setLeftEndEffector);
- subscribe("left_pr2arm_pos", leftArmPosMsg, &Pr2KinematicControllers::currentLeftArmPos);
- subscribe("right_pr2arm_pos", rightArmPosMsg, &Pr2KinematicControllers::currentRightArmPos);
+ subscribe("left_pr2arm_pos", leftArmPosMsg, &Pr2KinematicControllers::currentLeftArmPos); // configuration of left arm.
+ subscribe("right_pr2arm_pos", rightArmPosMsg, &Pr2KinematicControllers::currentRightArmPos); // configuration of right arm.
subscribe("interpolate_step_size", float64_msg, &Pr2KinematicControllers::setStepSize);
subscribe("interpolate_wait_time", float64_msg, &Pr2KinematicControllers::setWaitTime);
@@ -77,32 +89,42 @@
void currentLeftArmPos(void)
{
- // don't need to do anything -- we already have the data
}
void currentRightArmPos(void)
{
- // don't need to do anything -- we already have the data
+ JntArray q = JntArray(right_arm->num_joints_);
+ q(0) = rightArmPosMsg.turretAngle;
+ q(1) = rightArmPosMsg.shoulderLiftAngle;
+ q(2) = rightArmPosMsg.upperarmRollAngle;
+ q(3) = rightArmPosMsg.elbowAngle;
+ q(4) = rightArmPosMsg.forearmRollAngle;
+ q(5) = rightArmPosMsg.wristPitchAngle;
+ q(6) = rightArmPosMsg.wristRollAngle;
+
+ Frame f;
+ right_arm->computeFK(q,f);
+ pr2_msgs::EndEffectorState efs;
+ KDL_to_EndEffectorStateMsg(f, efs);
+ publish("rightarm_tooltip_cartesian",efs);
}
- void publishFrame(bool isRightArm, const Frame& f)
+ void KDL_to_EndEffectorStateMsg(const Frame& f, pr2_msgs::EndEffectorState &efs)
{
- pr2_msgs::EndEffectorState efs;
efs.set_rot_size(9);
efs.set_trans_size(3);
- std::cout << "Publishing rot ";
for(int i = 0; i < 9; i++)
- {
efs.rot[i] = f.M.data[i];
- std::cout << efs.rot[i] << " ";
- }
- std::cout << " trans ";
+
for(int i = 0; i < 3; i++)
- {
efs.trans[i] = f.p.data[i];
- std::cout << efs.trans[i] << " ";
- }
- std::cout << std::endl;
+ }
+
+ void publishFrame(bool isRightArm, const Frame& f)
+ {
+ pr2_msgs::EndEffectorState efs;
+ KDL_to_EndEffectorStateMsg(f, efs);
+
if(isRightArm)
publish("cmd_rightarm_cartesian",efs);
else
@@ -111,29 +133,18 @@
void RunControlLoop(bool isRightArm, const Frame& r, const std_msgs::PR2Arm& arm)
{
-
- std_msgs::Empty emp;
rosgazebo::VoidVoid::request req;
rosgazebo::VoidVoid::response res;
-// publish("reset_IK_guess",emp);
- if (ros::service::call("reset_IK_guess", req, res))
- printf("Success!\n");
- else
+
+ cout<<"RunControlLoop: rotation: "<<r.M<<"\n";
+
+ if (ros::service::call("reset_IK_guess", req, res)==false)
{
- printf("reset_IK_guess service failed.\nExiting..\n");
+ printf("[pr2_kinematic_controllers] <pr2_kinematic_controllers.cpp> reset_IK_guess service failed.\nExiting..\n");
exit(0);
}
-// if (ros::service::call("reset_IK_guess", req, res)==false)
-// {
-// printf("reset_IK_guess service failed.\nExiting..\n");
-// exit(0);
-// }
-
-
- PR2_kinematics pr2_kin;
- JntArray q = JntArray(pr2_kin.nJnts);
-
+ JntArray q = JntArray(right_arm->num_joints_);
q(0) = arm.turretAngle;
q(1) = arm.shoulderLiftAngle;
q(2) = arm.upperarmRollAngle;
@@ -143,14 +154,12 @@
q(6) = arm.wristRollAngle;
Frame f;
- pr2_kin.FK(q,f);
+ right_arm->computeFK(q,f);
Vector start = f.p;
Vector move = r.p-start;
double dist = move.Norm();
move = move/dist;
- std::cout << "Starting trans " << f.p.data[0] << " " << f.p.data[1] << " " << f.p.data[2] << std::endl;
-
RotationalInterpolation_SingleAxis rotInterpolater;
rotInterpolater.SetStartEnd(f.M, r.M);
double total_angle = rotInterpolater.Angle();
@@ -159,19 +168,76 @@
Vector target;
int nSteps = (int)(dist/step_size);
double angle_step = total_angle/nSteps;
- for(int i=0;i<nSteps;i++)
+ bool reachable = true;
+ for(int i=0;i<nSteps && reachable==true;i++)
{
+ printf("[pr2_kinematic_controllers] interpolating...\n");
f.p = start+(i+1)*move*step_size;
f.M = rotInterpolater.Pos(angle_step*(i+1));
- std::cout << "Starting trans " << f.p.data[0] << " " << f.p.data[1] << " " << f.p.data[2] << std::endl;
- publishFrame(isRightArm, f);
+
+ if (isRightArm)
+ reachable=SetRightArmCartesian(f); // services.
+ else
+ publishFrame(isRightArm, f); // old way of doing things. from interpolated_kinematic_controller.
+
usleep(wait_time*1e6);
}
+
f.p = r.p;
f.M = r.M;
- publishFrame(isRightArm, f);
+ if (isRightArm)
+ reachable=SetRightArmCartesian(f); // services.
+ else
+ publishFrame(isRightArm, f); // old way of doing things. from interpolated_kinematic_controller.
+
+ if (reachable==false)
+ printf("[pr2_kinematic_controllers] reachable became FALSE.\n");
}
+ bool SetRightArmCartesian(const Frame &f)
+ {
+ rosgazebo::MoveCartesian::request req;
+ rosgazebo::MoveCartesian::response res;
+ KDL_to_EndEffectorStateMsg(f, req.e);
+
+ if (ros::service::call("set_rightarm_cartesian", req, res)==false)
+ {
+ printf("[pr2_kinematic_controllers] <pr2_kinematic_controllers.cpp> set_rightarm_cartesian service failed.\nExiting..\n");
+ exit(0);
+ }
+
+ return (res.reachable==-1) ? false : true;
+ }
+
+ bool moveAlongGripper(pr2_kinematic_controllers::Float64Int32::request &req, pr2_kinematic_controllers::Float64Int32::response &res)
+ {
+ moveAlongGripper(req.f);
+ return true;
+ }
+
+ void moveAlongGripper(double dist)
+ {
+ JntArray q = JntArray(right_arm->num_joints_);
+
+ q(0) = rightArmPosMsg.turretAngle;
+ q(1) = rightArmPosMsg.shoulderLiftAngle;
+ q(2) = rightArmPosMsg.upperarmRollAngle;
+ q(3) = rightArmPosMsg.elbowAngle;
+ q(4) = rightArmPosMsg.forearmRollAngle;
+ q(5) = rightArmPosMsg.wristPitchAngle;
+ q(6) = rightArmPosMsg.wristRollAngle;
+ Frame f;
+ right_arm->computeFK(q,f);
+ cout<<"current end effector position: "<<f.p<<"\n";
+ cout<<"current end effector rotation: "<<f.M<<"\n";
+ Vector v(0,0,dist);
+ v = f*v;
+ cout<<"final end effector position: "<<v<<"\n";
+ cout<<"final end effector rotation: "<<f.M<<"\n";
+ f.p=v;
+ RunControlLoop(true, f, rightArmPosMsg);
+ }
+
private:
pr2_msgs::EndEffectorState _leftEndEffectorGoal;
@@ -180,6 +246,9 @@
double step_size;
double wait_time;
std_msgs::Float64 float64_msg;
+
+ RobotKinematics pr2_kin;
+ SerialChain *right_arm;
};
Modified: pkg/trunk/manip/pr2_kinematic_controllers/test/test_pr2_kin_con.cc
===================================================================
--- pkg/trunk/manip/pr2_kinematic_controllers/test/test_pr2_kin_con.cc 2008-08-08 22:27:44 UTC (rev 2836)
+++ pkg/trunk/manip/pr2_kinematic_controllers/test/test_pr2_kin_con.cc 2008-08-08 22:35:59 UTC (rev 2837)
@@ -14,7 +14,7 @@
//In shoulder frame x 0.562689
//In shoulder frame y -0.367447
//In shoulder frame z -0.369594
- Rotation r = Rotation::RotZ(DTOR(0));
+ Rotation r = Rotation::RotZ(deg2rad*0);
Vector v(.562689,-.367447,-.369594);
std::cout << " rot: " << std::endl;
Modified: pkg/trunk/manip/teleop_arm_keyboard/Makefile
===================================================================
--- pkg/trunk/manip/teleop_arm_keyboard/Makefile 2008-08-08 22:27:44 UTC (rev 2836)
+++ pkg/trunk/manip/teleop_arm_keyboard/Makefile 2008-08-08 22:35:59 UTC (rev 2837)
@@ -1,16 +1,2 @@
-SRC = teleop_arm_keyboard.cc
-OUT = teleop_arm_keyboard
-PKG = teleop_arm_keyboard
-CXX = g++
-all: $(PKG)
+include $(shell rospack find mk)/cmake.mk
-
-CFLAGS = -g -Wall $(shell rospack export/cpp/cflags $(PKG))
-
-LFLAGS = $(shell rospack export/cpp/lflags $(PKG))
-
-teleop_arm_keyboard: teleop_arm_keyboard.cc
- $(CXX) $(CFLAGS) -o $@ $< $(LFLAGS)
-
-clean:
- rm -rf *.o $(PKG) $(PKG).dSYM
Modified: pkg/trunk/robot_control_loops/pr2_gazebo/manifest.xml
===================================================================
--- pkg/trunk/robot_control_loops/pr2_gazebo/manifest.xml 2008-08-08 22:27:44 UTC (rev 2836)
+++ pkg/trunk/robot_control_loops/pr2_gazebo/manifest.xml 2008-08-08 22:35:59 UTC (rev 2837)
@@ -20,7 +20,8 @@
<depend package="libpr2HW" />
<depend package="math_utils" />
<depend package="string_utils" />
- <depend package="libKDL" />
+ <!-- <depend package="libKDL" /> -->
+ <depend package="robot_kinematics" />
<depend package="robot_mechanism_model" />
<depend package="hw_interface" />
<depend package="gazebo_hardware" />
Modified: pkg/trunk/robot_control_loops/pr2_gazebo/src/pr2_gazebo.cpp
===================================================================
--- pkg/trunk/robot_control_loops/pr2_gazebo/src/pr2_gazebo.cpp 2008-08-08 22:27:44 UTC (rev 2836)
+++ pkg/trunk/robot_control_loops/pr2_gazebo/src/pr2_gazebo.cpp 2008-08-08 22:35:59 UTC (rev 2837)
@@ -87,12 +87,19 @@
/* initialize controllers */
/* ...
[truncated message content] |
|
From: <adv...@us...> - 2008-08-08 22:52:02
|
Revision: 2838
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2838&view=rev
Author: advaitjain
Date: 2008-08-08 22:52:09 +0000 (Fri, 08 Aug 2008)
Log Message:
-----------
moved libKDL to deprecated
Added Paths:
-----------
pkg/trunk/deprecated/libKDL/
pkg/trunk/deprecated/libKDL/src/kdl_kinematics.cpp
pkg/trunk/deprecated/libKDL/test/pr2_kin_test.cpp
Removed Paths:
-------------
pkg/trunk/deprecated/libKDL/src/kdl_kinematics.cpp
pkg/trunk/deprecated/libKDL/test/pr2_kin_test.cpp
pkg/trunk/util/kinematics/libKDL/
Deleted: pkg/trunk/deprecated/libKDL/src/kdl_kinematics.cpp
===================================================================
--- pkg/trunk/util/kinematics/libKDL/src/kdl_kinematics.cpp 2008-08-08 22:04:51 UTC (rev 2829)
+++ pkg/trunk/deprecated/libKDL/src/kdl_kinematics.cpp 2008-08-08 22:52:09 UTC (rev 2838)
@@ -1,140 +0,0 @@
-
-#include "libKDL/kdl_kinematics.h"
-
-using namespace KDL;
-using namespace PR2;
-using namespace std;
-
-
-PR2_kinematics::PR2_kinematics()
-{
- //---------- create the chain for the PR2 ----------
- this->chain = new Chain();
- this->chain->addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(ARM_PAN_SHOULDER_PITCH_OFFSET.x,0.0,0.0))));
- this->chain->addSegment(Segment(Joint(Joint::RotY),Frame(Vector(ARM_SHOULDER_PITCH_ROLL_OFFSET.x,0.0,0.0))));
- this->chain->addSegment(Segment(Joint(Joint::RotX),Frame(Vector(ARM_SHOULDER_ROLL_ELBOW_PITCH_OFFSET.x,0.0,0.0))));
- this->chain->addSegment(Segment(Joint(Joint::RotY),Frame(Vector(ELBOW_PITCH_ELBOW_ROLL_OFFSET.x,0.0,0.0))));
- this->chain->addSegment(Segment(Joint(Joint::RotX),Frame(Vector(ELBOW_ROLL_WRIST_PITCH_OFFSET.x,0.0,0.0))));
- this->chain->addSegment(Segment(Joint(Joint::RotY),Frame(Vector(WRIST_PITCH_WRIST_ROLL_OFFSET.x,0.0,0.0))));
- this->chain->addSegment(Segment(Joint(Joint::RotX),Frame(Vector(WRIST_ROLL_GRIPPER_OFFSET.x,0.0,0.0))));
-
- this->nJnts = this->chain->getNrOfJoints();
- this->q_IK_guess = new JntArray(this->nJnts);
- this->q_IK_result = new JntArray(this->nJnts);
-
- //------ Forward Position Kinematics --------------
- this->fk_pos_solver = new ChainFkSolverPos_recursive(*this->chain);
- //------- IK
- this->ik_vel_solver = new ChainIkSolverVel_pinv(*this->chain);
- this->ik_pos_solver = new ChainIkSolverPos_NR(*this->chain, *this->fk_pos_solver, *this->ik_vel_solver);
-
-
- //---------- create a chain for the fore-arm camera -------------
- this->chain_forearm_camera = new Chain();
- this->chain_forearm_camera->addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(ARM_PAN_SHOULDER_PITCH_OFFSET.x,0.0,0.0))));
- this->chain_forearm_camera->addSegment(Segment(Joint(Joint::RotY),Frame(Vector(ARM_SHOULDER_PITCH_ROLL_OFFSET.x,0.0,0.0))));
- this->chain_forearm_camera->addSegment(Segment(Joint(Joint::RotX),Frame(Vector(ARM_SHOULDER_ROLL_ELBOW_PITCH_OFFSET.x,0.0,0.0))));
- this->chain_forearm_camera->addSegment(Segment(Joint(Joint::RotY),Frame(Vector(ELBOW_PITCH_ELBOW_ROLL_OFFSET.x,0.0,0.0))));
- this->chain_forearm_camera->addSegment(Segment(Joint(Joint::RotX),Frame(Rotation::RotY(-45*deg2rad),Vector(0.111825,0.0,0.02))));
-
- this->nJnts_forearm_camera = this->chain_forearm_camera->getNrOfJoints();
- //------ Forward Position Kinematics --------------
- this->fk_pos_solver_forearm_camera = new ChainFkSolverPos_recursive(*this->chain_forearm_camera);
- //------- IK
- this->ik_vel_solver_forearm_camera = new ChainIkSolverVel_pinv(*this->chain_forearm_camera);
- this->ik_pos_solver_forearm_camera = new ChainIkSolverPos_NR(*this->chain_forearm_camera, *this->fk_pos_solver_forearm_camera,
- *this->ik_vel_solver_forearm_camera);
-
-
-}
-
-PR2_kinematics::~PR2_kinematics()
-{
- delete this->chain;
- delete this->fk_pos_solver;
- delete this->ik_vel_solver;
- delete this->ik_pos_solver;
- delete this->q_IK_guess;
- delete this->q_IK_result;
-
- delete this->chain_forearm_camera;
- delete this->fk_pos_solver_forearm_camera;
- delete this->ik_vel_solver_forearm_camera;
- delete this->ik_pos_solver_forearm_camera;
-}
-
-
-/*
- * f - end effector frame (result of the fwd kinematics)
- * returns True if ok, False if error.
- */
-bool PR2_kinematics::FK(const JntArray &q, Frame &f)
-{
- if (this->fk_pos_solver->JntToCart(q,f) >= 0)
- return true;
- else
- return false;
-}
-
-/*
- * We might want to get rid of this function.
- * Hasn't been removed for compatibility with ArmController.
- */
-bool PR2_kinematics::IK(const JntArray &q_init, const Frame &f, JntArray &q_out)
-{
- if (this->ik_pos_solver->CartToJnt(q_init,f,q_out) >= 0)
- {
- angle_within_mod180(q_out, this->nJnts);
- return true;
- }
- else
- return false;
-}
-
-/*
- * uses internal state (q_IK_guess) as the seed for KDL's IK.
- * result is stored in q_IK_result
- */
-bool PR2_kinematics::IK(const Frame &f)
-{
- if (this->ik_pos_solver->CartToJnt(*this->q_IK_guess,f,*this->q_IK_result) >= 0)
- {
- angle_within_mod180(*this->q_IK_result, this->nJnts);
- return true;
- }
- else
- return false;
-}
-
-
-// returns a%b
-double modulus_double(double a, double b)
-{
- int quo = a/b;
- return a-b*quo;
-}
-
-double angle_within_mod180(double ang)
-{
- double rem = modulus_double(ang, 2*M_PI);
-
- if (rem>M_PI)
- rem -= 2*M_PI;
- else if (rem<(-1*M_PI))
- rem += 2*M_PI;
-
- return rem;
-}
-
-void angle_within_mod180(JntArray &q, int nJnts)
-{
- for(int i=0;i<nJnts;i++)
- {
- q(i) = angle_within_mod180(q(i));
-// printf(".. %f ..", q(i));
- }
-}
-
-
-
-
Copied: pkg/trunk/deprecated/libKDL/src/kdl_kinematics.cpp (from rev 2837, pkg/trunk/util/kinematics/libKDL/src/kdl_kinematics.cpp)
===================================================================
--- pkg/trunk/deprecated/libKDL/src/kdl_kinematics.cpp (rev 0)
+++ pkg/trunk/deprecated/libKDL/src/kdl_kinematics.cpp 2008-08-08 22:52:09 UTC (rev 2838)
@@ -0,0 +1,144 @@
+
+#include "libKDL/kdl_kinematics.h"
+
+using namespace KDL;
+using namespace PR2;
+using namespace std;
+
+
+PR2_kinematics::PR2_kinematics()
+{
+ //---------- create the chain for the PR2 ----------
+ this->chain = new Chain();
+ this->chain->addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(ARM_PAN_SHOULDER_PITCH_OFFSET.x,0.0,0.0))));
+ this->chain->addSegment(Segment(Joint(Joint::RotY),Frame(Vector(ARM_SHOULDER_PITCH_ROLL_OFFSET.x,0.0,0.0))));
+ this->chain->addSegment(Segment(Joint(Joint::RotX),Frame(Vector(ARM_SHOULDER_ROLL_ELBOW_PITCH_OFFSET.x,0.0,0.0))));
+ this->chain->addSegment(Segment(Joint(Joint::RotY),Frame(Vector(ELBOW_PITCH_ELBOW_ROLL_OFFSET.x,0.0,0.0))));
+ this->chain->addSegment(Segment(Joint(Joint::RotX),Frame(Vector(ELBOW_ROLL_WRIST_PITCH_OFFSET.x,0.0,0.0))));
+ this->chain->addSegment(Segment(Joint(Joint::RotY),Frame(Vector(WRIST_PITCH_WRIST_ROLL_OFFSET.x,0.0,0.0))));
+ this->chain->addSegment(Segment(Joint(Joint::RotX),Frame(Vector(WRIST_ROLL_GRIPPER_OFFSET.x,0.0,0.0))));
+
+ this->nJnts = this->chain->getNrOfJoints();
+ this->q_IK_guess = new JntArray(this->nJnts);
+ this->q_IK_result = new JntArray(this->nJnts);
+
+ //------ Forward Position Kinematics --------------
+ this->fk_pos_solver = new ChainFkSolverPos_recursive(*this->chain);
+ //------- IK
+ this->ik_vel_solver = new ChainIkSolverVel_pinv(*this->chain);
+ this->ik_pos_solver = new ChainIkSolverPos_NR(*this->chain, *this->fk_pos_solver, *this->ik_vel_solver);
+
+
+ //---------- create a chain for the fore-arm camera -------------
+ this->chain_forearm_camera = new Chain();
+ this->chain_forearm_camera->addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(ARM_PAN_SHOULDER_PITCH_OFFSET.x,0.0,0.0))));
+ this->chain_forearm_camera->addSegment(Segment(Joint(Joint::RotY),Frame(Vector(ARM_SHOULDER_PITCH_ROLL_OFFSET.x,0.0,0.0))));
+ this->chain_forearm_camera->addSegment(Segment(Joint(Joint::RotX),Frame(Vector(ARM_SHOULDER_ROLL_ELBOW_PITCH_OFFSET.x,0.0,0.0))));
+ this->chain_forearm_camera->addSegment(Segment(Joint(Joint::RotY),Frame(Vector(ELBOW_PITCH_ELBOW_ROLL_OFFSET.x,0.0,0.0))));
+ this->chain_forearm_camera->addSegment(Segment(Joint(Joint::RotX),Frame(Rotation::RotY(-45*deg2rad),Vector(0.111825,0.0,0.02))));
+
+ this->nJnts_forearm_camera = this->chain_forearm_camera->getNrOfJoints();
+ //------ Forward Position Kinematics --------------
+ this->fk_pos_solver_forearm_camera = new ChainFkSolverPos_recursive(*this->chain_forearm_camera);
+ //------- IK
+ this->ik_vel_solver_forearm_camera = new ChainIkSolverVel_pinv(*this->chain_forearm_camera);
+ this->ik_pos_solver_forearm_camera = new ChainIkSolverPos_NR(*this->chain_forearm_camera, *this->fk_pos_solver_forearm_camera,
+ *this->ik_vel_solver_forearm_camera);
+
+
+}
+
+PR2_kinematics::~PR2_kinematics()
+{
+ delete this->chain;
+ delete this->fk_pos_solver;
+ delete this->ik_vel_solver;
+ delete this->ik_pos_solver;
+ delete this->q_IK_guess;
+ delete this->q_IK_result;
+
+ delete this->chain_forearm_camera;
+ delete this->fk_pos_solver_forearm_camera;
+ delete this->ik_vel_solver_forearm_camera;
+ delete this->ik_pos_solver_forearm_camera;
+}
+
+
+/*
+ * f - end effector frame (result of the fwd kinematics)
+ * returns True if ok, False if error.
+ */
+bool PR2_kinematics::FK(const JntArray &q, Frame &f)
+{
+ if (this->fk_pos_solver->JntToCart(q,f) >= 0)
+ return true;
+ else
+ return false;
+}
+
+/*
+ * We might want to get rid of this function.
+ * Hasn't been removed for compatibility with ArmController.
+ */
+bool PR2_kinematics::IK(const JntArray &q_init, const Frame &f, JntArray &q_out)
+{
+ if (this->ik_pos_solver->CartToJnt(q_init,f,q_out) >= 0)
+ {
+ angle_within_mod180(q_out, this->nJnts);
+ return true;
+ }
+ else
+ {
+ printf("[libKDL]<kdl_kinematics.cpp>IK failed.\n");
+ return false;
+ }
+}
+
+/*
+ * uses internal state (q_IK_guess) as the seed for KDL's IK.
+ * result is stored in q_IK_result
+ */
+bool PR2_kinematics::IK(const Frame &f)
+{
+ if (this->ik_pos_solver->CartToJnt(*this->q_IK_guess,f,*this->q_IK_result) >= 0)
+ {
+ angle_within_mod180(*this->q_IK_result, this->nJnts);
+// cout<<"[libKDL]<kdl_kinematics.cpp> IK result: "<<*this->q_IK_result<<endl;
+ return true;
+ }
+ else
+ return false;
+}
+
+
+// returns a%b
+double modulus_double(double a, double b)
+{
+ int quo = a/b;
+ return a-b*quo;
+}
+
+double angle_within_mod180(double ang)
+{
+ double rem = modulus_double(ang, 2*M_PI);
+
+ if (rem>M_PI)
+ rem -= 2*M_PI;
+ else if (rem<(-1*M_PI))
+ rem += 2*M_PI;
+
+ return rem;
+}
+
+void angle_within_mod180(JntArray &q, int nJnts)
+{
+ for(int i=0;i<nJnts;i++)
+ {
+ q(i) = angle_within_mod180(q(i));
+// printf(".. %f ..", q(i));
+ }
+}
+
+
+
+
Deleted: pkg/trunk/deprecated/libKDL/test/pr2_kin_test.cpp
===================================================================
--- pkg/trunk/util/kinematics/libKDL/test/pr2_kin_test.cpp 2008-08-08 22:04:51 UTC (rev 2829)
+++ pkg/trunk/deprecated/libKDL/test/pr2_kin_test.cpp 2008-08-08 22:52:09 UTC (rev 2838)
@@ -1,61 +0,0 @@
-
-#include "libKDL/kdl_kinematics.h"
-
-#include <sys/time.h>
-#include <unistd.h>
-
-
-using namespace KDL;
-using namespace PR2;
-using namespace std;
-
-
-
-int main( int argc, char** argv )
-{
- PR2_kinematics pr2_kin;
- struct timeval t0, t1;
-
- JntArray pr2_config = JntArray(pr2_kin.nJnts);
- pr2_config(0) = 0.0, pr2_config(1) = -0, pr2_config(2)=0.0, pr2_config(3)=0.0;
- pr2_config(4) = 0.0, pr2_config(5) = deg2rad*0, pr2_config(6) = deg2rad*90.0;
- cout<<"Config of the arm:"<<pr2_config<<endl;
-
- Frame f;
- if (pr2_kin.FK(pr2_config,f))
- cout<<"End effector transformation:"<<f<<endl;
-
- pr2_config(0) = 0.1, pr2_config(1) = -1, pr2_config(2)=0.3, pr2_config(3)=0.3;
- pr2_config(4) = 0.2, pr2_config(5) = 0.5, pr2_config(6) = 0.0;
- cout<<"Config of the arm:"<<pr2_config<<endl;
-
- if (pr2_kin.FK(pr2_config,f))
- cout<<"End effector transformation:"<<f<<endl;
- else
- cout<<"Could not compute Fwd Kin."<<endl;
-
- (*pr2_kin.q_IK_guess)(0) = 0.1, (*pr2_kin.q_IK_guess)(1) = 0.0, (*pr2_kin.q_IK_guess)(2) = 0.0, (*pr2_kin.q_IK_guess)(3) = 0.0;
- (*pr2_kin.q_IK_guess)(4) = 0.0, (*pr2_kin.q_IK_guess)(5) = 0.0, (*pr2_kin.q_IK_guess)(6) = 0.0;
-
- gettimeofday(&t0,NULL);
- if (pr2_kin.IK(f) == true)
- {
- gettimeofday(&t1, NULL);
- cout<<"IK result:"<<*pr2_kin.q_IK_result<<endl;
- double time_taken = (t1.tv_sec*1000000+t1.tv_usec - (t0.tv_sec*1000000+t0.tv_usec))/1000.;
- printf("Time taken: %f ms\n", time_taken);
- }
- else
- cout<<"Could not compute Inv Kin."<<endl;
-
- //------ checking that IK returned a valid soln -----
- Frame f_ik;
- if (pr2_kin.FK(*pr2_kin.q_IK_result,f_ik))
- cout<<"End effector after IK:"<<f_ik<<endl;
- else
- cout<<"Could not compute Fwd Kin. (After IK)"<<endl;
-
-}
-
-
-
Copied: pkg/trunk/deprecated/libKDL/test/pr2_kin_test.cpp (from rev 2837, pkg/trunk/util/kinematics/libKDL/test/pr2_kin_test.cpp)
===================================================================
--- pkg/trunk/deprecated/libKDL/test/pr2_kin_test.cpp (rev 0)
+++ pkg/trunk/deprecated/libKDL/test/pr2_kin_test.cpp 2008-08-08 22:52:09 UTC (rev 2838)
@@ -0,0 +1,69 @@
+
+#include "libKDL/kdl_kinematics.h"
+
+#include <sys/time.h>
+#include <unistd.h>
+
+
+using namespace KDL;
+using namespace PR2;
+using namespace std;
+
+
+
+int main( int argc, char** argv )
+{
+ PR2_kinematics pr2_kin;
+ struct timeval t0, t1;
+
+ JntArray pr2_config = JntArray(pr2_kin.nJnts);
+ pr2_config(0) = 0.0, pr2_config(1) = -0, pr2_config(2)=0.0, pr2_config(3)=0.0;
+ pr2_config(4) = 0.0, pr2_config(5) = deg2rad*0, pr2_config(6) = deg2rad*0.0;
+ cout<<"Config of the arm:"<<pr2_config<<endl;
+
+// pr2_config(0) = -0.641904;
+// pr2_config(1) = -0.496190;
+// pr2_config(2) = +0.201855;
+// pr2_config(3) = +0.845452;
+// pr2_config(4) = -0.187794;
+// pr2_config(5) = +1.23572;
+// pr2_config(6) = -0.482375;
+
+ Frame f;
+ if (pr2_kin.FK(pr2_config,f))
+ cout<<"End effector transformation:"<<f<<endl;
+
+// pr2_config(0) = 0.1, pr2_config(1) = -1, pr2_config(2)=0.3, pr2_config(3)=0.3;
+// pr2_config(4) = 0.2, pr2_config(5) = 0.5, pr2_config(6) = 0.0;
+// cout<<"Config of the arm:"<<pr2_config<<endl;
+//
+// if (pr2_kin.FK(pr2_config,f))
+// cout<<"End effector transformation:"<<f<<endl;
+// else
+// cout<<"Could not compute Fwd Kin."<<endl;
+//
+// (*pr2_kin.q_IK_guess)(0) = 0.1, (*pr2_kin.q_IK_guess)(1) = 0.0, (*pr2_kin.q_IK_guess)(2) = 0.0, (*pr2_kin.q_IK_guess)(3) = 0.0;
+// (*pr2_kin.q_IK_guess)(4) = 0.0, (*pr2_kin.q_IK_guess)(5) = 0.0, (*pr2_kin.q_IK_guess)(6) = 0.0;
+//
+// gettimeofday(&t0,NULL);
+// if (pr2_kin.IK(f) == true)
+// {
+// gettimeofday(&t1, NULL);
+// cout<<"IK result:"<<*pr2_kin.q_IK_result<<endl;
+// double time_taken = (t1.tv_sec*1000000+t1.tv_usec - (t0.tv_sec*1000000+t0.tv_usec))/1000.;
+// printf("Time taken: %f ms\n", time_taken);
+// }
+// else
+// cout<<"Could not compute Inv Kin."<<endl;
+//
+// //------ checking that IK returned a valid soln -----
+// Frame f_ik;
+// if (pr2_kin.FK(*pr2_kin.q_IK_result,f_ik))
+// cout<<"End effector after IK:"<<f_ik<<endl;
+// else
+// cout<<"Could not compute Fwd Kin. (After IK)"<<endl;
+
+}
+
+
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2008-08-08 23:18:42
|
Revision: 2841
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2841&view=rev
Author: stuglaser
Date: 2008-08-08 23:18:51 +0000 (Fri, 08 Aug 2008)
Log Message:
-----------
Changed spawnController in MechanismControl to take a name for the controller.
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_actuators.h
pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp
pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_actuators.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_actuators.h 2008-08-08 23:01:41 UTC (rev 2840)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_actuators.h 2008-08-08 23:18:51 UTC (rev 2841)
@@ -60,8 +60,8 @@
Model *parent_model_;
HardwareInterface hw_;
- MechanismControlNode mc_;
- ros::node *rosnode_;
+ MechanismControl mc_;
+ MechanismControlNode mcn_;
// Each joint in joints_ corresponds to the joint with the same
// index in mech_joints_. The mech_joints_ vector exists so that
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp 2008-08-08 23:01:41 UTC (rev 2840)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp 2008-08-08 23:18:51 UTC (rev 2841)
@@ -48,23 +48,12 @@
GZ_REGISTER_DYNAMIC_CONTROLLER("gazebo_actuators", GazeboActuators);
GazeboActuators::GazeboActuators(Entity *parent)
- : Controller(parent), hw_(0), mc_(&hw_)
+ : Controller(parent), hw_(0), mc_(&hw_), mcn_(&mc_)
{
- this->parent_model_ = dynamic_cast<Model*>(this->parent);
+ this->parent_model_ = dynamic_cast<Model*>(this->parent);
- if (!this->parent_model_)
- gzthrow("GazeboActuators controller requires a Model as its parent");
-
- rosnode_ = ros::g_node;
- int argc = 0;
- char** argv = NULL;
- if (rosnode_ == NULL)
- {
- // start a ros node if none exist
- ros::init(argc,argv);
- rosnode_ = new ros::node("ros_gazebo",ros::node::DONT_HANDLE_SIGINT);
- printf("-------------------- starting node in GazeboActuators\n");
- }
+ if (!this->parent_model_)
+ gzthrow("GazeboActuators controller requires a Model as its parent");
}
GazeboActuators::~GazeboActuators()
Modified: pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
===================================================================
--- pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2008-08-08 23:01:41 UTC (rev 2840)
+++ pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2008-08-08 23:18:51 UTC (rev 2841)
@@ -49,6 +49,7 @@
#include <generic_controllers/controller.h>
#include "mechanism_control/ListControllerTypes.h"
+#include "mechanism_control/SpawnController.h"
typedef controller::Controller* (*ControllerAllocator)();
@@ -63,8 +64,8 @@
// Non real-time functions
bool init(TiXmlElement* config);
bool registerActuator(const std::string &name, int index);
- bool addController(controller::Controller *c);
- bool spawnController(const char* type, TiXmlElement* config);
+ bool addController(controller::Controller *c, const std::string &name);
+ bool spawnController(const std::string &type, const std::string &name, TiXmlElement *config);
mechanism::Robot model_;
@@ -78,21 +79,25 @@
const static int MAX_NUM_CONTROLLERS = 100;
ros::thread::mutex controllers_mutex_;
controller::Controller* controllers_[MAX_NUM_CONTROLLERS];
+ std::string controller_names_[MAX_NUM_CONTROLLERS];
};
/*
* Exposes MechanismControl's interface over ROS
*/
-class MechanismControlNode : public MechanismControl, public ros::node
+class MechanismControlNode : public ros::node
{
public:
- MechanismControlNode(HardwareInterface *hw);
+ MechanismControlNode(MechanismControl *mc);
virtual ~MechanismControlNode() {}
bool listControllerTypes(mechanism_control::ListControllerTypes::request &req,
mechanism_control::ListControllerTypes::response &resp);
+ bool spawnController(mechanism_control::SpawnController::request &req,
+ mechanism_control::SpawnController::response &resp);
private:
+ MechanismControl *mc_;
};
#endif /* MECHANISM_CONTROL_H */
Modified: pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-08-08 23:01:41 UTC (rev 2840)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-08-08 23:18:51 UTC (rev 2841)
@@ -141,7 +141,7 @@
controller::ControllerFactory::instance().registerType(type, f);
}
-bool MechanismControl::addController(controller::Controller *c)
+bool MechanismControl::addController(controller::Controller *c, const std::string &name)
{
//Add controller to list of controllers in realtime-safe manner;
controllers_mutex_.lock(); //This lock is only to prevent us from other non-realtime threads. The realtime thread may be spinning through the list of controllers while we are in here, so we need to keep that list always in a valid state. This is why we fully allocate and set up the controller before adding it into the list of active controllers.
@@ -152,6 +152,7 @@
{
spot_found = true;
controllers_[i] = c;
+ controller_names_[i] = name;
break;
}
}
@@ -166,22 +167,26 @@
return true;
}
-bool MechanismControl::spawnController(const char *type, TiXmlElement *config)
+bool MechanismControl::spawnController(const std::string &type,
+ const std::string &name,
+ TiXmlElement *config)
{
controller::Controller *c = controller::ControllerFactory::instance().create(type);
if (c == NULL)
return false;
c->initXml(&model_, config);
- return addController(c);
+ return addController(c, name);
}
-MechanismControlNode::MechanismControlNode(HardwareInterface *hw)
- : MechanismControl(hw), ros::node("MechanismControl")
+MechanismControlNode::MechanismControlNode(MechanismControl *mc)
+ : ros::node("MechanismControl"), mc_(mc)
{
+ assert(mc != NULL);
advertise_service("list_controller_types", &MechanismControlNode::listControllerTypes);
+ advertise_service("spawn_controller", &MechanismControlNode::spawnController);
}
@@ -195,3 +200,12 @@
return true;
}
+bool MechanismControlNode::spawnController(
+ mechanism_control::SpawnController::request &req,
+ mechanism_control::SpawnController::response &resp)
+{
+ TiXmlDocument doc;
+ doc.Parse(req.xml_config.c_str());
+ resp.ok = mc_->spawnController(req.type, req.name, doc.RootElement());
+ return true;
+}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rob...@us...> - 2008-08-08 23:49:56
|
Revision: 2845
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2845&view=rev
Author: rob_wheeler
Date: 2008-08-08 23:50:01 +0000 (Fri, 08 Aug 2008)
Log Message:
-----------
pr2_etherCat:
- extract configuration from xml file
- get current time in ethercat_hardware
mechanism_model:
- Initialize joints from xml element
mechanism_control:
- Move Joint initialization into Joint's initXml
- Fix transmission xml parsing
- When initializing actuators, push_back onto vector instead of assigning
generic_controllers:
- Replace all singing/all dancing JointController with simplified JointPositionController
ethercat_hardware:
- Register actuators with mechanism control
- Fix some sign problems in wg05 command packet
- Set HardwareInterface's current time in update loop
Modified Paths:
--------------
pkg/trunk/controllers/generic_controllers/CMakeLists.txt
pkg/trunk/controllers/generic_controllers/include/generic_controllers/controller.h
pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h
pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg05.h
pkg/trunk/drivers/motor/ethercat_hardware/manifest.xml
pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp
pkg/trunk/drivers/motor/ethercat_hardware/src/wg05.cpp
pkg/trunk/mechanism/hardware_interface/include/hardware_interface/hardware_interface.h
pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
pkg/trunk/mechanism/mechanism_model/CMakeLists.txt
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/joint.h
pkg/trunk/mechanism/mechanism_model/manifest.xml
pkg/trunk/mechanism/mechanism_model/src/simple_transmission.cpp
pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp
Added Paths:
-----------
pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_position_controller.h
pkg/trunk/controllers/generic_controllers/src/joint_position_controller.cpp
pkg/trunk/mechanism/mechanism_model/src/joint.cpp
Removed Paths:
-------------
pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_controller.h
pkg/trunk/controllers/generic_controllers/src/joint_controller.cpp
Modified: pkg/trunk/controllers/generic_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/generic_controllers/CMakeLists.txt 2008-08-08 23:44:45 UTC (rev 2844)
+++ pkg/trunk/controllers/generic_controllers/CMakeLists.txt 2008-08-08 23:50:01 UTC (rev 2845)
@@ -1,4 +1,4 @@
cmake_minimum_required(VERSION 2.6)
include(rosbuild)
rospack(generic_controllers)
-rospack_add_library(generic_controllers src/pid.cpp src/joint_controller.cpp)
+rospack_add_library(generic_controllers src/pid.cpp src/joint_position_controller.cpp)
Modified: pkg/trunk/controllers/generic_controllers/include/generic_controllers/controller.h
===================================================================
--- pkg/trunk/controllers/generic_controllers/include/generic_controllers/controller.h 2008-08-08 23:44:45 UTC (rev 2844)
+++ pkg/trunk/controllers/generic_controllers/include/generic_controllers/controller.h 2008-08-08 23:50:01 UTC (rev 2845)
@@ -45,7 +45,8 @@
#include <misc_utils/factory.h>
#include <mechanism_model/robot.h>
-class TiXmlElement;
+#include <tinyxml/tinyxml.h>
+//class TiXmlElement;
namespace controller
{
@@ -82,7 +83,6 @@
{
}
virtual void update(void) = 0;
- virtual void init(void) = 0;
virtual void initXml(mechanism::Robot *robot, TiXmlElement *config) = 0;
};
Deleted: pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_controller.h
===================================================================
--- pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_controller.h 2008-08-08 23:44:45 UTC (rev 2844)
+++ pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_controller.h 2008-08-08 23:50:01 UTC (rev 2845)
@@ -1,343 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2008, Willow Garage, Inc.
- * All rights reserved.
- *
- * 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 the Willow Garage 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.
- *********************************************************************/
-#pragma once
-/***************************************************/
-/*! \class controller::JointController
- \brief A Joint controller
-
- This class implements controller loops for
- PR2 Joint Control
-
- ASSUMES:
- Rotary joint
-
- Parameters to be set by SetParam
- PGain
- IGain
- DGain
- IMax
- IMin
-
- Parameters fetched from joint
- Time
- SaturationEffort
- MaxEffort
-
- Steps to bring a JointController online
- 1. Initialize gains via SetParam
- 2. Set the controller mode (SetMode(mode))
- 3. EnableController()
- 4. Set appropriate command (position, torque, velocity)
- 5. Call Update from Real Time loop
-
-
- */
-/***************************************************/
-#include <sys/types.h>
-#include <stdint.h>
-
-#include <generic_controllers/controller.h>
-#include <generic_controllers/pid.h>
-#include <math_utils/angles.h>
-#include <mechanism_model/joint.h>
-#include <string>
-#include <math.h>
-#include <urdf/URDF.h>
-
-namespace controller
-{
-
-class JointController : public Controller
-{
- static const double ACCELERATION_THRESHOLD = 0.1; //Distance threshold below which linear acceleration limit is active, if enabled
-public:
- /*!
- * \brief Default Constructor of the JointController class.
- *
- */
- JointController();
-
- /*!
- * \brief Destructor of the JointController class.
- */
- ~JointController();
-
- static Controller* create()
- {
- return new JointController();
- }
-
- void init(void);
- void initXml(mechanism::Robot *robot, TiXmlElement *config);
-
- /*!
- * \brief Functional way to initialize limits and gains.
- *
- */
- void init(PidControlParam pcp, ControllerControlMode mode, double time, double maxEffort, double minEffort,
- mechanism::Joint *joint);
-
- /*!
- * \brief Functional way to initialize limits and gains.
- *
- */
- void init(double time, mechanism::Joint *joint);
-
- /*!
- * \brief Functional way to initialize limits and gains.
- *
- */
- void init(double pGain, double iGain, double dGain, double windupMax, double windupMin, ControllerControlMode mode,
- double time, double maxEffort, double minEffort, mechanism::Joint *joint);
-
- //-------------------------------------------------------------------------//
- //MODE/ENABLE CALLS
- //-------------------------------------------------------------------------//
-
- /*!
- * \brief Switches command mode type (Torque, position, velocity control)
- *
- */
- ControllerControlMode setMode(ControllerControlMode mode);
-
- /*!
- * \brief Returns the current mode of the controller
- */
- ControllerControlMode getMode(void);
-
- /*!
- * \brief Allow controller to run
- */
- ControllerControlMode enableController();
-
- /*!
- * \brief Set torque to zero. Prevent controller from running
- */
- ControllerControlMode disableController();
-
- /*!
- * \brief Return true if last command saturated the torque
- *
- *
- */
- bool checkForSaturation(void);
-
- //-------------------------------------------------------------------------//
- //TORQUE CALLS
- //-------------------------------------------------------------------------//
- /*!
- * \brief Give a torque command to be issue on update (if in torque mode)
- *
- * \param torque Torque command to issue
- */
- ControllerErrorCode setTorqueCmd(double torque);
-
- /*!
- * \brief Fetch the latest user issued torque command
- *
- * \param double* torque Pointer to value to change
- */
- ControllerErrorCode getTorqueCmd(double *torque);
-
- /*!
- * \brief Get the actual torque of the joint motor.
- *
- * \param double* torque Pointer to value to change
- */
- ControllerErrorCode getTorqueAct(double *torque);
-
- //-------------------------------------------------------------------------//
- //POSITION CALLS
- //-------------------------------------------------------------------------//
-
- /*!
- * \brief Give set position of the joint for next update: revolute (angle) and prismatic (position)
- *
- * \param double pos Position command to issue
- */
- ControllerErrorCode setPosCmd(double pos);
-
- /*!
- * \brief Get latest position command to the joint: revolute (angle) and prismatic (position).
- * \param double* pos Pointer to value to change
- */
- ControllerErrorCode getPosCmd(double *pos);
-
- /*!
- * \brief Read the torque of the motor
- * \param double* pos Pointer to value to change
- */
- ControllerErrorCode getPosAct(double *pos);
-
- //-------------------------------------------------------------------------//
- //VELOCITY CALLS
- //-------------------------------------------------------------------------//
-
- /*!
- * \brief Set velocity command to the joint to be issue next update
- * \param double vel Velocity to issue next command
- */
- ControllerErrorCode setVelCmd(double vel);
-
- /*!
- * \brief Get latest velocity command to the joint
- * \param double* vel Pointer to value to change
- */
- ControllerErrorCode getVelCmd(double *vel);
-
- /*!
- * \brief Get actual velocity of the joint
- * \param double* vel Pointer to value to change
- */
- ControllerErrorCode getVelAct(double *vel);
-
- /*!
- * \brief Compute max velocity coming into the end stop to stop with linear velocity in endstop.
- *
- */
- double getMaxVelocity(void);
-
- //-------------------------------------------------------------------------//
- //UPDATE CALLS
- //-------------------------------------------------------------------------//
- /*!
- * \brief Issues commands to joint based on control mode
- *
- *
- */
-
- //Issues commands to the joint. Should be called at regular intervals
- virtual void update(void);
-
- //-------------------------------------------------------------------------//
- //PARAM SERVER CALLS
- //-------------------------------------------------------------------------//
- /*!
- * \brief Set parameters for this controller
- *
- * user can set maximum velocity
- * and maximum acceleration
- * constraints for this controller
- * \param string label Name of param to change
- * \param double value New value
- *<br>
- *<UL TYPE="none">
- *<LI> e.g. SetParam('PGain',10);
- *<LI> or SetParam('IGain',10);
- *<LI> or SetParam('DGain',1);
- *<LI> or SetParam('IMax', 100);
- *<LI> or SetParam('IMin',-100);
- *</UL>
- */
- ControllerErrorCode setParam(std::string label, double value);
-
- /*!
- * \brief Get parameters for this controller
- *
- * user can get maximum velocity
- * and maximum acceleration
- * constraints for this controller
- *<br>
- *<UL TYPE="none">
- *<UL TYPE="none">
- *<LI> e.g. GetParam('PGain',value);
- *<LI> or GetParam('IGain',value);
- *<LI> or GetParam('DGain',value);
- *<LI> or GetParam('IMax', value);
- *<LI> or GetParam('IMin', value);
- *</UL>
- */
- ControllerErrorCode getParam(std::string label, double* value);
-
- /*!
- * \brief Set the name of the controller
- *
- * \param name - std::string representation of the name of the controller
- */
- void setName(const std::string & name);
-
- /*!
- * \brief Return string name of the controller
- *
- * \return std::string representation of the name of the controller
- */
- std::string getName();
-
- bool cap_accel_; /*!<Flag to indicate whether we should cap acceleration.>*/
- double max_accel_; /*!<Maximum allowed acceleration/deceleration.>*/
-
- /*!
- * \brief load parameters from the XML file
- *
- * \param std::string representation of the filename for the XML file with complete path
- */
- ControllerErrorCode loadXML(std::string filename);
-
-private:
-
- /*!
- * \brief Actually issue torque set command of the joint motor.
- */
- void setJointEffort(double torque);
-
- mechanism::Joint* joint_; /*!< Joint we're controlling>*/
- Pid pid_controller_; /*!< Internal PID controller>*/
-
- double last_time_;/*!< Last time stamp of update> */
-
- ControllerControlMode control_mode_; /*!< Indicate current controller mode (torque, position, velocity)>*/
- double cmd_torque_;/*!< Last commanded torque>*/
- double cmd_pos_;/*!< Last commanded position> */
- double cmd_vel_;/*!< Last commanded Velocity> */
-
- bool saturation_flag_; /*!< Flag to indicate last command exceed torque limits and was truncated>*/
- bool enabled_; /*!<Can controller issue commands?>*/
-
- double p_gain_; /*!< Proportional gain>*/
- double i_gain_;/*!< Integral gain >*/
- double d_gain_;/*!< Derivative gain> */
- double windup_max_;/*!< Upper integral clamp> */
- double windup_min_;/*!< Lower integral clamp> */
- double max_effort_; /*!<Temporary (until param server) : local copy of max effort.>*/
- double min_effort_; /*!<Temporary (until param server): local copy of min effort.>*/
-
- std::map<std::string,std::string> param_map_;
- void loadParam(std::string label, double &value);
- void loadParam(std::string label, int &value);
- std::string name_; /*!< Namespace ID for this controller>*/
-
- mechanism::Robot *robot_;
-};
-
-}
Added: pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_position_controller.h
===================================================================
--- pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_position_controller.h (rev 0)
+++ pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_position_controller.h 2008-08-08 23:50:01 UTC (rev 2845)
@@ -0,0 +1,102 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * 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 the Willow Garage 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.
+ *********************************************************************/
+
+#ifndef JOINT_POSITION_CONTROLLER_H
+#define JOINT_POSITION_CONTROLLER_H
+
+#include <generic_controllers/controller.h>
+#include <generic_controllers/pid.h>
+
+namespace controller
+{
+
+class JointPositionController : public Controller
+{
+public:
+ /*!
+ * \brief Default Constructor of the JointController class.
+ *
+ */
+ JointPositionController();
+
+ /*!
+ * \brief Destructor of the JointController class.
+ */
+ ~JointPositionController();
+
+ /*!
+ * \brief Functional way to initialize limits and gains.
+ *
+ */
+ void init(double p_gain, double i_gain, double d_gain, double windup, double time, mechanism::Joint *joint);
+ void initXml(mechanism::Robot *robot, TiXmlElement *config);
+
+ /*!
+ * \brief Give set position of the joint for next update: revolute (angle) and prismatic (position)
+ *
+ * \param double pos Position command to issue
+ */
+ void setCommand(double command);
+
+ /*!
+ * \brief Get latest position command to the joint: revolute (angle) and prismatic (position).
+ */
+ double getCommand();
+
+ /*!
+ * \brief Read the torque of the motor
+ */
+ double getActual();
+
+ /*!
+ * \brief Issues commands to the joint. Should be called at regular intervals
+ */
+
+ virtual void update();
+
+private:
+ /*!
+ * \brief Actually issue torque set command of the joint motor.
+ */
+ void setJointEffort(double torque);
+
+ mechanism::Joint* joint_; /*!< Joint we're controlling>*/
+ Pid pid_controller_; /*!< Internal PID controller>*/
+ double last_time_; /*!< Last time stamp of update> */
+ double command_; /*!< Last commanded position> */
+ mechanism::Robot *robot_; /*!< Pointer to robot structure>*/
+};
+}
+
+#endif /* JOINT_POSITION_CONTROLLER_H */
Deleted: pkg/trunk/controllers/generic_controllers/src/joint_controller.cpp
===================================================================
--- pkg/trunk/controllers/generic_controllers/src/joint_controller.cpp 2008-08-08 23:44:45 UTC (rev 2844)
+++ pkg/trunk/controllers/generic_controllers/src/joint_controller.cpp 2008-08-08 23:50:01 UTC (rev 2845)
@@ -1,479 +0,0 @@
-/*********************************************************************
- * Software License Agreement (BSD License)
- *
- * Copyright (c) 2008, Willow Garage, Inc.
- * All rights reserved.
- *
- * 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 the Willow Garage 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.
- *********************************************************************/
-#include <generic_controllers/joint_controller.h>
-#define DEFAULTMAXACCEL 5
-//#define DEBUG 1
-//Todo:
-//1. Get and set params via server
-//2. Integrate Joint and robot objects
-//3. Integrate Motor controller time
-
-using namespace controller;
-
-ROS_REGISTER_CONTROLLER(JointController)
-
-//-------------------------------------------------------------------------//
-//CONSTRUCTION/DESTRUCTION CALLS
-//-------------------------------------------------------------------------//
-
-
-JointController::JointController()
-{
- //Instantiate PID class
- pid_controller_.initPid(0, 0, 0, 0, 0); //Constructor for pid controller
-
- //Set commands to zero
- cmd_torque_ = 0;
- cmd_pos_ = 0;
- cmd_vel_ = 0;
-
- control_mode_ = CONTROLLER_DISABLED;
-}
-
-JointController::~JointController()
-{
-}
-
-void JointController::init()
-{
-}
-
-void JointController::initXml(mechanism::Robot *robot, TiXmlElement *config)
-{
-}
-
-void JointController::init(double p_gain, double i_gain, double d_gain, double windup_max, double windup_min,
- ControllerControlMode mode, double time, double max_effort, double min_effort,
- mechanism::Joint *joint)
-{
- pid_controller_.initPid(p_gain, i_gain, d_gain, windup_max, windup_min); //Constructor for pid controller
-
- cmd_torque_ = 0;
- cmd_pos_ = 0;
- cmd_vel_ = 0;
-
- //Init time
- last_time_ = time;
-
- //Temporary: will transition to use param server
- p_gain_ = p_gain;
- i_gain_ = i_gain;
- d_gain_ = d_gain;
- windup_max_ = windup_max;
- windup_min_ = windup_min;
-
- min_effort_ = min_effort;
- max_effort_ = max_effort;
- joint_ = joint;
-
- control_mode_ = mode;
- enableController();
-}
-
-void JointController::init(PidControlParam pcp, ControllerControlMode mode, double time, double max_effort,
- double min_effort, mechanism::Joint *joint)
-{
- pid_controller_.initPid(pcp.p_gain_, pcp.i_gain_, pcp.d_gain_, pcp.windup_max_, pcp.windup_min_); //Constructor for pid controller
-
- cmd_torque_ = 0;
- cmd_pos_ = 0;
- cmd_vel_ = 0;
-
- //Init time
- last_time_ = time;
-
- //Temporary: will transition to use param server
- p_gain_ = pcp.p_gain_;
- i_gain_ = pcp.i_gain_;
- d_gain_ = pcp.d_gain_;
- windup_max_ = pcp.windup_max_;
- windup_min_ = pcp.windup_min_;
-
- min_effort_ = min_effort;
- max_effort_ = max_effort;
- joint_ = joint;
-
- control_mode_ = mode;
- enableController();
-}
-
-void JointController::init(double time, mechanism::Joint *joint)
-{
- pid_controller_.initPid(p_gain_, i_gain_, d_gain_, windup_max_, windup_min_); //Constructor for pid controller
-
- cmd_torque_ = 0;
- cmd_pos_ = 0;
- cmd_vel_ = 0;
-
- last_time_ = time;
- joint_ = joint;
- enableController();
-}
-
-//-------------------------------------------------------------------------//
-//MODE/ENABLE CALLS
-//-------------------------------------------------------------------------//
-
-//Set the controller control mode
-ControllerControlMode JointController::setMode(ControllerControlMode mode)
-{
- control_mode_ = mode;
- return CONTROLLER_MODE_SET;
-}
-
-//Getter for control mode
-ControllerControlMode JointController::getMode()
-{
- return control_mode_;
-}
-
-//Allow controller to function
-ControllerControlMode JointController::enableController()
-{
- enabled_ = true;
- return CONTROLLER_ENABLED;
-}
-
-//Disable functioning. Set joint torque to zero.
-ControllerControlMode JointController::disableController()
-{
- enabled_ = false;
- joint_->commanded_effort_ = 0; //Immediately set commanded Effort to 0
- control_mode_ = CONTROLLER_DISABLED;
- return CONTROLLER_DISABLED;
-}
-
-bool JointController::checkForSaturation(void)
-{
- return saturation_flag_;
-}
-
-//-------------------------------------------------------------------------//
-//TORQUE CALLS
-//-------------------------------------------------------------------------//
-ControllerErrorCode JointController::setTorqueCmd(double torque)
-{
- // double max_effort = joint->effortLimit;
-
- if (control_mode_ != CONTROLLER_TORQUE) //Make sure we're in torque command mode
- {
- return CONTROLLER_MODE_ERROR;
- }
-
- cmd_torque_ = torque;
-
- if (cmd_torque_ >= max_effort_)
- { //Truncate to positive limit
- cmd_torque_ = max_effort_;
- return CONTROLLER_TORQUE_LIMIT;
- }
- else if (cmd_torque_ <= min_effort_)
- { //Truncate to negative limit
- cmd_torque_ = min_effort_;
- return CONTROLLER_TORQUE_LIMIT;
- }
-
- return CONTROLLER_CMD_SET;
-}
-
-ControllerErrorCode JointController::getTorqueCmd(double *torque)
-{
- *torque = cmd_torque_;
- return CONTROLLER_ALL_OK;
-}
-
-ControllerErrorCode JointController::getTorqueAct(double *torque)
-{
- *torque = joint_->applied_effort_; //Read torque from joint
- return CONTROLLER_ALL_OK;
-}
-
-//-------------------------------------------------------------------------//
-//POSITION CALLS
-//-------------------------------------------------------------------------//
-
-
-//Query mode, then set desired position
-ControllerErrorCode JointController::setPosCmd(double pos)
-{
- if (control_mode_ != CONTROLLER_POSITION) //Make sure we're in position command mode
- {
- return CONTROLLER_MODE_ERROR;
- }
-
- cmd_pos_ = pos;
- if (cmd_pos_ >= joint_->joint_limit_max_ && joint_->type_ != mechanism::JOINT_CONTINUOUS)
- { //Truncate to positive limit
- cmd_pos_ = joint_->joint_limit_max_;
- return CONTROLLER_JOINT_LIMIT;
- }
- else if (cmd_pos_ <= joint_->joint_limit_min_ && joint_->type_ != mechanism::JOINT_CONTINUOUS)
- { //Truncate to negative limit
- cmd_pos_ = joint_->joint_limit_min_;
- return CONTROLLER_JOINT_LIMIT;
- }
- return CONTROLLER_CMD_SET;
-}
-
-//Return the current position command
-controller::ControllerErrorCode JointController::getPosCmd(double *pos)
-{
- *pos = cmd_pos_;
- return controller::CONTROLLER_ALL_OK;
-}
-
-//Query the joint for the actual position
-controller::ControllerErrorCode JointController::getPosAct(double *pos)
-{
- *pos = joint_->position_;
- return controller::CONTROLLER_ALL_OK;
-}
-
-//-------------------------------------------------------------------------//
-//VELOCITY CALLS
-//-------------------------------------------------------------------------//
-//Check mode, then set the commanded velocity
-ControllerErrorCode JointController::setVelCmd(double vel)
-{
- if (control_mode_ == CONTROLLER_VELOCITY || control_mode_ == ETHERDRIVE_SPEED)
- { //Make sure we're in velocity command mode
- cmd_vel_ = vel;
- return CONTROLLER_CMD_SET;
- }
- else
- return CONTROLLER_MODE_ERROR;
-}
-
-//Return the internally stored commanded velocity
-controller::ControllerErrorCode JointController::getVelCmd(double *vel)
-{
- *vel = cmd_vel_;
- return controller::CONTROLLER_ALL_OK;
-}
-
-//Query our joint for velocity
-controller::ControllerErrorCode JointController::getVelAct(double *vel)
-{
- *vel = joint_->velocity_;
- return controller::CONTROLLER_ALL_OK;
-}
-
-double JointController::getMaxVelocity()
-{
- double dis_to_min, dis_to_max, closest_limit;
- dis_to_min = fabs(math_utils::shortest_angular_distance(joint_->position_, joint_->joint_limit_min_));
- dis_to_max = fabs(math_utils::shortest_angular_distance(joint_->position_, joint_->joint_limit_max_));
- closest_limit = dis_to_min < dis_to_max ? dis_to_min : dis_to_max;
- return sqrt(fabs(closest_limit * max_accel_));
-}
-
-//-------------------------------------------------------------------------//
-//UPDATE CALLS
-//-------------------------------------------------------------------------//
-
-
-void JointController::update(void)
-{
- double error(0), time(0), current_torque_cmd(0);
- double max_velocity = cmd_vel_;
- double currentVoltageCmd, v_backemf, v_clamp_min, v_clamp_max, k;
-
- if (control_mode_ == controller::CONTROLLER_DISABLED)
- {
- printf("JointController.cpp: Error:: controller disabled\n");
- return; //If we're not initialized, don't try to interact
- }
- time = robot_->hw_->current_time_;
- switch (control_mode_)
- {
- case CONTROLLER_TORQUE: //Pass through torque command
- current_torque_cmd = cmd_torque_;
- break;
-
- case CONTROLLER_POSITION: //Close the loop around position
- if (joint_->type_ == mechanism::JOINT_ROTARY || joint_->type_ == mechanism::JOINT_CONTINUOUS)
- error = math_utils::shortest_angular_distance(cmd_pos_, joint_->position_);
- else
- error = joint_->position_ - cmd_pos_;
- current_torque_cmd = pid_controller_.updatePid(error, time - last_time_);
- break;
-
- case CONTROLLER_VELOCITY: //Close the loop around velocity
- if (cap_accel_)
- {
- max_velocity = getMaxVelocity(); //Check max velocity coming into wall
- if (fabs(cmd_vel_) > max_velocity)
- {
- cmd_vel_ = -max_velocity; //Truncate velocity smoothly
- }
- }
- error = joint_->velocity_ - cmd_vel_;
-
- current_torque_cmd = pid_controller_.updatePid(error, time - last_time_);
- ...
[truncated message content] |
|
From: <rob...@us...> - 2008-08-09 08:06:22
|
Revision: 2863
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2863&view=rev
Author: rob_wheeler
Date: 2008-08-09 08:06:24 +0000 (Sat, 09 Aug 2008)
Log Message:
-----------
Create a transmission factory and use it when initializing mechanism control
Modified Paths:
--------------
pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/transmission.h
pkg/trunk/mechanism/mechanism_model/manifest.xml
pkg/trunk/mechanism/mechanism_model/src/simple_transmission.cpp
pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp
Modified: pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
===================================================================
--- pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2008-08-09 06:13:28 UTC (rev 2862)
+++ pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2008-08-09 08:06:24 UTC (rev 2863)
@@ -31,7 +31,7 @@
* mc.registerActuator("AnActuatorName", 0);
* mc.registerActuator("AnotherActuatorName", 0);
* ...
- * mc.init(config);
+ * mc.initXml(config);
*
* mc.spawnController("JointController", controllerConfig);
*/
@@ -62,7 +62,7 @@
void update();
// Non real-time functions
- bool init(TiXmlElement* config);
+ bool initXml(TiXmlElement* config);
bool registerActuator(const std::string &name, int index);
bool addController(controller::Controller *c, const std::string &name);
bool spawnController(const std::string &type, const std::string &name, TiXmlElement *config);
Modified: pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-08-09 06:13:28 UTC (rev 2862)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-08-09 08:06:24 UTC (rev 2863)
@@ -51,14 +51,14 @@
return true;
}
-bool MechanismControl::init(TiXmlElement* config)
+bool MechanismControl::initXml(TiXmlElement* config)
{
bool successful = true;
+ ros::node *node = ros::node::instance();
TiXmlElement *elt;
- // Constructs the joints
- std::map<std::string, Joint*> joint_map;
+ // Construct the joints
for (elt = config->FirstChildElement("joint"); elt; elt = elt->NextSiblingElement("joint"))
{
Joint *j = new Joint;
@@ -67,41 +67,16 @@
j->initXml(elt);
}
- // Constructs the transmissions
+ // Construct the transmissions
elt = config->FirstChildElement("transmission");
for (; elt; elt = elt->NextSiblingElement("transmission"))
{
- if (0 == strcmp("SimpleTransmission", elt->Attribute("type")))
- {
- // Looks up the joint and the actuator used by the transmission.
- Robot::IndexMap::iterator joint_it =
- model_.joints_lookup_.find(elt->FirstChildElement("joint")->Attribute("name"));
- Robot::IndexMap::iterator actuator_it =
- model_.actuators_lookup_.find(elt->FirstChildElement("actuator")->Attribute("name"));
- if (joint_it == model_.joints_lookup_.end())
- {
- // TODO: report: The joint was not declared in the XML file
- printf("Unable to locate joint: %s\n", elt->FirstChildElement("joint")->Attribute("name"));
- continue;
- }
- if (actuator_it == model_.actuators_lookup_.end())
- {
- // TODO: report: The actuator was not registered with mechanism control.
- printf("Unable to locate actuator: %s\n", elt->FirstChildElement("actuator")->Attribute("name"));
- continue;
- }
- Transmission *tr =
- new SimpleTransmission(model_.joints_[joint_it->second], hw_->actuators_[actuator_it->second],
- atof(elt->FirstChildElement("mechanicalReduction")->GetText()),
- atof(elt->FirstChildElement("motorTorqueConstant")->GetText()),
- atof(elt->FirstChildElement("pulsesPerRevolution")->GetText()));
- model_.transmissions_.push_back(tr);
- }
- else
- {
- // TODO: report: Unknown transmission type
- successful = false;
- }
+ const char *type = elt->Attribute("type");
+ Transmission *t = TransmissionFactory::instance().create(type);
+ if (t == NULL)
+ node->log(ros::FATAL, "Unknown transmission type: %s\n", type);
+ t->initXml(elt, &model_);
+ model_.transmissions_.push_back(t);
}
initialized_ = true;
Modified: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/transmission.h
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/transmission.h 2008-08-09 06:13:28 UTC (rev 2862)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/transmission.h 2008-08-09 08:06:24 UTC (rev 2863)
@@ -34,17 +34,35 @@
#ifndef TRANSMISSION_H
#define TRANSMISSION_H
+#include <tinyxml/tinyxml.h>
+
+#include <misc_utils/factory.h>
+
#include "mechanism_model/joint.h"
#include "hardware_interface/hardware_interface.h"
namespace mechanism {
+class Robot;
+
+class Transmission;
+typedef Factory<Transmission> TransmissionFactory;
+
+#define ROS_REGISTER_TRANSMISSION(c) \
+ mechanism::Transmission *ROS_New_##c() { return new c(); } \
+ bool ROS_TRANSMISSION_##c = \
+ mechanism::TransmissionFactory::instance().registerType(#c, ROS_New_##c);
+
+
class Transmission
{
public:
Transmission() {}
virtual ~Transmission() {}
+ // Initialize transmission from XML data
+ virtual void initXml(TiXmlElement *config, Robot *robot) = 0;
+
// Uses encoder data to fill out joint position and velocities
virtual void propagatePosition() = 0;
@@ -67,6 +85,8 @@
SimpleTransmission(Joint *joint, Actuator *actuator, double mechanical_reduction, double motor_torque_constant, double pulses_per_revolution);
~SimpleTransmission() {}
+ void initXml(TiXmlElement *config, Robot *robot);
+
Actuator *actuator_;
Joint *joint_;
Modified: pkg/trunk/mechanism/mechanism_model/manifest.xml
===================================================================
--- pkg/trunk/mechanism/mechanism_model/manifest.xml 2008-08-09 06:13:28 UTC (rev 2862)
+++ pkg/trunk/mechanism/mechanism_model/manifest.xml 2008-08-09 08:06:24 UTC (rev 2863)
@@ -3,8 +3,10 @@
</description>
<author>Eric Berger be...@wi...</author>
<license>BSD</license>
+<depend package='roscpp'/>
<depend package='hardware_interface'/>
<depend package="stl_utils" />
+<depend package="misc_utils" />
<depend package="tinyxml" />
<url>http://pr.willowgarage.com</url>
<export>
Modified: pkg/trunk/mechanism/mechanism_model/src/simple_transmission.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_model/src/simple_transmission.cpp 2008-08-09 06:13:28 UTC (rev 2862)
+++ pkg/trunk/mechanism/mechanism_model/src/simple_transmission.cpp 2008-08-09 08:06:24 UTC (rev 2863)
@@ -31,13 +31,14 @@
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
-#include <mechanism_model/transmission.h>
-#include <mechanism_model/joint.h>
#include <math.h>
-#include <stdio.h>
+#include <ros/node.h>
+#include <mechanism_model/robot.h>
using namespace mechanism;
+ROS_REGISTER_TRANSMISSION(SimpleTransmission)
+
SimpleTransmission::SimpleTransmission(Joint *joint, Actuator *actuator,
double mechanical_reduction, double motor_torque_constant,
double pulses_per_revolution)
@@ -49,6 +50,14 @@
joint_ = joint;
}
+void SimpleTransmission::initXml(TiXmlElement *elt, Robot *robot)
+{
+ joint_ = robot->getJoint(elt->FirstChildElement("joint")->Attribute("name"));
+ actuator_ = robot->getActuator(elt->FirstChildElement("actuator")->Attribute("name"));
+ mechanical_reduction_ = atof(elt->FirstChildElement("mechanicalReduction")->GetText()),
+ motor_torque_constant_ = atof(elt->FirstChildElement("motorTorqueConstant")->GetText()),
+ pulses_per_revolution_ = atof(elt->FirstChildElement("pulsesPerRevolution")->GetText());
+}
void SimpleTransmission::propagatePosition()
{
Modified: pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp
===================================================================
--- pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp 2008-08-09 06:13:28 UTC (rev 2862)
+++ pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp 2008-08-09 08:06:24 UTC (rev 2863)
@@ -69,7 +69,7 @@
ec.initXml(root, mc);
// Initialize mechanism control from robot description
- mc.init(root);
+ mc.initXml(root);
// Spawn controllers
// TODO what file does this come from?
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2008-08-11 21:32:11
|
Revision: 2914
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2914&view=rev
Author: stuglaser
Date: 2008-08-11 21:32:13 +0000 (Mon, 11 Aug 2008)
Log Message:
-----------
MechanismControlNode's update function gets called now.
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp
pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp 2008-08-11 21:32:09 UTC (rev 2913)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp 2008-08-11 21:32:13 UTC (rev 2914)
@@ -155,7 +155,8 @@
// Runs Mechanism Control
//--------------------------------------------------
hw_.current_time_ = Simulator::Instance()->GetSimTime();
- mc_.update();
+ //mc_.update();
+ mcn_.update();
//--------------------------------------------------
// Takes in actuation commands
Modified: pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
===================================================================
--- pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2008-08-11 21:32:09 UTC (rev 2913)
+++ pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2008-08-11 21:32:13 UTC (rev 2914)
@@ -70,13 +70,13 @@
bool spawnController(const std::string &type, const std::string &name, TiXmlElement *config);
mechanism::Robot model_;
+ HardwareInterface *hw_;
// TODO: deprecated. Replaced by ControllerFactory
void registerControllerType(const std::string& type, ControllerAllocator f);
private:
bool initialized_;
- HardwareInterface *hw_;
const static int MAX_NUM_CONTROLLERS = 100;
ros::thread::mutex controllers_mutex_;
@@ -93,6 +93,10 @@
MechanismControlNode(MechanismControl *mc);
virtual ~MechanismControlNode() {}
+ bool initXml(TiXmlElement *config);
+
+ void update(); // Must be realtime safe
+
bool listControllerTypes(mechanism_control::ListControllerTypes::request &req,
mechanism_control::ListControllerTypes::response &resp);
bool listControllers(mechanism_control::ListControllers::request &req,
Modified: pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-08-11 21:32:09 UTC (rev 2913)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-08-11 21:32:13 UTC (rev 2914)
@@ -180,7 +180,16 @@
advertise_service("spawn_controller", &MechanismControlNode::spawnController);
}
+bool MechanismControlNode::initXml(TiXmlElement *config)
+{
+ return mc_->initXml(config);
+}
+void MechanismControlNode::update()
+{
+ mc_->update();
+}
+
bool MechanismControlNode::listControllerTypes(
mechanism_control::ListControllerTypes::request &req,
mechanism_control::ListControllerTypes::response &resp)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-08-11 22:00:49
|
Revision: 2922
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2922&view=rev
Author: gerkey
Date: 2008-08-11 22:00:42 +0000 (Mon, 11 Aug 2008)
Log Message:
-----------
Added blacklist files to broken packages
Added Paths:
-----------
pkg/trunk/3rdparty/estar/ROS_BUILD_BLACKLIST
pkg/trunk/3rdparty/libsunflower/ROS_BUILD_BLACKLIST
pkg/trunk/3rdparty/plplot/ROS_BUILD_BLACKLIST
pkg/trunk/controllers/testControllers/ROS_BUILD_BLACKLIST
pkg/trunk/deprecated/sbl_pr2arm_mpk/ROS_BUILD_BLACKLIST
pkg/trunk/deprecated/test_kinematic_planning/ROS_BUILD_BLACKLIST
pkg/trunk/drivers/cam/cv_cam/ROS_BUILD_BLACKLIST
pkg/trunk/drivers/cam/elphel_cam/ROS_BUILD_BLACKLIST
pkg/trunk/exec_trex/ROS_BUILD_BLACKLIST
pkg/trunk/nav/sbpl_2dnav/ROS_BUILD_BLACKLIST
pkg/trunk/simulators/nepumuk/ROS_BUILD_BLACKLIST
pkg/trunk/vision/laser_interface/ROS_BUILD_BLACKLIST
pkg/trunk/vision/urg_ipdcmot/ROS_BUILD_BLACKLIST
pkg/trunk/visualization/log_gui/ROS_BUILD_BLACKLIST
Added: pkg/trunk/3rdparty/estar/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/3rdparty/estar/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/3rdparty/estar/ROS_BUILD_BLACKLIST 2008-08-11 22:00:42 UTC (rev 2922)
@@ -0,0 +1,2 @@
+http://pr.willowgarage.com:8011/personalrobots-update-wgs7/builds/7/step-nepumuk%0A/0
+https://prdev.willowgarage.com/trac/personalrobots/ticket/157
Added: pkg/trunk/3rdparty/libsunflower/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/3rdparty/libsunflower/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/3rdparty/libsunflower/ROS_BUILD_BLACKLIST 2008-08-11 22:00:42 UTC (rev 2922)
@@ -0,0 +1 @@
+estar
Added: pkg/trunk/3rdparty/plplot/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/3rdparty/plplot/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/3rdparty/plplot/ROS_BUILD_BLACKLIST 2008-08-11 22:00:42 UTC (rev 2922)
@@ -0,0 +1,2 @@
+http://pr.willowgarage.com:8011/personalrobots-update-wgs7/builds/7/step-plplot%0A/0
+https://prdev.willowgarage.com/trac/personalrobots/ticket/194
Added: pkg/trunk/controllers/testControllers/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/controllers/testControllers/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/controllers/testControllers/ROS_BUILD_BLACKLIST 2008-08-11 22:00:42 UTC (rev 2922)
@@ -0,0 +1,2 @@
+http://pr.willowgarage.com:8011/personalrobots-update-wgs7/builds/7/step-testControllers%0A/0
+https://prdev.willowgarage.com/trac/personalrobots/ticket/151
Added: pkg/trunk/deprecated/sbl_pr2arm_mpk/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/deprecated/sbl_pr2arm_mpk/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/deprecated/sbl_pr2arm_mpk/ROS_BUILD_BLACKLIST 2008-08-11 22:00:42 UTC (rev 2922)
@@ -0,0 +1 @@
+http://pr.willowgarage.com:8011/personalrobots-update-wgs7/builds/7/step-sbl_pr2arm_mpk%0A/0
Added: pkg/trunk/deprecated/test_kinematic_planning/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/deprecated/test_kinematic_planning/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/deprecated/test_kinematic_planning/ROS_BUILD_BLACKLIST 2008-08-11 22:00:42 UTC (rev 2922)
@@ -0,0 +1 @@
+http://pr.willowgarage.com:8011/personalrobots-update-wgs7/builds/7/step-test_kinematic_planning%0A/0
Added: pkg/trunk/drivers/cam/cv_cam/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/drivers/cam/cv_cam/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/drivers/cam/cv_cam/ROS_BUILD_BLACKLIST 2008-08-11 22:00:42 UTC (rev 2922)
@@ -0,0 +1,2 @@
+http://pr.willowgarage.com:8011/personalrobots-update-wgs7/builds/7/step-cv_cam%0A/0
+https://prdev.willowgarage.com/trac/personalrobots/ticket/161
Added: pkg/trunk/drivers/cam/elphel_cam/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/drivers/cam/elphel_cam/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/drivers/cam/elphel_cam/ROS_BUILD_BLACKLIST 2008-08-11 22:00:42 UTC (rev 2922)
@@ -0,0 +1,2 @@
+http://pr.willowgarage.com:8011/personalrobots-update-wgs7/builds/7/step-elphel_cam%0A/0
+https://prdev.willowgarage.com/trac/personalrobots/ticket/162
Added: pkg/trunk/exec_trex/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/exec_trex/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/exec_trex/ROS_BUILD_BLACKLIST 2008-08-11 22:00:42 UTC (rev 2922)
@@ -0,0 +1,2 @@
+http://pr.willowgarage.com:8011/personalrobots-update-wgs7/builds/7/step-exec_trex%0A/0
+https://prdev.willowgarage.com/trac/personalrobots/ticket/144
Added: pkg/trunk/nav/sbpl_2dnav/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/nav/sbpl_2dnav/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/nav/sbpl_2dnav/ROS_BUILD_BLACKLIST 2008-08-11 22:00:42 UTC (rev 2922)
@@ -0,0 +1 @@
+http://pr.willowgarage.com:8011/personalrobots-update-wgs7/builds/7/step-sbpl_2dnav%0A/0
Added: pkg/trunk/simulators/nepumuk/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/simulators/nepumuk/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/simulators/nepumuk/ROS_BUILD_BLACKLIST 2008-08-11 22:00:42 UTC (rev 2922)
@@ -0,0 +1 @@
+estar
Added: pkg/trunk/vision/laser_interface/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/vision/laser_interface/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/vision/laser_interface/ROS_BUILD_BLACKLIST 2008-08-11 22:00:42 UTC (rev 2922)
@@ -0,0 +1,2 @@
+http://pr.willowgarage.com:8011/personalrobots-update-wgs7/builds/7/step-laser_interface%0A/0
+https://prdev.willowgarage.com/trac/personalrobots/ticket/193
Added: pkg/trunk/vision/urg_ipdcmot/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/vision/urg_ipdcmot/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/vision/urg_ipdcmot/ROS_BUILD_BLACKLIST 2008-08-11 22:00:42 UTC (rev 2922)
@@ -0,0 +1,2 @@
+http://pr.willowgarage.com:8011/personalrobots-update-wgs7/builds/7/step-urg_ipdcmot%0A/0
+https://prdev.willowgarage.com/trac/personalrobots/ticket/158
Added: pkg/trunk/visualization/log_gui/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/visualization/log_gui/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/visualization/log_gui/ROS_BUILD_BLACKLIST 2008-08-11 22:00:42 UTC (rev 2922)
@@ -0,0 +1,2 @@
+http://pr.willowgarage.com:8011/personalrobots-update-wgs7/builds/7/step-log_gui%0A/0
+https://prdev.willowgarage.com/trac/personalrobots/ticket/191
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
Thanks for helping keep SourceForge clean.
X