Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[1741] pkg/trunk
|
From: <hsu...@us...> - 2008-07-18 01:09:14
|
Revision: 1741
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1741&view=rev
Author: hsujohnhsu
Date: 2008-07-18 01:09:22 +0000 (Fri, 18 Jul 2008)
Log Message:
-----------
fixes to rosgazebonode so teleop_arm_keyboard runs.
updates to new controller loop.
Modified Paths:
--------------
pkg/trunk/demos/2dnav-gazebo/world/robot.world
pkg/trunk/robot/pr2_gazebo/src/test2.cpp
pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h
pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
Added Paths:
-----------
pkg/trunk/clean_rosgazebo.scp
Added: pkg/trunk/clean_rosgazebo.scp
===================================================================
--- pkg/trunk/clean_rosgazebo.scp (rev 0)
+++ pkg/trunk/clean_rosgazebo.scp 2008-07-18 01:09:22 UTC (rev 1741)
@@ -0,0 +1,35 @@
+#!/bin/bash
+
+#clean controllers
+(cd controllers/genericControllers ; rm -f lib/* ;make clean)
+(cd controllers/pr2Controllers ; rm -f lib/* ;make clean)
+
+#clean gazebo dependent stuff
+(cd drivers/robot/pr2/libpr2HW ; rm -f lib/* ;make clean)
+(cd drivers/robot/pr2/libpr2API ; rm -f lib/* ;make clean)
+
+(cd drivers/simulator/gazebo_plugin ; rm -f lib/* ;make clean)
+(cd drivers/simulator/gazebo_hardware ; rm -f lib/* ;make clean)
+(cd drivers/simulator/gazebo_sensors ; rm -f lib/* ;make clean)
+
+(cd simulators/rosgazebo ; rm -f lib/* ;make clean)
+(cd robot/pr2_gazebo ; rm -f lib/* ;make clean)
+
+#clean 2dnav stack stuff
+(cd nav/nav_view ; rm -f lib/* ;make clean)
+(cd nav/amcl_player ; rm -f lib/* ;make clean)
+(cd nav/wavefront_player ; rm -f lib/* ;make clean)
+(cd nav/map_server ; rm -f lib/* ;make clean)
+
+#clean keyboard controllers
+(cd nav/teleop_base_keyboard ; rm -f lib/* ;make clean)
+(cd manip/teleop_arm_keyboard ; rm -f lib/* ;make clean)
+
+# clean kinematics
+(cd util/kinematics/libKinematics ; rm -f lib/* ;make clean)
+(cd util/kinematics/libKDL ; rm -f lib/* ;make clean)
+
+# clean visualization
+(cd visualization/irrlicht_viewer ; rm -f lib/* ;make clean)
+(cd visualization/cloud_viewer ; rm -f lib/* ;make clean)
+
Property changes on: pkg/trunk/clean_rosgazebo.scp
___________________________________________________________________
Added: svn:executable
+ *
Modified: pkg/trunk/demos/2dnav-gazebo/world/robot.world
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/world/robot.world 2008-07-18 00:34:56 UTC (rev 1740)
+++ pkg/trunk/demos/2dnav-gazebo/world/robot.world 2008-07-18 01:09:22 UTC (rev 1741)
@@ -96,8 +96,8 @@
<hfov>90</hfov>
<nearClip>0.1</nearClip>
<farClip>20</farClip>
- <saveFrames>false</saveFrames>
- <saveFramePath>frames</saveFramePath>
+ <saveFrames>true</saveFrames>
+ <saveFramePath>frames_0</saveFramePath>
<updateRate>10.0</updateRate>
<controller:generic_camera name="cam1_controller">
<updateRate>10.0</updateRate>
Modified: pkg/trunk/robot/pr2_gazebo/src/test2.cpp
===================================================================
--- pkg/trunk/robot/pr2_gazebo/src/test2.cpp 2008-07-18 00:34:56 UTC (rev 1740)
+++ pkg/trunk/robot/pr2_gazebo/src/test2.cpp 2008-07-18 01:09:22 UTC (rev 1741)
@@ -47,11 +47,8 @@
#include <time.h>
#include <signal.h>
-//#include <RosGazeboNode/RosGazeboNode.h>
+pthread_mutex_t simMutex; //Mutex for sim R/W
- pthread_mutex_t simMutex; //Mutex for sim R/W
-mechanism::Joint* JointArray[PR2::MAX_JOINTS]; //Joint pointer array
-
void finalize(int)
{
fprintf(stderr,"Caught sig, clean-up and exit\n");
@@ -86,10 +83,6 @@
pthread_mutex_init(&simMutex, NULL);
- for (int i = 0;i<PR2::MAX_JOINTS;i++){
- JointArray[i] = new mechanism::Joint();
- }
-
ros::init(argc,argv);
/***************************************************************************************/
@@ -98,15 +91,6 @@
/* */
/***************************************************************************************/
printf("Creating robot\n");
- //PR2::PR2Robot* myPR2;
- // Initialize robot object
- //myPR2 = new PR2::PR2Robot();
- // Initialize connections
- //myPR2->InitializeRobot();
- // Set control mode for the base
- //myPR2->SetBaseControlMode(PR2::PR2_CARTESIAN_CONTROL);
- //myPR2->EnableGripperLeft();
- //myPR2->EnableGripperRight();
/***************************************************************************************/
/* */
@@ -128,32 +112,16 @@
}; // 0 for head, 1 for caster+arm
//int numJointsLookUp[] ={7, 28}; // joints per actuator array
int portLookUp[] = { // the way joints are listed in pr2.model
- PR2::CASTER_FL_STEER ,
- PR2::CASTER_FL_DRIVE_L ,
- PR2::CASTER_FL_DRIVE_R ,
- PR2::CASTER_FR_STEER ,
- PR2::CASTER_FR_DRIVE_L ,
- PR2::CASTER_FR_DRIVE_R ,
- PR2::CASTER_RL_STEER ,
- PR2::CASTER_RL_DRIVE_L ,
- PR2::CASTER_RL_DRIVE_R ,
- PR2::CASTER_RR_STEER ,
- PR2::CASTER_RR_DRIVE_L ,
- PR2::CASTER_RR_DRIVE_R ,
+ PR2::CASTER_FL_STEER , PR2::CASTER_FL_DRIVE_L , PR2::CASTER_FL_DRIVE_R ,
+ PR2::CASTER_FR_STEER , PR2::CASTER_FR_DRIVE_L , PR2::CASTER_FR_DRIVE_R ,
+ PR2::CASTER_RL_STEER , PR2::CASTER_RL_DRIVE_L , PR2::CASTER_RL_DRIVE_R ,
+ PR2::CASTER_RR_STEER , PR2::CASTER_RR_DRIVE_L , PR2::CASTER_RR_DRIVE_R ,
PR2::SPINE_ELEVATOR ,
- PR2::ARM_L_PAN ,
- PR2::ARM_L_SHOULDER_PITCH,
- PR2::ARM_L_SHOULDER_ROLL ,
- PR2::ARM_L_ELBOW_PITCH ,
- PR2::ARM_L_ELBOW_ROLL ,
- PR2::ARM_L_WRIST_PITCH ,
+ PR2::ARM_L_PAN , PR2::ARM_L_SHOULDER_PITCH, PR2::ARM_L_SHOULDER_ROLL ,
+ PR2::ARM_L_ELBOW_PITCH , PR2::ARM_L_ELBOW_ROLL , PR2::ARM_L_WRIST_PITCH ,
PR2::ARM_L_WRIST_ROLL ,
- PR2::ARM_R_PAN ,
- PR2::ARM_R_SHOULDER_PITCH,
- PR2::ARM_R_SHOULDER_ROLL ,
- PR2::ARM_R_ELBOW_PITCH ,
- PR2::ARM_R_ELBOW_ROLL ,
- PR2::ARM_R_WRIST_PITCH ,
+ PR2::ARM_R_PAN , PR2::ARM_R_SHOULDER_PITCH, PR2::ARM_R_SHOULDER_ROLL ,
+ PR2::ARM_R_ELBOW_PITCH , PR2::ARM_R_ELBOW_ROLL , PR2::ARM_R_WRIST_PITCH ,
PR2::ARM_R_WRIST_ROLL ,
PR2::ARM_L_GRIPPER - PR2::ARM_L_GRIPPER ,
PR2::ARM_R_GRIPPER - PR2::ARM_R_GRIPPER ,
@@ -167,32 +135,16 @@
int jointId[] = { // numbering system
- PR2::CASTER_FL_STEER ,
- PR2::CASTER_FL_DRIVE_L ,
- PR2::CASTER_FL_DRIVE_R ,
- PR2::CASTER_FR_STEER ,
- PR2::CASTER_FR_DRIVE_L ,
- PR2::CASTER_FR_DRIVE_R ,
- PR2::CASTER_RL_STEER ,
- PR2::CASTER_RL_DRIVE_L ,
- PR2::CASTER_RL_DRIVE_R ,
- PR2::CASTER_RR_STEER ,
- PR2::CASTER_RR_DRIVE_L ,
- PR2::CASTER_RR_DRIVE_R ,
+ PR2::CASTER_FL_STEER , PR2::CASTER_FL_DRIVE_L , PR2::CASTER_FL_DRIVE_R ,
+ PR2::CASTER_FR_STEER , PR2::CASTER_FR_DRIVE_L , PR2::CASTER_FR_DRIVE_R ,
+ PR2::CASTER_RL_STEER , PR2::CASTER_RL_DRIVE_L , PR2::CASTER_RL_DRIVE_R ,
+ PR2::CASTER_RR_STEER , PR2::CASTER_RR_DRIVE_L , PR2::CASTER_RR_DRIVE_R ,
PR2::SPINE_ELEVATOR ,
- PR2::ARM_L_PAN ,
- PR2::ARM_L_SHOULDER_PITCH,
- PR2::ARM_L_SHOULDER_ROLL ,
- PR2::ARM_L_ELBOW_PITCH ,
- PR2::ARM_L_ELBOW_ROLL ,
- PR2::ARM_L_WRIST_PITCH ,
+ PR2::ARM_L_PAN , PR2::ARM_L_SHOULDER_PITCH, PR2::ARM_L_SHOULDER_ROLL ,
+ PR2::ARM_L_ELBOW_PITCH , PR2::ARM_L_ELBOW_ROLL , PR2::ARM_L_WRIST_PITCH ,
PR2::ARM_L_WRIST_ROLL ,
- PR2::ARM_R_PAN ,
- PR2::ARM_R_SHOULDER_PITCH,
- PR2::ARM_R_SHOULDER_ROLL ,
- PR2::ARM_R_ELBOW_PITCH ,
- PR2::ARM_R_ELBOW_ROLL ,
- PR2::ARM_R_WRIST_PITCH ,
+ PR2::ARM_R_PAN , PR2::ARM_R_SHOULDER_PITCH, PR2::ARM_R_SHOULDER_ROLL ,
+ PR2::ARM_R_ELBOW_PITCH , PR2::ARM_R_ELBOW_ROLL , PR2::ARM_R_WRIST_PITCH ,
PR2::ARM_R_WRIST_ROLL ,
PR2::ARM_L_GRIPPER ,
PR2::ARM_R_GRIPPER ,
@@ -204,88 +156,48 @@
PR2::HEAD_PTZ_R_PAN ,
PR2::HEAD_PTZ_R_TILT };
- string etherIP[] = {"10.12.0.103",
- "10.12.0.103",
- "10.12.0.103",
- "10.12.0.103",
- "10.12.0.103",
- "10.12.0.103",
- "10.12.0.103",
- "10.12.0.103",
- "10.12.0.103",
- "10.12.0.103",
- "10.12.0.103",
- "10.12.0.103",
- "10.12.0.103",
- "10.12.0.103",
- "10.12.0.103",
- "10.12.0.103",
- "10.12.0.103",
- "10.12.0.103",
- "10.12.0.103",
- "10.12.0.103",
- "10.12.0.103",
- "10.12.0.103",
- "10.12.0.103",
- "10.12.0.103",
- "10.12.0.103",
- "10.12.0.103",
- "10.12.0.103",
- "10.12.0.103",
- "10.12.0.103",
- "10.12.0.103",
- "10.12.0.103",
- "10.12.0.103",
- "10.12.0.103",
- "10.12.0.103",
- "10.12.0.103",
- "10.12.0.103"
+ string etherIP[] = {"10.12.0.103", "10.12.0.103", "10.12.0.103", "10.12.0.103", "10.12.0.103", "10.12.0.103",
+ "10.12.0.103", "10.12.0.103", "10.12.0.103", "10.12.0.103", "10.12.0.103", "10.12.0.103",
+ "10.12.0.103", "10.12.0.103", "10.12.0.103", "10.12.0.103", "10.12.0.103", "10.12.0.103",
+ "10.12.0.103", "10.12.0.103", "10.12.0.103", "10.12.0.103", "10.12.0.103", "10.12.0.103",
+ "10.12.0.103", "10.12.0.103", "10.12.0.103", "10.12.0.103", "10.12.0.103", "10.12.0.103",
+ "10.12.0.103", "10.12.0.103", "10.12.0.103", "10.12.0.103", "10.12.0.103", "10.12.0.103"
};
- string hostIP[] = {"10.12.0.2 ",
- "10.12.0.2 ",
- "10.12.0.2 ",
- "10.12.0.2 ",
- "10.12.0.2 ",
- "10.12.0.2 ",
- "10.12.0.2 ",
- "10.12.0.2 ",
- "10.12.0.2 ",
- "10.12.0.2 ",
- "10.12.0.2 ",
- "10.12.0.2 ",
- "10.12.0.2 ",
- "10.12.0.2 ",
- "10.12.0.2 ",
- "10.12.0.2 ",
- "10.12.0.2 ",
- "10.12.0.2 ",
- "10.12.0.2 ",
- "10.12.0.2 ",
- "10.12.0.2 ",
- "10.12.0.2 ",
- "10.12.0.2 ",
- "10.12.0.2 ",
- "10.12.0.2 ",
- "10.12.0.2 ",
- "10.12.0.2 ",
- "10.12.0.2 ",
- "10.12.0.2 ",
- "10.12.0.2 ",
- "10.12.0.2 ",
- "10.12.0.2 ",
- "10.12.0.2 ",
- "10.12.0.2 ",
- "10.12.0.2 ",
- "10.12.0.2 "
+ string hostIP[] = {"10.12.0.2 ", "10.12.0.2 ", "10.12.0.2 ", "10.12.0.2 ", "10.12.0.2 ", "10.12.0.2 ",
+ "10.12.0.2 ", "10.12.0.2 ", "10.12.0.2 ", "10.12.0.2 ", "10.12.0.2 ", "10.12.0.2 ",
+ "10.12.0.2 ", "10.12.0.2 ", "10.12.0.2 ", "10.12.0.2 ", "10.12.0.2 ", "10.12.0.2 ",
+ "10.12.0.2 ", "10.12.0.2 ", "10.12.0.2 ", "10.12.0.2 ", "10.12.0.2 ", "10.12.0.2 ",
+ "10.12.0.2 ", "10.12.0.2 ", "10.12.0.2 ", "10.12.0.2 ", "10.12.0.2 ", "10.12.0.2 ",
+ "10.12.0.2 ", "10.12.0.2 ", "10.12.0.2 ", "10.12.0.2 ", "10.12.0.2 ", "10.12.0.2 "
};
/***************************************************************************************/
/* */
+ /* initialize robot model */
+ /* */
+ /* in Robot */
+ /* */
+ /***************************************************************************************/
+ mechanism::Joint* joint[PR2::MAX_JOINTS]; //Joint pointer array
+ for (int i = 0;i<PR2::MAX_JOINTS;i++){
+ joint[i] = new mechanism::Joint();
+ }
+
+ /***************************************************************************************/
+ /* */
/* initialize hardware */
/* */
+ /* User initialized */
+ /* */
+ /* single hardware interface object, containing array of actuators */
+ /* single gazebo hardware object */
+ /* */
/***************************************************************************************/
+ // a single HardwareInterface objects with numActuators actuators
HardwareInterface *hi = new HardwareInterface(numActuators);
+ // hi is passed to h during construction of h,
+ // mapping between actuators in h and actuators in hi defined by boardLookUp, portLookUp and jointId
GazeboHardware *h = new GazeboHardware(numBoards,numActuators, boardLookUp, portLookUp, jointId, etherIP, hostIP, hi);
- // connect to hardware
+ // connect to hardware (gazebo in this case)
h->init();
// access to all the gazebo sensors
@@ -295,7 +207,10 @@
/* */
/* initialize controllers */
/* */
+ /* TODO: move to mechanism_control */
+ /* */
/***************************************************************************************/
+ // stubs
CONTROLLER::ArmController myArm;
CONTROLLER::HeadController myHead;
CONTROLLER::SpineController mySpine;
@@ -303,25 +218,39 @@
CONTROLLER::LaserScannerController myLaserScanner;
CONTROLLER::GripperController myGripper;
- CONTROLLER::JointController *jc =new CONTROLLER::JointController(); //Test jointController
- jc->Init(1000,0,0,500,-500, CONTROLLER::CONTROLLER_POSITION,time,1000,-1000,1000,JointArray[PR2::ARM_L_SHOULDER_PITCH]);
- //void JointController::Init(double PGain, double IGain, double DGain, double IMax, double IMin, CONTROLLER_CONTROL_MODE mode, double time, double maxPositiveTorque, double maxNegativeTorque, double maxEffort, mechanism::Joint *joint) {
- // std::cout<<"****************"<<jc.SetTorqueCmd(-500.0)<<std::endl;
- // std::cout<<"****************"<<jc.SetPosCmd(0.0)<<std::endl;
+ // used here in this test
+ CONTROLLER::JointController* jc[PR2::MAX_JOINTS]; // One controller per joint
- /***************************************************************************************/
- /* */
- /* initialize robot model */
- /* */
- /***************************************************************************************/
- mechanism::Joint *joint = new mechanism::Joint();
+ // initialize each jointController jc[i], associate with a joint joint[i]
+ for (int i = 0;i<PR2::MAX_JOINTS;i++){
+ jc[i] = new CONTROLLER::JointController();
+ jc[i]->Init(1000,0,0,500,-500, CONTROLLER::CONTROLLER_POSITION,time,1000,-1000,1000,joint[i]);
+ }
+ //Explicitly initialize the controllers we wish to use. Don't forget to set the controllers to torque mode in the world file!
+ jc[PR2::ARM_L_PAN]->Init(100,0,0,500,-500, CONTROLLER::CONTROLLER_POSITION,time,100,-100,100,joint[PR2::ARM_L_PAN]);
+ jc[PR2::ARM_L_SHOULDER_PITCH]->Init(1000,0,0,500,-500, CONTROLLER::CONTROLLER_POSITION,time,1000,-1000,1000,joint[PR2::ARM_L_SHOULDER_PITCH]);
+ jc[PR2::ARM_L_SHOULDER_ROLL]->Init(100,0,0,500,-500, CONTROLLER::CONTROLLER_POSITION,time,100,-100,100,joint[PR2::ARM_L_SHOULDER_ROLL]);
+ jc[PR2::ARM_L_ELBOW_PITCH]->Init(300,0,0,500,-500, CONTROLLER::CONTROLLER_POSITION,time,100,-100,100,joint[PR2::ARM_L_ELBOW_PITCH]);
+ jc[PR2::ARM_L_ELBOW_ROLL]->Init(100,0,0,500,-500, CONTROLLER::CONTROLLER_POSITION,time,100,-100,100,joint[PR2::ARM_L_ELBOW_ROLL]);
+ jc[PR2::ARM_L_WRIST_PITCH]->Init(100,0,0,500,-500, CONTROLLER::CONTROLLER_POSITION,time,100,-100,100,joint[PR2::ARM_L_WRIST_PITCH]);
+ jc[PR2::ARM_L_WRIST_ROLL]->Init(100,0,0,500,-500, CONTROLLER::CONTROLLER_POSITION,time,100,-100,100,joint[PR2::ARM_L_WRIST_ROLL]);
+ //JointController::Init(double PGain, double IGain, double DGain, double IMax,
+ // double IMin, CONTROLLER_CONTROL_MODE mode, double time,
+ // double maxPositiveTorque, double maxNegativeTorque,
+ // double maxEffort, mechanism::Joint *joint)
+
/***************************************************************************************/
/* */
/* initialize transmission */
/* */
+ /* TODO: in Robot */
+ /* */
/***************************************************************************************/
- SimpleTransmission *st = new SimpleTransmission(joint,&hi->actuator[0],1,1,14000);
+ SimpleTransmission* st[PR2::MAX_JOINTS];
+ for (int i = 0;i<PR2::MAX_JOINTS;i++){
+ st[i] = new SimpleTransmission(joint[i],&hi->actuator[0],1,1,1);
+ }
//myPR2->hw.GetSimTime(&time);
@@ -401,17 +330,18 @@
// get encoder counts from hardware
h->updateState();
- // setup joint state from encoder counts
- st->propagatePosition();
- //update controller
- jc->Update();
+ for (int i = 0;i<PR2::MAX_JOINTS;i++){
+ // setup joint state from encoder counts
+ st[i]->propagatePosition();
+ //update controller
+ jc[i]->Update();
+ // update command current from joint command
+ st[i]->propagateEffort();
+ }
//cout << "pos:: " << joint->position << ", eff:: " << joint->commandedEffort << endl;
- // update command current from joint command
- st->propagateEffort();
-
// send command to hardware
h->sendCommand();
Modified: pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h
===================================================================
--- pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h 2008-07-18 00:34:56 UTC (rev 1740)
+++ pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h 2008-07-18 01:09:22 UTC (rev 1741)
@@ -107,6 +107,10 @@
// that stage should load.
RosGazeboNode(int argc, char** argv, const char* fname,
PR2::PR2Robot *myPR2,
+ CONTROLLER::JointController** ControllerArray
+ );
+ RosGazeboNode(int argc, char** argv, const char* fname,
+ PR2::PR2Robot *myPR2,
CONTROLLER::ArmController *myArm,
CONTROLLER::HeadController *myHead,
CONTROLLER::SpineController *mySpine,
@@ -114,9 +118,6 @@
CONTROLLER::LaserScannerController *myLaserScanner,
CONTROLLER::GripperController *myGripper
);
- ~RosGazeboNode();
- // Constructor; stage itself needs argc/argv. fname is the .world file
- // that stage should load.
RosGazeboNode(int argc, char** argv, const char* fname,
PR2::PR2Robot *myPR2,
CONTROLLER::ArmController *myArm,
@@ -128,6 +129,7 @@
CONTROLLER::JointController** ControllerArray
);
+ ~RosGazeboNode();
// advertise / subscribe models
int AdvertiseSubscribeMessages();
Modified: pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
===================================================================
--- pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp 2008-07-18 00:34:56 UTC (rev 1740)
+++ pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp 2008-07-18 01:09:22 UTC (rev 1741)
@@ -47,7 +47,6 @@
// this->PR2Copy->SetArmJointPosition(PR2::PR2_LEFT_ARM, jointPosition, jointSpeed);
*/
//*
- /*
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);
this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_R_SHOULDER_ROLL , this->rightarm.upperarmRollAngle, 0);
@@ -55,9 +54,8 @@
this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_R_ELBOW_ROLL , this->rightarm.forearmRollAngle, 0);
this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_R_WRIST_PITCH , this->rightarm.wristPitchAngle, 0);
this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_R_WRIST_ROLL , this->rightarm.wristRollAngle, 0);
- this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_R_GRIPPER , this->rightarm.gripperGapCmd, 0);
+ //this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_R_GRIPPER , this->rightarm.gripperGapCmd, 0);
this->PR2Copy->hw.CloseGripper(PR2::PR2_RIGHT_GRIPPER, this->rightarm.gripperGapCmd, this->rightarm.gripperForceCmd);
-*/
//*/
this->lock.unlock();
}
@@ -83,7 +81,6 @@
*/
//*
- /*
this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_L_PAN , this->leftarm.turretAngle, 0);
this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_L_SHOULDER_PITCH, this->leftarm.shoulderLiftAngle, 0);
this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_L_SHOULDER_ROLL , this->leftarm.upperarmRollAngle, 0);
@@ -93,7 +90,6 @@
this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_L_WRIST_ROLL , this->leftarm.wristRollAngle, 0);
// this->PR2Copy->SetJointServoCmd(PR2::ARM_L_GRIPPER , this->leftarm.gripperGapCmd, 0);
this->PR2Copy->hw.CloseGripper(PR2::PR2_LEFT_GRIPPER, this->leftarm.gripperGapCmd, this->leftarm.gripperForceCmd);
- */
//*/
this->lock.unlock();
}
@@ -156,6 +152,39 @@
RosGazeboNode::RosGazeboNode(int argc, char** argv, const char* fname,
PR2::PR2Robot *myPR2,
+ CONTROLLER::JointController** ControllerArray):
+ ros::node("rosgazebo"),tf(*this)
+{
+ // accept passed in robot
+ this->PR2Copy = myPR2;
+
+ //Store copy of Controller Array. Only interact with it during Update() calls.
+ this->ControllerArray = ControllerArray;
+
+ // 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>();
+
+ // FIXME: move this to Subscribe Models
+ param("tilting_laser/max_cloud_pts",max_cloud_pts, 10000);
+ this->cloud_pts->allocate(this->max_cloud_pts);
+ this->cloud_ch1->allocate(this->max_cloud_pts);
+
+ // initialize times
+ this->PR2Copy->GetTime(&(this->lastTime));
+ this->PR2Copy->GetTime(&(this->simTime));
+
+ //No new messages
+ newRightArmPos = false;
+ newLeftArmPos = false;
+
+}
+
+RosGazeboNode::RosGazeboNode(int argc, char** argv, const char* fname,
+ PR2::PR2Robot *myPR2,
CONTROLLER::ArmController *myArm,
CONTROLLER::HeadController *myHead,
CONTROLLER::SpineController *mySpine,
@@ -167,6 +196,9 @@
// accept passed in robot
this->PR2Copy = myPR2;
+ //did not receive an controller array.
+ this->ControllerArray = NULL;
+
// initialize random seed
srand(time(NULL));
@@ -314,8 +346,11 @@
/* Arm Updates */
/* */
/***************************************************************/
- if(newLeftArmPos)UpdateLeftArm();
- if(newRightArmPos)UpdateRightArm();
+ if (ControllerArray)
+ {
+ if(newLeftArmPos)UpdateLeftArm();
+ if(newRightArmPos)UpdateRightArm();
+ }
/***************************************************************/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
