Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[2302] pkg/trunk
|
From: <adv...@us...> - 2008-07-30 05:46:05
|
Revision: 2302
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2302&view=rev
Author: advaitjain
Date: 2008-07-30 05:46:12 +0000 (Wed, 30 Jul 2008)
Log Message:
-----------
added a new position sensor in gazebo_sensors and propagated it
all the way through. We can attach this sensor to any object
in the simulation and use it to get the the objects pose.
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h
pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp
pkg/trunk/drivers/simulator/gazebo_sensors/include/gazebo_sensors/gazebo_sensors.h
pkg/trunk/drivers/simulator/gazebo_sensors/src/gazebo_sensors.cpp
pkg/trunk/manip/overhead_grasp_behavior/src/overhead_grasp.cc
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_tablegrasp.world
pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h
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
pkg/trunk/vision/cam_viewer/cam_viewer.cpp
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h 2008-07-30 02:27:50 UTC (rev 2301)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h 2008-07-30 05:46:12 UTC (rev 2302)
@@ -553,6 +553,11 @@
public: PR2_ERROR_CODE GetBasePositionActual(double* x, double* y, double *z, double *roll, double *pitch, double *yaw);
/*! \fn
+ \brief Retrieve position and orientation of an object.
+ */
+ public: PR2_ERROR_CODE GetObjectPositionActual(double* x, double* y, double *z, double *roll, double *pitch, double *yaw);
+
+ /*! \fn
\brief - Run the robot
*/
public: PR2_ERROR_CODE RunRobot();
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-07-30 02:27:50 UTC (rev 2301)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-07-30 05:46:12 UTC (rev 2302)
@@ -918,7 +918,13 @@
return PR2_ALL_OK;
};
+PR2_ERROR_CODE PR2Robot::GetObjectPositionActual(double* x, double* y, double *z, double *roll, double *pitch, double *yaw)
+{
+ hw.GetObjectPositionGroundTruth(x,y,z,roll,pitch,yaw);
+ return PR2_ALL_OK;
+};
+
PR2_ERROR_CODE PR2Robot::GetLeftGripperCmd(double *gap,double *force)
{
hw.GetGripperCmd((PR2_MODEL_ID)PR2::PR2_LEFT_GRIPPER,gap,force);
Modified: pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h 2008-07-30 02:27:50 UTC (rev 2301)
+++ pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h 2008-07-30 05:46:12 UTC (rev 2302)
@@ -320,6 +320,11 @@
public: PR2_ERROR_CODE GetBasePositionGroundTruth(double* x, double* y, double *z, double *roll, double *pitch, double *yaw);
/*! \fn
+ \brief - Get ground truth object position
+ */
+ public: PR2_ERROR_CODE GetObjectPositionGroundTruth(double* x, double* y, double *z, double *roll, double *pitch, double *yaw);
+
+ /*! \fn
\brief Wait for Gazebo to update
TODO: put this in UpdateHW()?
*/
Modified: pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp 2008-07-30 02:27:50 UTC (rev 2301)
+++ pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp 2008-07-30 05:46:12 UTC (rev 2302)
@@ -46,6 +46,7 @@
gazebo::PositionIface *pr2LeftWristIface;
gazebo::PositionIface *pr2RightWristIface;
gazebo::PositionIface *pr2BaseIface;
+gazebo::PositionIface *posObjectIface;
gazebo::CameraIface *pr2WristCameraLeftIface;
gazebo::CameraIface *pr2WristCameraRightIface;
gazebo::CameraIface *pr2ForearmCameraLeftIface;
@@ -96,6 +97,8 @@
pr2RightWristIface = new gazebo::PositionIface();
pr2BaseIface = new gazebo::PositionIface();
+ posObjectIface = new gazebo::PositionIface();
+
int serverId = 0;
/// Connect to the libgazebo server
@@ -307,6 +310,17 @@
pr2BaseIface = NULL;
}
+ try
+ {
+ posObjectIface->Open(client, "p3d_object_position");
+ }
+ catch (std::string e)
+ {
+ std::cout << "Gazebo error: Unable to connect to the object position interface\n"
+ << e << "\n";
+ posObjectIface = NULL;
+ }
+
std::cout << "initial HW reads\n" << std::endl;
// fill in actuator data
for (int id = 0; id < PR2::HEAD_PTZ_R_TILT; id++)
@@ -1001,6 +1015,22 @@
return PR2_ALL_OK;
};
+PR2_ERROR_CODE PR2HW::GetObjectPositionGroundTruth(double* x, double* y, double *z, double *roll, double *pitch, double *yaw)
+{
+ if(posObjectIface == NULL)
+ return PR2_ALL_OK;
+
+ posObjectIface->Lock(1);
+ *x = posObjectIface->data->pose.pos.x;
+ *y = posObjectIface->data->pose.pos.y;
+ *z = posObjectIface->data->pose.pos.z;
+ *roll = posObjectIface->data->pose.roll;
+ *pitch = posObjectIface->data->pose.pitch;
+ *yaw = posObjectIface->data->pose.yaw;
+ posObjectIface->Unlock();
+ return PR2_ALL_OK;
+};
+
PR2_ERROR_CODE PR2HW::ClientWait()
{
// block until simulator update.
Modified: pkg/trunk/drivers/simulator/gazebo_sensors/include/gazebo_sensors/gazebo_sensors.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_sensors/include/gazebo_sensors/gazebo_sensors.h 2008-07-30 02:27:50 UTC (rev 2301)
+++ pkg/trunk/drivers/simulator/gazebo_sensors/include/gazebo_sensors/gazebo_sensors.h 2008-07-30 05:46:12 UTC (rev 2302)
@@ -78,7 +78,9 @@
gazebo::PositionIface *pr2RightWristIface;
gazebo::PositionIface *pr2BaseIface;
+ gazebo::PositionIface *posObjectIface;
+
/*! \fn
\brief Constructor
*/
@@ -170,6 +172,10 @@
*/
public: PR2::PR2_ERROR_CODE GetWristPoseGroundTruth(PR2::PR2_MODEL_ID id, double *x, double *y, double *z, double *roll, double *pitch, double *yaw);
+ /*! \fn
+ \brief - 3D position sensor can be attached to any object to get its position and orientation.
+ */
+ public: PR2::PR2_ERROR_CODE GetObjectPositionGroundTruth(double* x, double* y, double *z, double *roll, double *pitch, double *yaw);
/*! \fn
\brief Wait for Gazebo to update
Modified: pkg/trunk/drivers/simulator/gazebo_sensors/src/gazebo_sensors.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_sensors/src/gazebo_sensors.cpp 2008-07-30 02:27:50 UTC (rev 2301)
+++ pkg/trunk/drivers/simulator/gazebo_sensors/src/gazebo_sensors.cpp 2008-07-30 05:46:12 UTC (rev 2302)
@@ -33,6 +33,8 @@
pr2LeftWristIface = new gazebo::PositionIface();
pr2RightWristIface = new gazebo::PositionIface();
pr2BaseIface = new gazebo::PositionIface();
+
+ posObjectIface = new gazebo::PositionIface();
}
GazeboSensors::~GazeboSensors()
@@ -158,7 +160,18 @@
pr2BaseIface = NULL;
}
+ try
+ {
+ posObjectIface->Open(client, "p3d_object_position");
+ }
+ catch (std::string e)
+ {
+ std::cout << "Gazebo error: Unable to connect to the object position interface\n"
+ << e << "\n";
+ posObjectIface = NULL;
+ }
+
return PR2::PR2_ALL_OK;
};
@@ -352,6 +365,19 @@
return PR2::PR2_ALL_OK;
};
+PR2::PR2_ERROR_CODE GazeboSensors::GetObjectPositionGroundTruth(double* x, double* y, double *z, double *roll, double *pitch, double *yaw)
+{
+ posObjectIface->Lock(1);
+ *x = posObjectIface->data->pose.pos.x;
+ *y = posObjectIface->data->pose.pos.y;
+ *z = posObjectIface->data->pose.pos.z;
+ *roll = posObjectIface->data->pose.roll;
+ *pitch = posObjectIface->data->pose.pitch;
+ *yaw = posObjectIface->data->pose.yaw;
+ posObjectIface->Unlock();
+ return PR2::PR2_ALL_OK;
+};
+
PR2::PR2_ERROR_CODE GazeboSensors::ClientWait()
{
// block until simulator update.
Modified: pkg/trunk/manip/overhead_grasp_behavior/src/overhead_grasp.cc
===================================================================
--- pkg/trunk/manip/overhead_grasp_behavior/src/overhead_grasp.cc 2008-07-30 02:27:50 UTC (rev 2301)
+++ pkg/trunk/manip/overhead_grasp_behavior/src/overhead_grasp.cc 2008-07-30 05:46:12 UTC (rev 2302)
@@ -6,91 +6,110 @@
#include <libpr2API/pr2API.h>
#include <pr2_msgs/EndEffectorState.h>
+#include <std_msgs/Point3DFloat32.h>
#include <std_msgs/Float64.h>
#include <std_msgs/PR2Arm.h>
#include <unistd.h>
using namespace KDL;
-// coordinates of arm's origin in gazebo/arm coordinate frame (the axes of both frames are parallel)
-Vector gazebo_to_arm_vector = Vector(1.040-0.7987,-0.15,0.8269);
-void quit(int sig);
-void keyboardLoop(ros::node &n);
+class OverheadGrasper : public ros::node
+{
+ public:
+ // 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;
+ public:
+ OverheadGrasper(void) : ros::node("overhead_grasper")
+ {
+ advertise<std_msgs::PR2Arm>("cmd_rightarmconfig");
+ advertise<pr2_msgs::EndEffectorState>("right_pr2arm_set_end_effector");
+ advertise<std_msgs::Float64>("interpolate_step_size");
+ advertise<std_msgs::Float64>("interpolate_wait_time");
+ subscribe("object_position", objectPosMsg, &OverheadGrasper::objectPosition_cb);
-void positionArmCartesian(ros::node &n, Vector v, Rotation r)
-{
- 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];
- }
+ gazebo_to_arm_vector = Vector(1.040-0.7987,-0.15,0.8269);
+ }
- n.publish("right_pr2arm_set_end_effector",efs);
-}
+ void positionArmCartesian(Vector v, Rotation r)
+ {
+ 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];
+ }
+ publish("right_pr2arm_set_end_effector",efs);
+ }
-void overheadGraspPosture(ros::node &n)
-{
- std_msgs::PR2Arm rightarm;
- rightarm.turretAngle = deg2rad*-20;
- rightarm.shoulderLiftAngle = deg2rad*-20;
- rightarm.upperarmRollAngle = deg2rad*180;
- rightarm.elbowAngle = deg2rad*-30;
- rightarm.forearmRollAngle = 0;
- rightarm.wristPitchAngle = 0;
- rightarm.wristRollAngle = 0;
- rightarm.gripperForceCmd = 0;
- rightarm.gripperGapCmd = 0.2;
- n.publish("cmd_rightarmconfig",rightarm);
-}
+ void overheadGraspPosture()
+ {
+ std_msgs::PR2Arm rightarm;
+ rightarm.turretAngle = deg2rad*-20;
+ rightarm.shoulderLiftAngle = deg2rad*-20;
+ rightarm.upperarmRollAngle = deg2rad*180;
+ rightarm.elbowAngle = deg2rad*-30;
+ rightarm.forearmRollAngle = 0;
+ rightarm.wristPitchAngle = 0;
+ rightarm.wristRollAngle = 0;
+ rightarm.gripperForceCmd = 0;
+ rightarm.gripperGapCmd = 0.2;
+ publish("cmd_rightarmconfig",rightarm);
+ }
-// Vector v is in gazebo coordinate frame.
-void positionEyecamOverObject(ros::node &n, Vector v)
-{
- Vector v_arm = v - gazebo_to_arm_vector;
- v_arm.data[2] += 0.3; // I want end effector to be 0.5m above object.
- Rotation r = Rotation::RotY(DTOR(90)); // look down vertically
- cout<<"Going to: "<<v_arm<<endl;
- positionArmCartesian(n, v_arm, r);
-}
+ // Vector v is in gazebo coordinate frame.
+ void positionEyecamOverObject(Vector v)
+ {
+ 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
+ cout<<"Going to: "<<v_arm<<endl;
+ positionArmCartesian(v_arm, r);
+ }
+ void objectPosition_cb(void)
+ {
+ }
+
+};
+
+
+void quit(int sig);
+void keyboardLoop(OverheadGrasper &n);
+
+
int main(int argc, char **argv)
{
ros::init(argc, argv);
- ros::node mynode("overhead_grasp");
+ OverheadGrasper ohGrasper;
- mynode.advertise<std_msgs::PR2Arm>("cmd_rightarmconfig");
- mynode.advertise<pr2_msgs::EndEffectorState>("right_pr2arm_set_end_effector");
- mynode.advertise<std_msgs::Float64>("interpolate_step_size");
- mynode.advertise<std_msgs::Float64>("interpolate_wait_time");
-
std_msgs::Float64 float64_msg;
sleep(1);
double interpolate_step_size = 0.05;
float64_msg.data = interpolate_step_size;
- mynode.publish("interpolate_step_size", float64_msg);
+ ohGrasper.publish("interpolate_step_size", float64_msg);
double interpolate_wait_time = 1.0;
float64_msg.data = interpolate_wait_time;
- mynode.publish("interpolate_wait_time", float64_msg);
+ ohGrasper.publish("interpolate_wait_time", float64_msg);
//---- now for the keyboard driven state machine ------
while(1)
{
signal(SIGINT,quit);
- keyboardLoop(mynode);
+ keyboardLoop(ohGrasper);
}
return 0;
@@ -110,7 +129,7 @@
}
-void keyboardLoop(ros::node &n)
+void keyboardLoop(OverheadGrasper &n)
{
char c;
@@ -139,14 +158,17 @@
switch(c)
{
case '1':
- overheadGraspPosture(n);
+ n.overheadGraspPosture();
break;
case '2':
{
- Vector v(.835,-.45,0.70);
- positionEyecamOverObject(n, v);
+ Vector v(n.objectPosMsg.x,n.objectPosMsg.y,n.objectPosMsg.z);
+ n.positionEyecamOverObject(v);
}
break;
+ case '3':
+ printf("x: %.4f y: %.4f z: %.4f\n", n.objectPosMsg.x, n.objectPosMsg.y, n.objectPosMsg.z);
+ break;
default:
break;
}
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model 2008-07-30 02:27:50 UTC (rev 2301)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model 2008-07-30 05:46:12 UTC (rev 2302)
@@ -359,7 +359,7 @@
</body:box>
<body:empty name="forearm_cam_right_body">
- <xyz> 0.90225 -0.15 0.8369 </xyz>
+ <xyz> 0.92225 -0.15 0.8369 </xyz>
<rpy> 0.00 -45.0 0.00 </rpy>
<sensor:camera name="forearm_cam_right_sensor">
<imageSize>640 480</imageSize>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_tablegrasp.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_tablegrasp.world 2008-07-30 02:27:50 UTC (rev 2301)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_tablegrasp.world 2008-07-30 05:46:12 UTC (rev 2302)
@@ -47,6 +47,7 @@
<material>Gazebo/CloudySky</material>
</sky>
<gazeboPath>media</gazeboPath>
+ <shadowTechnique>none</shadowTechnique>
<grid>off</grid>
<maxUpdateRate>100</maxUpdateRate>
</rendering:ogre>
@@ -147,40 +148,56 @@
</body:box>
</model:physical>
- <!-- The small cylinder "cup" -->
- <model:physical name="cylinder1_model">
- <xyz> 2.5 0.0 0.9</xyz>
+ <!-- The small box "cup" -->
+ <model:physical name="object1_model">
+ <xyz> 0.835 -0.55 0.95</xyz>
+ <rpy> 0.0 0.0 30.0</rpy>
+ <body:box name="object1_body">
+ <geom:box name="object1_geom">
+ <kp>100000000.0</kp>
+ <kd>0.1</kd>
+ <mesh>default</mesh>
+ <size>0.1 0.03 0.03</size>
+ <mass> 0.05</mass>
+ <visual>
+ <size> 0.1 0.030 0.03</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+
+ <controller:P3D name="p3d_object_controller" plugin="libP3D.so">
+ <updateRate>100.0</updateRate>
+ <bodyName>object1_body</bodyName>
+ <interface:position name="p3d_object_position"/>
+ </controller:P3D>
+
+ </model:physical>
+
+
+
+ <!--
+ <model:physical name="object1_model">
+ <xyz> 1.035 -0.15 0.55</xyz>
<rpy> 0.0 0.0 0.0</rpy>
- <body:cylinder name="cylinder1_body">
- <geom:cylinder name="cylinder1_geom">
+ <body:box name="object1_body">
+ <geom:box name="object1_geom">
<kp>100000000.0</kp>
<kd>0.1</kd>
<mesh>default</mesh>
- <size>0.025 0.075</size>
+ <size>0.01 0.01 0.01</size>
<mass> 0.05</mass>
<visual>
- <size> 0.05 0.05 0.075</size>
+ <size> 0.01 0.010 0.01</size>
<material>Gazebo/PioneerBody</material>
- <mesh>unit_cylinder</mesh>
- </visual>
- </geom:cylinder>
- <geom:box name="cylinder1_base_geom">
- <kp>100000000.0</kp>
- <kd>0.1</kd>
- <mesh>default</mesh>
- <xyz>0.0 0.0 -0.033</xyz>
- <size>0.05 0.05 0.01</size>
- <mass> 0.01</mass>
- <visual>
- <size> 0.05 0.05 0.01</size>
- <material>Gazebo/Fish</material>
<mesh>unit_box</mesh>
</visual>
</geom:box>
- </body:cylinder>
+ </body:box>
</model:physical>
+ -->
-
<!-- The small ball -->
<model:physical name="sphere1_model">
<xyz> 2.5 -2.8 1.0</xyz>
@@ -293,12 +310,12 @@
<model:renderable name="directional_white">
<light>
<type>directional</type>
- <direction>0 -0.5 -0.5</direction>
+ <direction>0 0.0 -1.0</direction>
<diffuseColor>0.4 0.4 0.4</diffuseColor>
<specularColor>0.0 0.0 0.0</specularColor>
<attenuation>1 0.0 1.0 0.4</attenuation>
</light>
</model:renderable>
-
</gazebo:world>
+
Modified: pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h
===================================================================
--- pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h 2008-07-30 02:27:50 UTC (rev 2301)
+++ pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h 2008-07-30 05:46:12 UTC (rev 2302)
@@ -79,6 +79,8 @@
std_msgs::RobotBase2DOdom odomMsg;
rostools::Time timeMsg;
+ std_msgs::Point3DFloat32 objectPosMsg;
+
// A mutex to lock access to fields that are used in message callbacks
ros::thread::mutex lock;
Modified: pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
===================================================================
--- pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp 2008-07-30 02:27:50 UTC (rev 2301)
+++ pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp 2008-07-30 05:46:12 UTC (rev 2302)
@@ -47,7 +47,9 @@
// this->PR2Copy->SetArmJointPosition(PR2::PR2_LEFT_ARM, jointPosition, jointSpeed);
*/
+ printf("boo!\n");
if(!useControllerArray){
+ printf("hoo!\n");
this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_R_PAN , this->rightarm.turretAngle, 0);
this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_R_SHOULDER_PITCH, this->rightarm.shoulderLiftAngle, 0);
@@ -345,6 +347,7 @@
advertise<std_msgs::PR2Arm>("right_pr2arm_pos");
advertise<rostools::Time>("time");
advertise<std_msgs::Empty>("transform");
+ advertise<std_msgs::Point3DFloat32>("object_position");
subscribe("cmd_vel", velMsg, &RosGazeboNode::cmdvelReceived);
subscribe("cmd_leftarmconfig", leftarm, &RosGazeboNode::cmd_leftarmconfigReceived);
@@ -625,6 +628,17 @@
// those values are reused in the sendInverseEuler() call above.
publish("odom",this->odomMsg);
+ /***************************************************************/
+ /* */
+ /* 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);
+
/***************************************************************/
/* */
/* camera */
Modified: pkg/trunk/util/kinematics/libKDL/src/kdl_kinematics.cpp
===================================================================
--- pkg/trunk/util/kinematics/libKDL/src/kdl_kinematics.cpp 2008-07-30 02:27:50 UTC (rev 2301)
+++ pkg/trunk/util/kinematics/libKDL/src/kdl_kinematics.cpp 2008-07-30 05:46:12 UTC (rev 2302)
@@ -100,9 +100,6 @@
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<<"IK guess:"<<*this->q_IK_guess<<endl;
- cout<<"IK result:"<<*this->q_IK_result<<endl;
-
return true;
}
else
Modified: pkg/trunk/util/kinematics/libKDL/test/pr2_kin_test.cpp
===================================================================
--- pkg/trunk/util/kinematics/libKDL/test/pr2_kin_test.cpp 2008-07-30 02:27:50 UTC (rev 2301)
+++ pkg/trunk/util/kinematics/libKDL/test/pr2_kin_test.cpp 2008-07-30 05:46:12 UTC (rev 2302)
@@ -17,11 +17,18 @@
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;
- Frame f;
if (pr2_kin.FK(pr2_config,f))
cout<<"End effector transformation:"<<f<<endl;
else
Modified: pkg/trunk/vision/cam_viewer/cam_viewer.cpp
===================================================================
--- pkg/trunk/vision/cam_viewer/cam_viewer.cpp 2008-07-30 02:27:50 UTC (rev 2301)
+++ pkg/trunk/vision/cam_viewer/cam_viewer.cpp 2008-07-30 05:46:12 UTC (rev 2302)
@@ -12,18 +12,29 @@
{
public:
std_msgs::Image image_msg;
+ char key;
+ int imgnum;
CvView() : node("cam_viewer")
{
cvNamedWindow("cam_viewer", CV_WINDOW_AUTOSIZE);
subscribe("image", image_msg, &CvView::image_cb);
+ imgnum=0;
}
void image_cb()
{
IplImage *cv_image = cvCreateImage( cvSize( image_msg.width, image_msg.height), IPL_DEPTH_8U, 3);
memcpy(cv_image->imageData, image_msg.data, cv_image->imageSize);
cvShowImage("cam_viewer", cv_image);
- cvWaitKey(10);
+ key = cvWaitKey(10);
+ if (key==' ')
+ {
+ char imgname[30];
+ sprintf(imgname, "img_%d.png", imgnum);
+ cvSaveImage(imgname, cv_image);
+ imgnum++;
+ }
+
cvReleaseImage(&cv_image);
}
};
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
