Menu
▾
▴
personalrobots-commit
|
From: <hsu...@us...> - 2008-07-17 19:36:05
|
Revision: 1709
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1709&view=rev
Author: hsujohnhsu
Date: 2008-07-17 19:35:23 +0000 (Thu, 17 Jul 2008)
Log Message:
-----------
moved rosgazebo2.cc to pr2_gazebo test2.cpp
Modified Paths:
--------------
pkg/trunk/robot/pr2_gazebo/CMakeLists.txt
pkg/trunk/simulators/rosgazebo/CMakeLists.txt
Added Paths:
-----------
pkg/trunk/robot/pr2_gazebo/src/test2.cpp
Removed Paths:
-------------
pkg/trunk/simulators/rosgazebo/src/rosgazebo2.cc
Modified: pkg/trunk/robot/pr2_gazebo/CMakeLists.txt
===================================================================
--- pkg/trunk/robot/pr2_gazebo/CMakeLists.txt 2008-07-17 19:30:20 UTC (rev 1708)
+++ pkg/trunk/robot/pr2_gazebo/CMakeLists.txt 2008-07-17 19:35:23 UTC (rev 1709)
@@ -2,3 +2,4 @@
include(rosbuild)
rospack(pr2_gazebo)
rospack_add_executable(pr2_gazebo src/pr2_gazebo.cpp)
+rospack_add_executable(test2 src/test2.cpp)
Copied: pkg/trunk/robot/pr2_gazebo/src/test2.cpp (from rev 1707, pkg/trunk/simulators/rosgazebo/src/rosgazebo2.cc)
===================================================================
--- pkg/trunk/robot/pr2_gazebo/src/test2.cpp (rev 0)
+++ pkg/trunk/robot/pr2_gazebo/src/test2.cpp 2008-07-17 19:35:23 UTC (rev 1709)
@@ -0,0 +1,439 @@
+/*
+ * rosgazebo
+ * Copyright (c) 2008, Willow Garage, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+
+#include <pthread.h>
+
+// robot model
+#include <pr2Core/pr2Core.h>
+#include <robot_model/joint.h>
+
+// gazebo hardware, sensors
+#include <gazebo_hardware/gazebo_hardware.h>
+#include <gazebo_sensors/gazebo_sensors.h>
+
+
+// controller objects
+#include <pr2Controllers/ArmController.h>
+#include <pr2Controllers/HeadController.h>
+#include <pr2Controllers/SpineController.h>
+#include <pr2Controllers/BaseController.h>
+#include <pr2Controllers/LaserScannerController.h>
+#include <pr2Controllers/GripperController.h>
+
+
+// roscpp
+#include <ros/node.h>
+
+#include <time.h>
+#include <signal.h>
+
+//#include <RosGazeboNode/RosGazeboNode.h>
+
+ 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");
+ sleep(1);
+ exit(-1);
+}
+
+
+void *nonRealtimeLoop(void *rgn)
+{
+ std::cout << "Started nonRT loop" << std::endl;
+ while (1)
+ {
+ pthread_mutex_lock(&simMutex); //Lock for r/w
+ //((RosGazeboNode*)rgn)->Update();
+ pthread_mutex_unlock(&simMutex); //Unlock when done
+ // some time out for publishing ros info
+ usleep(10000);
+ }
+
+}
+
+int
+main(int argc, char** argv)
+{
+
+ double time;
+ double torque;
+
+ // we need 2 threads, one for RT and one for nonRT
+ pthread_t threads[2];
+
+ pthread_mutex_init(&simMutex, NULL);
+
+ for (int i = 0;i<PR2::MAX_JOINTS;i++){
+ JointArray[i] = new mechanism::Joint();
+ }
+
+ ros::init(argc,argv);
+
+ /***************************************************************************************/
+ /* */
+ /* The main simulator object */
+ /* */
+ /***************************************************************************************/
+ 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();
+
+ /***************************************************************************************/
+ /* */
+ /* hardware info, eventually retrieve from param server (from xml files) */
+ /* */
+ /***************************************************************************************/
+ int iB, iP;
+
+ int numBoards = 4; // 0 for head and 1 for arms, see pr2.model:Pr2_Actarray for now
+ int numActuators = PR2::MAX_JOINTS-2; // total number of actuators for both boards, last 2 in pr2Core (BASE_6DOF and PR2_WORLD don't count)
+
+ int boardLookUp[] ={1,1,1,1,1,1,1,1,1,1,1,1, // casters
+ 1, // spine elevator
+ 1,1,1,1,1,1,1, // left arm
+ 1,1,1,1,1,1,1, // right arm
+ 2, // left gripper
+ 3, // right gripper
+ 0,0,0,0,0,0,0 // head y/p, laser p, left ptz p/t, right ptz p/t
+ }; // 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::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_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_WRIST_ROLL ,
+ PR2::ARM_L_GRIPPER - PR2::ARM_L_GRIPPER ,
+ PR2::ARM_R_GRIPPER - PR2::ARM_R_GRIPPER ,
+ PR2::HEAD_YAW - PR2::HEAD_YAW ,
+ PR2::HEAD_PITCH - PR2::HEAD_YAW ,
+ PR2::HEAD_LASER_PITCH - PR2::HEAD_YAW ,
+ PR2::HEAD_PTZ_L_PAN - PR2::HEAD_YAW ,
+ PR2::HEAD_PTZ_L_TILT - PR2::HEAD_YAW ,
+ PR2::HEAD_PTZ_R_PAN - PR2::HEAD_YAW ,
+ PR2::HEAD_PTZ_R_TILT - PR2::HEAD_YAW };
+
+
+ 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::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_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_WRIST_ROLL ,
+ PR2::ARM_L_GRIPPER ,
+ PR2::ARM_R_GRIPPER ,
+ PR2::HEAD_YAW ,
+ PR2::HEAD_PITCH ,
+ PR2::HEAD_LASER_PITCH ,
+ PR2::HEAD_PTZ_L_PAN ,
+ PR2::HEAD_PTZ_L_TILT ,
+ 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 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 hardware */
+ /* */
+ /***************************************************************************************/
+ HardwareInterface *hi = new HardwareInterface(numActuators);
+ GazeboHardware *h = new GazeboHardware(numBoards,numActuators, boardLookUp, portLookUp, jointId, etherIP, hostIP, hi);
+ // connect to hardware
+ h->init();
+
+ // access to all the gazebo sensors
+ GazeboSensors *s = new GazeboSensors();
+
+ /***************************************************************************************/
+ /* */
+ /* initialize controllers */
+ /* */
+ /***************************************************************************************/
+ CONTROLLER::ArmController myArm;
+ CONTROLLER::HeadController myHead;
+ CONTROLLER::SpineController mySpine;
+ CONTROLLER::BaseController myBase;
+ 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;
+
+ /***************************************************************************************/
+ /* */
+ /* initialize robot model */
+ /* */
+ /***************************************************************************************/
+ mechanism::Joint *joint = new mechanism::Joint();
+
+ /***************************************************************************************/
+ /* */
+ /* initialize transmission */
+ /* */
+ /***************************************************************************************/
+ SimpleTransmission *st = new SimpleTransmission(joint,&hi->actuator[0],1,1,14000);
+
+ //myPR2->hw.GetSimTime(&time);
+
+ /***************************************************************************************/
+ /* */
+ /* initialize ROS Gazebo Nodes */
+ /* */
+ /***************************************************************************************/
+ printf("Creating node \n");
+ //RosGazeboNode rgn(argc,argv,argv[1],myPR2,&myArm,&myHead,&mySpine,&myBase,&myLaserScanner,&myGripper,JointArray);
+
+ /***************************************************************************************/
+ /* */
+ /* on termination... */
+ /* */
+ /***************************************************************************************/
+ signal(SIGINT, (&finalize));
+ signal(SIGQUIT, (&finalize));
+ signal(SIGTERM, (&finalize));
+
+ // see if we can subscribe models needed
+ //if (rgn.AdvertiseSubscribeMessages() != 0)
+ // exit(-1);
+
+ /***************************************************************************************/
+ /* */
+ /* Update ROS Gazebo Node */
+ /* contains controller pointers for the non-RT setpoints */
+ /* */
+ /***************************************************************************************/
+ //int rgnt = pthread_create(&threads[0],NULL, nonRealtimeLoop, (void *) (&rgn));
+ //if (rgnt)
+ //{
+ // printf("Could not start a separate thread for ROS Gazebo Node (code=%d)\n",rgnt);
+ // exit(-1);
+ //}
+
+ /***************************************************************************************/
+ /* */
+ /* RealTime loop using Gazebo ClientWait function call */
+ /* this is updated once every gazebo timestep (world time step size) */
+ /* */
+ /***************************************************************************************/
+ printf("Entering realtime loop\n");
+ double pos;
+ while(1)
+ {
+ //myPR2->hw.GetSimTime(&time);
+ //std::cout<<"Time:"<<time<<std::endl;
+
+ //pthread_mutex_trylock(&simMutex); //Try to lock here. But continue on if fails to enforce real time
+ // Update Controllers
+ // each controller will try to read new commands from shared memory with nonRT hooks,
+ // and skip update if locked by nonRT loop.
+
+ /*
+ myArm.Update();
+ myHead.Update();
+ mySpine.Update();
+ myBase.Update();
+ myLaserScanner.Update();
+ myGripper.Update();
+ */
+
+
+ // jc.GetTorqueCmd(&torque);
+ // std::cout<<"*"<<torque<<std::endl;
+ // std::cout<<JointArray[PR2::ARM_L_SHOULDER_PITCH]->position<<std::endl;
+ // TODO: Safety codes should go here...
+
+ // old: Send updated controller commands to hardware
+ // myPR2->hw.UpdateJointArray(JointArray);
+
+
+
+
+ // get encoder counts from hardware
+ h->updateState();
+
+ // setup joint state from encoder counts
+ st->propagatePosition();
+
+ //update controller
+ jc->Update();
+
+ //cout << "pos:: " << joint->position << ", eff:: " << joint->commandedEffort << endl;
+
+ // update command current from joint command
+ st->propagateEffort();
+
+ // send command to hardware
+ h->sendCommand();
+
+ // refresh hardware
+ h->tick();
+
+ //pthread_mutex_unlock(&simMutex); //Unlock after we're done with r/w
+
+ // wait for Gazebo time step
+ //myPR2->hw.ClientWait();
+ usleep(1000);
+
+ }
+
+ /***************************************************************************************/
+ /* */
+ /* have to call this explicitly for some reason. probably interference */
+ /* from signal handling in Stage / FLTK? */
+ /* */
+ /***************************************************************************************/
+ ros::msg_destruct();
+
+ exit(0);
+
+}
Modified: pkg/trunk/simulators/rosgazebo/CMakeLists.txt
===================================================================
--- pkg/trunk/simulators/rosgazebo/CMakeLists.txt 2008-07-17 19:30:20 UTC (rev 1708)
+++ pkg/trunk/simulators/rosgazebo/CMakeLists.txt 2008-07-17 19:35:23 UTC (rev 1709)
@@ -4,9 +4,6 @@
add_definitions(-Wall)
rospack_add_library(RosGazeboNode src/RosGazeboNode.cpp)
-rospack_add_executable(rosgazebo2 src/rosgazebo2.cc)
-target_link_libraries(rosgazebo2 RosGazeboNode)
-
rospack_add_executable(rosgazebo src/rosgazebo.cc)
target_link_libraries(rosgazebo RosGazeboNode)
Deleted: pkg/trunk/simulators/rosgazebo/src/rosgazebo2.cc
===================================================================
--- pkg/trunk/simulators/rosgazebo/src/rosgazebo2.cc 2008-07-17 19:30:20 UTC (rev 1708)
+++ pkg/trunk/simulators/rosgazebo/src/rosgazebo2.cc 2008-07-17 19:35:23 UTC (rev 1709)
@@ -1,439 +0,0 @@
-/*
- * rosgazebo
- * Copyright (c) 2008, Willow Garage, Inc.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- */
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <math.h>
-
-#include <pthread.h>
-
-// robot model
-#include <pr2Core/pr2Core.h>
-#include <robot_model/joint.h>
-
-// gazebo hardware, sensors
-#include <gazebo_hardware/gazebo_hardware.h>
-#include <gazebo_sensors/gazebo_sensors.h>
-
-
-// controller objects
-#include <pr2Controllers/ArmController.h>
-#include <pr2Controllers/HeadController.h>
-#include <pr2Controllers/SpineController.h>
-#include <pr2Controllers/BaseController.h>
-#include <pr2Controllers/LaserScannerController.h>
-#include <pr2Controllers/GripperController.h>
-
-
-// roscpp
-#include <ros/node.h>
-
-#include <time.h>
-#include <signal.h>
-
-//#include <RosGazeboNode/RosGazeboNode.h>
-
- 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");
- sleep(1);
- exit(-1);
-}
-
-
-void *nonRealtimeLoop(void *rgn)
-{
- std::cout << "Started nonRT loop" << std::endl;
- while (1)
- {
- pthread_mutex_lock(&simMutex); //Lock for r/w
- //((RosGazeboNode*)rgn)->Update();
- pthread_mutex_unlock(&simMutex); //Unlock when done
- // some time out for publishing ros info
- usleep(10000);
- }
-
-}
-
-int
-main(int argc, char** argv)
-{
-
- double time;
- double torque;
-
- // we need 2 threads, one for RT and one for nonRT
- pthread_t threads[2];
-
- pthread_mutex_init(&simMutex, NULL);
-
- for (int i = 0;i<PR2::MAX_JOINTS;i++){
- JointArray[i] = new mechanism::Joint();
- }
-
- ros::init(argc,argv);
-
- /***************************************************************************************/
- /* */
- /* The main simulator object */
- /* */
- /***************************************************************************************/
- 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();
-
- /***************************************************************************************/
- /* */
- /* hardware info, eventually retrieve from param server (from xml files) */
- /* */
- /***************************************************************************************/
- int iB, iP;
-
- int numBoards = 4; // 0 for head and 1 for arms, see pr2.model:Pr2_Actarray for now
- int numActuators = PR2::MAX_JOINTS-2; // total number of actuators for both boards, last 2 in pr2Core (BASE_6DOF and PR2_WORLD don't count)
-
- int boardLookUp[] ={1,1,1,1,1,1,1,1,1,1,1,1, // casters
- 1, // spine elevator
- 1,1,1,1,1,1,1, // left arm
- 1,1,1,1,1,1,1, // right arm
- 2, // left gripper
- 3, // right gripper
- 0,0,0,0,0,0,0 // head y/p, laser p, left ptz p/t, right ptz p/t
- }; // 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::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_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_WRIST_ROLL ,
- PR2::ARM_L_GRIPPER - PR2::ARM_L_GRIPPER ,
- PR2::ARM_R_GRIPPER - PR2::ARM_R_GRIPPER ,
- PR2::HEAD_YAW - PR2::HEAD_YAW ,
- PR2::HEAD_PITCH - PR2::HEAD_YAW ,
- PR2::HEAD_LASER_PITCH - PR2::HEAD_YAW ,
- PR2::HEAD_PTZ_L_PAN - PR2::HEAD_YAW ,
- PR2::HEAD_PTZ_L_TILT - PR2::HEAD_YAW ,
- PR2::HEAD_PTZ_R_PAN - PR2::HEAD_YAW ,
- PR2::HEAD_PTZ_R_TILT - PR2::HEAD_YAW };
-
-
- 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::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_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_WRIST_ROLL ,
- PR2::ARM_L_GRIPPER ,
- PR2::ARM_R_GRIPPER ,
- PR2::HEAD_YAW ,
- PR2::HEAD_PITCH ,
- PR2::HEAD_LASER_PITCH ,
- PR2::HEAD_PTZ_L_PAN ,
- PR2::HEAD_PTZ_L_TILT ,
- 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 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 ",
- ...
[truncated message content] |
|
From: <is...@us...> - 2008-07-17 23:27:32
|
Revision: 1727
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1727&view=rev
Author: isucan
Date: 2008-07-17 23:27:36 +0000 (Thu, 17 Jul 2008)
Log Message:
-----------
remove dependecy on freesolid
Modified Paths:
--------------
pkg/trunk/util/megamaid/src/vacuum/vacuum.cpp
pkg/trunk/util/wg_collision_space/CMakeLists.txt
pkg/trunk/util/wg_collision_space/manifest.xml
Removed Paths:
-------------
pkg/trunk/3rdparty/freesolid/
pkg/trunk/util/wg_collision_space/include/collisionspace/environmentSOLID.h
pkg/trunk/util/wg_collision_space/src/collisionspace/environmentSOLID.cpp
Modified: pkg/trunk/util/megamaid/src/vacuum/vacuum.cpp
===================================================================
--- pkg/trunk/util/megamaid/src/vacuum/vacuum.cpp 2008-07-17 23:09:32 UTC (rev 1726)
+++ pkg/trunk/util/megamaid/src/vacuum/vacuum.cpp 2008-07-17 23:27:36 UTC (rev 1727)
@@ -64,6 +64,7 @@
map_name(vac_topics[i]),
start))
throw std::runtime_error("couldn't open log file\n");
+
subscribe(vac_topics[i], bags[i], &Vacuum::dummy_cb);
}
}
Modified: pkg/trunk/util/wg_collision_space/CMakeLists.txt
===================================================================
--- pkg/trunk/util/wg_collision_space/CMakeLists.txt 2008-07-17 23:09:32 UTC (rev 1726)
+++ pkg/trunk/util/wg_collision_space/CMakeLists.txt 2008-07-17 23:27:36 UTC (rev 1727)
@@ -1,7 +1,6 @@
cmake_minimum_required(VERSION 2.6)
include(rosbuild)
rospack(wg_collision_space)
-rospack_add_library(CollisionSpace src/collisionspace/environmentODE.cpp
- src/collisionspace/environmentSOLID.cpp)
+rospack_add_library(CollisionSpace src/collisionspace/environmentODE.cpp)
rospack_add_executable(test_kinematic_ode src/test/test_kinematic_ode.cpp)
target_link_libraries(test_kinematic_ode CollisionSpace)
Deleted: pkg/trunk/util/wg_collision_space/include/collisionspace/environmentSOLID.h
===================================================================
--- pkg/trunk/util/wg_collision_space/include/collisionspace/environmentSOLID.h 2008-07-17 23:09:32 UTC (rev 1726)
+++ pkg/trunk/util/wg_collision_space/include/collisionspace/environmentSOLID.h 2008-07-17 23:27:36 UTC (rev 1727)
@@ -1,154 +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.
-*********************************************************************/
-
-#ifndef KINEMATIC_ENVIRONMENT_MODEL_SOLID_
-#define KINEMATIC_ENVIRONMENT_MODEL_SOLID_
-
-#include <collisionspace/environment.h>
-#include <robotmodels/kinematic.h>
-#include <SOLID/solid.h>
-
-/** @htmlinclude ../../manifest.html
-
- A class describing an environment for a kinematic robot using SOLID */
-
-class EnvironmentModelSOLID : public EnvironmentModel
-{
- public:
-
- /* dummy object for SOLID object references */
- struct SOLIDObject
- {
- unsigned int id;
- };
-
- /* an object in the world */
- struct Object
- {
- Object(void)
- {
- obj = new SOLIDObject();
- shape = NULL;
- }
-
- ~Object(void)
- {
- if (obj && shape)
- {
- dtDeleteObject(obj);
- dtDeleteShape(shape);
- }
- if (obj)
- delete obj;
- }
-
- SOLIDObject *obj;
- DtShapeRef shape;
- };
-
- class KinematicModelSOLID : public KinematicModel
- {
- public:
-
- KinematicModelSOLID(void) : KinematicModel()
- {
- }
-
- virtual ~KinematicModelSOLID(void)
- {
- for (unsigned int i = 0 ; i < m_kshapes.size() ; ++i)
- delete m_kshapes[i];
- }
-
- virtual void build(URDF &model, const char *group = NULL);
-
- unsigned int getObjectCount(void) const;
- Object* getObject(unsigned int index) const;
-
- void updateCollisionPositions(void);
-
- protected:
-
- struct kShape
- {
- kShape(void)
- {
- obj = new Object();
- link = NULL;
- }
-
- ~kShape(void)
- {
- if (obj)
- delete obj;
- }
-
- Object *obj;
- Link *link;
- };
-
- std::vector<kShape*> m_kshapes;
-
- void buildSOLIDShapes(Robot *robot);
- DtShapeRef buildSOLIDShape(Geometry *geom);
-
- };
-
-
- EnvironmentModelSOLID(void) : EnvironmentModel()
- {
- model = dynamic_cast<KinematicModel*>(&m_modelSOLID);
- }
-
- ~EnvironmentModelSOLID(void)
- {
- for (unsigned int i = 0 ; i < m_obstacles.size() ; ++i)
- delete m_obstacles[i];
- }
-
- /** Check if the model is in collision */
- virtual bool isCollision(void);
-
- /** Add a point cloud to the collision space */
- virtual void addPointCloud(unsigned int n, const double *points, double radius = 0.01);
-
- protected:
-
- KinematicModelSOLID m_modelSOLID;
- std::vector<Object*> m_obstacles;
-
-
-};
-
-#endif
Modified: pkg/trunk/util/wg_collision_space/manifest.xml
===================================================================
--- pkg/trunk/util/wg_collision_space/manifest.xml 2008-07-17 23:09:32 UTC (rev 1726)
+++ pkg/trunk/util/wg_collision_space/manifest.xml 2008-07-17 23:27:36 UTC (rev 1727)
@@ -7,7 +7,6 @@
<license>BSD</license>
<depend package="wg_robot_models"/>
- <depend package="freesolid"/>
<depend package="opende"/>
<depend package="drawstuff"/>
Deleted: pkg/trunk/util/wg_collision_space/src/collisionspace/environmentSOLID.cpp
===================================================================
--- pkg/trunk/util/wg_collision_space/src/collisionspace/environmentSOLID.cpp 2008-07-17 23:09:32 UTC (rev 1726)
+++ pkg/trunk/util/wg_collision_space/src/collisionspace/environmentSOLID.cpp 2008-07-17 23:27:36 UTC (rev 1727)
@@ -1,154 +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 <collisionspace/environmentSOLID.h>
-
-bool EnvironmentModelSOLID::isCollision(void)
-{
- return dtTest();
-}
-
-void EnvironmentModelSOLID::addPointCloud(unsigned int n, const double *points, double radius)
-{
- Object *obj = new Object();
- obj->shape = dtNewComplexShape();
-
- for (unsigned int i = 0 ; i < n ; ++i)
- {
- unsigned int i3 = i * 3;
- double x = points[i3 ];
- double y = points[i3 + 1];
- double z = points[i3 + 2];
-
- dtBegin(DT_SIMPLEX);
- dtVertex(x, y, z + radius);
- dtVertex(x - radius, y - radius, z - radius);
- dtVertex(x + radius, y - radius, z - radius);
- dtEnd();
-
- dtBegin(DT_SIMPLEX);
- dtVertex(x - radius, y - radius, z - radius);
- dtVertex(x + radius, y - radius, z - radius);
- dtVertex(x + radius, y + radius, z - radius);
- dtEnd();
-
- dtBegin(DT_SIMPLEX);
- dtVertex(x, y, z + radius);
- dtVertex(x + radius, y - radius, z - radius);
- dtVertex(x + radius, y + radius, z - radius);
- dtEnd();
-
- dtBegin(DT_SIMPLEX);
- dtVertex(x, y, z + radius);
- dtVertex(x + radius, y - radius, z - radius);
- dtVertex(x - radius, y - radius, z - radius);
- dtEnd();
- }
-
- dtEndComplexShape();
- dtCreateObject(obj->obj, obj->shape);
-}
-
-void EnvironmentModelSOLID::KinematicModelSOLID::build(URDF &model, const char *group)
-{
- KinematicModel::build(model, group);
- for (unsigned int i = 0 ; i < m_robots.size() ; ++i)
- buildSOLIDShapes(m_robots[i]);
- dtDisableCaching();
-}
-
-void EnvironmentModelSOLID::KinematicModelSOLID::buildSOLIDShapes(Robot *robot)
-{
- for (unsigned int i = 0 ; i < robot->links.size() ; ++i)
- {
- kShape *ks = new kShape();
- ks->link = robot->links[i];
- ks->obj->shape = buildSOLIDShape(robot->links[i]->geom);
- if (!ks->obj->shape)
- {
- delete ks;
- continue;
- }
-
- dtCreateObject(ks->obj->obj, ks->obj->shape);
- m_kshapes.push_back(ks);
- }
-}
-
-DtShapeRef EnvironmentModelSOLID::KinematicModelSOLID::buildSOLIDShape(Geometry *geom)
-{
- DtShapeRef g = NULL;
-
- switch (geom->type)
- {
- case Geometry::SPHERE:
- g = dtSphere(geom->size[0]);
- break;
- case Geometry::BOX:
- g = dtBox(geom->size[0], geom->size[1], geom->size[2]);
- break;
- case Geometry::CYLINDER:
- g = dtCylinder(geom->size[0], geom->size[1]);
- break;
- default:
- break;
- }
-
- return g;
-}
-
-void EnvironmentModelSOLID::KinematicModelSOLID::updateCollisionPositions(void)
-{
- for (unsigned int i = 0 ; i < m_kshapes.size() ; ++i)
- {
- dtSelectObject(m_kshapes[i]->obj->obj);
- libTF::Pose3D::Position pos;
- m_kshapes[i]->link->globalTrans.getPosition(pos);
- libTF::Pose3D::Quaternion quat;
- m_kshapes[i]->link->globalTrans.getQuaternion(quat);
- dtLoadIdentity();
- dtTranslate(pos.x, pos.y, pos.z);
- dtRotate(quat.x, quat.y, quat.z, quat.w);
- }
-}
-
-unsigned int EnvironmentModelSOLID::KinematicModelSOLID::getObjectCount(void) const
-{
- return m_kshapes.size();
-}
-
-EnvironmentModelSOLID::Object* EnvironmentModelSOLID::KinematicModelSOLID::getObject(unsigned int index) const
-{
- return m_kshapes[index]->obj;
-}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-07-18 00:08:33
|
Revision: 1737
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1737&view=rev
Author: isucan
Date: 2008-07-18 00:08:36 +0000 (Fri, 18 Jul 2008)
Log Message:
-----------
added namespace for robot models
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/util/collision_space/include/collision_space/environment.h
pkg/trunk/util/collision_space/include/collision_space/environmentODE.h
pkg/trunk/util/collision_space/src/collision_space/environmentODE.cpp
pkg/trunk/util/collision_space/src/test/test_kinematic_ode.cpp
pkg/trunk/util/robot_models/include/robot_models/kinematic.h
pkg/trunk/util/robot_models/src/robot_models/kinematic.cpp
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-07-18 00:04:22 UTC (rev 1736)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-07-18 00:08:36 UTC (rev 1737)
@@ -163,7 +163,6 @@
{
ompl::SpaceInformationKinematic_t si;
ompl::MotionPlanner_t mp;
- // collision space
};
std::vector<Model> models;
Modified: pkg/trunk/util/collision_space/include/collision_space/environment.h
===================================================================
--- pkg/trunk/util/collision_space/include/collision_space/environment.h 2008-07-18 00:04:22 UTC (rev 1736)
+++ pkg/trunk/util/collision_space/include/collision_space/environment.h 2008-07-18 00:08:36 UTC (rev 1737)
@@ -60,7 +60,7 @@
/** Add a point cloud to the collision space */
virtual void addPointCloud(unsigned int n, const double* points, double radius = 0.01) = 0;
- KinematicModel *model;
+ robot_models::KinematicModel *model;
};
Modified: pkg/trunk/util/collision_space/include/collision_space/environmentODE.h
===================================================================
--- pkg/trunk/util/collision_space/include/collision_space/environmentODE.h 2008-07-18 00:04:22 UTC (rev 1736)
+++ pkg/trunk/util/collision_space/include/collision_space/environmentODE.h 2008-07-18 00:08:36 UTC (rev 1737)
@@ -46,7 +46,7 @@
{
public:
- class KinematicModelODE : public KinematicModel
+ class KinematicModelODE : public robot_models::KinematicModel
{
public:
@@ -92,7 +92,7 @@
EnvironmentModelODE(void) : EnvironmentModel()
{
- model = dynamic_cast<KinematicModel*>(&m_modelODE);
+ model = dynamic_cast<robot_models::KinematicModel*>(&m_modelODE);
m_space = dHashSpaceCreate(0);
m_modelODE.setODESpace(m_space);
}
Modified: pkg/trunk/util/collision_space/src/collision_space/environmentODE.cpp
===================================================================
--- pkg/trunk/util/collision_space/src/collision_space/environmentODE.cpp 2008-07-18 00:04:22 UTC (rev 1736)
+++ pkg/trunk/util/collision_space/src/collision_space/environmentODE.cpp 2008-07-18 00:08:36 UTC (rev 1737)
@@ -37,7 +37,7 @@
void EnvironmentModelODE::KinematicModelODE::build(URDF &model, const char *group)
{
- KinematicModel::build(model, group);
+ robot_models::KinematicModel::build(model, group);
assert(m_space);
for (unsigned int i = 0 ; i < m_robots.size() ; ++i)
buildODEGeoms(m_robots[i]);
Modified: pkg/trunk/util/collision_space/src/test/test_kinematic_ode.cpp
===================================================================
--- pkg/trunk/util/collision_space/src/test/test_kinematic_ode.cpp 2008-07-18 00:04:22 UTC (rev 1736)
+++ pkg/trunk/util/collision_space/src/test/test_kinematic_ode.cpp 2008-07-18 00:08:36 UTC (rev 1737)
@@ -121,7 +121,7 @@
km.model->build(model);
printf("number of robots = %d\n", km.model->getRobotCount());
- KinematicModel::Robot *r = km.model->getRobot(0);
+ robot_models::KinematicModel::Robot *r = km.model->getRobot(0);
printf("state dimension = %d\n", r->stateDimension);
double *param = new double[r->stateDimension];
Modified: pkg/trunk/util/robot_models/include/robot_models/kinematic.h
===================================================================
--- pkg/trunk/util/robot_models/include/robot_models/kinematic.h 2008-07-18 00:04:22 UTC (rev 1736)
+++ pkg/trunk/util/robot_models/include/robot_models/kinematic.h 2008-07-18 00:08:36 UTC (rev 1737)
@@ -44,186 +44,192 @@
A class describing a kinematic robot model loaded from URDF */
-class KinematicModel
+namespace robot_models
{
- public:
-
- /** Possible geometry elements. These are assumed to be centered at origin (similar to ODE) */
- struct Geometry
- {
- Geometry(void)
- {
- type = UNKNOWN;
- size[0] = size[1] = size[2] = 0.0;
- }
-
- enum
- {
- UNKNOWN, BOX, CYLINDER, SPHERE
- } type;
- double size[3];
- };
- struct Joint;
- struct Link;
-
- /** A joint from the robot. Contains the transform applied by the joint type */
- struct Joint
+ class KinematicModel
{
- Joint(void)
- {
- before = after = NULL;
- usedParamStart = usedParamEnd = 0;
- axis[0] = axis[1] = axis[2] = 0.0;
- anchor[0] = anchor[1] = anchor[2] = 0.0;
- limit[0] = limit[1] = 0.0;
- type = UNKNOWN;
- }
+ public:
- ~Joint(void)
+
+ /** Possible geometry elements. These are assumed to be centered at origin (similar to ODE) */
+ struct Geometry
{
- if (after)
- delete after;
- }
+ Geometry(void)
+ {
+ type = UNKNOWN;
+ size[0] = size[1] = size[2] = 0.0;
+ }
+
+ enum
+ {
+ UNKNOWN, BOX, CYLINDER, SPHERE
+ } type;
+ double size[3];
+ };
- /* the links that this joint connects */
- Link *before;
- Link *after;
+ struct Joint;
+ struct Link;
- /* the range of indices in the parameter vector that
- needed to access information about the position of this
- joint */
- unsigned int usedParamStart;
- unsigned int usedParamEnd;
- bool active;
+ /** A joint from the robot. Contains the transform applied by the joint type */
+ struct Joint
+ {
+ Joint(void)
+ {
+ before = after = NULL;
+ usedParamStart = usedParamEnd = 0;
+ axis[0] = axis[1] = axis[2] = 0.0;
+ anchor[0] = anchor[1] = anchor[2] = 0.0;
+ limit[0] = limit[1] = 0.0;
+ type = UNKNOWN;
+ }
+
+ ~Joint(void)
+ {
+ if (after)
+ delete after;
+ }
+
+ /* the links that this joint connects */
+ Link *before;
+ Link *after;
+
+ /* the range of indices in the parameter vector that
+ needed to access information about the position of this
+ joint */
+ unsigned int usedParamStart;
+ unsigned int usedParamEnd;
+ bool active;
+
+ /* relevant joint information */
+ enum
+ {
+ UNKNOWN, FIXED, REVOLUTE, PRISMATIC, FLOATING
+ } type;
+ double axis[3];
+ double anchor[3];
+ double limit[2];
+
+ /* ----------------- Computed data -------------------*/
+
+ /* the local transform (computed by forward kinematics) */
+ libTF::Pose3D varTrans;
+ libTF::Pose3D globalTrans;
+
+ void computeTransform(const double *params);
+
+ };
- /* relevant joint information */
- enum
+ /** A link from the robot. Contains the constant transform applied to the link and its geometry */
+ struct Link
+ {
+ Link(void)
{
- UNKNOWN, FIXED, REVOLUTE, PRISMATIC, FLOATING
- } type;
- double axis[3];
- double anchor[3];
- double limit[2];
+ before = NULL;
+ geom = new Geometry();
+ }
+
+ ~Link(void)
+ {
+ if (geom)
+ delete geom;
+ for (unsigned int i = 0 ; i < after.size() ; ++i)
+ delete after[i];
+ }
+
+ /* joint that connects this link to the parent link */
+ Joint *before;
+
+ /* list of descending joints (each connects to a child link) */
+ std::vector<Joint*> after;
+
+ /* the constant transform applied to the link (local) */
+ libTF::Pose3D constTrans;
+
+ /* the constant transform applied to the collision geometry of the link (local) */
+ libTF::Pose3D constGeomTrans;
+
+ /* the geometry of the link */
+ Geometry *geom;
+
+ /* ----------------- Computed data -------------------*/
+
+ /* the global transform for this link (computed by forward kinematics) */
+ libTF::Pose3D globalTrans;
+
+ void computeTransform(const double *params);
+ };
- /* ----------------- Computed data -------------------*/
+ /** A robot structure */
+ struct Robot
+ {
+ Robot(void)
+ {
+ chain = NULL;
+ stateDimension = 0;
+ }
+
+ virtual ~Robot(void)
+ {
+ if (chain)
+ delete chain;
+ }
+
+ void computeTransforms(const double *params);
+
+ /** List of links in the robot */
+ std::vector<Link*> links;
+
+ /** List of leaf links (have no child links) */
+ std::vector<Link*> leafs;
+
+ /** The first joint in the robot -- the root */
+ Joint *chain;
+
+ /** Number of parameters needed to define the joints */
+ unsigned int stateDimension;
+
+ /** The bounding box for the set of parameters describing the
+ * joints. This array contains 2 * stateDimension elements:
+ * positions 2*i and 2*i+1 define the minimum and maximum
+ * values for the parameter. If both minimum and maximum are
+ * set to 0, the parameter is unbounded. */
+ std::vector<double> stateBounds;
+
+ /** Group of links corresponding to this robot (if any) */
+ std::string tag;
+
+ };
- /* the local transform (computed by forward kinematics) */
- libTF::Pose3D varTrans;
- libTF::Pose3D globalTrans;
-
- void computeTransform(const double *params);
-
- };
-
- /** A link from the robot. Contains the constant transform applied to the link and its geometry */
- struct Link
- {
- Link(void)
+ KinematicModel(void)
{
- before = NULL;
- geom = new Geometry();
+ m_verbose = false;
}
- ~Link(void)
+ virtual ~KinematicModel(void)
{
- if (geom)
- delete geom;
- for (unsigned int i = 0 ; i < after.size() ; ++i)
- delete after[i];
+ for (unsigned int i = 0 ; i < m_robots.size() ; ++i)
+ delete m_robots[i];
}
- /* joint that connects this link to the parent link */
- Joint *before;
+ virtual void build(URDF &model, const char *group = NULL);
- /* list of descending joints (each connects to a child link) */
- std::vector<Joint*> after;
+ unsigned int getRobotCount(void) const;
+ Robot* getRobot(unsigned int index) const;
- /* the constant transform applied to the link (local) */
- libTF::Pose3D constTrans;
-
- /* the constant transform applied to the collision geometry of the link (local) */
- libTF::Pose3D constGeomTrans;
-
- /* the geometry of the link */
- Geometry *geom;
+ protected:
- /* ----------------- Computed data -------------------*/
+ std::vector<Robot*> m_robots;
+ bool m_verbose;
- /* the global transform for this link (computed by forward kinematics) */
- libTF::Pose3D globalTrans;
-
- void computeTransform(const double *params);
- };
-
- /** A robot structure */
- struct Robot
- {
- Robot(void)
- {
- chain = NULL;
- stateDimension = 0;
- }
+ private:
- virtual ~Robot(void)
- {
- if (chain)
- delete chain;
- }
+ void buildChain(Robot *robot, Link *parent, Joint *joint, URDF::Link *urdfLink);
+ void buildChain(Robot *robot, Joint *parent, Link *link, URDF::Link *urdfLink);
- void computeTransforms(const double *params);
-
- /** List of links in the robot */
- std::vector<Link*> links;
-
- /** List of leaf links (have no child links) */
- std::vector<Link*> leafs;
-
- /** The first joint in the robot -- the root */
- Joint *chain;
-
- /** Number of parameters needed to define the joints */
- unsigned int stateDimension;
-
- /** The bounding box for the set of parameters describing the
- * joints. This array contains 2 * stateDimension elements:
- * positions 2*i and 2*i+1 define the minimum and maximum
- * values for the parameter. If both minimum and maximum are
- * set to 0, the parameter is unbounded. */
- std::vector<double> stateBounds;
-
- /** Group of links corresponding to this robot (if any) */
- std::string tag;
-
};
-
- KinematicModel(void)
- {
- m_verbose = false;
- }
-
- virtual ~KinematicModel(void)
- {
- for (unsigned int i = 0 ; i < m_robots.size() ; ++i)
- delete m_robots[i];
- }
-
- virtual void build(URDF &model, const char *group = NULL);
- unsigned int getRobotCount(void) const;
- Robot* getRobot(unsigned int index) const;
-
- protected:
-
- std::vector<Robot*> m_robots;
- bool m_verbose;
-
- private:
+}
- void buildChain(Robot *robot, Link *parent, Joint *joint, URDF::Link *urdfLink);
- void buildChain(Robot *robot, Joint *parent, Link *link, URDF::Link *urdfLink);
-
-};
-
#endif
Modified: pkg/trunk/util/robot_models/src/robot_models/kinematic.cpp
===================================================================
--- pkg/trunk/util/robot_models/src/robot_models/kinematic.cpp 2008-07-18 00:04:22 UTC (rev 1736)
+++ pkg/trunk/util/robot_models/src/robot_models/kinematic.cpp 2008-07-18 00:08:36 UTC (rev 1737)
@@ -35,13 +35,13 @@
#include <robot_models/kinematic.h>
#include <cstdio>
-void KinematicModel::Robot::computeTransforms(const double *params)
+void robot_models::KinematicModel::Robot::computeTransforms(const double *params)
{
chain->computeTransform(params);
}
// we can optimize things here... (when we use identity transforms, for example)
-void KinematicModel::Joint::computeTransform(const double *params)
+void robot_models::KinematicModel::Joint::computeTransform(const double *params)
{
switch (type)
{
@@ -73,7 +73,7 @@
after->computeTransform(params);
}
-void KinematicModel::Link::computeTransform(const double *params)
+void robot_models::KinematicModel::Link::computeTransform(const double *params)
{
globalTrans = before->globalTrans;
globalTrans.multiplyPose(constTrans);
@@ -82,7 +82,7 @@
globalTrans.multiplyPose(constGeomTrans);
}
-void KinematicModel::build(URDF &model, const char *group)
+void robot_models::KinematicModel::build(URDF &model, const char *group)
{
if (group)
{
@@ -110,17 +110,17 @@
}
}
-unsigned int KinematicModel::getRobotCount(void) const
+unsigned int robot_models::KinematicModel::getRobotCount(void) const
{
return m_robots.size();
}
-KinematicModel::Robot* KinematicModel::getRobot(unsigned int index) const
+robot_models::KinematicModel::Robot* robot_models::KinematicModel::getRobot(unsigned int index) const
{
return m_robots[index];
}
-void KinematicModel::buildChain(Robot *robot, Link *parent, Joint* joint, URDF::Link* urdfLink)
+void robot_models::KinematicModel::buildChain(Robot *robot, Link *parent, Joint* joint, URDF::Link* urdfLink)
{
joint->usedParamStart = robot->stateDimension;
joint->before = parent;
@@ -168,7 +168,7 @@
buildChain(robot, joint, joint->after, urdfLink);
}
-void KinematicModel::buildChain(Robot *robot, Joint *parent, Link* link, URDF::Link* urdfLink)
+void robot_models::KinematicModel::buildChain(Robot *robot, Joint *parent, Link* link, URDF::Link* urdfLink)
{
link->before = parent;
robot->links.push_back(link);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
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.
|
|
From: <hsu...@us...> - 2008-07-18 02:14:08
|
Revision: 1752
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1752&view=rev
Author: hsujohnhsu
Date: 2008-07-18 02:14:15 +0000 (Fri, 18 Jul 2008)
Log Message:
-----------
added a controller for each joint.
Modified Paths:
--------------
pkg/trunk/robot/pr2_gazebo/manifest.xml
pkg/trunk/robot/pr2_gazebo/src/test2.cpp
Added Paths:
-----------
pkg/trunk/controllers/rosControllers/
pkg/trunk/controllers/rosControllers/CMakeLists.txt
pkg/trunk/controllers/rosControllers/Makefile
pkg/trunk/controllers/rosControllers/include/
pkg/trunk/controllers/rosControllers/include/rosControllers/
pkg/trunk/controllers/rosControllers/include/rosControllers/RosJointController.h
pkg/trunk/controllers/rosControllers/manifest.xml
pkg/trunk/controllers/rosControllers/src/
pkg/trunk/controllers/rosControllers/src/RosJointController.cpp
Added: pkg/trunk/controllers/rosControllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/rosControllers/CMakeLists.txt (rev 0)
+++ pkg/trunk/controllers/rosControllers/CMakeLists.txt 2008-07-18 02:14:15 UTC (rev 1752)
@@ -0,0 +1,5 @@
+cmake_minimum_required(VERSION 2.6)
+include(rosbuild)
+rospack(rosControllers)
+add_definitions(-Wall)
+rospack_add_library(RosJointController src/RosJointController.cpp)
Added: pkg/trunk/controllers/rosControllers/Makefile
===================================================================
--- pkg/trunk/controllers/rosControllers/Makefile (rev 0)
+++ pkg/trunk/controllers/rosControllers/Makefile 2008-07-18 02:14:15 UTC (rev 1752)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Added: pkg/trunk/controllers/rosControllers/include/rosControllers/RosJointController.h
===================================================================
--- pkg/trunk/controllers/rosControllers/include/rosControllers/RosJointController.h (rev 0)
+++ pkg/trunk/controllers/rosControllers/include/rosControllers/RosJointController.h 2008-07-18 02:14:15 UTC (rev 1752)
@@ -0,0 +1,98 @@
+#pragma once
+/*
+ * ROS WRAPPER FOR JOINT CONTROLLER
+ *
+ * stubbed, to be filled in
+ *
+ * Copyright (c) 2008, Willow Garage, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+
+#include <genericControllers/JointController.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
+#include <rostools/Time.h>
+// roscpp - base
+#include <std_msgs/RobotBase2DOdom.h>
+#include <std_msgs/BaseVel.h>
+// roscpp - arm
+#include <std_msgs/PR2Arm.h>
+// roscpp - camera
+#include <std_msgs/Image.h>
+
+// for frame transforms
+#include <rosTF/rosTF.h>
+
+#include <time.h>
+#include <iostream>
+
+// Our node
+class RosJointController : public ros::node
+{
+ 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::Empty shutterMsg; // marks end of a cloud message
+ std_msgs::RobotBase2DOdom odomMsg;
+ rostools::Time timeMsg;
+
+ // A mutex to lock access to fields that are used in message callbacks
+ ros::thread::mutex lock;
+
+ // for frame transforms, publish frame transforms
+ rosTFServer tf;
+
+ public:
+ // Constructor; stage itself needs argc/argv. fname is the .world file
+ // that stage should load.
+ RosJointController(std::string jointName);
+ ~RosJointController();
+
+ // advertise / subscribe models
+ int advertiseSubscribeMessages();
+
+ void init(CONTROLLER::JointController *jc);
+ // Do one update of the simulator. May pause if the next update time
+ // has not yet arrived.
+ void update();
+
+ // Message callback for a std_msgs::BaseVel message, which set velocities.
+ void cmdReceived();
+
+ //Keep track of controllers
+ CONTROLLER::JointController* jc;
+};
+
+
+
+
Added: pkg/trunk/controllers/rosControllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/rosControllers/manifest.xml (rev 0)
+++ pkg/trunk/controllers/rosControllers/manifest.xml 2008-07-18 02:14:15 UTC (rev 1752)
@@ -0,0 +1,19 @@
+<package>
+ <description>ROS wrapper[s] that does the communications for [Joint]Controllers.</description>
+ <author>John M. Hsu</author>
+ <license>TBD</license>
+ <depend package="roscpp" />
+ <depend package="genericControllers" />
+ <depend package="pr2Controllers" />
+ <depend package="pr2Core" />
+ <depend package="std_msgs" />
+ <depend package="rosthread" />
+ <depend package="rosTF" />
+ <depend package="math_utils" />
+ <depend package="string_utils" />
+ <depend package="libKDL" />
+ <depend package="robot_model" />
+ <export>
+ <cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lRosJointController"/>
+ </export>
+</package>
Added: pkg/trunk/controllers/rosControllers/src/RosJointController.cpp
===================================================================
--- pkg/trunk/controllers/rosControllers/src/RosJointController.cpp (rev 0)
+++ pkg/trunk/controllers/rosControllers/src/RosJointController.cpp 2008-07-18 02:14:15 UTC (rev 1752)
@@ -0,0 +1,59 @@
+/*
+ * rosgazebo
+ * Copyright (c) 2008, Willow Garage, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include <rosControllers/RosJointController.h>
+
+void
+RosJointController::cmdReceived(){
+}
+
+
+
+RosJointController::RosJointController(std::string jointName) : ros::node(jointName),tf(*this)
+{
+}
+
+int
+RosJointController::advertiseSubscribeMessages()
+{
+ return(0);
+}
+
+RosJointController::~RosJointController()
+{
+}
+
+
+void
+RosJointController::init(CONTROLLER::JointController *jc)
+{
+ this->jc = jc;
+}
+
+
+void
+RosJointController::update()
+{
+ this->lock.lock();
+
+ this->lock.unlock();
+}
+
+
+
Modified: pkg/trunk/robot/pr2_gazebo/manifest.xml
===================================================================
--- pkg/trunk/robot/pr2_gazebo/manifest.xml 2008-07-18 01:50:03 UTC (rev 1751)
+++ pkg/trunk/robot/pr2_gazebo/manifest.xml 2008-07-18 02:14:15 UTC (rev 1752)
@@ -10,6 +10,7 @@
<depend package="libpr2API" />
<depend package="genericControllers" />
<depend package="pr2Controllers" />
+ <depend package="rosControllers" />
<depend package="pr2Core" />
<depend package="std_msgs" />
<depend package="rosthread" />
Modified: pkg/trunk/robot/pr2_gazebo/src/test2.cpp
===================================================================
--- pkg/trunk/robot/pr2_gazebo/src/test2.cpp 2008-07-18 01:50:03 UTC (rev 1751)
+++ pkg/trunk/robot/pr2_gazebo/src/test2.cpp 2008-07-18 02:14:15 UTC (rev 1752)
@@ -31,7 +31,6 @@
#include <gazebo_hardware/gazebo_hardware.h>
#include <gazebo_sensors/gazebo_sensors.h>
-
// controller objects
#include <pr2Controllers/ArmController.h>
#include <pr2Controllers/HeadController.h>
@@ -40,14 +39,16 @@
#include <pr2Controllers/LaserScannerController.h>
#include <pr2Controllers/GripperController.h>
+// ros node for controllers
+#include <rosControllers/RosJointController.h>
// roscpp
-#include <ros/node.h>
+//#include <ros/node.h>
#include <time.h>
#include <signal.h>
-pthread_mutex_t simMutex; //Mutex for sim R/W
+pthread_mutex_t simMutex[PR2::MAX_JOINTS]; //Mutex for sim R/W
void finalize(int)
{
@@ -57,14 +58,16 @@
}
-void *nonRealtimeLoop(void *rgn)
+void *nonRealtimeLoop(void *rjc)
{
std::cout << "Started nonRT loop" << std::endl;
while (1)
{
- pthread_mutex_lock(&simMutex); //Lock for r/w
- //((RosGazeboNode*)rgn)->Update();
- pthread_mutex_unlock(&simMutex); //Unlock when done
+ for (int i = 0;i<PR2::MAX_JOINTS;i++){
+ pthread_mutex_lock(&simMutex[i]); //Lock for r/w
+ ((RosJointController*)rjc)->update();
+ pthread_mutex_unlock(&simMutex[i]); //Unlock when done
+ }
// some time out for publishing ros info
usleep(10000);
}
@@ -81,8 +84,10 @@
// we need 2 threads, one for RT and one for nonRT
pthread_t threads[2];
- pthread_mutex_init(&simMutex, NULL);
-
+ for (int i = 0;i<PR2::MAX_JOINTS;i++){
+ pthread_mutex_init(&(simMutex[i]), NULL);
+ }
+
ros::init(argc,argv);
/***************************************************************************************/
@@ -223,9 +228,10 @@
// 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] = 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]);
@@ -235,6 +241,14 @@
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]);
+ jc[PR2::ARM_R_PAN]->Init(100,0,0,500,-500, CONTROLLER::CONTROLLER_POSITION,time,100,-100,100,joint[PR2::ARM_R_PAN]);
+ jc[PR2::ARM_R_SHOULDER_PITCH]->Init(1000,0,0,500,-500, CONTROLLER::CONTROLLER_POSITION,time,1000,-1000,1000,joint[PR2::ARM_R_SHOULDER_PITCH]);
+ jc[PR2::ARM_R_SHOULDER_ROLL]->Init(100,0,0,500,-500, CONTROLLER::CONTROLLER_POSITION,time,100,-100,100,joint[PR2::ARM_R_SHOULDER_ROLL]);
+ jc[PR2::ARM_R_ELBOW_PITCH]->Init(300,0,0,500,-500, CONTROLLER::CONTROLLER_POSITION,time,100,-100,100,joint[PR2::ARM_R_ELBOW_PITCH]);
+ jc[PR2::ARM_R_ELBOW_ROLL]->Init(100,0,0,500,-500, CONTROLLER::CONTROLLER_POSITION,time,100,-100,100,joint[PR2::ARM_R_ELBOW_ROLL]);
+ jc[PR2::ARM_R_WRIST_PITCH]->Init(100,0,0,500,-500, CONTROLLER::CONTROLLER_POSITION,time,100,-100,100,joint[PR2::ARM_R_WRIST_PITCH]);
+ jc[PR2::ARM_R_WRIST_ROLL]->Init(100,0,0,500,-500, CONTROLLER::CONTROLLER_POSITION,time,100,-100,100,joint[PR2::ARM_R_WRIST_ROLL]);
+
//JointController::Init(double PGain, double IGain, double DGain, double IMax,
// double IMin, CONTROLLER_CONTROL_MODE mode, double time,
// double maxPositiveTorque, double maxNegativeTorque,
@@ -259,9 +273,20 @@
/* initialize ROS Gazebo Nodes */
/* */
/***************************************************************************************/
- printf("Creating node \n");
- //RosGazeboNode rgn(argc,argv,argv[1],myPR2,&myArm,&myHead,&mySpine,&myBase,&myLaserScanner,&myGripper,JointArray);
+ RosJointController* rjc[PR2::MAX_JOINTS]; // One ros node per controller
+ for (int i = 0;i<PR2::MAX_JOINTS;i++){
+ // name for controller
+ char tmp[21];
+ sprintf(tmp,"joint_controller_%04d", i);
+ // instantiate
+ rjc[i] = new RosJointController(tmp);
+ rjc[i]->init(jc[i]);
+ // see if we can subscribe models needed
+ if (rjc[i]->advertiseSubscribeMessages() != 0)
+ exit(-1);
+ }
+
/***************************************************************************************/
/* */
/* on termination... */
@@ -271,22 +296,21 @@
signal(SIGQUIT, (&finalize));
signal(SIGTERM, (&finalize));
- // see if we can subscribe models needed
- //if (rgn.AdvertiseSubscribeMessages() != 0)
- // exit(-1);
-
/***************************************************************************************/
/* */
/* Update ROS Gazebo Node */
/* contains controller pointers for the non-RT setpoints */
/* */
/***************************************************************************************/
- //int rgnt = pthread_create(&threads[0],NULL, nonRealtimeLoop, (void *) (&rgn));
- //if (rgnt)
- //{
- // printf("Could not start a separate thread for ROS Gazebo Node (code=%d)\n",rgnt);
- // exit(-1);
- //}
+ int rjct[PR2::MAX_JOINTS];
+ for (int i=0;i<PR2::MAX_JOINTS;i++) {
+ rjct[i] = pthread_create(&threads[0],NULL, nonRealtimeLoop, (void *) (&(rjc[i])));
+ if (rjct[i])
+ {
+ printf("Could not start a separate thread for %d-th ROS Gazebo Node (code=%d)\n",i,rjct[i]);
+ exit(-1);
+ }
+ }
/***************************************************************************************/
/* */
@@ -301,7 +325,6 @@
//myPR2->hw.GetSimTime(&time);
//std::cout<<"Time:"<<time<<std::endl;
- //pthread_mutex_trylock(&simMutex); //Try to lock here. But continue on if fails to enforce real time
// Update Controllers
// each controller will try to read new commands from shared memory with nonRT hooks,
// and skip update if locked by nonRT loop.
@@ -315,18 +338,6 @@
myGripper.Update();
*/
-
- // jc.GetTorqueCmd(&torque);
- // std::cout<<"*"<<torque<<std::endl;
- // std::cout<<JointArray[PR2::ARM_L_SHOULDER_PITCH]->position<<std::endl;
- // TODO: Safety codes should go here...
-
- // old: Send updated controller commands to hardware
- // myPR2->hw.UpdateJointArray(JointArray);
-
-
-
-
// get encoder counts from hardware
h->updateState();
@@ -335,7 +346,11 @@
// setup joint state from encoder counts
st[i]->propagatePosition();
//update controller
- jc[i]->Update();
+ if (pthread_mutex_trylock(&simMutex[i])==0)
+ {
+ jc[i]->Update();
+ pthread_mutex_unlock(&simMutex[i]); //Unlock after we're done with r/w
+ }
// update command current from joint command
st[i]->propagateEffort();
}
@@ -348,7 +363,6 @@
// refresh hardware
h->tick();
- //pthread_mutex_unlock(&simMutex); //Unlock after we're done with r/w
// wait for Gazebo time step
//myPR2->hw.ClientWait();
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-07-18 18:32:32
|
Revision: 1767
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1767&view=rev
Author: isucan
Date: 2008-07-18 18:32:40 +0000 (Fri, 18 Jul 2008)
Log Message:
-----------
moved robot_models to robot_motion_planning_models
Modified Paths:
--------------
pkg/trunk/robot_models/robot_motion_planning_models/CMakeLists.txt
pkg/trunk/robot_models/robot_motion_planning_models/manifest.xml
pkg/trunk/util/collision_space/manifest.xml
Added Paths:
-----------
pkg/trunk/robot_models/
pkg/trunk/robot_models/robot_motion_planning_models/
Removed Paths:
-------------
pkg/trunk/util/robot_models/
Modified: pkg/trunk/robot_models/robot_motion_planning_models/CMakeLists.txt
===================================================================
--- pkg/trunk/util/robot_models/CMakeLists.txt 2008-07-18 17:04:34 UTC (rev 1766)
+++ pkg/trunk/robot_models/robot_motion_planning_models/CMakeLists.txt 2008-07-18 18:32:40 UTC (rev 1767)
@@ -1,4 +1,4 @@
cmake_minimum_required(VERSION 2.6)
include(rosbuild)
-rospack(robot_models)
-rospack_add_library(RobotModels src/robot_models/kinematic.cpp)
+rospack(robot_motion_planning_models)
+rospack_add_library(PlanningRobotModels src/robot_models/kinematic.cpp)
Modified: pkg/trunk/robot_models/robot_motion_planning_models/manifest.xml
===================================================================
--- pkg/trunk/util/robot_models/manifest.xml 2008-07-18 17:04:34 UTC (rev 1766)
+++ pkg/trunk/robot_models/robot_motion_planning_models/manifest.xml 2008-07-18 18:32:40 UTC (rev 1767)
@@ -11,7 +11,7 @@
<depend package="libTF"/>
<export>
- <cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lRobotModels"/>
+ <cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lPlanningRobotModels"/>
</export>
</package>
Modified: pkg/trunk/util/collision_space/manifest.xml
===================================================================
--- pkg/trunk/util/collision_space/manifest.xml 2008-07-18 17:04:34 UTC (rev 1766)
+++ pkg/trunk/util/collision_space/manifest.xml 2008-07-18 18:32:40 UTC (rev 1767)
@@ -6,7 +6,7 @@
<author>Ioan Sucan/is...@wi...</author>
<license>BSD</license>
- <depend package="robot_models"/>
+ <depend package="robot_motion_planning_models"/>
<depend package="opende"/>
<depend package="drawstuff"/>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-07-18 18:35:22
|
Revision: 1768
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1768&view=rev
Author: isucan
Date: 2008-07-18 18:35:28 +0000 (Fri, 18 Jul 2008)
Log Message:
-----------
renamed include path from robot_models to planning_models
Modified Paths:
--------------
pkg/trunk/robot_models/robot_motion_planning_models/CMakeLists.txt
pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp
pkg/trunk/util/collision_space/include/collision_space/environment.h
Added Paths:
-----------
pkg/trunk/robot_models/robot_motion_planning_models/include/planning_models/
pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/
Removed Paths:
-------------
pkg/trunk/robot_models/robot_motion_planning_models/include/robot_models/
pkg/trunk/robot_models/robot_motion_planning_models/src/robot_models/
Modified: pkg/trunk/robot_models/robot_motion_planning_models/CMakeLists.txt
===================================================================
--- pkg/trunk/robot_models/robot_motion_planning_models/CMakeLists.txt 2008-07-18 18:32:40 UTC (rev 1767)
+++ pkg/trunk/robot_models/robot_motion_planning_models/CMakeLists.txt 2008-07-18 18:35:28 UTC (rev 1768)
@@ -1,4 +1,4 @@
cmake_minimum_required(VERSION 2.6)
include(rosbuild)
rospack(robot_motion_planning_models)
-rospack_add_library(PlanningRobotModels src/robot_models/kinematic.cpp)
+rospack_add_library(PlanningRobotModels src/planning_models/kinematic.cpp)
Modified: pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp
===================================================================
--- pkg/trunk/robot_models/robot_motion_planning_models/src/robot_models/kinematic.cpp 2008-07-18 18:32:40 UTC (rev 1767)
+++ pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp 2008-07-18 18:35:28 UTC (rev 1768)
@@ -32,7 +32,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
-#include <robot_models/kinematic.h>
+#include <planning_models/kinematic.h>
#include <cstdio>
void robot_models::KinematicModel::Robot::computeTransforms(const double *params)
Modified: pkg/trunk/util/collision_space/include/collision_space/environment.h
===================================================================
--- pkg/trunk/util/collision_space/include/collision_space/environment.h 2008-07-18 18:32:40 UTC (rev 1767)
+++ pkg/trunk/util/collision_space/include/collision_space/environment.h 2008-07-18 18:35:28 UTC (rev 1768)
@@ -35,7 +35,7 @@
#ifndef KINEMATIC_ENVIRONMENT_MODEL_
#define KINEMATIC_ENVIRONMENT_MODEL_
-#include <robot_models/kinematic.h>
+#include <planning_models/kinematic.h>
/** @htmlinclude ../../manifest.html
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-07-18 19:14:13
|
Revision: 1773
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1773&view=rev
Author: isucan
Date: 2008-07-18 19:14:17 +0000 (Fri, 18 Jul 2008)
Log Message:
-----------
renamed robot_model package to robot_mechanism_model and moved it to the robot_models directory (conflicting names with robot models for motion planning). The include path now is <mechanism_model/...> instead of <robot_model/...>. If code appears to be broken, just do make clean in the directory of the package that does not build
Modified Paths:
--------------
pkg/trunk/controllers/genericControllers/include/genericControllers/JointController.h
pkg/trunk/controllers/genericControllers/manifest.xml
pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/BaseController.h
pkg/trunk/controllers/pr2Controllers/src/BaseController.cpp
pkg/trunk/controllers/rosControllers/manifest.xml
pkg/trunk/controllers/testControllers/manifest.xml
pkg/trunk/controllers/testControllers/src/test_base.cpp
pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h
pkg/trunk/drivers/robot/pr2/libpr2HW/manifest.xml
pkg/trunk/mechanism/mechanism_control/include/mechanism_control/base_control.h
pkg/trunk/mechanism/mechanism_control/manifest.xml
pkg/trunk/robot/pr2_gazebo/src/test2.cpp
pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h
pkg/trunk/simulators/rosgazebo/manifest.xml
pkg/trunk/simulators/rosgazebo/src/rosgazebo.cc
Added Paths:
-----------
pkg/trunk/robot_models/robot_mechanism_model/
pkg/trunk/robot_models/robot_mechanism_model/CMakeLists.txt
pkg/trunk/robot_models/robot_mechanism_model/Makefile
pkg/trunk/robot_models/robot_mechanism_model/include/
pkg/trunk/robot_models/robot_mechanism_model/include/mechanism_model/
pkg/trunk/robot_models/robot_mechanism_model/include/mechanism_model/joint.h
pkg/trunk/robot_models/robot_mechanism_model/include/mechanism_model/link.h
pkg/trunk/robot_models/robot_mechanism_model/include/mechanism_model/robot.h
pkg/trunk/robot_models/robot_mechanism_model/include/mechanism_model/transmission.h
pkg/trunk/robot_models/robot_mechanism_model/manifest.xml
pkg/trunk/robot_models/robot_mechanism_model/src/
pkg/trunk/robot_models/robot_mechanism_model/src/CoupledTransmission.cpp
pkg/trunk/robot_models/robot_mechanism_model/src/SimpleTransmission.cpp
Removed Paths:
-------------
pkg/trunk/mechanism/robot_model/
Modified: pkg/trunk/controllers/genericControllers/include/genericControllers/JointController.h
===================================================================
--- pkg/trunk/controllers/genericControllers/include/genericControllers/JointController.h 2008-07-18 18:55:15 UTC (rev 1772)
+++ pkg/trunk/controllers/genericControllers/include/genericControllers/JointController.h 2008-07-18 19:14:17 UTC (rev 1773)
@@ -74,7 +74,7 @@
#include <genericControllers/Controller.h>
#include <genericControllers/Pid.h>
#include <math_utils/angles.h>
-#include <robot_model/joint.h>
+#include <mechanism_model/joint.h>
#include <string>
using namespace mechanism;
Modified: pkg/trunk/controllers/genericControllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/genericControllers/manifest.xml 2008-07-18 18:55:15 UTC (rev 1772)
+++ pkg/trunk/controllers/genericControllers/manifest.xml 2008-07-18 19:14:17 UTC (rev 1773)
@@ -6,7 +6,7 @@
<license>BSD</license>
<depend package="pr2Core" />
<depend package="math_utils" />
- <depend package="robot_model" />
+ <depend package="robot_mechanism_model" />
<url>http://pr.willowgarage.com</url>
<repository>http://pr.willowgarage.com/repos</repository>
<export>
Modified: pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/BaseController.h
===================================================================
--- pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/BaseController.h 2008-07-18 18:55:15 UTC (rev 1772)
+++ pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/BaseController.h 2008-07-18 19:14:17 UTC (rev 1773)
@@ -23,8 +23,8 @@
#include <genericControllers/Controller.h>
#include <genericControllers/JointController.h>
-#include <robot_model/joint.h>
-#include <robot_model/robot.h>
+#include <mechanism_model/joint.h>
+#include <mechanism_model/robot.h>
using namespace std;
Modified: pkg/trunk/controllers/pr2Controllers/src/BaseController.cpp
===================================================================
--- pkg/trunk/controllers/pr2Controllers/src/BaseController.cpp 2008-07-18 18:55:15 UTC (rev 1772)
+++ pkg/trunk/controllers/pr2Controllers/src/BaseController.cpp 2008-07-18 19:14:17 UTC (rev 1773)
@@ -6,7 +6,7 @@
#include <sys/time.h>
-#include <robot_model/joint.h>
+#include <mechanism_model/joint.h>
#define BASE_NUM_JOINTS 12
Modified: pkg/trunk/controllers/rosControllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/rosControllers/manifest.xml 2008-07-18 18:55:15 UTC (rev 1772)
+++ pkg/trunk/controllers/rosControllers/manifest.xml 2008-07-18 19:14:17 UTC (rev 1773)
@@ -12,7 +12,7 @@
<depend package="math_utils" />
<depend package="string_utils" />
<depend package="libKDL" />
- <depend package="robot_model" />
+ <depend package="robot_mechanism_model" />
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lRosJointController"/>
</export>
Modified: pkg/trunk/controllers/testControllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/testControllers/manifest.xml 2008-07-18 18:55:15 UTC (rev 1772)
+++ pkg/trunk/controllers/testControllers/manifest.xml 2008-07-18 19:14:17 UTC (rev 1773)
@@ -7,7 +7,7 @@
<depend package="pr2Controllers" />
<depend package="genericControllers" />
<depend package="hw_interface" />
- <depend package="robot_model" />
+ <depend package="robot_mechanism_model" />
<url>http://pr.willowgarage.com</url>
<repository>http://pr.willowgarage.com/repos</repository>
<export>
Modified: pkg/trunk/controllers/testControllers/src/test_base.cpp
===================================================================
--- pkg/trunk/controllers/testControllers/src/test_base.cpp 2008-07-18 18:55:15 UTC (rev 1772)
+++ pkg/trunk/controllers/testControllers/src/test_base.cpp 2008-07-18 19:14:17 UTC (rev 1773)
@@ -2,7 +2,7 @@
#include <hw_interface/hardware_interface.h>
#include <pr2Controllers/BaseController.h>
#include <genericControllers/JointController.h>
-#include <robot_model/joint.h>
+#include <mechanism_model/joint.h>
#include <sys/time.h>
#include <signal.h>
Modified: pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h 2008-07-18 18:55:15 UTC (rev 1772)
+++ pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h 2008-07-18 19:14:17 UTC (rev 1773)
@@ -43,7 +43,7 @@
#include <string>
#include <list>
#include <vector>
-#include <robot_model/joint.h>
+#include <mechanism_model/joint.h>
namespace PR2
{
Modified: pkg/trunk/drivers/robot/pr2/libpr2HW/manifest.xml
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2HW/manifest.xml 2008-07-18 18:55:15 UTC (rev 1772)
+++ pkg/trunk/drivers/robot/pr2/libpr2HW/manifest.xml 2008-07-18 19:14:17 UTC (rev 1773)
@@ -11,7 +11,7 @@
<depend package="gazebo"/>
<depend package="pr2Core"/>
<depend package="libKinematics"/>
-<depend package="robot_model"/>
+<depend package="robot_mechanism_model"/>
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lpr2HW"/>
</export>
Modified: pkg/trunk/mechanism/mechanism_control/include/mechanism_control/base_control.h
===================================================================
--- pkg/trunk/mechanism/mechanism_control/include/mechanism_control/base_control.h 2008-07-18 18:55:15 UTC (rev 1772)
+++ pkg/trunk/mechanism/mechanism_control/include/mechanism_control/base_control.h 2008-07-18 19:14:17 UTC (rev 1773)
@@ -28,7 +28,7 @@
#ifndef MECHANISM_CONTROL_H
#define MECHANISM_CONTROL_H
-#include <robot_model/robot.h>
+#include <mechanism_model/robot.h>
#include <rosthread/mutex.h>
#include <map>
#include <pr2Controllers/BaseController.h>
Modified: pkg/trunk/mechanism/mechanism_control/manifest.xml
===================================================================
--- pkg/trunk/mechanism/mechanism_control/manifest.xml 2008-07-18 18:55:15 UTC (rev 1772)
+++ pkg/trunk/mechanism/mechanism_control/manifest.xml 2008-07-18 19:14:17 UTC (rev 1773)
@@ -8,7 +8,7 @@
<depend package="rosthread"/>
<depend package="std_msgs"/>
<depend package="hw_interface"/>
-<depend package="robot_model"/>
+<depend package="robot_mechanism_model"/>
<depend package="pr2Controllers"/>
<depend package="etherdrive_hardware"/>
Modified: pkg/trunk/robot/pr2_gazebo/src/test2.cpp
===================================================================
--- pkg/trunk/robot/pr2_gazebo/src/test2.cpp 2008-07-18 18:55:15 UTC (rev 1772)
+++ pkg/trunk/robot/pr2_gazebo/src/test2.cpp 2008-07-18 19:14:17 UTC (rev 1773)
@@ -25,7 +25,7 @@
// robot model
#include <pr2Core/pr2Core.h>
-#include <robot_model/joint.h>
+#include <mechanism_model/joint.h>
// gazebo hardware, sensors
#include <gazebo_hardware/gazebo_hardware.h>
Added: pkg/trunk/robot_models/robot_mechanism_model/CMakeLists.txt
===================================================================
--- pkg/trunk/robot_models/robot_mechanism_model/CMakeLists.txt (rev 0)
+++ pkg/trunk/robot_models/robot_mechanism_model/CMakeLists.txt 2008-07-18 19:14:17 UTC (rev 1773)
@@ -0,0 +1,4 @@
+cmake_minimum_required(VERSION 2.6)
+include(rosbuild)
+rospack(robot_mechanism_model)
+rospack_add_library(mechanism_model src/SimpleTransmission.cpp)
Added: pkg/trunk/robot_models/robot_mechanism_model/Makefile
===================================================================
--- pkg/trunk/robot_models/robot_mechanism_model/Makefile (rev 0)
+++ pkg/trunk/robot_models/robot_mechanism_model/Makefile 2008-07-18 19:14:17 UTC (rev 1773)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Added: pkg/trunk/robot_models/robot_mechanism_model/include/mechanism_model/joint.h
===================================================================
--- pkg/trunk/robot_models/robot_mechanism_model/include/mechanism_model/joint.h (rev 0)
+++ pkg/trunk/robot_models/robot_mechanism_model/include/mechanism_model/joint.h 2008-07-18 19:14:17 UTC (rev 1773)
@@ -0,0 +1,57 @@
+
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2008, Eric Berger
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of Stanford University nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+#ifndef JOINT_H
+#define JOINT_H
+
+namespace mechanism {
+ class Joint{
+ public:
+
+ void enforceLimits();
+
+ char *name;
+ int type;
+
+ //Update every cycle from input data
+ bool initialized;
+ double position;
+ double velocity;
+ double appliedEffort;
+
+ //Written every cycle out to motor boards
+ double commandedEffort;
+
+ //Never changes
+ double jointLimitMin;
+ double jointLimitMax;
+ double effortLimit;
+ double velocityLimit;
+ };
+}
+
+#endif
Added: pkg/trunk/robot_models/robot_mechanism_model/include/mechanism_model/link.h
===================================================================
--- pkg/trunk/robot_models/robot_mechanism_model/include/mechanism_model/link.h (rev 0)
+++ pkg/trunk/robot_models/robot_mechanism_model/include/mechanism_model/link.h 2008-07-18 19:14:17 UTC (rev 1773)
@@ -0,0 +1,41 @@
+
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2008, Eric Berger
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of Stanford University nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+#ifndef LINK_H
+#define LINK_H
+
+#include "mechanism_model/joint.h"
+
+namespace mechanism {
+ class Link{
+ char *name;
+ //Pointer to KDL link
+ Joint *joint;
+ };
+}
+
+#endif
Added: pkg/trunk/robot_models/robot_mechanism_model/include/mechanism_model/robot.h
===================================================================
--- pkg/trunk/robot_models/robot_mechanism_model/include/mechanism_model/robot.h (rev 0)
+++ pkg/trunk/robot_models/robot_mechanism_model/include/mechanism_model/robot.h 2008-07-18 19:14:17 UTC (rev 1773)
@@ -0,0 +1,53 @@
+
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2008, Eric Berger
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of Stanford University nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+//The robot model is populated by the control code infrastructure and used by all the controllers to read mechanism state and command mechanism motion.
+
+#ifndef ROBOT_H
+#define ROBOT_H
+
+#include "mechanism_model/link.h"
+#include "mechanism_model/joint.h"
+#include "mechanism_model/transmission.h"
+
+namespace mechanism {
+ class Robot{
+ public:
+ Robot(char *ns){};
+ ~Robot(){};
+ char *name;
+ Link *link;
+ int numLinks;
+ Joint *joint;
+ int numJoints;
+ SimpleTransmission *transmission;
+ int numTransmissions;
+ };
+}
+
+#endif
Added: pkg/trunk/robot_models/robot_mechanism_model/include/mechanism_model/transmission.h
===================================================================
--- pkg/trunk/robot_models/robot_mechanism_model/include/mechanism_model/transmission.h (rev 0)
+++ pkg/trunk/robot_models/robot_mechanism_model/include/mechanism_model/transmission.h 2008-07-18 19:14:17 UTC (rev 1773)
@@ -0,0 +1,99 @@
+
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2008, Eric Berger
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of Stanford University nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+#ifndef TRANSMISSION_H
+#define TRANSMISSION_H
+
+#include "mechanism_model/joint.h"
+#include "hw_interface/hardware_interface.h"
+
+namespace mechanism {
+
+ /* class Transmission{
+
+ public:
+
+ Transmission(){};
+
+ virtual ~Transmission(){};
+
+ virtual void propagatePosition(); //Use encoder data to fill out joint position and velocities
+
+ virtual void propagateEffort(); //Use commanded joint efforts to fill out commanded motor currents
+
+ };
+ */
+ class SimpleTransmission{
+
+ public:
+
+ SimpleTransmission(Joint *joint, Actuator *actuator, double mechanicalReduction, double motorTorqueConstant, double ticksPerRadian);
+
+ SimpleTransmission(){};
+
+ ~SimpleTransmission(){};
+
+ Actuator *actuator;
+
+ Joint *joint;
+
+ double mechanicalReduction;
+
+ double motorTorqueConstant;
+
+ double pulsesPerRevolution;
+
+ void propagatePosition();
+
+ void propagateEffort();
+ };
+ /*
+ class CoupledTransmission : public Transmission{
+
+ public:
+
+ void CoupledTranmission(Actuator *actuator, Joint *joint, double mechanicalReduction, double motorTorqueConstant);
+
+ };
+
+ class NonlinearTransmission : public Transmission{
+
+ public:
+
+ NonlinearTransmission(Actuator *actuator, Joint *joint, double mechanicalReduction, double motorTorqueConstant);
+
+ Actuator *actuator;
+
+ Joint *joint;
+
+ // ?? Lookup table
+ };
+ */
+}
+
+#endif
Added: pkg/trunk/robot_models/robot_mechanism_model/manifest.xml
===================================================================
--- pkg/trunk/robot_models/robot_mechanism_model/manifest.xml (rev 0)
+++ pkg/trunk/robot_models/robot_mechanism_model/manifest.xml 2008-07-18 19:14:17 UTC (rev 1773)
@@ -0,0 +1,11 @@
+<package>
+<description brief="Mechanism model">
+</description>
+<author>Eric Berger be...@wi...</author>
+<license>BSD</license>
+<depend package='hw_interface'/>
+<url>http://pr.willowgarage.com</url>
+<export>
+ <cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lmechanism_model -Wl,-rpath,${prefix}/lib"/>
+</export>
+</package>
Added: pkg/trunk/robot_models/robot_mechanism_model/src/CoupledTransmission.cpp
===================================================================
--- pkg/trunk/robot_models/robot_mechanism_model/src/CoupledTransmission.cpp (rev 0)
+++ pkg/trunk/robot_models/robot_mechanism_model/src/CoupledTransmission.cpp 2008-07-18 19:14:17 UTC (rev 1773)
@@ -0,0 +1,46 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2008, Sachin Chitta, Jimmy Sastra
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of Stanford University nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+// Simple transmission implementation
+#include <mechanism/transmission.h>
+
+CoupledTransmission(Actuator *actuator, Joint *joint, double mechanicalReduction, double motorTorqueConstant){
+ this->actuator = actuator;
+ this->joint = joint;
+ this->mechanicalReduction = mechanicalReduction;
+ this->motorTorqueConstant = motorTorqueConstant;
+ this->ticksPerRadian = ticksPerRadian;
+}
+
+void CoupledTransmission::propagatePosition(){
+ this->joint.position = this->ticksPerRadian * this->actuator.state.encoderCount;
+ this->joint.appliedEffort = this->actuator.lastMeasuredCurrent * (motorTorqueConstant * mechanicalReduction);
+}
+
+void CoupledTransmission::propagateEffort(){
+ this->actuator.command.current = this->joint.commandedEffort/(motorTorqueConstant * mechanicalReduction);
+}
Added: pkg/trunk/robot_models/robot_mechanism_model/src/SimpleTransmission.cpp
===================================================================
--- pkg/trunk/robot_models/robot_mechanism_model/src/SimpleTransmission.cpp (rev 0)
+++ pkg/trunk/robot_models/robot_mechanism_model/src/SimpleTransmission.cpp 2008-07-18 19:14:17 UTC (rev 1773)
@@ -0,0 +1,57 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2008, Sachin Chitta, Jimmy Sastra
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of Stanford University nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+// Simple transmission implementation
+#include <mechanism_model/transmission.h>
+#include <mechanism_model/joint.h>
+#include <math.h>
+#include <stdio.h>
+
+using namespace mechanism;
+
+SimpleTransmission::SimpleTransmission(Joint *joint, Actuator *actuator, double mechanicalReduction, double motorTorqueConstant, double pulsesPerRevolution)
+{
+ this->joint = joint;
+ this->actuator = actuator;
+ this->mechanicalReduction = mechanicalReduction;
+ this->motorTorqueConstant = motorTorqueConstant;
+ this->pulsesPerRevolution = pulsesPerRevolution;
+}
+
+
+void SimpleTransmission::propagatePosition(){
+ this->joint->position = ((double)this->actuator->state.encoderCount*2*M_PI)/(pulsesPerRevolution * mechanicalReduction);
+ this->joint->velocity = ((double)this->actuator->state.encoderVelocity*2*M_PI)/(pulsesPerRevolution * mechanicalReduction);
+ this->joint->appliedEffort = this->actuator->state.lastMeasuredCurrent * (motorTorqueConstant * mechanicalReduction);
+}
+
+void SimpleTransmission::propagateEffort(){
+ this->actuator->command.current = this->joint->commandedEffort/(motorTorqueConstant * mechanicalReduction);
+#ifdef DEBUG
+ printf("Transmission:: %f , %f\n",this->joint->commandedEffort, this->actuator->command.current);
+#endif
+}
Modified: pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h
===================================================================
--- pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h 2008-07-18 18:55:15 UTC (rev 1772)
+++ pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h 2008-07-18 19:14:17 UTC (rev 1773)
@@ -35,7 +35,7 @@
#include <pr2Controllers/GripperController.h>
#include "ringbuffer.h"
-#include "robot_model/joint.h"
+#include "mechanism_model/joint.h"
// roscpp
#include <ros/node.h>
// roscpp - laser
Modified: pkg/trunk/simulators/rosgazebo/manifest.xml
===================================================================
--- pkg/trunk/simulators/rosgazebo/manifest.xml 2008-07-18 18:55:15 UTC (rev 1772)
+++ pkg/trunk/simulators/rosgazebo/manifest.xml 2008-07-18 19:14:17 UTC (rev 1773)
@@ -15,7 +15,7 @@
<depend package="math_utils" />
<depend package="string_utils" />
<depend package="libKDL" />
- <depend package="robot_model" />
+ <depend package="robot_mechanism_model" />
<depend package="hw_interface" />
<depend package="gazebo_hardware" />
<depend package="gazebo_sensors" />
Modified: pkg/trunk/simulators/rosgazebo/src/rosgazebo.cc
===================================================================
--- pkg/trunk/simulators/rosgazebo/src/rosgazebo.cc 2008-07-18 18:55:1...
[truncated message content] |
|
From: <is...@us...> - 2008-07-18 21:09:51
|
Revision: 1777
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1777&view=rev
Author: isucan
Date: 2008-07-18 21:09:58 +0000 (Fri, 18 Jul 2008)
Log Message:
-----------
reorganized collision space (hopefully, one of the last iterations)
Modified Paths:
--------------
pkg/trunk/robot_models/robot_motion_planning_models/include/planning_models/kinematic.h
pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp
Added Paths:
-----------
pkg/trunk/util/collision_space/src/collision_space/environment.cpp
Modified: pkg/trunk/robot_models/robot_motion_planning_models/include/planning_models/kinematic.h
===================================================================
--- pkg/trunk/robot_models/robot_motion_planning_models/include/planning_models/kinematic.h 2008-07-18 21:09:22 UTC (rev 1776)
+++ pkg/trunk/robot_models/robot_motion_planning_models/include/planning_models/kinematic.h 2008-07-18 21:09:58 UTC (rev 1777)
@@ -44,7 +44,7 @@
A class describing a kinematic robot model loaded from URDF */
-namespace robot_models
+namespace planning_models
{
Modified: pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp
===================================================================
--- pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp 2008-07-18 21:09:22 UTC (rev 1776)
+++ pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp 2008-07-18 21:09:58 UTC (rev 1777)
@@ -35,13 +35,13 @@
#include <planning_models/kinematic.h>
#include <cstdio>
-void robot_models::KinematicModel::Robot::computeTransforms(const double *params)
+void planning_models::KinematicModel::Robot::computeTransforms(const double *params)
{
chain->computeTransform(params);
}
// we can optimize things here... (when we use identity transforms, for example)
-void robot_models::KinematicModel::Joint::computeTransform(const double *params)
+void planning_models::KinematicModel::Joint::computeTransform(const double *params)
{
switch (type)
{
@@ -73,7 +73,7 @@
after->computeTransform(params);
}
-void robot_models::KinematicModel::Link::computeTransform(const double *params)
+void planning_models::KinematicModel::Link::computeTransform(const double *params)
{
globalTrans = before->globalTrans;
globalTrans.multiplyPose(constTrans);
@@ -82,7 +82,7 @@
globalTrans.multiplyPose(constGeomTrans);
}
-void robot_models::KinematicModel::build(robot_desc::URDF &model, const char *group)
+void planning_models::KinematicModel::build(robot_desc::URDF &model, const char *group)
{
if (group)
{
@@ -110,17 +110,17 @@
}
}
-unsigned int robot_models::KinematicModel::getRobotCount(void) const
+unsigned int planning_models::KinematicModel::getRobotCount(void) const
{
return m_robots.size();
}
-robot_models::KinematicModel::Robot* robot_models::KinematicModel::getRobot(unsigned int index) const
+planning_models::KinematicModel::Robot* planning_models::KinematicModel::getRobot(unsigned int index) const
{
return m_robots[index];
}
-void robot_models::KinematicModel::buildChain(Robot *robot, Link *parent, Joint* joint, robot_desc::URDF::Link* urdfLink)
+void planning_models::KinematicModel::buildChain(Robot *robot, Link *parent, Joint* joint, robot_desc::URDF::Link* urdfLink)
{
joint->usedParamStart = robot->stateDimension;
joint->before = parent;
@@ -168,7 +168,7 @@
buildChain(robot, joint, joint->after, urdfLink);
}
-void robot_models::KinematicModel::buildChain(Robot *robot, Joint *parent, Link* link, robot_desc::URDF::Link* urdfLink)
+void planning_models::KinematicModel::buildChain(Robot *robot, Joint *parent, Link* link, robot_desc::URDF::Link* urdfLink)
{
link->before = parent;
robot->links.push_back(link);
Added: pkg/trunk/util/collision_space/src/collision_space/environment.cpp
===================================================================
--- pkg/trunk/util/collision_space/src/collision_space/environment.cpp (rev 0)
+++ pkg/trunk/util/collision_space/src/collision_space/environment.cpp 2008-07-18 21:09:58 UTC (rev 1777)
@@ -0,0 +1,44 @@
+/*********************************************************************
+* 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 <collision_space/environment.h>
+
+unsigned int collision_space::EnvironmentModel::addRobotModel(robot_desc::URDF &pmodel, const char *group)
+{
+ planning_models::KinematicModel *m = new planning_models::KinematicModel();
+ m->build(pmodel, group);
+ unsigned int pos = models.size();
+ models.push_back(m);
+ return pos;
+}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-07-18 23:45:33
|
Revision: 1794
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1794&view=rev
Author: isucan
Date: 2008-07-18 23:45:42 +0000 (Fri, 18 Jul 2008)
Log Message:
-----------
more incremental updates
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/robot_models/robot_motion_planning_models/include/planning_models/kinematic.h
pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-07-18 23:27:55 UTC (rev 1793)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-07-18 23:45:42 UTC (rev 1794)
@@ -139,7 +139,7 @@
{
// const int dim = req.start_state.vals_size;
// ompl::SpaceInformationKinematic::GoalStateKinematic_t goal = new ompl::SpaceInformationKinematic::GoalStateKinematic(m_si);
- // m_si->setGoal(goal);
+ //m_si->setGoal(goal);
/*
std::vector<double*> path;
@@ -189,16 +189,7 @@
model->collisionSpaceID = m_collisionSpace->addRobotModel(*file, groups[i].c_str());
m_models[name + "::" + groups[i]] = model;
createMotionPlanningInstances(model);
-
- // pass it to the collision space to create one more kinematic model
- // find the state dimension & bounding box, set up the space information
- // create a motion planner for this space info
-
-
- // todo: kinematic model should print what each state dimension means
-
- }
-
+ }
}
private:
@@ -232,6 +223,30 @@
unsigned int collisionSpaceID;
};
+ class SpaceInformationNode : public ompl::SpaceInformationKinematic
+ {
+ public:
+ SpaceInformationNode(planning_models::KinematicModel *km) : SpaceInformationKinematic()
+ {
+ m_stateDimension = km->stateDimension;
+ m_stateComponent.resize(m_stateDimension);
+
+ for (unsigned int i = 0 ; i < m_stateDimension ; ++i)
+ {
+ m_stateComponent[i].type = StateComponent::NORMAL;
+ m_stateComponent[i].minValue = km->stateBounds[i*2 ];
+ m_stateComponent[i].maxValue = km->stateBounds[i*2 + 1];
+ m_stateComponent[i].resolution = (m_stateComponent[i].maxValue - m_stateComponent[i].minValue) / 20.0;
+ if (m_stateComponent[i].resolution == 0.0)
+ m_stateComponent[i].resolution = 0.1; // this happens for floating joints
+ }
+ }
+
+ virtual ~SpaceInformationNode(void)
+ {
+ }
+ };
+
std_msgs::PointCloudFloat32 m_cloud;
collision_space::EnvironmentModel *m_collisionSpace;
std::map<std::string, Model*> m_models;
@@ -240,24 +255,19 @@
static bool isStateValid(const ompl::SpaceInformationKinematic::StateKinematic_t state, void *data)
{
Model *model = reinterpret_cast<Model*>(data);
-
- return false;
+ return model->collisionSpace->isCollision(model->collisionSpaceID);
}
void createMotionPlanningInstances(Model* model)
{
+ model->collisionSpace = m_collisionSpace;
+
Planner p;
- p.si = new ompl::SpaceInformationKinematic();
+ p.si = new SpaceInformationNode(m_collisionSpace->models[model->collisionSpaceID]);
+ p.si->setStateValidFn(isStateValid, reinterpret_cast<void*>(model));
p.mp = new ompl::RRT(p.si);
- p.type = 0;
-
+ p.type = 0;
model->planners.push_back(p);
- model->collisionSpace = m_collisionSpace;
-
- planning_models::KinematicModel *km = m_collisionSpace->models[model->collisionSpaceID];
- p.si->setStateValidFn(isStateValid, reinterpret_cast<void*>(model));
-
-
}
};
Modified: pkg/trunk/robot_models/robot_motion_planning_models/include/planning_models/kinematic.h
===================================================================
--- pkg/trunk/robot_models/robot_motion_planning_models/include/planning_models/kinematic.h 2008-07-18 23:27:55 UTC (rev 1793)
+++ pkg/trunk/robot_models/robot_motion_planning_models/include/planning_models/kinematic.h 2008-07-18 23:45:42 UTC (rev 1794)
@@ -204,7 +204,8 @@
KinematicModel(void)
{
- m_verbose = false;
+ stateDimension = 0;
+ m_verbose = true;
}
virtual ~KinematicModel(void)
@@ -215,14 +216,23 @@
virtual void build(robot_desc::URDF &model, const char *group = NULL);
+ void setVerbose(bool verbose);
+
unsigned int getRobotCount(void) const;
Robot* getRobot(unsigned int index) const;
+ void computeTransforms(const double *params);
+
+ /** Cumulative state dimension */
+ unsigned int stateDimension;
+ /** Cumulative state bounds */
+ std::vector<double> stateBounds;
+
protected:
std::vector<Robot*> m_robots;
bool m_verbose;
-
+
private:
void buildChain(Robot *robot, Link *parent, Joint *joint, robot_desc::URDF::Link *urdfLink);
Modified: pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp
===================================================================
--- pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp 2008-07-18 23:27:55 UTC (rev 1793)
+++ pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp 2008-07-18 23:45:42 UTC (rev 1794)
@@ -40,6 +40,12 @@
chain->computeTransform(params);
}
+void planning_models::KinematicModel::computeTransforms(const double *params)
+{
+ for (unsigned int i = 0 ; i < m_robots.size() ; ++i)
+ m_robots[i]->chain->computeTransform(params + i);
+}
+
// we can optimize things here... (when we use identity transforms, for example)
void planning_models::KinematicModel::Joint::computeTransform(const double *params)
{
@@ -82,6 +88,11 @@
globalTrans.multiplyPose(constGeomTrans);
}
+void planning_models::KinematicModel::setVerbose(bool verbose)
+{
+ m_verbose = verbose;
+}
+
void planning_models::KinematicModel::build(robot_desc::URDF &model, const char *group)
{
if (group)
@@ -108,6 +119,12 @@
m_robots.push_back(rb);
}
}
+
+ for (unsigned int i = 0 ; i < m_robots.size() ; ++i)
+ {
+ stateDimension += m_robots[i]->stateDimension;
+ stateBounds.insert(stateBounds.end(), m_robots[i]->stateBounds.begin(), m_robots[i]->stateBounds.end());
+ }
}
unsigned int planning_models::KinematicModel::getRobotCount(void) const
@@ -164,6 +181,15 @@
break;
}
joint->active = joint->usedParamEnd > joint->usedParamStart;
+ if (m_verbose && joint->usedParamEnd > joint->usedParamStart)
+ {
+ printf("Joint '%s' connects link '%s' to link '%s' and uses state coordinates: ",
+ urdfLink->joint->name.c_str(), urdfLink->parentName.c_str(), urdfLink->name.c_str());
+ for (unsigned int i = joint->usedParamStart ; i < joint->usedParamEnd ; ++i)
+ printf("%d ", i);
+ printf("\n");
+ }
+
robot->stateDimension = joint->usedParamEnd;
buildChain(robot, joint, joint->after, urdfLink);
}
@@ -173,9 +199,6 @@
link->before = parent;
robot->links.push_back(link);
- if (m_verbose)
- printf("Created link '%s'\n", urdfLink->name.c_str());
-
/* copy relevant data */
switch (urdfLink->collision->geometry->type)
{
@@ -220,8 +243,6 @@
if (!found)
continue;
}
- if (m_verbose)
- printf("Connecting to link '%s'\n", urdfLink->children[i]->name.c_str());
Joint *newJoint = new Joint();
buildChain(robot, link, newJoint, urdfLink->children[i]);
link->after.push_back(newJoint);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-07-21 20:35:57
|
Revision: 1847
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1847&view=rev
Author: hsujohnhsu
Date: 2008-07-21 20:36:06 +0000 (Mon, 21 Jul 2008)
Log Message:
-----------
moving robot description parser.
Added Paths:
-----------
pkg/trunk/robot_descriptions/
pkg/trunk/robot_descriptions/wg_robot_description_parser/
Removed Paths:
-------------
pkg/trunk/util/wg_robot_desc/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-07-21 20:42:11
|
Revision: 1848
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1848&view=rev
Author: hsujohnhsu
Date: 2008-07-21 20:42:17 +0000 (Mon, 21 Jul 2008)
Log Message:
-----------
moving xml files to a better place.
Added Paths:
-----------
pkg/trunk/robot_descriptions/wg_robot_description/
pkg/trunk/robot_descriptions/wg_robot_description/pr2/
pkg/trunk/robot_descriptions/wg_robot_description/pr2/actuators.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/cameras.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
pkg/trunk/robot_descriptions/wg_robot_description/send.xml
Removed Paths:
-------------
pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/actuators.xml
pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/cameras.xml
pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2.xml
pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/send.xml
pkg/trunk/robot_descriptions/wg_robot_description_parser/src/urdf/MathExpression.cpp
Deleted: pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/actuators.xml
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/actuators.xml 2008-07-21 20:36:06 UTC (rev 1847)
+++ pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/actuators.xml 2008-07-21 20:42:17 UTC (rev 1848)
@@ -1,35 +0,0 @@
-<?xml version="1.0"?>
-
-<robot name="pr2">
-
- <joint name="joint_shoulder_pan_left">
- <actuator name="actuator_shoulder_pan_left_1">
- <motor>MAXON</motor>
- <ip> IP_ADDRESS </ip>
- <port> 0 </port>
- <reduction>10</reduction>
- <polymap>-1 -0.2 -0.5 </polymap>
- </actuator>
-
- <actuator name="actuator_shoulder_pan_left_2">
- <motor>MAXON</motor>
- <ip> IP_ADDRESS </ip>
- <port> 0 </port>
- <reduction>10</reduction>
- <polymap>1 0.2 0.5 </polymap>
- </actuator>
-
- </joint>
-
-
- <joint name="joint_left_gripper">
- <actuator name="">
- <motor>MAXON</motor>
- <ip> IP_ADDRESS </ip>
- <port> 0 </port>
- <reduction>10</reduction>
- <lookup> gripper_lookup </lookup>
- </actuator>
- </joint>
-
-</robot>
Deleted: pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/cameras.xml
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/cameras.xml 2008-07-21 20:36:06 UTC (rev 1847)
+++ pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/cameras.xml 2008-07-21 20:42:17 UTC (rev 1848)
@@ -1,39 +0,0 @@
-<params>
- <double name="camera_models/videre_left/width"> 640 </double>
- <double name="camera_models/videre_left/height"> 480 </double>
- <double name="camera_models/videre_left/focal_x"> 962.472 </double>
- <double name="camera_models/videre_left/focal_y"> 962.961 </double>
- <double name="camera_models/videre_left/center_x"> 312.410 </double>
- <double name="camera_models/videre_left/center_y"> 242.114 </double>
- <double name="camera_models/videre_left/radial_1"> -0.441686 </double>
- <double name="camera_models/videre_left/radial_2"> 0.302547 </double>
-
- <double name="camera_models/videre_right/width"> 640 </double>
- <double name="camera_models/videre_right/height"> 480 </double>
- <double name="camera_models/videre_right/focal_x"> 947.233 </double>
- <double name="camera_models/videre_right/focal_y"> 946.236 </double>
- <double name="camera_models/videre_right/center_x"> 337.313 </double>
- <double name="camera_models/videre_right/center_y"> 223.764 </double>
- <double name="camera_models/videre_right/radial_1"> -0.419809 </double>
- <double name="camera_models/videre_right/radial_2"> 0.246824 </double>
-
- <double name="camera_models/videre_stereo/translation_x"> .0606940 </double>
- <double name="camera_models/videre_stereo/translation_y"> -.000401150 </double>
- <double name="camera_models/videre_stereo/translation_z"> -.00225697 </double>
-
- <double name="camera_models/videre_stereo/rotation_0_0"> 0.999802 </double>
- <double name="camera_models/videre_stereo/rotation_1_0"> -0.000745101 </double>
- <double name="camera_models/videre_stereo/rotation_2_0"> 0.0198657 </double>
-
- <double name="camera_models/videre_stereo/rotation_0_1"> 0.000869400 </double>
- <double name="camera_models/videre_stereo/rotation_1_1"> 0.999980 </double>
- <double name="camera_models/videre_stereo/rotation_2_1"> -0.00624909 </double>
-
- <double name="camera_models/videre_stereo/rotation_0_2"> -0.0198607 </double>
- <double name="camera_models/videre_stereo/rotation_1_2"> 0.00626513 </double>
- <double name="camera_models/videre_stereo/rotation_2_2"> 0.999783 </double>
-
- <string name="camera_models/videre_stereo/left_cam"> videre_left </string>
- <string name="camera_models/videre_stereo/right_cam"> videre_right </string>
-</params>
-
Deleted: pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2.xml
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2.xml 2008-07-21 20:36:06 UTC (rev 1847)
+++ pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2.xml 2008-07-21 20:42:17 UTC (rev 1848)
@@ -1,872 +0,0 @@
-<?xml version="1.0"?>
-
-<robot name="pr2"><!-- name of the robot-->
-
-<!-- Begin template definitions for ease of reuse -->
-
-
-<!-- Begin constant dimensions -->
- <constants>
-
- <wheel_length>0.03</wheel_length>
- <wheel_radius>0.08</wheel_radius>
-
- <shoulder_pan_length>0.60</shoulder_pan_length>
- <shoulder_pan_radius>0.13</shoulder_pan_radius>
-
- <shoulder_pitch_length>0.10</shoulder_pitch_length>
- <shoulder_pitch_radius>0.12</shoulder_pitch_radius>
-
- <shoulder_roll_x>0.35</shoulder_roll_x>
- <shoulder_roll_y>0.12</shoulder_roll_y>
- <shoulder_roll_z>0.15</shoulder_roll_z>
-
- <elbow_pitch_length>0.08</elbow_pitch_length>
- <elbow_pitch_radius>0.1</elbow_pitch_radius>
-
- <forearm_roll_x>0.27</forearm_roll_x>
- <forearm_roll_y>0.12</forearm_roll_y>
- <forearm_roll_z>0.08</forearm_roll_z>
-
- <wrist_pitch_length>0.031</wrist_pitch_length>
- <wrist_pitch_radius>0.1</wrist_pitch_radius>
-
- <wrist_roll_x>0.11</wrist_roll_x>
- <wrist_roll_y>0.10</wrist_roll_y>
- <wrist_roll_z>0.05</wrist_roll_z>
-
- <base_length>0.5</base_length>
- <base_width>0.5</base_width>
- <base_height>0.1</base_height>
-
- <wheel_clearance_offset>0.02</wheel_clearance_offset> <!-- wheel clearance from bottom of base to ground-->
-
- <torso_length>0.3</torso_length>
- <torso_width>0.05</torso_width>
- <torso_height>0.8</torso_height>
-
- <head_pan_length>0.1</head_pan_length>
- <head_pan_radius>0.5</head_pan_radius>
-
- <head_tilt_length>0.25</head_tilt_length>
- <head_tilt_width>0.1</head_tilt_width>
- <head_tilt_height>0.15</head_tilt_height>
-
- <base_torso_offset_x>-0.05</base_torso_offset_x>
- <base_torso_offset_y>0.0</base_torso_offset_y>
- <base_torso_offset_z>0.30981</base_torso_offset_z>
-
- <torso_head_pan_offset_x>-0.1829361-base_torso_offset_x</torso_head_pan_offset_x>
- <torso_head_pan_offset_y>0</torso_head_pan_offset_y>
- <torso_head_pan_offset_z>0.80981-base_torso_offset_z</torso_head_pan_offset_z>
- </constants>
-<!-- End constant dimensions -->
-
- <templates>
- <define template="pr2_caster_visual">
- <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 -->
- <material>Gazebo/Grey</material>
- <geometry type="mesh" name="pr2_caster_mesh_file">
- <filename>{ROS_PKG}/demos/2dnav-gazebo/world/Media/models/pr2_caster.mesh</filename><!-- mesh specified using an obj file -->
- </geometry>
- </define>
-
- <define template="pr2_caster_collision">
- <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 -->
- <material>Gazebo/Grey</material>
- <geometry type="box"><!-- geometry specified using a simple geometric object -->
- <size>0.1 0.08 0.1</size>
- </geometry>
- </define>
-
- <define template="pr2_caster_inertial">
- <mass> 3.473082 </mass>
- <com> 0.007342 0.00015 0.095236 </com> <!-- position of the center of mass with respect to the link's own anchor in a local coordinate frame -->
- <inertia> 0.012411765597 -0.000711733678 0.00050272983 0.015218160428 -0.000004273467 0.011763977943 </inertia>
- </define>
-
-
- <define template="pr2_caster_joint" type="revolute">
- <axis> 0 0 1 </axis> <!-- direction of the joint in the local coordinate frame -->
- <anchor>0 0 0</anchor> <!-- point on the joint relative to the local coordinate frame -->
- <calibration>1.5 -1 </calibration> <!-- Calibration stop/flag indicating location and then direction -->
- </define>
-
- <define template="pr2_wheel_visual">
- <xyz>0 0 0</xyz> <!-- location defined with respect to the link origin in a local coordinate frame -->
- <rpy>M_PI/2 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 -->
- <material>Gazebo/Grey</material>
- <geometry type="cylinder"><!-- geometry specified using a simple geometric object -->
- <size>wheel_radius wheel_length</size>
- </geometry>
- </define>
-
- <define template="pr2_wheel_inertial">
- <mass> 0.44036 </mass>
- <com> 0.000008 0.000008 0.008475 </com> <!-- position of the center of mass with respect to the link's own anchor in a local coordinate frame -->
- <inertia> 0.012411765597 -0.000711733678 0.00050272983 0.015218160428 -0.000004273467 0.011763977943 </inertia>
- </define>
-
- <define template="pr2_wheel_joint" type="revolute">
- <axis> 0 1 0 </axis> <!-- direction of the joint in the local coordinate frame -->
- <anchor>0 0 0</anchor> <!-- point on the joint relative to the local coordinate frame -->
- <calibration>1.5 -1 </calibration> <!-- Calibration stop/flag indicating location and then direction -->
- </define>
-
- <define template="pr2_shoulder_pan_joint" type="revolute">
- <axis> 0 0 1 </axis> <!-- direction of the joint in a global coordinate frame -->
- <anchor>0 0 0 </anchor> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
- <limit> -M_PI/2 M_PI/2 </limit>
- <calibration>1.5 -1 </calibration> <!-- Calibration stop/flag indicating location and then direction -->
- </define>
-
- <define template="pr2_shoulder_pan_inertial">
- <mass> 16.29553 </mass>
- <com> -0.005215 -0.030552 -0.175743 </com> <!-- position of the center of mass with respect to the link's own anchor in a local coordinate frame -->
- <inertia>0.793291393007 0.003412032973 0.0096614481 0.818419457224 -0.033999499551 0.13915007406 </inertia>
- </define>
-
- <define template="pr2_shoulder_pan_visual">
- <xyz>0 0 shoulder_pan_length/2 </xyz> <!-- location defined with respect to the link origin in a local coordinate frame -->
- <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/Grey</material>
- <geometry type="cylinder">
- <size> shoulder_pan_radius shoulder_pan_length</size>
- </geometry>
- </define>
-
- <define template="pr2_shoulder_pitch_joint" type="revolute">
- <axis> 0 1 0 </axis> <!-- direction of the joint in a global coordinate frame -->
- <anchor>0 0 0 </anchor> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
- <limit> -0.6109 1.3090 </limit>
- <calibration>1.5 -1 </calibration> <!-- Calibration stop/flag indicating location and then direction -->
- </define>
-
- <define template="pr2_shoulder_pitch_inertial">
- <mass> 2.41223 </mass>
- <com> -0.035895 0.014422 -0.028263</com> <!-- position of the center of mass with respect to the link's own anchor in a local coordinate frame -->
- <inertia> 0.016640333585 0.002696462583 0.001337742275 0.017232603914 -0.003605106514 0.018723553425</inertia>
- </define>
-
- <define template="pr2_shoulder_pitch_visual">
- <xyz>shoulder_pitch_length/2 0 0 </xyz> <!-- location defined with respect to the link origin in a local coordinate frame -->
- <rpy>M_PI/2 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 -->
- <material>Gazebo/Grey</material>
- <geometry type="cylinder">
- <size> shoulder_pitch_radius shoulder_pitch_length </size>
- </geometry>
- </define>
-
- <define template="pr2_shoulder_roll_joint" type="revolute">
- <axis> 1 0 0 </axis> <!-- direction of the joint in a global coordinate frame -->
- <anchor>0 0 0 </anchor> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
- <limit> -2.3562 2.3562 </limit>
- <calibration>1.5 -1 </calibration> <!-- Calibration stop/flag indicating location and then direction -->
- </define>
-
- <define template="pr2_shoulder_roll_inertial">
- <mass> 4.9481 </mass>
- <com> 0.001205 -0.016293 0.21227 </com> <!-- position of the center of mass with respect to the link's own anchor in a local coordinate frame -->
- <inertia> 0.073060715309 0.000547745916 0.000119476885 0.072124510748 0.001544932307 0.013383284908</inertia>
- </define>
-
- <define template="pr2_shoulder_roll_visual">
- <xyz>shoulder_roll_x/2 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 -->
- <material>Gazebo/Grey</material>
- <geometry type="box">
- <size> shoulder_roll_x shoulder_roll_y shoulder_roll_z </size>
- </geometry>
- </define>
-
- <define template="pr2_elbow_pitch_joint" type="revolute">
- <axis> 0 1 0 </axis> <!-- direction of the joint in a global coordinate frame -->
- <anchor>0 0 0 </anchor> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
- <limit> -M_PI/2 0.8727 </limit>
- <calibration>1.5 -1 </calibration> <!-- Calibration stop/flag indicating location and then direction -->
- </define>
-
- <define template="pr2_elbow_pitch_inertial">
- <mass> 1.689246 </mass>
- <com> -0.011638 0.013173 0.001228 </com> <!-- position of the center of mass with respect to the link's own anchor in a local coordinate frame -->
- <inertia> 0.003275298548 0.000292046674 -0.000077359282 0.003236815206 -0.000001162155 0.00410053774 </inertia>
- </define>
-
- <define template="pr2_elbow_pitch_visual">
- <xyz>elbow_pitch_length/2 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 -->
- <material>Gazebo/Grey</material>
- <geometry type="cylinder">
- <size>elbow_pitch_radius elbow_pitch_length </size>
- </geometry>
- </define>
-
- <define template="pr2_forearm_roll_joint" type="revolute">
- <axis> 1 0 0 </axis> <!-- direction of the joint in a global coordinate frame -->
- <anchor>0 0 0 </anchor> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
- <calibration>1.5 -1 </calibration> <!-- Calibration stop/flag indicating location and then direction -->
- </define>
-
- <define template="pr2_forearm_roll_inertial">
- <mass> 1.804155 </mass>
- <com> -0.000058 0.013779 0.179474 </com> <!-- position of the center of mass with respect to the link's own anchor in a local coordinate frame -->
- <inertia> 0.012430552544 -0.000003671102 0.000029379389 0.013567548848 -0.000427679042 0.001755089866 </inertia>
- </define>
-
- <define template="pr2_forearm_roll_visual">
- <xyz>forearm_roll_x/2 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 -->
- <material>Gazebo/Grey</material>
- <geometry type="box">
- <size>forearm_roll_x forearm_roll_y forearm_roll_z </size>
- </geometry>
- </define>
-
- <define template="pr2_wrist_pitch_joint" type="revolute">
- <axis> 0 1 0 </axis> <!-- direction of the joint in a global coordinate frame -->
- <anchor>0 0 0 </anchor> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
- <calibration>1.5 -1 </calibration> <!-- Calibration stop/flag indicating location and then direction -->
- </define>
-
- <define template="pr2_wrist_pitch_inertial">
- <mass> 1.804155 </mass>
- <com> -0.000058 0.013779 0.179474 </com> <!-- position of the center of mass with respect to the link's own anchor in a local coordinate frame -->
- <inertia> 0.012430552544 -0.000003671102 0.000029379389 0.013567548848 -0.000427679042 0.001755089866 </inertia>
- </define>
-
- <define template="pr2_wrist_pitch_visual">
- <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 -->
- <material>Gazebo/Grey</material>
- <geometry type="cylinder">
- <size>wrist_pitch_radius wrist_pitch_length</size>
- </geometry>
- </define>
-
-
- <define template="pr2_wrist_roll_joint" type="revolute">
- <axis> 0 1 0 </axis> <!-- direction of the joint in a global coordinate frame -->
- <anchor>0 0 0 </anchor> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
- <calibration>1.5 -1 </calibration> <!-- Calibration stop/flag indicating location and then direction -->
- </define>
-
- <define template="pr2_wrist_roll_inertial">
- <mass> 1.804155 </mass>
- <com> -0.000058 0.013779 0.179474 </com> <!-- position of the center of mass with respect to the link's own anchor in a local coordinate frame -->
- <inertia> 0.012430552544 -0.000003671102 0.000029379389 0.013567548848 -0.000427679042 0.001755089866 </inertia>
- </define>
-
- <define template="pr2_wrist_roll_visual">
- <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 -->
- <material>Gazebo/Grey</material>
- <geometry type="box">
- <size>wrist_roll_x wrist_roll_y wrist_roll_z </size>
- </geometry>
- </define>
- </templates>
-<!-- End template definitions for ease of reuse -->
-
-
-<!-- Begin base definition -->
-
- <link name="base"><!-- link specifying the base of the robot -->
-
- <parent> world</parent>
-
- <rpy> 0 0 0 </rpy> <!-- rotation of a local coordinate frame attached to the link with respect to a global coordinate frame -->
- <xyz> 0 0 wheel_clearance_offset</xyz> <!-- position of a local coordinate frame attached to the link with respect to the parent link's coordinate frame -->
-
- <joint type="floating" name="joint_base">
- <anchor>0.25 0.1 0.3 </anchor> <!-- Initial position of the base in a global coordinate frame -->
- </joint>
-
- <inertial>
- <mass> 70.750964 </mass>
- <com> -0.061523 0.000942 0.293569 </com> <!-- position of the center of mass with respect to the link's own anchor in a local coordinate frame -->
- <inertia>5.652232699207 -0.009719934438 1.293988226423 5.669473158652 -0.007379583694 3.683196351726 </inertia>
- </inertial>
-
- <visual>
- <xyz>0 0 base_height/2</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 -->
- <material>Gazebo/Grey</material>
- <geometry type="box">
- <size>base_length base_width base_height</size>
- </geometry>
- </visual>
-
- <collision>
- <xyz>0 0 base_height/2</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 -->
- <material>Gazebo/Grey</material>
- <geometry type="box">
- <size>base_length base_width base_height</size>
- </geometry>
- </collision>
-
- </link>
-
- <link name="caster_front_left">
- <parent> base</parent>
- <rpy> 0 0 0 </rpy> <!-- rotation of a local coordinate frame attached to the link with respect to the parent's frame -->
- <xyz> 0.2255 0.2255 0.08069 </xyz> <!-- position of a local coordinate frame attached to the link with respect to parent's frame-->
- <joint clone="pr2_caster_joint"/>
- <inertial clone="pr2_caster_inertial"/>
- <visual clone="pr2_caster_visual"/>
- <collision clone="pr2_caster_collision"/>
- </link>
-
-
- <link name="caster_front_right">
- <parent> base</parent>
- <rpy> 0 0 0 </rpy> <!-- rotation of a local coordinate frame attached to the link with respect to the parent's frame -->
- <xyz> 0.2255 -0.2255 0.08069 </xyz> <!-- position of a local coordinate frame attached to the link with respect to parent's frame-->
-
- <joint clone="pr2_caster_joint"/>
- <inertial clone="pr2_caster_inertial"/>
- <visual clone="pr2_caster_visual"/>
- <collision clone="pr2_caster_collision"/>
- </link>
-
- <link name="caster_rear_left">
- <parent> base</parent>
-
- <rpy> 0 0 0 </rpy> <!-- rotation of a local coordinate frame attached to the link with respect to the parent's frame -->
- <xyz> -0.2255 0.2255 0.08069 </xyz> <!-- position of a local coordinate frame attached to the link with respect to parent's frame-->
-
- <joint clone="pr2_caster_joint"/>
- <inertial clone="pr2_caster_inertial"/>
- <visual clone="pr2_caster_visual"/>
- <collision clone="pr2_caster_collision"/>
- </link>
-
- <link name="caster_rear_right">
- <parent> base</parent>
-
- <rpy> 0 0 0 </rpy> <!-- rotation of a local coordinate frame attached to the link with respect to the parent's frame -->
- <xyz> -0.2255 -0.2255 0.08069 </xyz> <!-- position of a local coordinate frame attached to the link with respect to parent's frame-->
-
- <joint clone="pr2_caster_joint"/>
- <inertial clone="pr2_caster_inertial"/>
- <visual clone="pr2_caster_visual"/>
- <collision clone="pr2_caster_collision"/>
- </link>
-
- <link name="wheel_front_left_l">
- <parent> caster_front_left</parent>
-
- <rpy> 0 0 0 </rpy> <!-- rotation of a local coordinate frame attached to the link with respect to the parent's frame -->
- <xyz> 0 0.049 0 </xyz> <!-- position of a local coordinate frame attached to the link with respect to parent's frame-->
-
- <joint clone="pr2_wheel_joint"/>
- <inertial clone="pr2_wheel_inertial"/>
- <visual clone="pr2_wheel_visual"/>
- <collision clone="pr2_wheel_visual"/>
- </link>
-
- <link name="wheel_front_left_r">
- <parent> caster_front_left</parent>
-
- <rpy> 0 0 0 </rpy> <!-- rotation of a local coordinate frame attached to the link with respect to the parent's frame -->
- <xyz> 0 -0.049 0 </xyz> <!-- position of a local coordinate frame attached to the link with respect to parent's frame-->
-
- <joint clone="pr2_wheel_joint"/>
- <inertial clone="pr2_wheel_inertial"/>
- <visual clone="pr2_wheel_visual"/>
- <collision clone="pr2_wheel_visual"/>
- </link>
-
- <link name="wheel_front_right_l">
- <parent> caster_front_right</parent>
-
- <rpy> 0 0 0 </rpy> <!-- rotation of a local coordinate frame attached to the link with respect to the parent's frame -->
- <xyz> 0 0.049 0 </xyz> <!-- position of a local coordinate frame attached to the link with respect to parent's frame-->
-
- <joint clone="pr2_wheel_joint"/>
- <inertial clone="pr2_wheel_inertial"/>
- <visual clone="pr2_wheel_visual"/>
- <collision clone="pr2_wheel_visual"/>
- </link>
-
-
- <link name="wheel_front_right_r">
- <parent> caster_front_right</parent>
-
- <rpy> 0 0 0 </rpy> <!-- rotation of a local coordinate frame attached to the link with respect to the parent's frame -->
- <xyz> 0 -0.049 0 </xyz> <!-- position of a local coordinate frame attached to the link with respect to parent's frame-->
-
- <joint clone="pr2_wheel_joint"/>
- <inertial clone="pr2_wheel_inertial"/>
- <visual clone="pr2_wheel_visual"/>
- <collision clone="pr2_wheel_visual"/>
- </link>
-
- <link name="wheel_rear_left_l">
- <parent> caster_rear_left</parent>
-
- <rpy> 0 0 0 </rpy> <!-- rotation of a local coordinate frame attached to the link with respect to the parent's frame -->
- <xyz> 0 0.049 0 </xyz> <!-- position of a local coordinate frame attached to the link with respect to parent's frame-->
-
- <joint clone="pr2_wheel_joint"/>
- <inertial clone="pr2_wheel_inertial"/>
- <visual clone="pr2_wheel_visual"/>
- <collision clone="pr2_wheel_visual"/>
- </link>
-
- <link name="wheel_rear_left_r">
- <parent> caster_rear_left</parent>
-
- <rpy> 0 0 0 </rpy> <!-- rotation of a local coordinate frame attached to the link with respect to the parent's frame -->
- <xyz> 0 -0.049 0 </xyz> <!-- position of a local coordinate frame attached to the link with respect to parent's frame-->
-
- <joint clone="pr2_wheel_joint"/>
- <inertial clone="pr2_wheel_inertial"/>
- <visual clone="pr2_wheel_visual"/>
- <collision clone="pr2_wheel_visual"/>
- </link>
-
- <link name="wheel_rear_right_l">
- <parent> caster_rear_right</parent>
-
- <rpy> 0 0 0 </rpy> <!-- rotation of a local coordinate frame attached to the link with respect to the parent's frame -->
- <xyz> 0 0.049 0 </xyz> <!-- position of a local coordinate frame attached to the link with respect to parent's frame-->
-
- <joint clone="pr2_wheel_joint"/>
- <inertial clone="pr2_wheel_inertial"/>
- <visual clone="pr2_wheel_visual"/>
- <collision clone="pr2_wheel_visual"/>
- </link>
-
- <link name="wheel_rear_right_r">
- <parent> caster_rear_right</parent>
-
- <rpy> 0 0 0 </rpy> <!-- rotation of a local coordinate frame attached to the link with respect to the parent's frame -->
- <xyz> 0 -0.049 0 </xyz> <!-- position of a local coordinate frame attached to the link with respect to parent's frame-->
-
- <joint clone="pr2_wheel_joint"/>
- <inertial clone="pr2_wheel_inertial"/>
- <visual clone="pr2_wheel_visual"/>
- <collision clone="pr2_wheel_visual"/>
- </link>
-
-<!-- End base definition -->
-
-
-<!-- Begin torso definition -->
-
- <link name="torso">
- <parent> base </parent>
- <rpy> 0 0 0 </rpy> <!-- rotation of a local coordinate frame attached to the link with respect to the parent's frame -->
- <xyz> base_torso_offset_x base_torso_offset_y base_torso_offset_z</xyz> <!-- position of a local coordinate frame attached to the link with respect to parent's frame-->
-
- <joint type="prismatic">
- <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.030 0.396 </limit>
- </joint>
-
- <inertial>
- <mass> 36.248046 </mass>
- <com> -0.085285 -0.002393 -0.149724 </com> <!-- position of the center of mass with respect to the link's own anchor in a local coordinate frame -->
- <inertia> 2.771653750257 0.004284522609 -0.160418504506 2.510019507959 0.029664468704 0.526432355569</inertia>
- </inertial>
-
- <visual>
- <xyz>0.0 0.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 -->
- <material>Gazebo/Grey</material>
- <geometry type="box">
- <size> torso_length torso_width torso_height </size>
- </geometry>
- </visual>
-
- <collision>
- <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 -->
- <material>Gazebo/Grey</material>
- <geometry type="box">
- <size> torso_length torso_width torso_height </size>
- </geometry>
- </collision>
- </link>
-
-<!-- End torso definition -->
-
-
-
-<!-- Begin arm definitions -->
-
-
- <link name="shoulder_pan_left"><!-- link specifying the shoulder of the robot -->
- <parent> torso</parent>
- <rpy> 0 0 0 </rpy> <!-- rotation of a local coordinate frame attached to the link with respect to the parent's frame -->
- <xyz> 0 0.1329361 0</xyz> <!-- position of a local coordinate frame attached to the link with respect to parent's frame-->
-
- <joint clone="pr2_shoulder_pan_joint" name="joint_shoulder_pan_left"/>
- <inertial clone="pr2_shoulder_pan_inertial"/>
- <visual clone="pr2_shoulder_pan_visual"/>
- <c...
[truncated message content] |
|
From: <is...@us...> - 2008-07-21 20:50:22
|
Revision: 1854
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1854&view=rev
Author: isucan
Date: 2008-07-21 20:50:29 +0000 (Mon, 21 Jul 2008)
Log Message:
-----------
fixed building after rename/move of packages
Modified Paths:
--------------
pkg/trunk/robot_descriptions/wg_robot_description_parser/CMakeLists.txt
pkg/trunk/robot_models/robot_motion_planning_models/manifest.xml
Modified: pkg/trunk/robot_descriptions/wg_robot_description_parser/CMakeLists.txt
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description_parser/CMakeLists.txt 2008-07-21 20:49:06 UTC (rev 1853)
+++ pkg/trunk/robot_descriptions/wg_robot_description_parser/CMakeLists.txt 2008-07-21 20:50:29 UTC (rev 1854)
@@ -1,6 +1,6 @@
cmake_minimum_required(VERSION 2.6)
include(rosbuild)
-rospack(wg_robot_desc)
+rospack(wg_robot_description_parser)
rospack_add_library(URDF src/urdf/URDF.cpp)
rospack_add_executable(parse src/test/parse.cpp)
target_link_libraries(parse URDF)
Modified: pkg/trunk/robot_models/robot_motion_planning_models/manifest.xml
===================================================================
--- pkg/trunk/robot_models/robot_motion_planning_models/manifest.xml 2008-07-21 20:49:06 UTC (rev 1853)
+++ pkg/trunk/robot_models/robot_motion_planning_models/manifest.xml 2008-07-21 20:50:29 UTC (rev 1854)
@@ -7,7 +7,7 @@
<license>BSD</license>
<url>http://pr.willowgarage.com/wiki/RobotDescriptionFormat</url>
- <depend package="wg_robot_desc"/>
+ <depend package="wg_robot_description_parser"/>
<depend package="libTF"/>
<export>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-07-21 20:48:59
|
Revision: 1853
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1853&view=rev
Author: hsujohnhsu
Date: 2008-07-21 20:49:06 +0000 (Mon, 21 Jul 2008)
Log Message:
-----------
move Media, world and models to a better place.
Modified Paths:
--------------
pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg.xml
pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo.xml
Added Paths:
-----------
pkg/trunk/robot_descriptions/gazebo_robot_description/
pkg/trunk/robot_descriptions/gazebo_robot_description/Makefile
pkg/trunk/robot_descriptions/gazebo_robot_description/manifest.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_torque.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_torque_control.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_torque_position.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_tray1.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_tray2.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_tray3.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_tray_hand.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_trimesh.model
Removed Paths:
-------------
pkg/trunk/demos/2dnav-gazebo/world/
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_torque.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_torque_control.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_torque_position.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_tray1.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_tray2.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_tray3.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_tray_hand.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_trimesh.model
Modified: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg.xml 2008-07-21 20:47:26 UTC (rev 1852)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg.xml 2008-07-21 20:49:06 UTC (rev 1853)
@@ -1,8 +1,8 @@
<launch>
<group name="wg">
- <node pkg="pr2_gazebo" type="run-gazebo.sh" args="$(find 2dnav-gazebo)/world/robot_wg.world" respawn="true" />
- <node pkg="map_server" type="map_server" args="$(find 2dnav-gazebo)/world/Media/materials/textures/willowMap.png 0.1" respawn="false" />
+ <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="wavefront_player" type="wavefront_player" respawn="false" />
Modified: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo.xml 2008-07-21 20:47:26 UTC (rev 1852)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo.xml 2008-07-21 20:49:06 UTC (rev 1853)
@@ -1,8 +1,8 @@
<launch>
<group name="wg">
- <node pkg="pr2_gazebo" type="run-gazebo.sh" args="$(find 2dnav-gazebo)/world/robot.world" respawn="true" />
- <node pkg="map_server" type="map_server" args="$(find 2dnav-gazebo)/world/Media/materials/textures/map3.png 0.1" respawn="false" />
+ <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="wavefront_player" type="wavefront_player" respawn="false" />
Added: pkg/trunk/robot_descriptions/gazebo_robot_description/Makefile
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/Makefile (rev 0)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/Makefile 2008-07-21 20:49:06 UTC (rev 1853)
@@ -0,0 +1,5 @@
+all:
+ @echo Nothing to make
+
+clean:
+ @echo nothing to clean
\ No newline at end of file
Added: pkg/trunk/robot_descriptions/gazebo_robot_description/manifest.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/manifest.xml (rev 0)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/manifest.xml 2008-07-21 20:49:06 UTC (rev 1853)
@@ -0,0 +1,8 @@
+<package>
+ <description>Gazebo world and model files for pr2.</description>
+ <author>John Hsu</author>
+ <license>BSD</license>
+ <url>http://pr.willowgarage.com/wiki/FIXME</url>
+ <depend package="gazebo"/>
+ <depend package="gazebo_plugin"/>
+</package>
Deleted: pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/world/pr2.model 2008-07-21 20:30:36 UTC (rev 1846)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model 2008-07-21 20:49:06 UTC (rev 1853)
@@ -1,2037 +0,0 @@
-<?xml version="1.0"?>
-
-<!-- PR2 model -->
-<model:physical name="pr2_model"
- xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
- xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
- xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
- xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
- xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
- xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
- xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
- >
-
- <xyz>0.0 0.0 0.02</xyz>
- <rpy>0.0 0.0 0.0 </rpy>
-
- <canonicalBody>base_body</canonicalBody>
-
- <body:box name="base_body">
- <xyz>0.175 0.0 0.15 </xyz> <!-- base bottom is h/2 + some height for wheel clearance-->
- <rpy>0.0 0.0 0.0 </rpy>
-
- <!-- report when this body collides with static bodies -->
- <reportStaticCollision>true</reportStaticCollision>
-
- <geom:box name="base_body_geom">
-
- <massMatrix>true</massMatrix> <!-- user provide full mass and inertia matrix -->
- <mass>70.750964</mass>
- <ixx>5.6522326992070</ixx>
- <ixy>-0.009719934438</ixy>
- <ixz>1.2939882264230</ixz>
- <iyy>5.6694731586520</iyy>
- <iyz>-0.007379583694</iyz>
- <izz>3.6831963517260</izz>
- <cx>-0.061523</cx>
- <cy> 0.000942</cy>
- <cz> 0.293569</cz>
-
-
- <xyz> 0.0 0.00 0.000</xyz>
- <size>0.65 0.65 0.26</size>
- <mul>1</mul>
- <!--color> 0.0 1.0 0.0</color-->
- <visual>
- <xyz>0.0 0.0 -0.13</xyz> <!-- shift center of model to match the actual model -->
- <!--size>0.65 0.65 0.65</size-->
- <scale>0.001 0.001 0.001</scale>
- <rpy> 90.0 0.0 90.0 </rpy>
- <mesh>pr2_base.mesh</mesh>
- <material>Gazebo/Shell</material>
- </visual>
- </geom:box>
-
- </body:box>
-
-
- <!-- Hokuyo laser body -->
- <model:physical name="base_laser_model">
- <canonicalBody>base_laser_body</canonicalBody>
- <body:box name="base_laser_body">
- <xyz>0.45 0.0 0.27</xyz> <!-- from the robot coordinate system (center bottom of base box)-->
- <rpy>0.0 0.0 0.0</rpy>
-
- <geom:box name="base_laser_box">
- <xyz>0.0 0.0 0.02</xyz>
- <rpy>0 0 0</rpy>
- <size>0.05 0.05 0.041</size>
- <mass>0.12</mass>
-
- <visual>
- <scale>0.05 0.05 0.041</scale>
- <mesh>unit_box</mesh>
- <material>Gazebo/Blue</material>
- </visual>
-
- </geom:box>
- <geom:cylinder name="base_laser_cylinder1">
- <xyz>0.0 0.0 0.041</xyz>
- <rpy>0 0 0</rpy>
- <size>0.02 0.013</size>
- <mass>0.02</mass>
-
- <visual>
- <rpy>0 0 0</rpy>
- <scale>0.04 0.04 0.013</scale>
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Black</material>
- </visual>
-
- </geom:cylinder>
-
- <geom:cylinder name="base_laser_cylinder2">
- <xyz>0.0 0.0 0.054</xyz>
- <rpy>0 0 0</rpy>
- <size>0.018 0.009</size>
- <mass>0.02</mass>
-
- <visual>
- <rpy>0 0 0</rpy>
- <size>0.036 0.036 0.009</size>
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Black</material>
- </visual>
-
- </geom:cylinder>
-
- <geom:cylinder name="base_laser_cylinder3">
- <xyz>0.0 0.0 0.063</xyz>
- <rpy>0 0 0</rpy>
- <size>0.0175 0.008</size>
- <mass>0.02</mass>
-
- <visual>
- <rpy>0 0 0</rpy>
- <size>0.035 0.035 0.008</size>
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Shell</material>
- </visual>
-
- </geom:cylinder>
-
-
- <sensor:ray name="base_laser_1">
- <rayCount>68</rayCount>
- <rangeCount>683</rangeCount>
- <laserCount>1</laserCount>
- <origin>0.0 0.0 0.05</origin>
-
- <minAngle>-135</minAngle>
- <maxAngle>135</maxAngle>
-
- <minRange>0.05</minRange>
- <maxRange>10.0</maxRange>
- <updateRate>10.0</updateRate>
- <controller:sicklms200_laser name="base_laser_controller_1">
- <interface:laser name="base_laser_iface_1"/>
- <updateRate>10</updateRate>
- </controller:sicklms200_laser>
- </sensor:ray>
- <!--
- -->
- <!--
- <sensor:ray2 name="laser_1">
- <rayCount>683</rayCount>
- <rangeCount>683</rangeCount>
- <laserCount>1</laserCount>
- <origin>0.0 0.0 0.05</origin>
-
- <minAngle>-135</minAngle>
- <maxAngle>135</maxAngle>
-
- <minRange>0.05</minRange>
- <maxRange>10.0</maxRange>
- <controller:sick2lms200_laser name="laser_controller_1">
- <interface:laser name="laser_iface_1"/>
- <updateRate>10</updateRate>
- </controller:sick2lms200_laser>
- </sensor:ray2>
- -->
-
- </body:box>
-
- <!-- attach hokuyo to torso -->
- <attach>
- <parentBody>base_body</parentBody>
- <myBody>base_laser_body</myBody>
- </attach>
- </model:physical>
-
-
-
- <!-- ========== spine and shoulder =========== -->
- <canonicalBody>torso_body</canonicalBody>
-
- <body:box name="torso_body">
- <xyz>0.056 0.0 0.45</xyz>
- <rpy>0.0 0.0 0.0</rpy>
-
- <!-- report when this body collides with static bodies -->
- <reportStaticCollision>true</reportStaticCollision>
-
- <geom:box name="torso_geom">
- <xyz> 0.0 0.0 0.0 </xyz>
- <size>0.1 0.2 0.6 </size>
- <mass>1</mass>
- <visual>
- <!-- x y z -->
- <xyz> 0.1 0.0 -0.425 </xyz>
- <!--size>0.6 0.8 0.6 </size-->
- <scale>0.001 0.001 0.001</scale>
- <rpy>90.0 0.0 90.0 </rpy>
- <mesh>pr2_body.mesh</mesh>
- <material>Gazebo/PioneerBody</material>
- </visual>
- </geom:box>
-
- <geom:box name="shoulder_geom">
- <xyz> 0.1 0.00 0.54</xyz>
- <size>0.3 0.65 0.02</size>
- <mass>1</mass>
- <visual>
- <!--size>0.3 0.65 0.02</size-->
- <size>0.0 0.0 0.0</size>
- <mesh>unit_box</mesh>
- <material>Gazebo/Shell</material>
- </visual>
- </geom:box>
-
- </body:box>
-
- <joint:slider name="torso_spine_slider">
- <body2>base_body</body2>
- <body1>torso_body</body1>
- <anchor>torso_body</anchor>
- <lowStop> 0.0 </lowStop>
- <highStop> 0.5 </highStop>
- <axis>0.0 0.0 1.0 </axis>
- <axisOffset> 0.0 0.0 0.0 </axisOffset>
- </joint:slider>
-
- <!-- ========== Right Arm =========== -->
- <body:cylinder name="shoulder_pan_body_right">
- <xyz> 0.1829361 -0.1329361 0.64 </xyz>
- <rpy> 0.0 0.0 0.0 </rpy>
- <geom:cylinder name="shoulder_pan_body_right_geom">
-
- <massMatrix>true</massMatrix> <!-- user provide full mass and inertia matrix -->
- <mass> 16.29553 </mass> <!-- who would have thought we have 6 digits of precision!? -->
- <ixx> 0.793291393007 </ixx>
- <ixy> 0.003412032973 </ixy>
- <ixz> 0.0096614481 </ixz>
- <iyy> 0.818419457224</iyy>
- <iyz> -0.033999499551</iyz>
- <izz> 0.13915007406 </izz>
- <cx> -0.005215 </cx>
- <cy> -0.030552 </cy>
- <cz> -0.175743 </cz>
-
- <kp>1000000.0</kp>
- <kd>1.0</kd>
- <size> 0.13 0.60</size> <!-- radius and length -->
- <visual>
- <xyz> 0.00 0.0 0.185 </xyz>
- <rpy> 90.0 0.0 90.0 </rpy>
- <!--size> 0.26 0.6 0.36</size-->
- <scale>0.001 0.001 0.001</scale>
- <mesh>pr2_sh_turret.mesh</mesh>
- <material>Gazebo/Green</material>
- </visual>
- </geom:cylinder>
- </body:cylinder>
-
- <body:cylinder name="upperarm_joint_right">
- <xyz> 0.285 -0.15 0.8269 </xyz>
- <rpy> 90.0 0.0 0.0 </rpy>
- <geom:cylinder name="upperarm_joint_right_geom">
-
- <massMatrix>true</massMatrix> <!-- user provide full mass and inertia matrix -->
- <mass> 2.41223</mass> <!-- who would have thought we have 6 digits of precision!? -->
- <ixx> 0.016640333585 </ixx>
- <ixy> 0.002696462583 </ixy>
- <ixz> 0.001337742275 </ixz>
- <iyy> 0.017232603914 </iyy>
- <iyz> -0.003605106514 </iyz>
- <izz> 0.018723553425 </izz>
- <cx> -0.035895 </cx>
- <cy> 0.014422 </cy>
- <cz> -0.028263</cz>
-
- <kp>1000000.0</kp>
- <kd>1.0</kd>
- <size> 0.12 0.10</size> <!-- radius and length -->
- <visual>
- <xyz> 0.0 0.0 0.0 </xyz>
- <rpy> 0.0 90.0 0.0 </rpy>
- <scale>0.001 0.001 0.001</scale>
- <mesh>pr2_sh_pitch.mesh</mesh>
- <material>Gazebo/PioneerBody</material>
- </visual>
- </geom:cylinder>
- </body:cylinder>
-
- <body:box name="upperarm_right">
- <xyz> 0.575 -0.15 0.8269 </xyz>
- <rpy> 0.0 0.0 0.0 </rpy>
- <geom:box name="upperarm_right_geom">
-
- <massMatrix>true</massMatrix> <!-- user provide full mass and inertia matrix -->
- <mass> 4.9481 </mass> <!-- who would have thought we have 6 digits of precision!? -->
- <ixx> 0.073060715309 </ixx>
- <ixy> 0.000547745916 </ixy>
- <ixz> 0.000119476885 </ixz>
- <iyy> 0.072124510748 </iyy>
- <iyz> 0.001544932307 </iyz>
- <izz> 0.013383284908 </izz>
- <cx> 0.001205 </cx>
- <cy> -0.016293 </cy>
- <cz> 0.21227</cz>
-
- <kp>1000000.0</kp>
- <kd>1.0</kd>
- <size> 0.35 0.12 0.15</size>
- <visual>
- <xyz> -0.20 0.0 0.0 </xyz>
- <rpy>180.0 -90.0 0.0 </rpy>
- <scale>0.001 0.001 0.001</scale>
- <mesh>pr2_sh_roll.mesh</mesh>
- <material>Gazebo/Shell</material>
- </visual>
- </geom:box>
- </body:box>
-
- <body:cylinder name="elbow_right">
- <xyz> 0.6850 -0.15 0.8269 </xyz>
- <rpy> 90.0 0.0 0.0 </rpy>
- <geom:cylinder name="elbow_right_geom">
-
- <massMatrix>true</massMatrix> <!-- user provide full mass and inertia matrix -->
- <mass> 1.689246</mass> <!-- who would have thought we have 6 digits of precision!? -->
- <ixx> 0.003275298548 </ixx>
- <ixy> 0.000292046674 </ixy>
- <ixz> -0.000077359282 </ixz>
- <iyy> 0.003236815206 </iyy>
- <iyz> -0.000001162155 </iyz>
- <izz> 0.00410053774 </izz>
- <cx> -0.011638 </cx>
- <cy> 0.013173 </cy>
- <cz> 0.001228</cz>
-
- <kp>1000000.0</kp>
- <kd>1.0</kd>
- <size> 0.08 0.1</size> <!-- radius and length -->
- <visual>
- <xyz> -0.135 0.0 0.0 </xyz>
- <rpy> 0.0 -90.0 0.0 </rpy>
- <scale>0.001 0.001 0.001</scale>
- <mesh>pr2_elbow_pitch.mesh</mesh>
- <material>Gazebo/PioneerBody</material>
- </visual>
- </geom:cylinder>
- </body:cylinder>
-
- <body:box name="forearm_right">
- <xyz> 0.90225 -0.15 0.8269 </xyz>
- <rpy> 0.0 0.0 0.0 </rpy>
- <geom:box name="forearm_right_geom">
-
- <massMatrix>true</massMatrix> <!-- user provide full mass and inertia matrix -->
- <mass> 1.804155 </mass> <!-- who would have thought we have 6 digits of precision!? -->
- <ixx> 0.012430552544 </ixx>
- <ixy> -0.000003671102 </ixy>
- <ixz> 0.000029379389 </ixz>
- <iyy> 0.013567548848 </iyy>
- <iyz> -0.000427679042 </iyz>
- <izz> 0.001755089866 </izz>
- <cx> -0.000058 </cx>
- <cy> 0.013779 </cy>
- <cz> 0.179474 </cz>
-
- <kp>1000000.0</kp>
- <kd>1.0</kd>
- <size> 0.27 0.12 0.08</size>
- <visual>
- <xyz> 0.11 0.0 0.0 </xyz>
- <rpy> 0.0 -90.0 -90.0 </rpy>
- <scale>0.001 0.001 0.001</scale>
- <mesh>pr2_forearm_roll.mesh</mesh>
- <material>Gazebo/Shell</material>
- </visual>
- </geom:box>
- </body:box>
-
- <body:cylinder name="wrist_right">
- <xyz> 1.008 -0.15 0.8269 </xyz>
- <rpy> 90.0 0.0 0.0 </rpy>
- <geom:cylinder name="wrist_right_geom">
-
- <massMatrix>true</massMatrix> <!-- user provide full mass and inertia matrix -->
- <mass> 1.804155 </mass> <!-- who would have thought we have 6 digits of precision!? -->
- <ixx> 0.012430552544 </ixx>
- <ixy> -0.000003671102 </ixy>
- <ixz> 0.000029379389 </ixz>
- <iyy> 0.013567548848 </iyy>
- <iyz> -0.000427679042 </iyz>
- <izz> 0.001755089866 </izz>
- <cx> 0.0 </cx>
- <cy> 0.0 </cy>
- <cz> 0.0 </cz>
-
- <kp>1000000.0</kp>
- <kd>1.0</kd>
- <size> 0.031 0.1</size> <!-- radius and length -->
- <visual>
- <xyz> 0.008 0.0 0.0 </xyz>
- <rpy> 90.0 0.0 -90.0 </rpy>
- <scale>0.001 0.001 0.001</scale>
- <mesh>pr2_wrist_pitch.mesh</mesh>
- <material>Gazebo/PioneerBody</material>
- </visual>
- </geom:cylinder>
- </body:cylinder>
-
- <body:box name="palm_right">
- <xyz> 1.10 -0.15 0.8269</xyz>
- <rpy> 0.0 0.0 0.0 </rpy>
- <geom:box name="palm_right_geom">
-
- <massMatrix>true</massMatrix> <!-- user provide full mass and inertia matrix -->
- <mass> 1.804155 </mass> <!-- who would have thought we have 6 digits of precision!? -->
- <ixx> 0.012430552544 </ixx>
- <ixy> -0.000003671102 </ixy>
- <ixz> 0.000029379389 </ixz>
- <iyy> 0.013567548848 </iyy>
- <iyz> -0.000427679042 </iyz>
- <izz> 0.001755089866 </izz>
- <cx> 0.0 </cx>
- <cy> 0.0 </cy>
- <cz> 0.0 </cz>
-
- <kp>1000000.0</kp>
- <kd>1.0</kd>
- <size> 0.11 0.10 0.05</size>
- <visual>
- <xyz> -0.035 0.0 0.0 </xyz>
- <rpy> 0.0 90.0 90.0 </rpy>
- <scale>0.001 0.001 0.001</scale>
- <mesh>pr2_wrist_roll.mesh</mesh>
- <material>Gazebo/Shell</material>
- </visual>
- </geom:box>
- </body:box>
-
- <body:box name="finger_1_right">
- <xyz> 1.18 -0.17 0.8269 </xyz>
- <rpy> 0.0 0.0 0.0 </rpy>
- <geom:box name="finger_1_right_geom">
- <kp>100000000.0</kp>
- <kd>1.0</kd>
- <size> 0.05 0.01 0.025</size>
- <mass>0.1</mass>
- <visual>
- <size> 0.05 0.01 0.025</size>
- <mesh>unit_box</mesh>
- <material>Gazebo/PioneerBody</material>
- </visual>
- </geom:box>
- </body:box>
-
- <body:box name="finger_2_right">
- <xyz> 1.18 -0.13 0.8269 </xyz>
- <rpy> 0.0 0.0 0.0 </rpy>
- <geom:box name="finger_2_right_geom">
- <kp>100000000.0</kp>
- <kd>1.0</kd>
- <size> 0.05 0.01 0.025</size>
- <mass>0.1</mass>
- <visual>
- <size> 0.05 0.01 0.025</size>
- <mesh>unit_box</mesh>
- <material>Gazebo/Blue</material>
- </visual>
- </geom:box>
- </body:box>
-
-
-
-
-
-
- <joint:hinge name="shoulder_pan_right">
- <body1>shoulder_pan_body_right</body1>
- <body2>torso_body</body2>
- <anchor>shoulder_pan_body_right</anchor>
- <axis> 0.0 0.0 1.0 </axis>
- </joint:hinge>
-
- <joint:hinge name="upperarm_pitch_joint_right">
- <body2>shoulder_pan_body_right</body2>
- <body1>upperarm_joint_right</body1>
- <anchor>upperarm_joint_right</anchor>
- <axis> 0.0 1.0 0.0 </axis>
- </joint:hinge>
-
- <joint:hinge name="upperarm_roll_joint_right">
- <body1>upperarm_right</body1>
- <body2>upperarm_joint_right</body2>
- <anchor>upperarm_joint_right</anchor>
- <axis> 1.0 0.0 0.0 </axis>
- </joint:hinge>
-
- <joint:hinge name="elbow_pitch_joint_right">
- <body1>elbow_right</body1>
- <body2>upperarm_right</body2>
- <anchor>elbow_right</anchor>
- <axis> 0.0 1.0 0.0 </axis>
- </joint:hinge>
-
- <joint:hinge name="forearm_roll_joint_right">
- <body1>forearm_right</body1>
- <body2>elbow_right</body2>
- <anchor>elbow_right</anchor>
- <axis> 1.0 0.0 0.0 </axis>
- </joint:hinge>
-
- <joint:hinge name="wrist_pitch_joint_right">
- <body1>wrist_right</body1>
- <body2>forearm_right</body2>
- <anchor>wrist_right</anchor>
- <axis> 0.0 1.0 0.0 </axis>
- </joint:hinge>
-
- <joint:hinge name="palm_roll_joint_right">
- <body1>palm_right</body1>
- <body2>wrist_right</body2>
- <anchor>palm_right</anchor>
- <axis> 1.0 0.0 0.0 </axis>
- <axisOffset> 0.0 0.0 0.0 </axisOffset>
- </joint:hinge>
-
- <joint:slider name="finger_1_slider_right">
- <body2>finger_1_right</body2>
- <body1>palm_right</body1>
- <anchor>palm_right</anchor>
- <lowStop> -0.02 </lowStop> <!-- red close -->
- <highStop> 0.05 </highStop> <!-- red open -->
- <axis>0.0 1.0 0.0 </axis>
- </joint:slider>
-
- <joint:slider name="finger_2_slider_right">
- <body2>finger_2_right</body2>
- <body1>palm_right</body1>
- <anchor>palm_right</anchor>
- <lowStop> -0.05 </lowStop> <!-- white open -->
- <highStop> 0.02 </highStop> <!-- white close -->
- <axis>0.0 1.0 0.0 </axis>
- </joint:slider>
-
- <!-- ========== Left Arm =========== -->
- <body:cylinder name="shoulder_pan_body_left">
- <xyz> 0.1829361 0.1329361 0.64 </xyz>
- <rpy> 0.0 0.0 0.0 </rpy>
- <geom:cylinder name="shoulder_pan_body_left_geom">
-
- <massMatrix>true</massMatrix> <!-- user provide full mass and inertia matrix -->
- <mass> 16.29553 </mass> <!-- who would have thought we have 6 digits of precision!? -->
- <ixx> 0.793291393007 </ixx>
- <ixy> 0.003412032973 </ixy>
- <ixz> 0.0096614481 </ixz>
- <iyy> 0.818419457224</iyy>
- <iyz> -0.033999499551</iyz>
- <izz> 0.13915007406 </izz>
- <cx> -0.005215 </cx>
- <cy> -0.030552 </cy>
- <cz> -0.175743 </cz>
-
- <kp>1000000.0</kp>
- <kd>1.0</kd>
- <size> 0.13 0.6</size> <!-- radius and length -->
- <visual>
- <xyz> 0.00 0.0 0.185 </xyz>
- <rpy> 90.0 0.0 90.0 </rpy>
- <!--size> 0.26 0.6 0.36</size-->
- <scale>0.001 0.001 0.001</scale>
- <mesh>pr2_sh_turret.mesh</mesh>
- <material>Gazebo/Green</material>
- </visual>
- </geom:cylinder>
- </body:cylinder>
-
- <body:cylinder name="upperarm_joint_left">
- <xyz> 0.285 0.15 0.8269 </xyz>
- <rpy> 90.0 0.0 0.0 </rpy>
- <geom:cylinder name="upperarm_joint_left_geom">
-
- <massMatrix>true</massMatrix> <!-- user provide full mass and inertia matrix -->
- <mass> 2.41223</mass> <!-- who would have thought we have 6 digits of precision!? -->
- <ixx> 0.016640333585 </ixx>
- <ixy> 0.002696462583 </ixy>
- <ixz> 0.001337742275 </ixz>
- <iyy> 0.017232603914 </iyy>
- <iyz> -0.003605106514 </iyz>
- <izz> 0.018723553425 </izz>
- <cx> -0.035895 </cx>
- <cy> 0.014422 </cy>
- <cz> -0.028263</cz>
-
- <kp>1000000.0</kp>
- <kd>1.0</kd>
- <size> 0.12 0.10</size> <!-- radius and length -->
- <visual>
- <xyz> 0.0 0.0 0.0 </xyz>
- <rpy> 0.0 90.0 0.0 </rpy>
- <scale>0.001 0.001 0.001</scale>
- <mesh>pr2_sh_pitch.mesh</mesh>
- <material>Gazebo/PioneerBody</material>
- </visual>
- </geom:cylinder>
- </body:cylinder>
-
- <body:box name="upperarm_left">
- <xyz> 0.575 0.15 0.8269 </xyz>
- <rpy> 0.0 0.0 0.0 </rpy>
- <geom:box name="upperarm_left_geom">
-
- <massMatrix>true</massMatrix> <!-- user provide full mass and inertia matrix -->
- <mass> 4.9481 </mass> <!-- who would have thought we have 6 digits of precision!? -->
- <ixx> 0.073060715309 </ixx>
- <ixy> 0.000547745916 </ixy>
- <ixz> 0.000119476885 </ixz>
- <iyy> 0.072124510748 </iyy>
- <iyz> 0.001544932307 </iyz>
- <izz> 0.013383284908 </izz>
- <cx> 0.001205 </cx>
- <cy> -0.016293 </cy>
- <cz> 0.21227</cz>
-
- <kp>1000000.0</kp>
- <kd>1.0</kd>
- <size> 0.35 0.12 0.15</size>
- <visual>
- <xyz> -0.20 0.0 0.0 </xyz>
- <rpy>180.0 -90.0 0.0 </rpy>
- <scale>0.001 0.001 0.001</scale>
- <mesh>pr2_sh_roll.mesh</mesh>
- <material>Gazebo/Shell</material>
- </visual>
- </geom:box>
- </body:box>
-
- <body:cylinder name="elbow_left">
- <xyz> 0.6850 0.15 0.8269 </xyz>
- <rpy> 90.0 0.0 0.0 </rpy>
- <geom:cylinder name="elbow_left_geom">
-
- <massMatrix>true</massMatrix> <!-- user provide full mass and inertia matrix -->
- <mass> 1.689246</mass> <!-- who would have thought we have 6 digits of precision!? -->
- <ixx> 0.003275298548 </ixx>
- <ixy> 0.000292046674 </ixy>
- <ixz> -0.000077359282 </ixz>
- <iyy> 0.003236815206 </iyy>
- <iyz> -0.000001162155 </iyz>
- <izz> 0.00410053774 </izz>
- <cx> -0.011638 </cx>
- <cy> 0.013173 </cy>
- <cz> 0.001228</cz>
-
- <kp>1000000.0</kp>
- <kd>1.0</kd>
- <size> 0.08 0.1</size> <!-- radius and length -->
- <visual>
- <xyz> -0.135 0.0 0.0 </xyz>
- <rpy> 0.0 -90.0 0.0 </rpy>
- <scale>0.001 0.001 0.001</scale>
- <mesh>pr2_elbow_pitch.mesh</mesh>
- <material>Gazebo/PioneerBody</material>
- </visual>
- </geom:cylinder>
- </body:cylinder>
-
- <body:box name="forearm_left">
- <xyz> 0.90225 0.15 0.8269 </xyz>
- <rpy> 0.0 0.0 0.0 </rpy>
- <geom:box name="forearm_left_geom">
-
- <massMatrix>true</massMatrix> <!-- user provide full mass and inertia matrix -->
- <mass> 1.804155 </mass> <!-- who would have thought we have 6 digits of precision!? -->
- <ixx> 0.012430552544 </ixx>
- <ixy> -0.000003671102 </ixy>
- <ixz> 0.000029379389 </ixz>
- <iyy> 0.013567548848 </iyy>
- <iyz> -0.000427679042 </iyz>
- <izz> 0.001755089866 </izz>
- <cx> -0.000058 </cx>
- <cy> 0.013779 </cy>
- <cz> 0.179474 </cz>
-
- <kp>1000000.0</kp>
- <kd>1.0</kd>
- <size> 0.27 0.12 0.08</size>
- <visual>
- <xyz> 0.11 0.0 0.0 </xyz>
- <rpy> 0.0 -90.0 -90.0 </rpy>
- <scale>0.001 0.001 0.001</scale>
- <mesh>pr2_forearm_roll.mesh</mesh>
- <material>Gazebo/Shell</material>
- </visual>
- </geom:box>
- </body:box>
-
- <body:cylinder name="wrist_left">
- <xyz> 1.008 0.15 0.8269 </xyz>
- <rpy> 90.0 0.0 0.0 </rpy>
- <geom:cylinder name="wrist_left_geom">
-
- <massMatrix>true</massMatrix> <!-- user provide full mass and inertia matrix -->
- <mass> 1.804155 </mass> <!-- who would have thought we have 6 digits of precision!? -->
- <ixx> 0.012430552544 </ixx>
- <ixy> -0.000003671102 </ixy>
- <ixz> 0.000029379389 </ixz>
- <iyy> 0.013567548848 </iyy>
- <iyz> -0.000427679042 </iyz>
- <izz> 0.001755089866 </izz>
- <cx> 0.0 </cx>
- <cy> 0.0 </cy>
- <cz> 0.0 </cz>
-
- <kp>1000000.0</kp>
- <kd>1.0</kd>
- <size> 0.031 0.1</size> <!-- radius and length -->
- <visual>
- <xyz> 0.003 0.0 0.0 </xyz>
- <rpy> 90.0 0.0 -90.0 </rpy>
- <scale>0.001 0.001 0.001</scale>
- <mesh>pr2_wrist_pitch.mesh</mesh>
- <material>Gazebo/PioneerBody</material>
- </visual>
- </geom:cylinder>
- </body:cylinder>
-
- <body:box name="palm_left">
- <xyz> 1.10 0.15 0.8269</xyz>
- <rpy> 0.0 0.0 0.0 </rpy>
- <geom:box name="palm_left_geom">
-
- <massMatrix>true</massMatrix> <!-- user provide full mass and inertia matrix -->
- <mass> 1.804155 </mass> <!-- who would have thought we have 6 digits of precision!? -->
- <ixx> 0.012430552544 </ixx>
- <ixy> -0.000003671102 </ixy>
- <ixz> 0.000029379389 </ixz>
- <iyy> 0.013567548848 </iyy>
- <iyz> -0.000427679042 </iyz>
- <izz> 0.001755089866 </izz>
- <cx> 0.0 </cx>
- <cy> 0.0 </cy>
- <cz> 0.0 </cz>
-
- <kp>1000000.0</kp>
- <kd>1.0</kd>
- <size> 0.11 0.10 0.05</size>
- <visual>
- <xyz> -0.035 0.0 0.0 </xyz>
- <rpy> 0.0 90.0 90.0 </rpy>
- <scale>0.001 0.001 0.001</scale>
- <mesh>pr2_wrist_roll.mesh</mesh>
- ...
[truncated message content] |
|
From: <hsu...@us...> - 2008-07-21 21:10:37
|
Revision: 1861
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1861&view=rev
Author: hsujohnhsu
Date: 2008-07-21 21:10:46 +0000 (Mon, 21 Jul 2008)
Log Message:
-----------
update gazebo world file location.
Modified Paths:
--------------
pkg/trunk/robot_control_loops/pr2_gazebo/setup.bash
pkg/trunk/robot_control_loops/pr2_gazebo/setup.tcsh
pkg/trunk/simulators/rosgazebo/setup.bash
pkg/trunk/simulators/rosgazebo/setup.tcsh
Modified: pkg/trunk/robot_control_loops/pr2_gazebo/setup.bash
===================================================================
--- pkg/trunk/robot_control_loops/pr2_gazebo/setup.bash 2008-07-21 21:10:20 UTC (rev 1860)
+++ pkg/trunk/robot_control_loops/pr2_gazebo/setup.bash 2008-07-21 21:10:46 UTC (rev 1861)
@@ -6,7 +6,7 @@
export SIM_PLUGIN=`rospack find gazebo_plugin`
export PR2API=`rospack find libpr2API`
export PR2HW=`rospack find libpr2HW`
-export PR2MEDIA=`rospack find 2dnav-gazebo`/world
+export PR2MEDIA=`rospack find gazebo_robot_description`/world
export LD_LIBRARY_PATH=''
Modified: pkg/trunk/robot_control_loops/pr2_gazebo/setup.tcsh
===================================================================
--- pkg/trunk/robot_control_loops/pr2_gazebo/setup.tcsh 2008-07-21 21:10:20 UTC (rev 1860)
+++ pkg/trunk/robot_control_loops/pr2_gazebo/setup.tcsh 2008-07-21 21:10:46 UTC (rev 1861)
@@ -6,7 +6,7 @@
setenv SIM_PLUGIN `rospack find gazebo_plugin`
setenv PR2API `rospack find libpr2API`
setenv PR2HW `rospack find libpr2HW`
-setenv PR2MEDIA `rospack find 2dnav-gazebo`/world
+setenv PR2MEDIA `rospack find gazebo_robot_description`/world
if (! $?LD_LIBRARY_PATH) setenv LD_LIBRARY_PATH ''
setenv LD_LIBRARY_PATH ''
Modified: pkg/trunk/simulators/rosgazebo/setup.bash
===================================================================
--- pkg/trunk/simulators/rosgazebo/setup.bash 2008-07-21 21:10:20 UTC (rev 1860)
+++ pkg/trunk/simulators/rosgazebo/setup.bash 2008-07-21 21:10:46 UTC (rev 1861)
@@ -6,7 +6,7 @@
export SIM_PLUGIN=`rospack find gazebo_plugin`
export PR2API=`rospack find libpr2API`
export PR2HW=`rospack find libpr2HW`
-export PR2MEDIA=`rospack find 2dnav-gazebo`/world
+export PR2MEDIA=`rospack find gazebo_robot_description`/world
export LD_LIBRARY_PATH=''
Modified: pkg/trunk/simulators/rosgazebo/setup.tcsh
===================================================================
--- pkg/trunk/simulators/rosgazebo/setup.tcsh 2008-07-21 21:10:20 UTC (rev 1860)
+++ pkg/trunk/simulators/rosgazebo/setup.tcsh 2008-07-21 21:10:46 UTC (rev 1861)
@@ -6,7 +6,7 @@
setenv SIM_PLUGIN `rospack find gazebo_plugin`
setenv PR2API `rospack find libpr2API`
setenv PR2HW `rospack find libpr2HW`
-setenv PR2MEDIA `rospack find 2dnav-gazebo`/world
+setenv PR2MEDIA `rospack find gazebo_robot_description`/world
if (! $?LD_LIBRARY_PATH) setenv LD_LIBRARY_PATH ''
setenv LD_LIBRARY_PATH ''
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-07-21 22:19:41
|
Revision: 1871
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1871&view=rev
Author: hsujohnhsu
Date: 2008-07-21 22:19:50 +0000 (Mon, 21 Jul 2008)
Log Message:
-----------
updates to full point cloud publish. attempt to correct behavior.
Modified Paths:
--------------
pkg/trunk/3rdparty/opende/Makefile
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model
pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h
pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
Modified: pkg/trunk/3rdparty/opende/Makefile
===================================================================
--- pkg/trunk/3rdparty/opende/Makefile 2008-07-21 22:19:33 UTC (rev 1870)
+++ pkg/trunk/3rdparty/opende/Makefile 2008-07-21 22:19:50 UTC (rev 1871)
@@ -2,7 +2,8 @@
WITH_DRAWSTUFF = yes
CFG_OPTIONS = --with-arch=nocona --enable-release --disable-debug --enable-double-precision
-REVISION = -r 1520
+#REVISION = -r 1520
+REVISION = -r 1534 #this seems to have the bNormalize error removed.
ifeq ($(WITH_DRAWSTUFF), yes)
CFG_OPTIONS += --with-drawstuff=X11
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model 2008-07-21 22:19:33 UTC (rev 1870)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model 2008-07-21 22:19:50 UTC (rev 1871)
@@ -1263,11 +1263,11 @@
<iClamp>0</iClamp>
</joint>
<joint name="hokuyo_pitch_joint">
- <saturationTorque>1000</saturationTorque>
- <pGain>1</pGain>
+ <saturationTorque>500</saturationTorque>
+ <pGain>2</pGain>
<dGain>0</dGain>
- <iGain>0</iGain>
- <iClamp>0</iClamp>
+ <iGain>1</iGain>
+ <iClamp>20</iClamp>
</joint>
<joint name="head_cam_left_yaw_joint">
<saturationTorque>100</saturationTorque>
Modified: pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h
===================================================================
--- pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h 2008-07-21 22:19:33 UTC (rev 1870)
+++ pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h 2008-07-21 22:19:50 UTC (rev 1871)
@@ -175,8 +175,12 @@
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-07-21 22:19:33 UTC (rev 1870)
+++ pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp 2008-07-21 22:19:50 UTC (rev 1871)
@@ -217,9 +217,11 @@
// 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, 10000);
+ 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);
@@ -257,9 +259,11 @@
// 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, 10000);
+ 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);
@@ -398,7 +402,7 @@
tmp_cloud_pt.z = tmp_range * cos(laser_yaw) * sin(laser_pitch);
// add gaussian noise
- const double sigma = 0.02; // 2 centimeter sigma
+ const double sigma = 0.002; // 2 millimeters sigma
tmp_cloud_pt.x = tmp_cloud_pt.x + GaussianKernel(0,sigma);
tmp_cloud_pt.y = tmp_cloud_pt.y + GaussianKernel(0,sigma);
tmp_cloud_pt.z = tmp_cloud_pt.z + GaussianKernel(0,sigma);
@@ -409,6 +413,9 @@
// push pcd point into structure
this->cloud_pts->add((std_msgs::Point3DFloat32)tmp_cloud_pt);
this->cloud_ch1->add(this->intensities[i]);
+
+ this->full_cloud_pts->push_back((std_msgs::Point3DFloat32)tmp_cloud_pt);
+ this->full_cloud_ch1->push_back(this->intensities[i]);
}
/***************************************************************/
/* */
@@ -422,10 +429,10 @@
this->cloudMsg.chan[0].name = "intensities";
this->cloudMsg.chan[0].set_vals_size(this->cloud_ch1->length);
- this->full_cloudMsg.set_pts_size(this->cloud_pts->length);
+ 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->cloud_ch1->length);
+ this->full_cloudMsg.chan[0].set_vals_size(this->full_cloud_ch1->size());
for(int i=0;i< this->cloud_pts->length ;i++)
{
@@ -433,15 +440,16 @@
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];
+ }
- this->full_cloudMsg.pts[i].x = this->cloud_pts->buffer[i].x;
- this->full_cloudMsg.pts[i].y = this->cloud_pts->buffer[i].y;
- this->full_cloudMsg.pts[i].z = this->cloud_pts->buffer[i].z;
- this->full_cloudMsg.chan[0].vals[i] = this->cloud_ch1->buffer[i];
+ for(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("cloud",this->cloudMsg);
- publish("full_cloud",this->full_cloudMsg);
- //publish("shutter",this->shutterMsg);
}
@@ -587,14 +595,17 @@
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 , 1000.0);
- this->PR2Copy->hw.SetJointGains(PR2::HEAD_LASER_PITCH, 10.0, 0.0, 0.0);
+ 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);
if (dAngle * simPitchRate < 0.0)
{
dAngle = -dAngle;
+ publish("full_cloud",this->full_cloudMsg);
publish("shutter",this->shutterMsg);
+ 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
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <MP...@us...> - 2008-07-22 19:44:49
|
Revision: 1906
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1906&view=rev
Author: MPPics
Date: 2008-07-22 19:44:56 +0000 (Tue, 22 Jul 2008)
Log Message:
-----------
Fixes to rosgazebo and image viewing.
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
pkg/trunk/drivers/robot/pr2/libpr2API/src/test/test_FK.cpp
pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp
pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h
pkg/trunk/simulators/rosgazebo/manifest.xml
pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
pkg/trunk/visualization/irrlicht_viewer/ILClient.cc
pkg/trunk/visualization/irrlicht_viewer/cloudGenerator.cc
pkg/trunk/visualization/pr2_gui/src/Vis3d.h
pkg/trunk/visualization/pr2_gui/src/launcherimpl.cpp
pkg/trunk/visualization/pr2_gui/src/launcherimpl.h
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-07-22 19:04:16 UTC (rev 1905)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-07-22 19:44:56 UTC (rev 1906)
@@ -334,9 +334,9 @@
{
NEWMAT::Matrix g(4,4);
g = myHead.ComputeFK(rS.headJntAngles,id-JointStart[HEAD]+1);
- g(1,4) = g(1,4) + BASE_HEAD_OFFSET.x;
- g(2,4) = g(2,4) + BASE_HEAD_OFFSET.y;
- g(3,4) = g(3,4) + BASE_HEAD_OFFSET.z;
+ g(1,4) = g(1,4) + BASE_TORSO_OFFSET.x + TORSO_HEAD_OFFSET.x;
+ g(2,4) = g(2,4) + BASE_TORSO_OFFSET.y + TORSO_HEAD_OFFSET.y;
+ g(3,4) = g(3,4) + BASE_TORSO_OFFSET.z + TORSO_HEAD_OFFSET.z;
return g;
};
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/src/test/test_FK.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/test/test_FK.cpp 2008-07-22 19:04:16 UTC (rev 1905)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/test/test_FK.cpp 2008-07-22 19:44:56 UTC (rev 1906)
@@ -62,11 +62,10 @@
unsigned long time_now_nsec;
// get time from robot
-/* myPR2.InitializeRobot();
- myPR2.GetTime(&time_now);
- time_now_sec = (unsigned long)floor(time_now);
- time_now_nsec = (unsigned long)floor( 1e9 * ( time_now - (double)time_now_sec) );
-*/
+ // myPR2.InitializeRobot();
+ // myPR2.GetTime(&time_now);
+ // time_now_sec = (unsigned long)floor(time_now);
+ // time_now_nsec = (unsigned long)floor( 1e9 * ( time_now - (double)time_now_sec) );
// unsigned long time_now_sec = 0;
// unsigned long time_now_nsec = 0;
@@ -81,24 +80,36 @@
for(ii = (int) PR2::CASTER_FL_STEER; ii <= (int) PR2::CASTER_RR_DRIVE_R; ii++)
{
myPR2.ComputeBodyPose((PR2::PR2_JOINT_ID)ii,rS,&x,&y,&z,&roll,&pitch,&yaw);
-// tfServer.sendEuler((unsigned int) ii,(unsigned int) PR2::PR2_WORLD,x,y,z,yaw,pitch,roll,time_now_sec,time_now_nsec);
- tfServer.sendEuler((unsigned int) ii,(unsigned int) PR2::PR2_WORLD,x,y,z,yaw,pitch,roll,0,0);
- }
+ // tfServer.sendEuler((unsigned int) ii,(unsigned int) PR2::PR2_WORLD,x,y,z,yaw,pitch,roll,time_now_sec,time_now_nsec);
+ //tfServer.sendEuler((unsigned int) ii+1,(unsigned int) PR2::PR2_WORLD+1,x,y,z,yaw,pitch,roll,0,0);
+ tfServer.sendEuler((unsigned int) ii+1,(unsigned int) PR2::PR2_WORLD+1,ii,0,0,0,0,0,0,0);
+ cout << "Base:: " << ii << endl;
+ cout << "pos:: (" << x << "," << y << "," << z << ")" << endl;
+ cout << "rot:: (" << roll << "," << pitch << "," << yaw << ")" << endl;
+ }
// Publish the arms
for(ii = (int) PR2::ARM_L_PAN; ii <= (int) PR2::ARM_R_WRIST_ROLL; ii++)
{
myPR2.ComputeBodyPose((PR2::PR2_JOINT_ID)ii,rS,&x,&y,&z,&roll,&pitch,&yaw);
-// tfServer.sendEuler((unsigned int) ii,(unsigned int) PR2::PR2_WORLD,x,y,z,yaw,pitch,roll,time_now_sec,time_now_nsec);
- tfServer.sendEuler((unsigned int) ii,(unsigned int) PR2::PR2_WORLD,x,y,z,yaw,pitch,roll,0,0);
- }
+ // tfServer.sendEuler((unsigned int) ii,(unsigned int) PR2::PR2_WORLD,x,y,z,yaw,pitch,roll,time_now_sec,time_now_nsec);
+ //tfServer.sendEuler((unsigned int) ii+1,(unsigned int) PR2::PR2_WORLD+1,x,y,z,yaw,pitch,roll,0,0);
+ tfServer.sendEuler((unsigned int) ii+1,(unsigned int) PR2::PR2_WORLD+1,0,0,0,0,0,0,0,0);
+ cout << "Arm:: " << ii << endl;
+ cout << "pos:: (" << x << "," << y << "," << z << ")" << endl;
+ cout << "rot:: (" << roll << "," << pitch << "," << yaw << ")" << endl;
+ }
// Publish the head
for(ii = (int) PR2::HEAD_YAW; ii <= (int) PR2::HEAD_PITCH; ii++)
{
myPR2.ComputeBodyPose((PR2::PR2_JOINT_ID)ii,rS,&x,&y,&z,&roll,&pitch,&yaw);
-// tfServer.sendEuler((unsigned int) ii,(unsigned int) PR2::PR2_WORLD,x,y,z,yaw,pitch,roll,time_now_sec,time_now_nsec);
- tfServer.sendEuler((unsigned int) ii,(unsigned int) PR2::PR2_WORLD,x,y,z,yaw,pitch,roll,0,0);
+ // tfServer.sendEuler((unsigned int) ii,(unsigned int) PR2::PR2_WORLD,x,y,z,yaw,pitch,roll,time_now_sec,time_now_nsec);
+ //tfServer.sendEuler((unsigned int) ii+1,(unsigned int) PR2::PR2_WORLD+1,x,y,z,yaw,pitch,roll,0,0);
+ tfServer.sendEuler((unsigned int) ii+1,(unsigned int) PR2::PR2_WORLD+1,0,0,0,0,0,0,0,0);
+ cout << "Head:: " << ii << endl;
+ cout << "pos:: (" << x << "," << y << "," << z << ")" << endl;
+ cout << "rot:: (" << roll << "," << pitch << "," << yaw << ")" << endl;
}
usleep(1000000);
cout << "Publishing:: " << endl;
Modified: pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp 2008-07-22 19:04:16 UTC (rev 1905)
+++ pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp 2008-07-22 19:44:56 UTC (rev 1906)
@@ -677,8 +677,8 @@
pr2CameraIface->Lock(1);
*width = (uint32_t)pr2CameraIface->data->width;
*height = (uint32_t)pr2CameraIface->data->height;
- *compression = "jpeg";
- *colorspace = "rgb"; //"mono";
+ *compression = "raw";
+ *colorspace = "rgb24"; //"mono";
*data_size = pr2CameraIface->data->image_size;
// on first pass, the sensor does not update after cameraIface is opened.
@@ -689,7 +689,7 @@
uint32_t buf_size = (*width) * (*height) * (*depth);
// copy the image into local buffer
-#if 0
+#if 1
//buf = (void*)(pr2CameraIface->data->image);
memcpy(buf,pr2CameraIface->data->image,buf_size);
#else
Modified: pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h 2008-07-22 19:04:16 UTC (rev 1905)
+++ pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h 2008-07-22 19:44:56 UTC (rev 1906)
@@ -331,9 +331,9 @@
const point3 TORSO_RIGHT_ARM_PAN_OFFSET = { 0 , -0.1329361, 0 };
const point3 ARM_PAN_SHOULDER_PITCH_OFFSET = {0.1 ,0 ,0.5 }; // FIXME: what is z?
const point3 ARM_SHOULDER_PITCH_ROLL_OFFSET = {0 ,0 ,0 };
- const point3 ARM_SHOULDER_ROLL_ELBOW_PITCH_OFFSET = {0.475 ,0 ,0 };
+ const point3 ARM_SHOULDER_ROLL_ELBOW_PITCH_OFFSET = {0.385 ,0 ,0 };//{0.475 ,0 ,0 };
const point3 ELBOW_PITCH_ELBOW_ROLL_OFFSET = {0 ,0 ,0 };//{0.09085 ,0 ,0 };
- const point3 ELBOW_ROLL_WRIST_PITCH_OFFSET = {0.2237 ,0 ,0 };
+ const point3 ELBOW_ROLL_WRIST_PITCH_OFFSET = {0.3137 ,0 ,0 };
const point3 WRIST_PITCH_WRIST_ROLL_OFFSET = {0 ,0 ,0 };
const point3 WRIST_ROLL_GRIPPER_OFFSET = {0 ,0 ,0 };
const point3 SPINE_RIGHT_ARM_OFFSET = {0.0 ,-0.15 ,0.68 };
@@ -344,6 +344,8 @@
const point3 HEAD_PAN_HEAD_PITCH_OFFSET = {0,0,0};
- const point3 BASE_HEAD_OFFSET = {-.1829361, 0, 0.80981};
+ const point3 TORSO_HEAD_OFFSET = {-.02, 0, 0.80981};
+ const point3 TORSO_TILT_LASER_OFFSET = {.07, 0, .68};
+ const point3 BASE_BASE_LASER_OFFSET = {.26, 0,.28};
}
#endif
Modified: pkg/trunk/simulators/rosgazebo/manifest.xml
===================================================================
--- pkg/trunk/simulators/rosgazebo/manifest.xml 2008-07-22 19:04:16 UTC (rev 1905)
+++ pkg/trunk/simulators/rosgazebo/manifest.xml 2008-07-22 19:44:56 UTC (rev 1906)
@@ -13,6 +13,7 @@
<depend package="std_msgs" />
<depend package="rosthread" />
<depend package="rosTF" />
+ <depend package="gazebo_plugin" />
<depend package="math_utils" />
<depend package="string_utils" />
<depend package="libKDL" />
Modified: pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
===================================================================
--- pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp 2008-07-22 19:04:16 UTC (rev 1905)
+++ pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp 2008-07-22 19:44:56 UTC (rev 1906)
@@ -288,10 +288,12 @@
advertise<std_msgs::Image>("image");
advertise<std_msgs::PointCloudFloat32>("cloud");
advertise<std_msgs::PointCloudFloat32>("full_cloud");
+ advertise<std_msgs::PointCloudFloat32>("cloudStereo");
advertise<std_msgs::Empty>("shutter");
advertise<std_msgs::PR2Arm>("left_pr2arm_pos");
advertise<std_msgs::PR2Arm>("right_pr2arm_pos");
advertise<rostools::Time>("time");
+ advertise<std_msgs::Empty>("transform");
subscribe("cmd_vel", velMsg, &RosGazeboNode::cmdvelReceived);
subscribe("cmd_leftarmconfig", leftarm, &RosGazeboNode::cmd_leftarmconfigReceived);
@@ -402,7 +404,7 @@
tmp_cloud_pt.z = tmp_range * cos(laser_yaw) * sin(laser_pitch);
// add gaussian noise
- const double sigma = 0.002; // 2 millimeters sigma
+ const double sigma = 0.002; // 2 millimeter sigma
tmp_cloud_pt.x = tmp_cloud_pt.x + GaussianKernel(0,sigma);
tmp_cloud_pt.y = tmp_cloud_pt.y + GaussianKernel(0,sigma);
tmp_cloud_pt.z = tmp_cloud_pt.z + GaussianKernel(0,sigma);
@@ -695,31 +697,34 @@
roll,
odomMsg.header.stamp.sec,
odomMsg.header.stamp.nsec);
+ //std::cout << "base y p r " << yaw << " " << pitch << " " << roll << std::endl;
// base = center of the bottom of the base box
// torso = midpoint of bottom of turrets
tf.sendEuler(PR2::FRAMEID_TORSO,
PR2::FRAMEID_BASE,
- PR2::BASE_LEFT_ARM_OFFSET.x,
+ PR2::BASE_TORSO_OFFSET.x,
+ PR2::BASE_TORSO_OFFSET.y,
+ PR2::BASE_TORSO_OFFSET.z, /* FIXME: spine elevator not accounted for */
0.0,
- PR2::BASE_LEFT_ARM_OFFSET.z, /* FIXME: spine elevator not accounted for */
0.0,
0.0,
- 0.0,
odomMsg.header.stamp.sec,
odomMsg.header.stamp.nsec);
// arm_l_turret = bottom of left turret
tf.sendEuler(PR2::FRAMEID_ARM_L_TURRET,
PR2::FRAMEID_TORSO,
- 0.0,
- PR2::BASE_LEFT_ARM_OFFSET.y,
- 0.0,
+ PR2::TORSO_LEFT_ARM_PAN_OFFSET.x,
+ PR2::TORSO_LEFT_ARM_PAN_OFFSET.y,
+ PR2::TORSO_LEFT_ARM_PAN_OFFSET.z,
larm.turretAngle,
+ //0.0,
0.0,
0.0,
odomMsg.header.stamp.sec,
odomMsg.header.stamp.nsec);
+ //std::cout << "left pan angle " << larm.turretAngle << std::endl;
// arm_l_shoulder = center of left shoulder pitch bracket
tf.sendEuler(PR2::FRAMEID_ARM_L_SHOULDER,
@@ -821,9 +826,9 @@
// arm_r_turret = bottom of right turret
tf.sendEuler(PR2::FRAMEID_ARM_R_TURRET,
PR2::FRAMEID_TORSO,
- 0.0,
- PR2::BASE_RIGHT_ARM_OFFSET.y,
- 0.0,
+ PR2::TORSO_RIGHT_ARM_PAN_OFFSET.x,
+ PR2::TORSO_RIGHT_ARM_PAN_OFFSET.y,
+ PR2::TORSO_RIGHT_ARM_PAN_OFFSET.z,
rarm.turretAngle,
0.0,
0.0,
@@ -929,12 +934,12 @@
// FIXME: not implemented
tf.sendEuler(PR2::FRAMEID_HEAD_PAN_BASE,
PR2::FRAMEID_TORSO,
+ PR2::TORSO_HEAD_OFFSET.x,
0.0,
+ PR2::TORSO_HEAD_OFFSET.z,
0.0,
- 1.0,
0.0,
0.0,
- 0.0,
odomMsg.header.stamp.sec,
odomMsg.header.stamp.nsec);
@@ -964,10 +969,10 @@
// FIXME: not implemented
tf.sendEuler(PR2::FRAMEID_TILT_LASER_BLOCK,
- PR2::FRAMEID_BASE,
- 0.035,
+ PR2::FRAMEID_TORSO,
+ PR2::TORSO_TILT_LASER_OFFSET.x,
0.0,
- 0.26,
+ PR2::TORSO_TILT_LASER_OFFSET.z,
0.0,
0.0,
0.0,
@@ -977,9 +982,9 @@
// FIXME: not implemented
tf.sendEuler(PR2::FRAMEID_BASE_LASER_BLOCK,
PR2::FRAMEID_BASE,
- 0.035,
+ PR2::BASE_BASE_LASER_OFFSET.x,
0.0,
- 0.26,
+ PR2::BASE_BASE_LASER_OFFSET.z,
0.0,
0.0,
0.0,
@@ -1001,9 +1006,9 @@
PR2::BASE_BODY_OFFSETS[0].x,
PR2::BASE_BODY_OFFSETS[0].y,
PR2::BASE_BODY_OFFSETS[0].z,
+ tmpSteerFL,
0.0,
0.0,
- tmpSteerFL,
odomMsg.header.stamp.sec,
odomMsg.header.stamp.nsec);
tf.sendEuler(PR2::FRAMEID_CASTER_FL_WHEEL_L,
@@ -1032,9 +1037,9 @@
PR2::BASE_BODY_OFFSETS[3].x,
PR2::BASE_BODY_OFFSETS[3].y,
PR2::BASE_BODY_OFFSETS[3].z,
+ tmpSteerFR,
0.0,
0.0,
- tmpSteerFR,
odomMsg.header.stamp.sec,
odomMsg.header.stamp.nsec);
tf.sendEuler(PR2::FRAMEID_CASTER_FR_WHEEL_L,
@@ -1063,9 +1068,9 @@
PR2::BASE_BODY_OFFSETS[6].x,
PR2::BASE_BODY_OFFSETS[6].y,
PR2::BASE_BODY_OFFSETS[6].z,
+ tmpSteerRL,
0.0,
0.0,
- tmpSteerRL,
odomMsg.header.stamp.sec,
odomMsg.header.stamp.nsec);
tf.sendEuler(PR2::FRAMEID_CASTER_RL_WHEEL_L,
@@ -1094,9 +1099,9 @@
PR2::BASE_BODY_OFFSETS[9].x,
PR2::BASE_BODY_OFFSETS[9].y,
PR2::BASE_BODY_OFFSETS[9].z,
+ tmpSteerRR,
0.0,
0.0,
- tmpSteerRR,
odomMsg.header.stamp.sec,
odomMsg.header.stamp.nsec);
tf.sendEuler(PR2::FRAMEID_CASTER_RR_WHEEL_L,
@@ -1121,7 +1126,7 @@
odomMsg.header.stamp.nsec);
-
+ publish("transform",this->shutterMsg);
this->lock.unlock();
}
Modified: pkg/trunk/visualization/irrlicht_viewer/ILClient.cc
===================================================================
--- pkg/trunk/visualization/irrlicht_viewer/ILClient.cc 2008-07-22 19:04:16 UTC (rev 1905)
+++ pkg/trunk/visualization/irrlicht_viewer/ILClient.cc 2008-07-22 19:44:56 UTC (rev 1906)
@@ -62,7 +62,8 @@
s_pRenderer = NULL;
pthread_mutex_destroy(s_pSingletonMutex);
delete s_pSingletonMutex;
- } else {
+ }
+ else {
// Release the lock
pthread_mutex_unlock(s_pSingletonMutex);
}
Modified: pkg/trunk/visualization/irrlicht_viewer/cloudGenerator.cc
===================================================================
--- pkg/trunk/visualization/irrlicht_viewer/cloudGenerator.cc 2008-07-22 19:04:16 UTC (rev 1905)
+++ pkg/trunk/visualization/irrlicht_viewer/cloudGenerator.cc 2008-07-22 19:44:56 UTC (rev 1906)
@@ -1,5 +1,6 @@
#include <ros/node.h>
#include <rostime/clock.h>
+//#include <rostools/Time.h>
#include <std_msgs/PointCloudFloat32.h>
#include <std_msgs/Empty.h>
Modified: pkg/trunk/visualization/pr2_gui/src/Vis3d.h
===================================================================
--- pkg/trunk/visualization/pr2_gui/src/Vis3d.h 2008-07-22 19:04:16 UTC (rev 1905)
+++ pkg/trunk/visualization/pr2_gui/src/Vis3d.h 2008-07-22 19:44:56 UTC (rev 1906)
@@ -31,8 +31,6 @@
/**
@mainpage
-@htmlinclude manifest.html
-
@b Vis3d is a 3D visualization of the robot's current state and sensor feedback using the Irrlicht rendering engine.
<hr>
Modified: pkg/trunk/visualization/pr2_gui/src/launcherimpl.cpp
===================================================================
--- pkg/trunk/visualization/pr2_gui/src/launcherimpl.cpp 2008-07-22 19:04:16 UTC (rev 1905)
+++ pkg/trunk/visualization/pr2_gui/src/launcherimpl.cpp 2008-07-22 19:44:56 UTC (rev 1906)
@@ -4,7 +4,8 @@
:
launcher( parent )
{
-
+ PTZLCodec = new ImageCodec<std_msgs::Image>(&PTZLImage);
+
wxInitAllImageHandlers();
LeftDock_FGS->Hide(HeadLaser_RB,true);
LeftDock_FGS->Hide(Visualization_SBS,true);
@@ -259,7 +260,7 @@
tiltPTZL_S->Enable(true);
zoomPTZL_S->Enable(true);
PTZL_B->Enable(true);
- myNode->subscribe("PTZL_image", PTZLImage, &LauncherImpl::incomingPTZLImageConn,this);
+ myNode->subscribe("image", PTZLImage, &LauncherImpl::incomingPTZLImageConn,this);
myNode->subscribe("PTZL_state", PTZL_state, &LauncherImpl::incomingPTZLState,this);
myNode->advertise<std_msgs::PTZActuatorCmd>("PTZL_cmd");
@@ -297,19 +298,22 @@
{
if(PTZL_GET_NEW_IMAGE)
{
- PTZL_GET_NEW_IMAGE = false;
- const uint32_t count = PTZLImage.get_data_size();
- delete PTZLImageData;
+ PTZL_GET_NEW_IMAGE = false;
+ if(PTZLImage.compression == string("raw"))
+ {
+ PTZLCodec->inflate();
+ PTZLImage.compression = string("jpeg");
+ if(!PTZLCodec->deflate(100))
+ return;
+ }
+ const uint32_t count = PTZLImage.get_data_size();
+ delete PTZLImageData;
PTZLImageData = new uint8_t[count];
memcpy(PTZLImageData, PTZLImage.data, sizeof(uint8_t) * count);
wxMemoryInputStream mis(PTZLImageData,PTZLImage.get_data_size());
delete PTZL_im;
- PTZL_im = new wxImage(mis,wxBITMAP_TYPE_JPEG,-1);
-
- //Event stuff
- wxCommandEvent PTZL_Event(wxEVT_COMMAND_BUTTON_CLICKED, PTZL_B->GetId());
- PTZL_Event.SetEventObject(this);
- this->AddPendingEvent(PTZL_Event);
+ PTZL_im = new wxImage(mis,wxBITMAP_TYPE_ANY,-1);
+
if(PTZL_bmp == NULL){
std::cout << "Layout\n";
wxSize size(PTZLImage.width,PTZLImage.height);
@@ -318,6 +322,11 @@
Layout();
Fit();
}
+
+ //Event stuff
+ wxCommandEvent PTZL_Event(wxEVT_COMMAND_BUTTON_CLICKED, PTZL_B->GetId());
+ PTZL_Event.SetEventObject(this);
+ this->AddPendingEvent(PTZL_Event);
}
//else
//std::cout << "!\n";
@@ -383,19 +392,21 @@
if(PTZR_GET_NEW_IMAGE)
{
PTZR_GET_NEW_IMAGE = false;
+ if(PTZRImage.compression == string("raw"))
+ {
+ PTZRCodec->inflate();
+ PTZRImage.compression = string("jpeg");
+ if(!PTZRCodec->deflate(100))
+ return;
+ }
const uint32_t count = PTZRImage.get_data_size();
delete PTZRImageData;
PTZRImageData = new uint8_t[count];
memcpy(PTZRImageData, PTZRImage.data, sizeof(uint8_t) * count);
wxMemoryInputStream mis(PTZRImageData,PTZRImage.get_data_size());
delete PTZR_im;
- PTZR_im = new wxImage(mis,wxBITMAP_TYPE_JPEG,-1);
-
- //Event stuff
- wxCommandEvent PTZR_Event(wxEVT_COMMAND_BUTTON_CLICKED, PTZR_B->GetId());
- PTZR_Event.SetEventObject(this);
- this->AddPendingEvent(PTZR_Event);
- if(PTZR_bmp == NULL){
+ PTZR_im = new wxImage(mis,wxBITMAP_TYPE_ANY,-1);
+ if(PTZR_bmp == NULL){
std::cout << "Layout\n";
wxSize size(PTZRImage.width,PTZRImage.height);
PTZR_B->SetMinSize(size);
@@ -403,6 +414,10 @@
Layout();
Fit();
}
+ //Event stuff
+ wxCommandEvent PTZR_Event(wxEVT_COMMAND_BUTTON_CLICKED, PTZR_B->GetId());
+ PTZR_Event.SetEventObject(this);
+ this->AddPendingEvent(PTZR_Event);
}
//else
//std::cout << "!\n";
@@ -446,19 +461,21 @@
if(WristL_GET_NEW_IMAGE)
{
WristL_GET_NEW_IMAGE = false;
+ if(WristLImage.compression == string("raw"))
+ {
+ WristLCodec->inflate();
+ WristLImage.compression = string("jpeg");
+ if(!WristLCodec->deflate(100))
+ return;
+ }
const uint32_t count = WristLImage.get_data_size();
delete WristLImageData;
WristLImageData = new uint8_t[count];
memcpy(WristLImageData, WristLImage.data, sizeof(uint8_t) * count);
wxMemoryInputStream mis(WristLImageData,WristLImage.get_data_size());
delete WristL_im;
- WristL_im = new wxImage(mis,wxBITMAP_TYPE_JPEG,-1);
-
- //Event stuff
- wxCommandEvent WristL_Event(wxEVT_COMMAND_BUTTON_CLICKED, WristL_B->GetId());
- WristL_Event.SetEventObject(this);
- this->AddPendingEvent(WristL_Event);
- if(WristL_bmp == NULL){
+ WristL_im = new wxImage(mis,wxBITMAP_TYPE_ANY,-1);
+ if(WristL_bmp == NULL){
std::cout << "Layout\n";
wxSize size(WristLImage.width,WristLImage.height);
WristL_B->SetMinSize(size);
@@ -466,6 +483,11 @@
Layout();
Fit();
}
+ //Event stuff
+ wxCommandEvent WristL_Event(wxEVT_COMMAND_BUTTON_CLICKED, WristL_B->GetId());
+ WristL_Event.SetEventObject(this);
+ this->AddPendingEvent(WristL_Event);
+
}
//else
//std::cout << "!\n";
@@ -510,19 +532,21 @@
if(WristR_GET_NEW_IMAGE)
{
WristR_GET_NEW_IMAGE = false;
+ if(WristRImage.compression == string("raw"))
+ {
+ WristRCodec->inflate();
+ WristRImage.compression = string("jpeg");
+ if(!WristRCodec->deflate(100))
+ return;
+ }
const uint32_t count = WristRImage.get_data_size();
delete WristRImageData;
WristRImageData = new uint8_t[count];
memcpy(WristRImageData, WristRImage.data, sizeof(uint8_t) * count);
wxMemoryInputStream mis(WristRImageData,WristRImage.get_data_size());
delete WristR_im;
- WristR_im = new wxImage(mis,wxBITMAP_TYPE_JPEG,-1);
-
- //Event stuff
- wxCommandEvent WristR_Event(wxEVT_COMMAND_BUTTON_CLICKED, WristR_B->GetId());
- WristR_Event.SetEventObject(this);
- this->AddPendingEvent(WristR_Event);
- if(WristR_bmp == NULL){
+ WristR_im = new wxImage(mis,wxBITMAP_TYPE_ANY,-1);
+ if(WristR_bmp == NULL){
std::cout << "Layout\n";
wxSize size(WristRImage.width,WristRImage.height);
WristR_B->SetMinSize(size);
@@ -530,6 +554,11 @@
Layout();
Fit();
}
+ //Event stuff
+ wxCommandEvent WristR_Event(wxEVT_COMMAND_BUTTON_CLICKED, WristR_B->GetId());
+ WristR_Event.SetEventObject(this);
+ this->AddPendingEvent(WristR_Event);
+
}
//else
//std::cout << "!\n";
Modified: pkg/trunk/visualization/pr2_gui/src/launcherimpl.h
===================================================================
--- pkg/trunk/visualization/pr2_gui/src/launcherimpl.h 2008-07-22 19:04:16 UTC (rev 1905)
+++ pkg/trunk/visualization/pr2_gui/src/launcherimpl.h 2008-07-22 19:44:56 UTC (rev 1906)
@@ -1,7 +1,62 @@
#ifndef __launcherimpl__
#define __launcherimpl__
+///////////////////////////////////////////////////////////////////////////////
+//
+// Copyright (C) 2008, Willow Garage Inc.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of Stanford University nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
/**
+@mainpage
+
+@htmlinclude manifest.html
+
+@b Subclass of the Launcher window
+
+<hr>
+
+@section topic ROS topics
+
+Subscribes to (name/type):
+- @b "PTZL_image"/std_msgs::Image : Image received from the left PTZ
+- @b "PTZR_image"/std_msgs::Image : Image received from the right PTZ
+- @b "WristL_image"/std_msgs::Image : Image received from the left wrist camera
+- @b "WristR_image"/std_msgs::Image : Image received from the right wrist camera
+- @b "PTZL_state"/std_msgs::PTZActuatorState : Receives state from the left PTZ
+- @b "PTZR_state"/std_msgs::PTZActuatorState : Receives state from the right PTZ
+
+Publishes to (name/type):
+- @b "PTZR_cmd"/std_msgs::std_msgs::PTZActuatorCmd : Moves the right PTZ to the given position
+- @b "PTZL_cmd"/std_msgs::std_msgs::PTZActuatorCmd : Moves the left PTZ to the given position
+
+
+@todo
+- Turn me into subclasses and change widgets to the ros panels
+**/
+
+/**
@file
Subclass of launcher, which is generated by wxFormBuilder.
*/
@@ -24,67 +79,126 @@
{
protected:
// Handlers for launcher events.
+ ///Enables and disables the head hokuyo point cloud
void startStopHeadPtCld( wxCommandEvent& event );
+ ///Enables and disables the base mounted hokuyo laser scan
void startStopFloorPtCld( wxCommandEvent& event );
+ ///Enables and disables the stereo vision point cloud
void startStopStereoPtCld( wxCommandEvent& event );
+ ///Enables and disables the 3d model
void startStopModel( wxCommandEvent& event );
+ ///Enables and disalbes the UCS
void startStopUCS( wxCommandEvent& event );
+ ///Enables and disables the grid
void startStopGrid( wxCommandEvent& event );
+ ///Enables and disables the user inserted objects
void startStopObjects( wxCommandEvent& event );
+ ///Changes to the selected view
void viewChanged( wxCommandEvent& event );
+ ///Changes the head hokuyo scan type
void HeadLaserChanged( wxCommandEvent& event );
+ ///Enables or disables the 3d visualization
void startStop_Visualization( wxCommandEvent& event );
+ ///Enables or disables the topdown 2d visualization
void startStop_Topdown( wxCommandEvent& event );
+ ///Enables or disables the left PTZ camera
void startStop_PTZL( wxCommandEvent& event );
+ ///Enables or disables the right PTZ camera
void startStop_PTZR( wxCommandEvent& event );
+ ///Enables or disables the left wrist camera
void startStop_WristL( wxCommandEvent& event );
+ ///Enables or disables the right wrist camera
void startStop_WristR( wxCommandEvent& event );
+ //Stops the robot from moving more (not implemented)
void EmergencyStop( wxCommandEvent& event );
+ ///Draws the left PTZ image
void PTZLDrawPic( wxCommandEvent& event );
+ ///Draws the right PTZ image
void PTZRDrawPic( wxCommandEvent& event );
+ ///Draws the right wrist image
void WristRDrawPic( wxCommandEvent& event );
+ ///Draws the left wrist image
void WristLDrawPic( wxCommandEvent& event );
+ ///Writes to the gui's console (instead of the command line)
void consoleOut(wxString Line);
+ ///(Callback) Gets an image for the left PTZ
void incomingPTZLImageConn();
+ ///(Callback) Gets an image for the right PTZ
void incomingPTZRImageConn();
+ ///(Callback) Gets an image for the right wrist
void incomingWristRImageConn();
+ ///(Callback) Gets an image for the left wrist
void incomingWristLImageConn();
+ ///(Callback) Gets an state for the left PTZ
void incomingPTZLState();
+ ///(Callback) Gets an state for the right PTZ
void incomingPTZRState();
+ ///(Publisher) Sends a position command to the right PTZ
void PTZR_ptzChanged(wxScrollEvent& event);
+ ///(Publisher) Sends a position command to the left PTZ
void PTZL_ptzChanged(wxScrollEvent& event);
public:
//Variables
+ ///ROS node
ros::node *myNode;
+ ///Image for the left PTZ
std_msgs::Image PTZLImage;
+ ///Image for the right PTZ
std_msgs::Image PTZRImage;
+ ///Image for the left wrist
std_msgs::Image WristLImage;
+ ///Image for the right wrist
std_msgs::Image WristRImage;
+ ///Command message shared between the PTZs
std_msgs::PTZActuatorCmd ptz_cmd;
+ ///State of the left PTZ
std_msgs::PTZActuatorState PTZL_state;
+ ///State of the right PTZ
std_msgs::PTZActuatorState PTZR_state;
+ ///3d visualization object (irrlicht window)
Vis3d *vis3d_Window;
+ ///Copy of the left PTZ image data
uint8_t *PTZLImageData;
+ ///Copy of the right PTZ image data
uint8_t *PTZRImageData;
+ ///Copy of the left wrist image data
uint8_t *WristLImageData;
+ ///Copy of the right wrist image data
uint8_t *WristRImageData;
+ ///Flag stating a new frame can be displayed for the left PTZ
bool PTZL_GET_NEW_IMAGE;
+ ///Flag stating a new frame can be displayed for the right PTZ
bool PTZR_GET_NEW_IMAGE;
+ ///Flag stating a new frame can be displayed for the right wrist
bool WristR_GET_NEW_IMAGE;
+ ///Flag stating a new frame can be displayed for the left wrist
bool WristL_GET_NEW_IMAGE;
+ ///wx bitmap object necessary for displaying camera/pictures
wxBitmap *PTZL_bmp;
+ ///wx bitmap object necessary for displaying camera/pictures
wxBitmap *PTZR_bmp;
+ ///wx bitmap object necessary for displaying camera/pictures
wxBitmap *WristR_bmp;
+ ///wx bitmap object necessary for displaying camera/pictures
wxBitmap *WristL_bmp;
+ ///wx ima...
[truncated message content] |
|
From: <cma...@us...> - 2008-07-22 21:32:12
|
Revision: 1926
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1926&view=rev
Author: cmatei18
Date: 2008-07-22 21:32:16 +0000 (Tue, 22 Jul 2008)
Log Message:
-----------
First commit of 0th order object detector and grasp planner.
Added Paths:
-----------
pkg/trunk/grasp_module/
pkg/trunk/grasp_module/grasp_planner/
pkg/trunk/grasp_module/grasp_planner/CMakeLists.txt
pkg/trunk/grasp_module/grasp_planner/Makefile
pkg/trunk/grasp_module/grasp_planner/include/
pkg/trunk/grasp_module/grasp_planner/include/graspPlanner.h
pkg/trunk/grasp_module/grasp_planner/include/graspPoint.h
pkg/trunk/grasp_module/grasp_planner/manifest.xml
pkg/trunk/grasp_module/grasp_planner/src/
pkg/trunk/grasp_module/grasp_planner/src/graspPlanner.cpp
pkg/trunk/grasp_module/grasp_planner/src/graspPoint.cpp
pkg/trunk/grasp_module/object_detector/
pkg/trunk/grasp_module/object_detector/CMakeLists.txt
pkg/trunk/grasp_module/object_detector/Makefile
pkg/trunk/grasp_module/object_detector/include/
pkg/trunk/grasp_module/object_detector/include/object.h
pkg/trunk/grasp_module/object_detector/include/objectDetector.h
pkg/trunk/grasp_module/object_detector/manifest.xml
pkg/trunk/grasp_module/object_detector/src/
pkg/trunk/grasp_module/object_detector/src/object.cpp
pkg/trunk/grasp_module/object_detector/src/objectDetector.cpp
Added: pkg/trunk/grasp_module/grasp_planner/CMakeLists.txt
===================================================================
--- pkg/trunk/grasp_module/grasp_planner/CMakeLists.txt (rev 0)
+++ pkg/trunk/grasp_module/grasp_planner/CMakeLists.txt 2008-07-22 21:32:16 UTC (rev 1926)
@@ -0,0 +1,5 @@
+cmake_minimum_required(VERSION 2.6)
+include(rosbuild)
+rospack(object_detector)
+add_definitions(-Wall)
+rospack_add_library(graspplanner src/graspPoint.cpp src/graspPlanner.cpp)
Added: pkg/trunk/grasp_module/grasp_planner/Makefile
===================================================================
--- pkg/trunk/grasp_module/grasp_planner/Makefile (rev 0)
+++ pkg/trunk/grasp_module/grasp_planner/Makefile 2008-07-22 21:32:16 UTC (rev 1926)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Added: pkg/trunk/grasp_module/grasp_planner/include/graspPlanner.h
===================================================================
--- pkg/trunk/grasp_module/grasp_planner/include/graspPlanner.h (rev 0)
+++ pkg/trunk/grasp_module/grasp_planner/include/graspPlanner.h 2008-07-22 21:32:16 UTC (rev 1926)
@@ -0,0 +1,26 @@
+#ifndef _graspplanner_h_
+#define _graspplanner_h_
+
+#include <vector>
+
+namespace grasp_module {
+
+class GraspPoint;
+class Object;
+
+/*!
+ This class computes GraspPoints from Objects.
+ */
+class GraspPlanner {
+ private:
+ public:
+ //! Constructor does absolutely nothing for now.
+ GraspPlanner(){}
+
+ //! Returns a list of grasp points for a given object
+ std::vector<GraspPoint*> *getGraspPoints(const Object *obj, int resolution = 8);
+};
+
+} //namespace grasp_module
+#endif
+
Added: pkg/trunk/grasp_module/grasp_planner/include/graspPoint.h
===================================================================
--- pkg/trunk/grasp_module/grasp_planner/include/graspPoint.h (rev 0)
+++ pkg/trunk/grasp_module/grasp_planner/include/graspPoint.h 2008-07-22 21:32:16 UTC (rev 1926)
@@ -0,0 +1,102 @@
+#ifndef _grasppoint_h_
+#define _grasppoint_h_
+
+#include <libTF/libTF.h>
+
+namespace NEWMAT {
+ class Matrix;
+}
+
+namespace grasp_module {
+
+/*! A GraspPoint stores the information needed to grasp an object at
+ a certain location.
+
+ For the moment, we are storing this information in the form of a
+ transformation matrix that aligns the gripper with the desired
+ grasping location.
+
+ COORDINATE SYSTEM CONVENTION: here we assume that the GRIPPER
+ coordinate system is as follows:
+
+ - origin is in the middle of the empty space between the jaws
+
+ - x axis is pointing forward (towards the object to be grasped)
+
+ - y axis is along the axis of translation of the parallel jaws
+
+ - z axis is the cross product of the two
+
+ If for example grasping a TALL VERTICAL object such as a bottle,
+ with the jaws on each side of it, we would have the x axis pointing
+ towards the object, the y axis pointing to the left and the z axis
+ pointing up. The origin would be somewhere inside the bottle,
+ presumably at its centroid.
+
+ If this is not what the gripper coordinate system should be, this
+ class as well as the grasp planner need to be modified to reflect
+ that.
+
+ In order to perform a grasp when starting from an instance of the
+ GraspPoint, the following steps have to be performed:
+
+ 1. Place the gripper at the 3D location and orientation specified by
+ the GraspPoint.
+
+ 2. Close the gripper.
+
+ 3. Hope for the best.
+
+ */
+class GraspPoint {
+ private:
+ //! The inner grasp point transform
+ libTF::TransformReference mTran;
+ //! The quality value, between 0 and 1. Higher value means better grasp.
+ float mQuality;
+ public:
+ static const unsigned int baseFrame;
+ static const unsigned int graspFrame;
+ static const unsigned int newFrame;
+
+ //! Empty constructor initializes GraspPoint to identity and quality to 0
+ GraspPoint();
+ //! Constructor taking all parameters
+ GraspPoint(const libTF::TFPoint &c,
+ const libTF::TFVector &app, const libTF::TFVector &up) {
+ setTran(c,app,up);
+ }
+ GraspPoint(const NEWMAT::Matrix *M) {setTran(M);}
+
+ //! Set the grasp point transform using a 4x4 NEWMAT matrix
+ void setTran(const NEWMAT::Matrix *M);
+ //! Set the transform by specifying position and approach (x) and up (z) directions
+ void setTran(const libTF::TFPoint &c,
+ const libTF::TFVector &app, const libTF::TFVector &up);
+
+ //! Returns the inner transform as a row-major float[16]. Caller should free this memory
+ void getTran(float **f);
+ //! Returns the inner transform as a NEWMAT::Matrix
+ void getTran(NEWMAT::Matrix *M);
+
+ //! Set the quality of this grasp. Caps value between 0 and 1
+ void setQuality(float q){if (q<0) q=0; if (q>1) q=1; mQuality = q;}
+ //! Get the quality of this grasp.
+ float getQuality() const {return mQuality;}
+ //! For sorting based on relative quality values.
+ bool operator < (const GraspPoint &rhs) const {return mQuality < rhs.mQuality;}
+};
+
+//!Convenience predicate for sorting containers of pointers to GraspPoints
+ bool sortGraspPredicate(const GraspPoint *lhs, const GraspPoint *rhs);
+
+//a couple of conveniece fctns
+ float norm(const libTF::TFVector &f);
+ libTF::TFVector normalize(const libTF::TFVector &f);
+ float dot(const libTF::TFVector &f1, const libTF::TFVector &f2);
+ libTF::TFVector cross(const libTF::TFVector &f1, const libTF::TFVector &f2);
+
+
+} //namespace grasp_module
+
+#endif
Added: pkg/trunk/grasp_module/grasp_planner/manifest.xml
===================================================================
--- pkg/trunk/grasp_module/grasp_planner/manifest.xml (rev 0)
+++ pkg/trunk/grasp_module/grasp_planner/manifest.xml 2008-07-22 21:32:16 UTC (rev 1926)
@@ -0,0 +1,11 @@
+<package>
+<description>Simple grasp planner based on object centroid and principal components</description>
+<author>Matei Ciocarlie</author>
+<license>BSD</license>
+<depend package="libTF" />
+<depend package="newmat10" />
+<depend package="object_detector" />
+<export>
+ <cpp cflags="-I${prefix}/include" lflags=" -L${prefix}/lib -lgraspplanner -Wl,-rpath,${prefix}/lib"/>
+</export>
+</package>
Added: pkg/trunk/grasp_module/grasp_planner/src/graspPlanner.cpp
===================================================================
--- pkg/trunk/grasp_module/grasp_planner/src/graspPlanner.cpp (rev 0)
+++ pkg/trunk/grasp_module/grasp_planner/src/graspPlanner.cpp 2008-07-22 21:32:16 UTC (rev 1926)
@@ -0,0 +1,109 @@
+#include "graspPlanner.h"
+
+#include <libTF/libTF.h>
+
+#include "object.h"
+#include "graspPoint.h"
+
+#include <algorithm> //for sort
+
+namespace grasp_module{
+
+/*! Given an object, this function will return a vector of the
+ best grasp points it can compute for it. It is the responsability of
+ the caller to free the returned memory.
+
+ The grasp points returned are as follows: The origin of the gripper
+ is placed at the centroid of the object. The "up" direction of the
+ gripper is aligned with the dominant axis (meaning the jaws are on
+ either side of the dominant axis). This is guaranteed for all grasps
+ returned.
+
+ A number of grasps are then obtained by rotating the gripper around
+ the dominant axis of the object. The starting point is the grasp
+ where the approach direction is along the second dominant axis of
+ the object - this is considered the most desirable grasp. Starting
+ from this position, more grasps are obtained by rotating around the
+ dominant axis.
+
+ \param obj - target object
+
+ \param resolution - number of grasps returned. If \a resolution = 1
+ the function only returns the most desirable grasp, with the
+ approach direction along the second dominant axis of the object. If
+ \a resolution > 1 it will sample along the "equator" of the object
+ and return exactly \resolution grasps, spaced equally apart around
+ the object.
+
+ All the grasps have a quality value between 0 and 1 set depending on
+ angular distance between approach direction and second dominant axis
+ of the object. Higher values mean better grasps, and the most
+ desirable grasp with quality = 1 is again considered to be the one
+ along the second dominant axis.
+
+ Returned vector is sorted in ascending quality of the grasps.
+*/
+
+std::vector<GraspPoint*> *GraspPlanner::getGraspPoints(const Object *obj, int resolution)
+{
+ std::vector<GraspPoint*> *grasps = new std::vector<GraspPoint*>;
+ if (resolution < 1) resolution = 1;
+
+ libTF::TFPoint c = obj->getCentroid();
+ libTF::TFVector a1,a2,a3;
+ //axes should already be orthonormal
+ obj->getAxes(a1,a2,a3);
+
+ libTF::TFVector app,up;
+
+ //the only fixed constraint is that we want the jaws to be on either side
+ //of the dominant axis. This should never change
+ up = a1;
+
+ //for starters let's approach along the second axis. This should be the most
+ //desirable grasp, with the jaws along the least dominant axis
+ app = a2;
+
+ //this is the base grasp that all others are computed relative to
+ GraspPoint baseGrasp(c,app,up);
+
+ NEWMAT::Matrix B;
+ baseGrasp.getTran(&B);
+
+ //std::cerr << "B:" << std::endl << B;
+
+ libTF::TransformReference baseTran;
+ baseTran.setWithMatrix( GraspPoint::graspFrame, GraspPoint::baseFrame, B, 0);
+
+ float angleStep = 2.0 * M_PI / resolution;
+ //get other grasp points by rotating the base grasp around its up (or z) direction
+ for (int i=0; i<resolution; i++) {
+
+ //NEWMAT::Matrix M0 = baseTran.getMatrix(GraspPoint::graspFrame, GraspPoint::baseFrame, 0);
+ //std::cerr << "M0:" << std::endl << M0 << std::endl;
+
+ baseTran.setWithEulers( GraspPoint::newFrame, GraspPoint::graspFrame,
+ 0, 0, 0,
+ i * angleStep, 0, 0,
+ 0 );
+
+ //NEWMAT::Matrix M1 = baseTran.getMatrix(GraspPoint::baseFrame, GraspPoint::graspFrame, 0);
+ //std::cerr << "M1:" << std::endl << M1 << std::endl;
+
+ //NEWMAT::Matrix M2 = baseTran.getMatrix(GraspPoint::graspFrame, GraspPoint::newFrame, 0);
+ //std::cerr << "M2:" << std::endl << M2 << std::endl;
+
+ NEWMAT::Matrix M = baseTran.getMatrix(GraspPoint::baseFrame, GraspPoint::newFrame, 0);
+ //std::cerr << "M:" << std::endl << M << std::endl;
+
+ GraspPoint *newGrasp = new GraspPoint(&M);
+ newGrasp->setQuality( fabs( cos(i * angleStep) ) );
+ grasps->push_back(newGrasp);
+ }
+
+ std::sort(grasps->begin(), grasps->end(), sortGraspPredicate);
+ return grasps;
+}
+
+
+} //namespace grasp_module
Added: pkg/trunk/grasp_module/grasp_planner/src/graspPoint.cpp
===================================================================
--- pkg/trunk/grasp_module/grasp_planner/src/graspPoint.cpp (rev 0)
+++ pkg/trunk/grasp_module/grasp_planner/src/graspPoint.cpp 2008-07-22 21:32:16 UTC (rev 1926)
@@ -0,0 +1,148 @@
+#include "graspPoint.h"
+
+#include "newmat10/newmatap.h"
+
+namespace grasp_module {
+
+// A couple of convenience fctns until libTF gets this functionality as well
+float norm(const libTF::TFVector &f)
+{
+ return sqrt( f.x*f.x + f.y*f.y + f.z*f.z);
+}
+
+libTF::TFVector normalize(const libTF::TFVector &f)
+{
+ float n = norm(f);
+ libTF::TFVector p;
+ p.x = f.x / n;
+ p.y = f.y / n;
+ p.z = f.z / n;
+ return p;
+}
+
+float dot(const libTF::TFVector &f1, const libTF::TFVector &f2)
+{
+ return f1.x*f2.x + f1.y*f2.y + f1.z*f2.z;
+}
+
+libTF::TFVector cross(const libTF::TFVector &f1, const libTF::TFVector &f2)
+{
+ libTF::TFVector c;
+ c.x = f1.y * f2.z - f1.z * f2.y;
+ c.y = f1.z * f2.x - f1.x * f2.z;
+ c.z = f1.x * f2.y - f1.y * f2.x;
+ return c;
+}
+
+// Back to the GraspPoint
+
+const unsigned int GraspPoint::baseFrame = 1;
+const unsigned int GraspPoint::graspFrame = 2;
+const unsigned int GraspPoint::newFrame = 3;
+
+ bool sortGraspPredicate(const GraspPoint *lhs, const GraspPoint *rhs) {
+ return (*lhs) < (*rhs);
+ }
+
+GraspPoint::GraspPoint()
+{
+ NEWMAT::Matrix I(4,4);
+ for (int i=0; i<4; i++) {
+ for (int j=0; j<4; j++) {
+ if (i==j) I.element(i,j) = 1;
+ else I.element(i,j) = 0;
+ }
+ }
+ setTran(&I);
+ mQuality = 0;
+}
+
+/*! Sets the inner transform using a 4x4 NEWMAT matrix \a M. No
+ checking is performed at all - the matrix is copied as is.
+ */
+void GraspPoint::setTran(const NEWMAT::Matrix *M)
+{
+ mTran.setWithMatrix( graspFrame, baseFrame, *M, 0);
+}
+
+/*! Sets the inner transform, but also normalizes everything and makes
+ sure is forms a well-behaved right-handed coordinate system.
+
+ \param c - the translation for the grasp point
+
+ \param app - the approach direction, which is made to correspond
+ to the x axis of the gripper. It is normalized, but other than
+ that its direction is left untouched.
+
+ \param up - the up direction, which is made to correspond to the z
+ axis of the gripper. It is normalized, and if it is not
+ perpendicular to \a app it is modified to make a correct
+ coordinate system tranform.
+
+ */
+void GraspPoint::setTran(const libTF::TFPoint &c,
+ const libTF::TFVector &app, const libTF::TFVector &up)
+{
+ libTF::TFVector xaxis, yaxis, zaxis;
+
+ //fprintf(stderr,"App: %f %f %f \n",app.x, app.y, app.z);
+ //fprintf(stderr," Up: %f %f %f \n",up.x, up.y, up.z);
+
+
+ xaxis = normalize(app);
+ zaxis = normalize(up);
+ yaxis = normalize( cross(zaxis,xaxis) );
+ zaxis = cross(xaxis,yaxis);
+
+ NEWMAT::Matrix M(4,4);
+
+ M.element(0,3) = c.x;
+ M.element(1,3) = c.y;
+ M.element(2,3) = c.z;
+ M.element(3,3) = 1.0;
+
+ M.element(0,0) = xaxis.x;
+ M.element(1,0) = xaxis.y;
+ M.element(2,0) = xaxis.z;
+ M.element(3,0) = 0.0;
+
+ M.element(0,1) = yaxis.x;
+ M.element(1,1) = yaxis.y;
+ M.element(2,1) = yaxis.z;
+ M.element(3,1) = 0.0;
+
+ M.element(0,2) = zaxis.x;
+ M.element(1,2) = zaxis.y;
+ M.element(2,2) = zaxis.z;
+ M.element(3,2) = 0.0;
+
+ setTran(&M);
+
+ /*
+ std::cerr << "Matrix:" << std::endl << M;
+ NEWMAT::Matrix Q(4,4);
+ getTran(&Q);
+ std::cerr << "Output matrix:" << std::endl << Q;
+ */
+}
+
+void GraspPoint::getTran(float **f)
+{
+ *f = new float[16];
+ NEWMAT::Matrix M(4,4);
+
+ getTran(&M);
+ for (int i=0; i<4; i++) {
+ for (int j=0; j<4; j++) {
+ (*f)[4*i+j] = M.element(i,j);
+ }
+ }
+}
+
+void GraspPoint::getTran(NEWMAT::Matrix *M)
+{
+ //get matrix is not const...
+ *M = mTran.getMatrix(baseFrame,graspFrame, 0);
+}
+
+} //namespace grasp_module
Added: pkg/trunk/grasp_module/object_detector/CMakeLists.txt
===================================================================
--- pkg/trunk/grasp_module/object_detector/CMakeLists.txt (rev 0)
+++ pkg/trunk/grasp_module/object_detector/CMakeLists.txt 2008-07-22 21:32:16 UTC (rev 1926)
@@ -0,0 +1,5 @@
+cmake_minimum_required(VERSION 2.6)
+include(rosbuild)
+rospack(object_detector)
+add_definitions(-Wall)
+rospack_add_library(objdetector src/object.cpp src/objectDetector.cpp)
Added: pkg/trunk/grasp_module/object_detector/Makefile
===================================================================
--- pkg/trunk/grasp_module/object_detector/Makefile (rev 0)
+++ pkg/trunk/grasp_module/object_detector/Makefile 2008-07-22 21:32:16 UTC (rev 1926)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Added: pkg/trunk/grasp_module/object_detector/include/object.h
===================================================================
--- pkg/trunk/grasp_module/object_detector/include/object.h (rev 0)
+++ pkg/trunk/grasp_module/object_detector/include/object.h 2008-07-22 21:32:16 UTC (rev 1926)
@@ -0,0 +1,75 @@
+#ifndef _object_h_
+#define _object_h_
+
+#include "assert.h"
+
+#include <libTF/libTF.h>
+
+namespace grasp_module {
+
+/*! This class represents an abstraction of an object, as opposed to
+ a set of raw sensor data. It is designed to be an intermediate step
+ between the raw data and the grasp planner: the raw data is
+ segmented into instances of this class, which are in turn passed to
+ the grasp planner.
+
+ For now, this can be considered a stub, holding a very simple
+ implementation. It is designed to work with a simple segmentation of
+ connected componetns from point clouds. It holds the centroid and
+ the principal axes of the object as detected in the point cloud.
+
+ We are working for now with the principle that we only add more data
+ if we need it. In the future, we might therefore add more labels,
+ data points, etc. as needed.
+ */
+
+class Object {
+ private:
+ //! Object centroid
+ libTF::TFPoint mCentroid;
+ //! Object principal axes, ordered from most dominant (\a mA1 ) to least dominant (\a mA3 )
+ libTF::TFVector mA1, mA2, mA3;
+
+ //! Copy another object
+ void copyFrom(const Object &o) {
+ mCentroid = o.mCentroid;
+ mA1 = o.mA1; mA2 = o.mA2; mA3 = o.mA3; }
+
+ public:
+ //! Constructor using centroid and axes information
+ Object(const libTF::TFPoint &c, const libTF::TFVector &a1,
+ const libTF::TFVector &a2, const libTF::TFVector &a3) {
+ setCentroid(c);
+ setAxes(a1,a2,a3);
+ }
+ //! Copy constructor
+ Object(const Object &o){copyFrom(o);}
+ //! Assignment operator
+ Object& operator=(const Object &o){copyFrom(o); return *this;}
+
+ //! Set the centroid of the object
+ void setCentroid(const libTF::TFPoint &c){mCentroid = c;}
+ //! Set the principal axes of the object, from most dominant (\a a1 ) to least dominant ( \a a3 )
+ void setAxes(const libTF::TFVector &a1,
+ const libTF::TFVector &a2,
+ const libTF::TFVector &a3){mA1 = a1; mA2 = a2; mA3 = a3;}
+ //! Set the centroid of this object
+ libTF::TFPoint getCentroid() const {return mCentroid;}
+ //! Get the principal axes of the object, from most dominant (\a a1 ) to least dominant ( \a a3 )
+ void getAxes(libTF::TFVector &a1,
+ libTF::TFVector &a2,
+ libTF::TFVector &a3) const {a1 = mA1; a2 = mA2; a3 = mA3;}
+ //! Get one principal axis. Legal values of \a i are between 1 (most dominant axis) and 3 (least dominant axis)
+ libTF::TFVector getAxis(int i) const {
+ switch(i) {
+ case 1: return mA1; break;
+ case 2: return mA2; break;
+ case 3: return mA3; break;
+ default: assert(0);}
+ }
+
+};
+
+} //namespace grasp_module
+
+#endif
Added: pkg/trunk/grasp_module/object_detector/include/objectDetector.h
===================================================================
--- pkg/trunk/grasp_module/object_detector/include/objectDetector.h (rev 0)
+++ pkg/trunk/grasp_module/object_detector/include/objectDetector.h 2008-07-22 21:32:16 UTC (rev 1926)
@@ -0,0 +1,67 @@
+#ifndef _objectdetector_h_
+#define _objectdetector_h_
+
+/**
+ @mainpage
+ @b A simple object detector that cleans up point clouds
+ and then segments into connected components. See ObjectDetector
+ class for details.
+ **/
+
+#include <vector>
+
+class SmartScan;
+
+namespace grasp_module {
+
+class Object;
+
+/*!
+ A simple detector for objects inside point clouds.
+
+ The main function of this class is \a getObjects(...) which takes in
+ a point cloud and returns the Objects found inside. However, this
+ functionality needs a set of parameters. The empty constructor will
+ initialize all these parameters to some default value, but it is
+ recommended to study the parameters and decide what are the optimal
+ values for the desired application.
+
+ The \a getObjects(...) function also makes a series of assumptions
+ about that is in the point cloud. In this sense, this is a very
+ naive object detector, more of a stub for better things to come. See
+ \a getObjects(...) function for all details.
+ */
+class ObjectDetector {
+ private:
+ //! Parameter for segmentation operation
+ float mGrazThresh;
+ //! Parameter for segmentation operation
+ float mGrazRad;
+ //! Parameter for segmentation operation
+ int mGrazNbrs;
+ //! Parameter for segmentation operation
+ float mRemThresh;
+ //! Parameter for segmentation operation
+ float mFitThresh;
+ //! Parameter for segmentation operation
+ float mConnThresh;
+ //! Parameter for segmentation operation
+ int mSizeThresh;
+
+ public:
+ //! Constructor. Will initialize all unspecified parameters to a default value.
+ ObjectDetector();
+ //! Core function of this class, segments a point cloud into objects.
+ std::vector<Object*> *getObjects(SmartScan *scan);
+
+ //! Set parameters for removing grazing points
+ void setParamGrazing(float thresh, float rad, int nbrs);
+ //! Set paramentes for plane detection and removal
+ void setParamPlane(float thresh, float fit);
+ //! Set paramenters for connected components
+ void setParamComponents(float thresh, int points);
+};
+
+} //namespace grasp_module
+
+#endif
Added: pkg/trunk/grasp_module/object_detector/manifest.xml
===================================================================
--- pkg/trunk/grasp_module/object_detector/manifest.xml (rev 0)
+++ pkg/trunk/grasp_module/object_detector/manifest.xml 2008-07-22 21:32:16 UTC (rev 1926)
@@ -0,0 +1,12 @@
+<package>
+<description>Simple object detector based on point clouds</description>
+<author>Matei Ciocarlie</author>
+<license>BSD</license>
+<depend package="roscpp" />
+<depend package="std_msgs" />
+<depend package="libTF" />
+<depend package="scan_utils" />
+<export>
+ <cpp cflags="-I${prefix}/include" lflags=" -L${prefix}/lib -lobjdetector -Wl,-rpath,${prefix}/lib"/>
+</export>
+</package>
Added: pkg/trunk/grasp_module/object_detector/src/object.cpp
===================================================================
--- pkg/trunk/grasp_module/object_detector/src/object.cpp (rev 0)
+++ pkg/trunk/grasp_module/object_detector/src/object.cpp 2008-07-22 21:32:16 UTC (rev 1926)
@@ -0,0 +1,11 @@
+#include "object.h"
+
+namespace grasp_module {
+
+/*
+all the code fits into the header file for now, so there's nothing
+here. As we add more complex functionality, it will find its place in
+here.
+*/
+
+} //namespace grasp_module
Added: pkg/trunk/grasp_module/object_detector/src/objectDetector.cpp
===================================================================
--- pkg/trunk/grasp_module/object_detector/src/objectDetector.cpp (rev 0)
+++ pkg/trunk/grasp_module/object_detector/src/objectDetector.cpp 2008-07-22 21:32:16 UTC (rev 1926)
@@ -0,0 +1,168 @@
+#include "objectDetector.h"
+
+#include "smartScan.h"
+#include "object.h"
+
+namespace grasp_module {
+
+/*! Empty constructor sets default values for all parameters. These
+ should at least be a good starting point for most situations. See
+ \a setParamXX(...) in this class for parameter details.
+ */
+ObjectDetector::ObjectDetector()
+{
+ mGrazThresh = 10;
+ mGrazRad = 0.01;
+ mGrazNbrs = 5;
+ mRemThresh = 0.02;
+ mFitThresh = 0.7;
+ mConnThresh = 0.02;
+ mSizeThresh = 300;
+}
+
+/*! This function returns the vector of \a Objects found in \a
+ scan. It proceeds through the following steps:
+
+ 1. Removal of ghosting artifacts. We assume point cloud is coming
+ from Hokuyo scanner, which produces artifacts. This step uses \a
+ SmartScan::removeGrazingPoints(...) to eliminate these.
+
+2. Table detection. We assume that a) the object all lie on a planar
+surface such as a table; b) the scan has been pre-cropped so that the
+table is the dominant plane in the scan (no walls, floors etc). This
+step detects the dominant plane in the scan and then removes it.
+
+3. Decompose scan into connected components. Once the table has been
+removed, the connected components that are left should be the objects
+we are interested in. Of course, we might still have stuff under the
+table or way above - in future work we will also do more cropping here
+based on the detected table.
+
+4. Discard potential objects that do not have enough points to be
+considered reliable.
+
+5. Compute controids and principal axes for what's left, using \a
+SmartScan::principalAxes(...)
+
+One potential problem is that this does not leave the scan untouched
+(it has to remove things). Maybe in the future we'll also write a
+version that takes in a \a const \a SmartScan at the price of a couple
+of extra copies.
+ */
+
+std::vector<Object*> *ObjectDetector::getObjects(SmartScan *scan)
+{
+ std::vector<Object*> *objects = new std::vector<Object*>;
+
+ if (mGrazThresh != 0) {
+ fprintf(stderr,"Removing grazing points...");
+ //clean up grazing points and outliers
+ scan->removeGrazingPoints(mGrazThresh, true, mGrazRad, mGrazNbrs); \
+ fprintf(stderr," done.\n");
+ }
+
+ if (mRemThresh != 0) {
+ //find table
+ fprintf(stderr,"Finding table...");
+ std_msgs::Point3DFloat32 planePoint, planeNormal;
+ float fitValue = scan->ransacPlane(planePoint, planeNormal);
+ fprintf(stderr," done.\n");
+
+ //clean up table
+ if (fitValue > mFitThresh) {
+ fprintf(stderr,"Removing table...");
+ scan->removePlane(planePoint, planeNormal, mRemThresh);
+ fprintf(stderr," done.\n");
+ } else {
+ fprintf(stderr,"Plane fit under threshold, we are not removing it...\n");
+ }
+ }
+
+ //remove outliers again
+ fprintf(stderr,"Removing outliers...");
+ scan->removeOutliers(mGrazRad, mGrazNbrs);
+ fprintf(stderr," done.\n");
+
+ //get connected components
+ fprintf(stderr,"Computing connected components...");
+ std::vector<SmartScan*> *components = scan->connectedComponents(mConnThresh);
+ fprintf(stderr," done.\n");
+
+ libTF::TFPoint c;
+ libTF::TFVector a1,a2,a3;
+ SmartScan *comp;
+ fprintf(stderr,"Creating objects...");
+ while (!components->empty()) {
+ comp = components->back();
+ components->pop_back();
+
+ //discard low-volume components
+ if (comp->size() > mSizeThresh) {
+ //compute centroid and principal axes
+ c = comp->centroid();
+ comp->principalAxes(a1,a2,a3);
+
+ //create objects
+ objects->push_back( new Object(c,a1,a2,a3) );
+ }
+ delete comp;
+ }
+ fprintf(stderr," done.\n");
+
+ return objects;
+}
+
+
+/*! There parameters are passed to \a SmartScan::removeGrazingPoints(...)
+
+ \param thresh - distance between point normal and scan
+ perpendicular direction for removal, in degrees. Set to 0 if you
+ want to completely skip the step of removing grazing
+ points. Default = 10
+
+ \param rad \param nbrs - radius and number of neighbors for
+ computing normals. Defaults = 0.01 and 5
+
+ See \a SmartScan::removeGrazingPoints(...) for details
+ */
+void ObjectDetector::setParamGrazing(float thresh, float rad, int nbrs)
+{
+ mGrazThresh = thresh;
+ mGrazRad = rad;
+ mGrazNbrs = nbrs;
+}
+
+/*! These parameters are passed to \a SmartScan::removePlane(...)
+
+ \param thresh - points that are closer than this value to the
+ detected plane of the table are removed. Set to 0 if you want the
+ plane detection and removal skipped entirely. Default = 0.02
+
+ \param fit - if the plane contains less than \a fit * total points
+ in the scan, we consider that the scan actually does not have a
+ dominant plane (such as a table) and therefore we skip the plane
+ removal stage. This value is directly compared to the return value of
+ \a SmartScan::ransacPlane(...). Default = 0.7
+ */
+void ObjectDetector::setParamPlane(float thresh, float fit)
+{
+ mRemThresh = thresh;
+ mFitThresh = fit;
+}
+
+/*! These parameters are passed to \a SmartScan::connectedComponents(...)
+
+ \param thresh - two components are considered connected if they are
+ closer than this value. Default = 0.02
+
+ \param points - only components with more point than this value are
+ kept. Default = 300
+
+ */
+void ObjectDetector::setParamComponents(float thresh, int points)
+{
+ mConnThresh = thresh;
+ mSizeThresh = points;
+}
+
+} //namespace grasp_module
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-07-23 02:26:16
|
Revision: 1961
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1961&view=rev
Author: hsujohnhsu
Date: 2008-07-23 02:26:23 +0000 (Wed, 23 Jul 2008)
Log Message:
-----------
Slow scan rate 1/60Hz for RosGazeboNode.
added math_utils for pr2Controllers.
updates to include flags for gazebo cpp export in manifest.xml to support Ros_Camera plugin.
Laser scan repeated scanning prevention.
Modified Paths:
--------------
pkg/trunk/3rdparty/gazebo/manifest.xml
pkg/trunk/controllers/pr2Controllers/manifest.xml
pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt
pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml
pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
Modified: pkg/trunk/3rdparty/gazebo/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/gazebo/manifest.xml 2008-07-23 02:10:50 UTC (rev 1960)
+++ pkg/trunk/3rdparty/gazebo/manifest.xml 2008-07-23 02:26:23 UTC (rev 1961)
@@ -11,7 +11,7 @@
<depend package="opende" />
<depend package="ogre" />
<export>
- <cpp lflags="-Wl,-rpath,${prefix}/gazebo/lib -L${prefix}/gazebo/lib -lgazebo -lgazeboServer" cflags="-I${prefix}/gazebo/include"/>
+ <cpp lflags="-Wl,-rpath,${prefix}/gazebo/lib -L${prefix}/gazebo/lib -lgazebo -lgazeboServer" cflags="-I${prefix}/gazebo/include -I${prefix}/gazebo-svn/server/sensors -I${prefix}/gazebo-svn/server/sensors/camera -I${prefix}/gazebo-svn/server -I${prefix}/gazebo-svn/server/rendering"/>
</export>
</package>
Modified: pkg/trunk/controllers/pr2Controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/pr2Controllers/manifest.xml 2008-07-23 02:10:50 UTC (rev 1960)
+++ pkg/trunk/controllers/pr2Controllers/manifest.xml 2008-07-23 02:26:23 UTC (rev 1961)
@@ -12,6 +12,7 @@
<depend package="hw_interface" />
<depend package="wg_robot_description_parser" />
<depend package="joy" />
+ <depend package="math_utils" />
<url>http://pr.willowgarage.com</url>
<repository>http://pr.willowgarage.com/repos</repository>
<export>
Modified: pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp 2008-07-23 02:10:50 UTC (rev 1960)
+++ pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp 2008-07-23 02:26:23 UTC (rev 1961)
@@ -481,28 +481,41 @@
gazebo::LaserIface *tmpLaserIface;
- double tmpTime;
- GetSimTime(&tmpTime);
-
switch (id)
{
case LASER_HEAD:
- if (tmpTime == this->lastTiltLaserTime)
- tmpLaserIface = NULL;
- else
+ if (pr2LaserIface->Lock(1))
{
- tmpLaserIface = pr2LaserIface;
- this->lastTiltLaserTime = tmpTime;
+ if (pr2LaserIface->data->head.time == this->lastTiltLaserTime)
+ {
+ tmpLaserIface = NULL;
+ }
+ else
+ {
+ tmpLaserIface = pr2LaserIface;
+ this->lastTiltLaserTime = pr2LaserIface->data->head.time;
+ }
+ pr2LaserIface->Unlock();
}
+ else
+ tmpLaserIface = NULL;
break;
case LASER_BASE:
- if (tmpTime == this->lastBaseLaserTime)
- tmpLaserIface = NULL;
- else
+ if (pr2BaseLaserIface->Lock(1))
{
- tmpLaserIface = pr2BaseLaserIface;
- this->lastBaseLaserTime = tmpTime;
+ if (pr2BaseLaserIface->data->head.time == this->lastBaseLaserTime)
+ {
+ tmpLaserIface = NULL;
+ }
+ else
+ {
+ tmpLaserIface = pr2BaseLaserIface;
+ this->lastBaseLaserTime = pr2BaseLaserIface->data->head.time;
+ }
+ pr2BaseLaserIface->Unlock();
}
+ else
+ tmpLaserIface = NULL;
break;
default:
tmpLaserIface = NULL;
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt 2008-07-23 02:10:50 UTC (rev 1960)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt 2008-07-23 02:26:23 UTC (rev 1961)
@@ -3,6 +3,7 @@
rospack(gazebo_plugin)
rospack_add_library(Pr2_Actarray src/Pr2_Actarray.cc)
rospack_add_library(Pr2_Gripper src/Pr2_Gripper.cc)
+rospack_add_library(Ros_Camera src/Ros_Camera.cc)
rospack_add_library(P3D src/P3D.cc)
# This target requires adding player to the manifest.xml.
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml 2008-07-23 02:10:50 UTC (rev 1960)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml 2008-07-23 02:26:23 UTC (rev 1961)
@@ -13,6 +13,8 @@
<depend package="pr2Core"/>
<depend package="newmat10"/>
<depend package="genericControllers"/>
+<depend package="roscpp"/>
+<depend package="std_msgs"/>
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lPr2_Actarray -lPr2_Gripper -lP3D"/>
</export>
Modified: pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
===================================================================
--- pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp 2008-07-23 02:10:50 UTC (rev 1960)
+++ pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp 2008-07-23 02:26:23 UTC (rev 1961)
@@ -588,7 +588,7 @@
/***************************************************************/
static double dAngle = -1;
double simPitchFreq,simPitchAngle,simPitchRate,simPitchTimeScale,simPitchAmp,simPitchOffset;
- simPitchFreq = 1.0/10.0;
+ simPitchFreq = 1.0/60.0;
simPitchTimeScale = 2.0*M_PI*simPitchFreq;
simPitchAmp = M_PI / 8.0;
simPitchOffset = -M_PI / 8.0;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-07-23 17:30:33
|
Revision: 1978
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1978&view=rev
Author: tfoote
Date: 2008-07-23 17:30:38 +0000 (Wed, 23 Jul 2008)
Log Message:
-----------
switching to namelookup from #defined frame IDs. tested with 2dnav-stage and 2dnav-gazebo
Modified Paths:
--------------
pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo.xml
pkg/trunk/demos/2dnav-stage/2dnav-stage.xml
pkg/trunk/drivers/laser/sicktoolbox_wrapper/manifest.xml
pkg/trunk/drivers/laser/sicktoolbox_wrapper/ros/sicklms/sicklms.cpp
pkg/trunk/drivers/robot/erratic_player/erratic_player.cc
pkg/trunk/drivers/robot/pr2/tilting_laser/src/tilting_laser/tilting_laser.cpp
pkg/trunk/drivers/robot/segway_apox/segway.cpp
pkg/trunk/exec_trex/CalcGraspPositionConstraint.cc
pkg/trunk/exec_trex/ROSNode.cc
pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cc
pkg/trunk/nav/amcl_player/amcl_player.cc
pkg/trunk/nav/nav_view/nav_view.cpp
pkg/trunk/nav/wavefront_player/wavefront_player.cc
pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
pkg/trunk/simulators/rosstage/rosstage.cc
pkg/trunk/util/transforms/namelookup/include/namelookup/nameLookupClient.hh
pkg/trunk/util/transforms/rosTF/include/rosTF/rosTF.h
pkg/trunk/util/transforms/rosTF/src/rosTF.cpp
pkg/trunk/visualization/pr2_gui/src/Vis3d.cpp
Modified: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo.xml 2008-07-23 17:30:27 UTC (rev 1977)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo.xml 2008-07-23 17:30:38 UTC (rev 1978)
@@ -1,6 +1,7 @@
<launch>
<group name="wg">
+ <node pkg="namelookup" type="namelookup_server" respawn="false"/>
<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" />
Modified: pkg/trunk/demos/2dnav-stage/2dnav-stage.xml
===================================================================
--- pkg/trunk/demos/2dnav-stage/2dnav-stage.xml 2008-07-23 17:30:27 UTC (rev 1977)
+++ pkg/trunk/demos/2dnav-stage/2dnav-stage.xml 2008-07-23 17:30:38 UTC (rev 1978)
@@ -1,6 +1,7 @@
<launch>
<group name="wg">
+ <node pkg="namelookup" type="namelookup_server" respawn="true"/>
<node pkg="rosstage" type="rosstage" args="$(find 2dnav-stage)/willow-erratic.world" respawn="false" />
<node pkg="map_server" type="map_server" args="$(find 2dnav-stage)/willow-full.png 0.1" respawn="false" />
<node pkg="amcl_player" type="amcl_player" respawn="false" />
Modified: pkg/trunk/drivers/laser/sicktoolbox_wrapper/manifest.xml
===================================================================
--- pkg/trunk/drivers/laser/sicktoolbox_wrapper/manifest.xml 2008-07-23 17:30:27 UTC (rev 1977)
+++ pkg/trunk/drivers/laser/sicktoolbox_wrapper/manifest.xml 2008-07-23 17:30:38 UTC (rev 1978)
@@ -5,6 +5,7 @@
</description>
<author>Morgan Quigley</author>
<license>BSD</license>
+ <depend package="namelookup"/>
<depend package="sicktoolbox"/>
<depend package="roscpp"/>
<depend package="std_msgs"/>
Modified: pkg/trunk/drivers/laser/sicktoolbox_wrapper/ros/sicklms/sicklms.cpp
===================================================================
--- pkg/trunk/drivers/laser/sicktoolbox_wrapper/ros/sicklms/sicklms.cpp 2008-07-23 17:30:27 UTC (rev 1977)
+++ pkg/trunk/drivers/laser/sicktoolbox_wrapper/ros/sicklms/sicklms.cpp 2008-07-23 17:30:38 UTC (rev 1978)
@@ -34,6 +34,7 @@
#include "ros/node.h"
#include "std_msgs/LaserScan.h"
#include "rosTF/rosTF.h"
+#include "namelookup/nameLookupClient.hh"
using namespace SickToolbox;
using namespace std;
@@ -43,7 +44,7 @@
got_ctrlc = true;
}
-class SickNode : public ros::node
+class SickNode : public ros::node, public nameLookupClient
{
public:
std_msgs::LaserScan scan_msg;
@@ -51,9 +52,9 @@
string port;
int baud;
double last_print_time;
- SickNode() : ros::node("sicklms"), scan_count(0), last_print_time(0)
+ SickNode() : ros::node("sicklms"), scan_count(0), last_print_time(0), nameLookupClient(*(ros::node*)this)
{
- scan_msg.header.frame_id = FRAMEID_LASER;
+ scan_msg.header.frame_id = this->lookup("FRAMEID_LASER");
advertise<std_msgs::LaserScan>("scan");
param("sicklms/port", port, string("/dev/ttyUSB1"));
param("sicklms/baud", baud, 500000);
Modified: pkg/trunk/drivers/robot/erratic_player/erratic_player.cc
===================================================================
--- pkg/trunk/drivers/robot/erratic_player/erratic_player.cc 2008-07-23 17:30:27 UTC (rev 1977)
+++ pkg/trunk/drivers/robot/erratic_player/erratic_player.cc 2008-07-23 17:30:38 UTC (rev 1978)
@@ -288,7 +288,7 @@
en.odom.vel.th = pdata->vel.pa;
en.odom.stall = pdata->stall;
- en.odom.header.frame_id = FRAMEID_ODOM;
+ en.odom.header.frame_id = tf.lookup("FRAMEID_ODOM");
en.odom.header.stamp.sec = (long long unsigned int)floor(hdr->timestamp);
en.odom.header.stamp.sec = (long long unsigned int)((hdr->timestamp - floor(hdr->timestamp)) * 1000000000ULL);
@@ -297,16 +297,16 @@
// Publish the new data
en.publish("odom", en.odom);
- en.tf.sendInverseEuler(FRAMEID_ODOM,
- FRAMEID_ROBOT,
- pdata->pos.px,
- pdata->pos.py,
- 0.0,
- pdata->pos.pa,
- 0.0,
- 0.0,
- (long long unsigned int)floor(hdr->timestamp),
- (long long unsigned int)((hdr->timestamp - floor(hdr->timestamp)) * 1000000000ULL));
+ en.tf.sendInverseEuler("FRAMEID_ODOM",
+ "FRAMEID_ROBOT",
+ pdata->pos.px,
+ pdata->pos.py,
+ 0.0,
+ pdata->pos.pa,
+ 0.0,
+ 0.0,
+ ros::Time((long long unsigned int)floor(hdr->timestamp),
+ (long long unsigned int)((hdr->timestamp - floor(hdr->timestamp)) * 1000000000ULL)));
std::cout <<"Sent 32" <<std::endl;
Modified: pkg/trunk/drivers/robot/pr2/tilting_laser/src/tilting_laser/tilting_laser.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/tilting_laser/src/tilting_laser/tilting_laser.cpp 2008-07-23 17:30:27 UTC (rev 1977)
+++ pkg/trunk/drivers/robot/pr2/tilting_laser/src/tilting_laser/tilting_laser.cpp 2008-07-23 17:30:38 UTC (rev 1978)
@@ -169,11 +169,11 @@
last_motor_time = ros::Time::now();
next_time = ros::Time::now();
- tf.setWithEulers(FRAMEID_LASER2, FRAMEID_TILT_STAGE,
+ tf.setWithEulers(tf.lookup("FRAMEID_LASER2"), tf.lookup("FRAMEID_TILT_STAGE"),
0.0, 0, .02,
0.0, 0.0, 0.0,
last_motor_time.to_ull());
- tf.setWithEulers(FRAMEID_TILT_STAGE, FRAMEID_TILT_BASE,
+ tf.setWithEulers(tf.lookup("FRAMEID_TILT_STAGE"), tf.lookup("FRAMEID_TILT_BASE"),
0, 0, 0,
0, 0, 0,
last_motor_time.to_ull());
@@ -234,9 +234,9 @@
u.y = 0;
u.z = 0;
u.time = scans.header.stamp.to_ull();
- u.frame = FRAMEID_TILT_STAGE;
+ u.frame = tf.lookup("FRAMEID_TILT_STAGE");
- libTF::TFVector v = tf.transformVector(FRAMEID_TILT_BASE,u);
+ libTF::TFVector v = tf.transformVector("FRAMEID_TILT_BASE",u);
double ang = atan2(v.z,v.x);
@@ -288,7 +288,7 @@
long long inc = scans.time_increment * i * 1000000000;
unsigned long long t = scans.header.stamp.to_ull() + inc;
- rot_point = tf.getMatrix(FRAMEID_TILT_BASE,FRAMEID_LASER2,t) * point;
+ rot_point = tf.getMatrix(tf.lookup("FRAMEID_TILT_BASE"),tf.lookup("FRAMEID_LASER2"),t) * point;
if (valid)
{
Modified: pkg/trunk/drivers/robot/segway_apox/segway.cpp
===================================================================
--- pkg/trunk/drivers/robot/segway_apox/segway.cpp 2008-07-23 17:30:27 UTC (rev 1977)
+++ pkg/trunk/drivers/robot/segway_apox/segway.cpp 2008-07-23 17:30:38 UTC (rev 1978)
@@ -64,7 +64,7 @@
tf(*this),
req_timeout(false)
{
- odom.header.frame_id = FRAMEID_ODOM;
+ odom.header.frame_id = tf.lookup("FRAMEID_ODOM");
advertise("odom", odom);
subscribe("cmd_vel", cmd_vel, &Segway::cmd_vel_cb);
subscribe("operating_mode", op_mode, &Segway::op_mode_cb);
@@ -299,16 +299,15 @@
odom.pos.th = odom_yaw;
publish("odom", odom);
- tf.sendInverseEuler(FRAMEID_ODOM,
- FRAMEID_ROBOT,
+ tf.sendInverseEuler("FRAMEID_ODOM",
+ "FRAMEID_ROBOT",
odom.pos.x,
odom.pos.y,
0.0,
odom.pos.th,
0,
0,
- odom.header.stamp.sec,
- odom.header.stamp.nsec);
+ odom.header.stamp);
}
}
last_foreaft = rmp.foreaft;
Modified: pkg/trunk/exec_trex/CalcGraspPositionConstraint.cc
===================================================================
--- pkg/trunk/exec_trex/CalcGraspPositionConstraint.cc 2008-07-23 17:30:27 UTC (rev 1977)
+++ pkg/trunk/exec_trex/CalcGraspPositionConstraint.cc 2008-07-23 17:30:38 UTC (rev 1978)
@@ -50,12 +50,12 @@
aPose.pitch = 0;
aPose.yaw = 0;
aPose.time = 0;
- aPose.frame = FRAMEID_ARM_R_SHOULDER;
+ aPose.frame = tf.lookup("FRAMEID_ARM_R_SHOULDER");
std::cout << "Before\n";
ROSNodeId rni = ROSNode::request();
- libTF::TFPose inShoulderFrame = rni->tf.transformPose(FRAMEID_ODOM, aPose);
+ libTF::TFPose inShoulderFrame = rni->tf.transformPose(tf.lookup("FRAMEID_ODOM"), aPose);
std::cout << "In shoulder frame in base x " << inShoulderFrame.x << std::endl;
std::cout << "In shoulder frame in base y " << inShoulderFrame.y << std::endl;
@@ -76,7 +76,7 @@
bPose.pitch = 0;
bPose.yaw = 0;
bPose.time = 0;
- bPose.frame = FRAMEID_ARM_R_HAND;
+ bPose.frame = tf.loookup("FRAMEID_ARM_R_HAND");
libTF::TFPose inHandFrame = rni->tf.transformPose(FRAMEID_ODOM, bPose);
Modified: pkg/trunk/exec_trex/ROSNode.cc
===================================================================
--- pkg/trunk/exec_trex/ROSNode.cc 2008-07-23 17:30:27 UTC (rev 1977)
+++ pkg/trunk/exec_trex/ROSNode.cc 2008-07-23 17:30:38 UTC (rev 1978)
@@ -69,8 +69,9 @@
// m_rcs_obs.valid = 1;
//copied from wavefront_planner.cc
- this->tf.setWithEulers(FRAMEID_LASER,
- FRAMEID_ROBOT,
+ //TODO change this to broadcast.
+ this->tf.setWithEulers(tf.lookup("FRAMEID_LASER"),
+ tf.lookup("FRAMEID_ROBOT"),
0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0);
this->laser_hitpts_size = this->laser_hitpts_len = 0;
@@ -361,11 +362,11 @@
robotPose.x = 0;
robotPose.y = 0;
robotPose.yaw = 0;
- robotPose.frame = FRAMEID_ROBOT;
+ robotPose.frame = tf.lookup("FRAMEID_ROBOT");
robotPose.time = laserMsg.header.stamp.sec * 1000000000ULL +
laserMsg.header.stamp.nsec; ///HACKE FIXME we should be able to get time somewhere else
try {
- global_pose = this->tf.transformPose2D(FRAMEID_MAP, robotPose);
+ global_pose = this->tf.transformPose2D("FRAMEID_MAP", robotPose);
debugMsg("ROSNode::VS", "New global pose x " << global_pose.x << " y " << global_pose.y);
@@ -705,7 +706,7 @@
local.yaw = 0;
try
{
- global = this->tf.transformPose2D(FRAMEID_MAP, local);
+ global = this->tf.transformPose2D("FRAMEID_MAP", local);
}
catch(libTF::TransformReference::LookupException& ex)
{
Modified: pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cc
===================================================================
--- pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cc 2008-07-23 17:30:27 UTC (rev 1977)
+++ pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cc 2008-07-23 17:30:38 UTC (rev 1978)
@@ -156,9 +156,9 @@
aPose.pitch = 0;
aPose.yaw = 0;
aPose.time = 0;
- aPose.frame = FRAMEID_ARM_R_HAND;
+ aPose.frame = tf.lookup("FRAMEID_ARM_R_HAND");
- libTF::TFPose inOdomFrame = tf.transformPose(FRAMEID_ODOM, aPose);
+ libTF::TFPose inOdomFrame = tf.transformPose("FRAMEID_ODOM", aPose);
std::cout << "In odom frame x " << inOdomFrame.x << std::endl;
std::cout << "In odom frame y " << inOdomFrame.y << std::endl;
@@ -174,9 +174,9 @@
aPose.pitch = 0;
aPose.yaw = 0;
aPose.time = 0;
- aPose.frame = FRAMEID_ODOM;
+ aPose.frame = tf.lookup("FRAMEID_ODOM");
- libTF::TFPose inOdomFrame = tf.transformPose(FRAMEID_ARM_R_SHOULDER, aPose);
+ libTF::TFPose inOdomFrame = tf.transformPose("FRAMEID_ARM_R_SHOULDER", aPose);
std::cout << "In shoulder frame x " << inOdomFrame.x << std::endl;
std::cout << "In shoulder frame y " << inOdomFrame.y << std::endl;
Modified: pkg/trunk/nav/amcl_player/amcl_player.cc
===================================================================
--- pkg/trunk/nav/amcl_player/amcl_player.cc 2008-07-23 17:30:27 UTC (rev 1977)
+++ pkg/trunk/nav/amcl_player/amcl_player.cc 2008-07-23 17:30:38 UTC (rev 1978)
@@ -370,16 +370,16 @@
// publish new transform map->odom
- this->tf->sendEuler(FRAMEID_ROBOT,
- FRAMEID_MAP,
+ this->tf->sendEuler("FRAMEID_ROBOT",
+ "FRAMEID_MAP",
pdata->pos.px,
pdata->pos.py,
0.0,
pdata->pos.pa,
0.0,
0.0,
- (long long unsigned int)floor(hdr->timestamp),
- (long long unsigned int)((hdr->timestamp - floor(hdr->timestamp)) * 1000000000ULL));
+ ros::Time((long long unsigned int)floor(hdr->timestamp),
+ (long long unsigned int)((hdr->timestamp - floor(hdr->timestamp)) * 1000000000ULL)));
/*
printf("lpose: (%.3f %.3f %.3f) @ (%llu:%llu)\n",
Modified: pkg/trunk/nav/nav_view/nav_view.cpp
===================================================================
--- pkg/trunk/nav/nav_view/nav_view.cpp 2008-07-23 17:30:27 UTC (rev 1977)
+++ pkg/trunk/nav/nav_view/nav_view.cpp 2008-07-23 17:30:38 UTC (rev 1978)
@@ -321,10 +321,10 @@
robotPose.x = 0;
robotPose.y = 0;
robotPose.yaw = 0;
- robotPose.frame = FRAMEID_ROBOT;
+ robotPose.frame = tf.lookup("FRAMEID_ROBOT");
robotPose.time = 0;
- libTF::TFPose2D mapPose = tf.transformPose2D(FRAMEID_MAP, robotPose);
+ libTF::TFPose2D mapPose = tf.transformPose2D("FRAMEID_MAP", robotPose);
glPushMatrix();
glTranslatef(mapPose.x, mapPose.y, 0);
Modified: pkg/trunk/nav/wavefront_player/wavefront_player.cc
===================================================================
--- pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-07-23 17:30:27 UTC (rev 1977)
+++ pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-07-23 17:30:38 UTC (rev 1978)
@@ -362,11 +362,13 @@
this->firstodom = true;
+
+ //TODO: broadcast this
// Static robot->laser transform
double laser_x_offset;
param("laser_x_offset", laser_x_offset, 0.05);
- this->tf.setWithEulers(FRAMEID_LASER,
- FRAMEID_ROBOT,
+ this->tf.setWithEulers(tf.lookup("FRAMEID_LASER"),
+ tf.lookup("FRAMEID_ROBOT"),
laser_x_offset, 0.0, 0.0, 0.0, 0.0, 0.0, 0);
advertise<std_msgs::Planner2DState>("state");
@@ -459,7 +461,7 @@
try
{
- global_cloud = this->tf.transformPointCloud(FRAMEID_MAP, local_cloud);
+ global_cloud = this->tf.transformPointCloud("FRAMEID_MAP", local_cloud);
}
catch(libTF::TransformReference::LookupException& ex)
{
@@ -468,7 +470,7 @@
}
catch(libTF::Pose3DCache::ExtrapolateException& ex)
{
- //puts("extrapolation required");
+ // puts("extrapolation required");
continue;
}
@@ -592,13 +594,13 @@
robotPose.x = 0;
robotPose.y = 0;
robotPose.yaw = 0;
- robotPose.frame = FRAMEID_ROBOT;
+ robotPose.frame = tf.lookup("FRAMEID_ROBOT");
robotPose.time = 0; // request most recent pose
//robotPose.time = laserMsg.header.stamp.sec * 1000000000ULL +
// laserMsg.header.stamp.nsec; ///HACKE FIXME we should be able to get time somewhere else
try
{
- global_pose = this->tf.transformPose2D(FRAMEID_MAP, robotPose);
+ global_pose = this->tf.transformPose2D("FRAMEID_MAP", robotPose);
}
catch(libTF::TransformReference::LookupException& ex)
{
Modified: pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
===================================================================
--- pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp 2008-07-23 17:30:27 UTC (rev 1977)
+++ pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp 2008-07-23 17:30:38 UTC (rev 1978)
@@ -478,7 +478,7 @@
this->laserMsg.intensities[i] = this->intensities[i];
}
- this->laserMsg.header.frame_id = FRAMEID_LASER;
+ this->laserMsg.header.frame_id = tf.lookup("FRAMEID_LASER");
this->laserMsg.header.stamp.sec = (unsigned long)floor(this->simTime);
this->laserMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->simTime - this->laserMsg.header.stamp.sec) );
@@ -511,7 +511,7 @@
// this->odomMsg.stall = this->positionmodel->Stall();
// TODO: get the frame ID from somewhere
- this->odomMsg.header.frame_id = FRAMEID_ODOM;
+ this->odomMsg.header.frame_id = tf.lookup("FRAMEID_ODOM");
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) );
@@ -521,16 +521,15 @@
/* frame transforms */
/* */
/***************************************************************/
- tf.sendInverseEuler(FRAMEID_ODOM,
- FRAMEID_ROBOT,
+ tf.sendInverseEuler("FRAMEID_ODOM",
+ "FRAMEID_ROBOT",
odomMsg.pos.x,
odomMsg.pos.y,
0.0,
odomMsg.pos.th,
0.0,
0.0,
- odomMsg.header.stamp.sec,
- odomMsg.header.stamp.nsec);
+ odomMsg.header.stamp);
// This publish call resets odomMsg.header.stamp.sec and
// odomMsg.header.stamp.nsec to zero. Thus, it must be called *after*
@@ -685,34 +684,32 @@
/* */
/***************************************************************/
//this->PR2Copy->GetBasePositionActual(&x,&y,&z,&roll,&pitch,&yaw); // actual CoM of base
- tf.sendEuler(PR2::FRAMEID_BASE,
- FRAMEID_ODOM,
+ tf.sendEuler("FRAMEID_BASE",
+ "FRAMEID_ODOM",
x,
y,
z-0.13, /* half height of base box */
yaw,
pitch,
roll,
- odomMsg.header.stamp.sec,
- odomMsg.header.stamp.nsec);
+ 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
- tf.sendEuler(PR2::FRAMEID_TORSO,
- PR2::FRAMEID_BASE,
+ tf.sendEuler("FRAMEID_TORSO",
+ "FRAMEID_BASE",
PR2::BASE_TORSO_OFFSET.x,
PR2::BASE_TORSO_OFFSET.y,
PR2::BASE_TORSO_OFFSET.z, /* FIXME: spine elevator not accounted for */
0.0,
0.0,
0.0,
- odomMsg.header.stamp.sec,
- odomMsg.header.stamp.nsec);
+ odomMsg.header.stamp);
// arm_l_turret = bottom of left turret
- tf.sendEuler(PR2::FRAMEID_ARM_L_TURRET,
- PR2::FRAMEID_TORSO,
+ tf.sendEuler("FRAMEID_ARM_L_TURRET",
+ "FRAMEID_TORSO",
PR2::TORSO_LEFT_ARM_PAN_OFFSET.x,
PR2::TORSO_LEFT_ARM_PAN_OFFSET.y,
PR2::TORSO_LEFT_ARM_PAN_OFFSET.z,
@@ -720,274 +717,251 @@
//0.0,
0.0,
0.0,
- odomMsg.header.stamp.sec,
- odomMsg.header.stamp.nsec);
+ odomMsg.header.stamp);
//std::cout << "left pan angle " << larm.turretAngle << std::endl;
// arm_l_shoulder = center of left shoulder pitch bracket
- tf.sendEuler(PR2::FRAMEID_ARM_L_SHOULDER,
- PR2::FRAMEID_ARM_L_TURRET,
+ tf.sendEuler("FRAMEID_ARM_L_SHOULDER",
+ "FRAMEID_ARM_L_TURRET",
PR2::ARM_PAN_SHOULDER_PITCH_OFFSET.x,
PR2::ARM_PAN_SHOULDER_PITCH_OFFSET.y,
PR2::ARM_PAN_SHOULDER_PITCH_OFFSET.z,
0.0,
larm.shoulderLiftAngle,
0.0,
- odomMsg.header.stamp.sec,
- odomMsg.header.stamp.nsec);
+ odomMsg.header.stamp);
// arm_l_upperarm = upper arm with roll DOF, at shoulder pitch center
- tf.sendEuler(PR2::FRAMEID_ARM_L_UPPERARM,
- PR2::FRAMEID_ARM_L_SHOULDER,
+ tf.sendEuler("FRAMEID_ARM_L_UPPERARM",
+ "FRAMEID_ARM_L_SHOULDER",
PR2::ARM_SHOULDER_PITCH_ROLL_OFFSET.x,
PR2::ARM_SHOULDER_PITCH_ROLL_OFFSET.y,
PR2::ARM_SHOULDER_PITCH_ROLL_OFFSET.z,
0.0,
0.0,
larm.upperarmRollAngle,
- odomMsg.header.stamp.sec,
- odomMsg.header.stamp.nsec);
+ odomMsg.header.stamp);
//frameid_arm_l_elbow = elbow pitch bracket center of rotation
- tf.sendEuler(PR2::FRAMEID_ARM_L_ELBOW,
- PR2::FRAMEID_ARM_L_UPPERARM,
+ tf.sendEuler("FRAMEID_ARM_L_ELBOW",
+ "FRAMEID_ARM_L_UPPERARM",
PR2::ARM_SHOULDER_ROLL_ELBOW_PITCH_OFFSET.x,
PR2::ARM_SHOULDER_ROLL_ELBOW_PITCH_OFFSET.y,
PR2::ARM_SHOULDER_ROLL_ELBOW_PITCH_OFFSET.z,
0.0,
larm.elbowAngle,
0.0,
- odomMsg.header.stamp.sec,
- odomMsg.header.stamp.nsec);
+ odomMsg.header.stamp);
//frameid_arm_l_forearm = forearm roll DOR, at elbow pitch center
- tf.sendEuler(PR2::FRAMEID_ARM_L_FOREARM,
- PR2::FRAMEID_ARM_L_ELBOW,
+ tf.sendEuler("FRAMEID_ARM_L_FOREARM",
+ "FRAMEID_ARM_L_ELBOW",
PR2::ELBOW_PITCH_ELBOW_ROLL_OFFSET.x,
PR2::ELBOW_PITCH_ELBOW_ROLL_OFFSET.y,
PR2::ELBOW_PITCH_ELBOW_ROLL_OFFSET.z,
0.0,
0.0,
larm.forearmRollAngle,
- odomMsg.header.stamp.sec,
- odomMsg.header.stamp.nsec);
+ odomMsg.header.stamp);
// arm_l_wrist = wrist pitch DOF.
- tf.sendEuler(PR2::FRAMEID_ARM_L_WRIST,
- PR2::FRAMEID_ARM_L_FOREARM,
+ tf.sendEuler("FRAMEID_ARM_L_WRIST",
+ "FRAMEID_ARM_L_FOREARM",
PR2::ELBOW_ROLL_WRIST_PITCH_OFFSET.x,
PR2::ELBOW_ROLL_WRIST_PITCH_OFFSET.y,
PR2::ELBOW_ROLL_WRIST_PITCH_OFFSET.z,
0.0,
larm.wristPitchAngle,
0.0,
- odomMsg.header.stamp.sec,
- odomMsg.header.stamp.nsec);
+ odomMsg.header.stamp);
// arm_l_hand = hand roll DOF, center at wrist pitch center
- tf.sendEuler(PR2::FRAMEID_ARM_L_HAND,
- PR2::FRAMEID_ARM_L_WRIST,
+ tf.sendEuler("FRAMEID_ARM_L_HAND",
+ "FRAMEID_ARM_L_WRIST",
PR2::WRIST_PITCH_WRIST_ROLL_OFFSET.x,
PR2::WRIST_PITCH_WRIST_ROLL_OFFSET.y,
PR2::WRIST_PITCH_WRIST_ROLL_OFFSET.z,
0.0,
0.0,
larm.wristRollAngle,
- odomMsg.header.stamp.sec,
- odomMsg.header.stamp.nsec);
+ odomMsg.header.stamp);
// FIXME: not implemented
- tf.sendEuler(PR2::FRAMEID_ARM_L_FINGER_1,
- PR2::FRAMEID_ARM_L_HAND,
+ tf.sendEuler("FRAMEID_ARM_L_FINGER_1",
+ "FRAMEID_ARM_L_HAND",
0.05,
0.025,
0.0,
0.0,
0.0,
0.0,
- odomMsg.header.stamp.sec,
- odomMsg.header.stamp.nsec);
+ odomMsg.header.stamp);
// FIXME: not implemented
- tf.sendEuler(PR2::FRAMEID_ARM_L_FINGER_2,
- PR2::FRAMEID_ARM_L_HAND,
+ tf.sendEuler("FRAMEID_ARM_L_FINGER_2",
+ "FRAMEID_ARM_L_HAND",
0.05,
-0.025,
0.0,
0.0,
0.0,
0.0,
- odomMsg.header.stamp.sec,
- odomMsg.header.stamp.nsec);
+ odomMsg.header.stamp);
// arm_r_turret = bottom of right turret
- tf.sendEuler(PR2::FRAMEID_ARM_R_TURRET,
- PR2::FRAMEID_TORSO,
+ tf.sendEuler("FRAMEID_ARM_R_TURRET",
+ "FRAMEID_TORSO",
PR2::TORSO_RIGHT_ARM_PAN_OFFSET.x,
PR2::TORSO_RIGHT_ARM_PAN_OFFSET.y,
PR2::TORSO_RIGHT_ARM_PAN_OFFSET.z,
rarm.turretAngle,
0.0,
0.0,
- odomMsg.header.stamp.sec,
- odomMsg.header.stamp.nsec);
+ odomMsg.header.stamp);
// arm_r_shoulder = center of right shoulder pitch bracket
- tf.sendEuler(PR2::FRAMEID_ARM_R_SHOULDER,
- PR2::FRAMEID_ARM_R_TURRET,
+ tf.sendEuler("FRAMEID_ARM_R_SHOULDER",
+ "FRAMEID_ARM_R_TURRET",
PR2::ARM_PAN_SHOULDER_PITCH_OFFSET.x,
PR2::ARM_PAN_SHOULDER_PITCH_OFFSET.y,
PR2::ARM_PAN_SHOULDER_PITCH_OFFSET.z,
0.0,
rarm.shoulderLiftAngle,
0.0,
- odomMsg.header.stamp.sec,
- odomMsg.header.stamp.nsec);
+ odomMsg.header.stamp);
// arm_r_upperarm = upper arm with roll DOF, at shoulder pitch center
- tf.sendEuler(PR2::FRAMEID_ARM_R_UPPERARM,
- PR2::FRAMEID_ARM_R_SHOULDER,
+ tf.sendEuler("FRAMEID_ARM_R_UPPERARM",
+ "FRAMEID_ARM_R_SHOULDER",
PR2::ARM_SHOULDER_PITCH_ROLL_OFFSET.x,
PR2::ARM_SHOULDER_PITCH_ROLL_OFFSET.y,
PR2::ARM_SHOULDER_PITCH_ROLL_OFFSET.z,
0.0,
0.0,
rarm.upperarmRollAngle,
- odomMsg.header.stamp.sec,
- odomMsg.header.stamp.nsec);
+ odomMsg.header.stamp);
//frameid_arm_r_elbow = elbow pitch bracket center of rotation
- tf.sendEuler(PR2::FRAMEID_ARM_R_ELBOW,
- PR2::FRAMEID_ARM_R_UPPERARM,
+ tf.sendEuler("FRAMEID_ARM_R_ELBOW",
+ "FRAMEID_ARM_R_UPPERARM",
PR2::ARM_SHOULDER_ROLL_ELBOW_PITCH_OFFSET.x,
PR2::ARM_SHOULDER_ROLL_ELBOW_PITCH_OFFSET.y,
PR2::ARM_SHOULDER_ROLL_ELBOW_PITCH_OFFSET.z,
0.0,
rarm.elbowAngle,
0.0,
- odomMsg.header.stamp.sec,
- odomMsg.header.stamp.nsec);
+ odomMsg.header.stamp);
//frameid_arm_r_forearm = forearm roll DOR, at elbow pitch center
- tf.sendEuler(PR2::FRAMEID_ARM_R_FOREARM,
- PR2::FRAMEID_ARM_R_ELBOW,
+ tf.sendEuler("FRAMEID_ARM_R_FOREARM",
+ "FRAMEID_ARM_R_ELBOW",
PR2::ELBOW_PITCH_ELBOW_ROLL_OFFSET.x,
PR2::ELBOW_PITCH_ELBOW_ROLL_OFFSET.y,
PR2::ELBOW_PITCH_ELBOW_ROLL_OFFSET.z,
0.0,
0.0,
rarm.forearmRollAngle,
- odomMsg.header.stamp.sec,
- odomMsg.header.stamp.nsec);
+ odomMsg.header.stamp);
// arm_r_wrist = wrist pitch DOF.
- tf.sendEuler(PR2::FRAMEID_ARM_R_WRIST,
- PR2::FRAMEID_ARM_R_FOREARM,
+ tf.sendEuler("FRAMEID_ARM_R_WRIST",
+ "FRAMEID_ARM_R_FOREARM",
PR2::ELBOW_ROLL_WRIST_PITCH_OFFSET.x,
PR2::ELBOW_ROLL_WRIST_PITCH_OFFSET.y,
PR2::ELBOW_ROLL_WRIST_PITCH_OFFSET.z,
0.0,
rarm.wristPitchAngle,
0.0,
- odomMsg.header.stamp.sec,
- odomMsg.header.stamp.nsec);
+ odomMsg.header.stamp);
// arm_r_hand = hand roll DOF, center at wrist pitch center
- tf.sendEuler(PR2::FRAMEID_ARM_R_HAND,
- PR2::FRAMEID_ARM_R_WRIST,
+ tf.sendEuler("FRAMEID_ARM_R_HAND",
+ "FRAMEID_ARM_R_WRIST",
PR2::WRIST_PITCH_WRIST_ROLL_OFFSET.x,
PR2::WRIST_PITCH_WRIST_ROLL_OFFSET.y,
PR2::WRIST_PITCH_WRIST_ROLL_OFFSET.z,
0.0,
0.0,
rarm.wristRollAngle,
- odomMsg.header.stamp.sec,
- odomMsg.header.stamp.nsec);
+ odomMsg.header.stamp);
// FIXME: not implemented
- tf.sendEuler(PR2::FRAMEID_ARM_R_FINGER_1,
- PR2::FRAMEID_ARM_R_HAND,
+ tf.sendEuler("FRAMEID_ARM_R_FINGER_1",
+ "FRAMEID_ARM_R_HAND",
0.05,
0.025,
0.0,
0.0,
0.0,
0.0,
- odomMsg.header.stamp.sec,
- odomMsg.header.stamp.nsec);
+ odomMsg.header.stamp);
// FIXME: not implemented
- tf.sendEuler(PR2::FRAMEID_ARM_R_FINGER_2,
- PR2::FRAMEID_ARM_R_HAND,
+ tf.sendEuler("FRAMEID_ARM_R_FINGER_2",
+ "FRAMEID_ARM_R_HAND",
...
[truncated message content] |
|
From: <hsu...@us...> - 2008-07-23 19:25:36
|
Revision: 1985
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1985&view=rev
Author: hsujohnhsu
Date: 2008-07-23 19:25:45 +0000 (Wed, 23 Jul 2008)
Log Message:
-----------
changed actarray into one list.
added PTZ controls as implemented in Gazebo.
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp
pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h
pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Misc.h
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model
Modified: pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp 2008-07-23 19:24:37 UTC (rev 1984)
+++ pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp 2008-07-23 19:25:45 UTC (rev 1985)
@@ -34,15 +34,16 @@
gazebo::Client *client;
gazebo::SimulationIface *simIface;
gazebo::PR2ArrayIface *pr2Iface;
-gazebo::PR2ArrayIface *pr2HeadIface;
gazebo::PR2GripperIface *pr2GripperLeftIface;
gazebo::PR2GripperIface *pr2GripperRightIface;
gazebo::LaserIface *pr2LaserIface;
gazebo::LaserIface *pr2BaseLaserIface;
gazebo::CameraIface *pr2CameraIface;
gazebo::CameraIface *pr2CameraGlobalIface;
-gazebo::CameraIface *pr2CameraHeadLeftIface;
-gazebo::CameraIface *pr2CameraHeadRightIface;
+gazebo::CameraIface *pr2PTZCameraLeftIface;
+gazebo::CameraIface *pr2PTZCameraRightIface;
+gazebo::PTZIface *pr2PTZLeftIface;
+gazebo::PTZIface *pr2PTZRightIface;
gazebo::PositionIface *pr2LeftWristIface;
gazebo::PositionIface *pr2RightWristIface;
gazebo::PositionIface *pr2BaseIface;
@@ -74,14 +75,15 @@
client = new gazebo::Client();
simIface = new gazebo::SimulationIface();
pr2Iface = new gazebo::PR2ArrayIface();
- pr2HeadIface = new gazebo::PR2ArrayIface();
pr2GripperLeftIface = new gazebo::PR2GripperIface();
pr2GripperRightIface = new gazebo::PR2GripperIface();
pr2LaserIface = new gazebo::LaserIface();
pr2BaseLaserIface = new gazebo::LaserIface();
pr2CameraGlobalIface = new gazebo::CameraIface();
- pr2CameraHeadLeftIface = new gazebo::CameraIface();
- pr2CameraHeadRightIface = new gazebo::CameraIface();
+ pr2PTZCameraLeftIface = new gazebo::CameraIface();
+ pr2PTZCameraRightIface = new gazebo::CameraIface();
+ pr2PTZLeftIface = new gazebo::PTZIface();
+ pr2PTZRightIface = new gazebo::PTZIface();
pr2LeftWristIface = new gazebo::PositionIface();
pr2RightWristIface = new gazebo::PositionIface();
@@ -120,17 +122,28 @@
<< e << "\n";
}
- /// Open the Position interface
+ /// Open the PTZ Position interface
try
{
- pr2HeadIface->Open(client, "pr2_head_iface");
+ pr2PTZLeftIface->Open(client, "ptz_left_iface");
}
catch (std::string e)
{
- std::cout << "Gazebo error: Unable to connect to the pr2 head interface\n"
+ std::cout << "Gazebo error: Unable to connect to the pr2 Left PTZ interface\n"
<< e << "\n";
}
+ /// Open the PTZ Position interface
+ try
+ {
+ pr2PTZRightIface->Open(client, "ptz_right_iface");
+ }
+ catch (std::string e)
+ {
+ std::cout << "Gazebo error: Unable to connect to the pr2 Right PTZ interface\n"
+ << e << "\n";
+ }
+
/// Open the Position interface for gripper left
try
{
@@ -179,7 +192,7 @@
}
- /// Open the camera interface for hokuyo
+ /// Open the camera interfaces
try
{
pr2CameraGlobalIface->Open(client, "cam1_iface_0");
@@ -193,24 +206,24 @@
try
{
- pr2CameraHeadLeftIface->Open(client, "head_cam_left_iface_0");
+ pr2PTZCameraLeftIface->Open(client, "ptz_cam_left_iface");
}
catch (std::string e)
{
std::cout << "Gazebo error: Unable to connect to the pr2 camera interface\n"
<< e << "\n";
- pr2CameraHeadLeftIface = NULL;
+ pr2PTZCameraLeftIface = NULL;
}
try
{
- pr2CameraHeadRightIface->Open(client, "head_cam_right_iface_0");
+ pr2PTZCameraRightIface->Open(client, "ptz_cam_right_iface");
}
catch (std::string e)
{
std::cout << "Gazebo error: Unable to connect to the pr2 camera interface\n"
<< e << "\n";
- pr2CameraHeadRightIface = NULL;
+ pr2PTZCameraRightIface = NULL;
}
try
@@ -248,23 +261,10 @@
std::cout << "initial HW reads\n" << std::endl;
// fill in actuator data
- for (int id = PR2::CASTER_FL_STEER; id < PR2::HEAD_PTZ_R_TILT; id++)
+ for (int id = PR2::HEAD_YAW; id < PR2::HEAD_PTZ_R_TILT; id++)
{
- if(IsHead((PR2::PR2_JOINT_ID)id))
+ if(IsGripperLeft((PR2::PR2_JOINT_ID)id))
{
- pr2HeadIface->Lock(1);
- this->jointData[id].cmdEnableMotor = pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdEnableMotor ;
- this->jointData[id].controlMode = pr2HeadIface->data->actuators[id-JointStart[HEAD]].controlMode ;
- this->jointData[id].pGain = pr2HeadIface->data->actuators[id-JointStart[HEAD]].pGain ;
- this->jointData[id].iGain = pr2HeadIface->data->actuators[id-JointStart[HEAD]].iGain ;
- this->jointData[id].dGain = pr2HeadIface->data->actuators[id-JointStart[HEAD]].dGain ;
- this->jointData[id].cmdPosition = pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdPosition ;
- this->jointData[id].cmdSpeed = pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdSpeed ;
- this->jointData[id].cmdEffectorForce = pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdEffectorForce ;
- pr2HeadIface->Unlock();
- }
- else if(IsGripperLeft((PR2::PR2_JOINT_ID)id))
- {
pr2GripperLeftIface->Lock(1);
this->jointData[id].cmdEnableMotor = pr2GripperLeftIface->data->cmdEnableMotor ;
this->jointData[id].controlMode = pr2GripperLeftIface->data->controlMode ;
@@ -361,9 +361,9 @@
PR2_ERROR_CODE PR2HW::SetJointGains(PR2_JOINT_ID id, double pGain, double iGain, double dGain)
{
- this->jointData[id-JointStart[HEAD]].pGain = pGain;
- this->jointData[id-JointStart[HEAD]].iGain = iGain;
- this->jointData[id-JointStart[HEAD]].dGain = dGain;
+ this->jointData[id].pGain = pGain;
+ this->jointData[id].iGain = iGain;
+ this->jointData[id].dGain = dGain;
return PR2_ALL_OK;
};
@@ -677,10 +677,10 @@
pr2CameraIface = pr2CameraGlobalIface;
break;
case CAMERA_HEAD_LEFT:
- pr2CameraIface = pr2CameraHeadLeftIface;
+ pr2CameraIface = pr2PTZCameraLeftIface;
break;
case CAMERA_HEAD_RIGHT:
- pr2CameraIface = pr2CameraHeadRightIface;
+ pr2CameraIface = pr2PTZCameraRightIface;
break;
default:
pr2CameraIface = pr2CameraGlobalIface;
@@ -827,18 +827,10 @@
{
//std::cout << "updating HW receive\n" << std::endl;
// receive data from hardware
- for (int id = PR2::CASTER_FL_STEER; id < PR2::HEAD_PTZ_R_TILT; id++)
+ for (int id = PR2::HEAD_YAW; id < PR2::HEAD_PTZ_R_TILT; id++)
{
- if(IsHead((PR2::PR2_JOINT_ID)id))
+ if(IsGripperLeft((PR2::PR2_JOINT_ID)id))
{
- pr2HeadIface->Lock(1);
- this->jointData[id].actualPosition = pr2HeadIface->data->actuators[id-JointStart[HEAD]].actualPosition ;
- this->jointData[id].actualSpeed = pr2HeadIface->data->actuators[id-JointStart[HEAD]].actualSpeed ;
- this->jointData[id].actualEffectorForce = pr2HeadIface->data->actuators[id-JointStart[HEAD]].actualEffectorForce;
- pr2HeadIface->Unlock();
- }
- else if(IsGripperLeft((PR2::PR2_JOINT_ID)id))
- {
pr2GripperLeftIface->Lock(1);
this->jointData[id].actualPosition = ( pr2GripperLeftIface->data->actualFingerPosition[0] - 0.015) // FIXME: not debugged
+(-pr2GripperLeftIface->data->actualFingerPosition[1] + 0.015);
@@ -886,23 +878,10 @@
//std::cout << "updating HW send\n" << std::endl;
// send commands to hardware
- for (int id = PR2::CASTER_FL_STEER; id < PR2::HEAD_PTZ_R_TILT; id++)
+ for (int id = PR2::HEAD_YAW; id < PR2::HEAD_PTZ_R_TILT; id++)
{
- if(IsHead((PR2::PR2_JOINT_ID)id))
+ if(IsGripperLeft((PR2::PR2_JOINT_ID)id))
{
- pr2HeadIface->Lock(1);
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdEnableMotor = this->jointData[id].cmdEnableMotor ;
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].controlMode = this->jointData[id].controlMode ;
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].pGain = this->jointData[id].pGain ;
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].iGain = this->jointData[id].iGain ;
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].dGain = this->jointData[id].dGain ;
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdPosition = this->jointData[id].cmdPosition ;
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdSpeed = this->jointData[id].cmdSpeed ;
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdEffectorForce = this->jointData[id].cmdEffectorForce;
- pr2HeadIface->Unlock();
- }
- else if(IsGripperLeft((PR2::PR2_JOINT_ID)id))
- {
pr2GripperLeftIface->Lock(1);
pr2GripperLeftIface->data->cmdEnableMotor = this->jointData[id].cmdEnableMotor ;
pr2GripperLeftIface->data->controlMode = this->jointData[id].controlMode ;
@@ -951,20 +930,10 @@
{
// std::cout << "updating Joint receive\n" << std::endl;
// receive data from hardware
- for (int id = PR2::CASTER_FL_STEER; id < PR2::HEAD_PTZ_R_TILT; id++)
+ for (int id = PR2::HEAD_YAW; id < PR2::HEAD_PTZ_R_TILT; id++)
{
- if(IsHead((PR2::PR2_JOINT_ID)id))
+ if(IsGripperLeft((PR2::PR2_JOINT_ID)id))
{
-
- pr2HeadIface->Lock(1);
- jointArray[id]->position = pr2HeadIface->data->actuators[id-JointStart[HEAD]].actualPosition ;
- jointArray[id]->velocity = pr2HeadIface->data->actuators[id-JointStart[HEAD]].actualSpeed ;
- jointArray[id]->appliedEffort = pr2HeadIface->data->actuators[id-JointStart[HEAD]].actualEffectorForce;
- pr2HeadIface->Unlock();
-
- }
- else if(IsGripperLeft((PR2::PR2_JOINT_ID)id))
- {
/* //Don't look at grippers for now
pr2GripperLeftIface->Lock(1);
this->jointArray[id].actualPosition = ( pr2GripperLeftIface->data->actualFingerPosition[0] - 0.015) // FIXME: not debugged
@@ -1017,27 +986,11 @@
// std::cout << "updating Joint send\n" << std::endl;
// send commands to hardware
- for (int id = PR2::CASTER_FL_STEER; id < PR2::HEAD_PTZ_R_TILT; id++)
+ for (int id = PR2::HEAD_YAW; id < PR2::HEAD_PTZ_R_TILT; id++)
{
- if(IsHead((PR2::PR2_JOINT_ID)id))
+ if(IsGripperLeft((PR2::PR2_JOINT_ID)id))
{
- pr2HeadIface->Lock(1);
-
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdEnableMotor = jointArray[id]->initialized;
/*
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].controlMode = this->jointData[id].controlMode ;
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].pGain = this->jointData[id].pGain ;
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].iGain = this->jointData[id].iGain ;
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].dGain = this->jointData[id].dGain ;
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdPosition = this->jointData[id].cmdPosition ;
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdSpeed = this->jointData[id].cmdSpeed ;
- */
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdEffectorForce = jointArray[id]->commandedEffort;
- pr2HeadIface->Unlock();
- }
- else if(IsGripperLeft((PR2::PR2_JOINT_ID)id))
- {
- /*
pr2GripperLeftIface->Lock(1);
pr2GripperLeftIface->data->cmdEnableMotor = this->jointData[id].cmdEnableMotor ;
pr2GripperLeftIface->data->controlMode = this->jointData[id].controlMode ;
Modified: pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h 2008-07-23 19:24:37 UTC (rev 1984)
+++ pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h 2008-07-23 19:25:45 UTC (rev 1985)
@@ -64,6 +64,9 @@
};
enum PR2_JOINT_ID{
+ HEAD_YAW ,
+ HEAD_PITCH ,
+ HEAD_LASER_PITCH ,
CASTER_FL_STEER , // 0
CASTER_FL_DRIVE_L , // 1
CASTER_FL_DRIVE_R , // 2
@@ -93,9 +96,6 @@
ARM_R_WRIST_ROLL ,
ARM_L_GRIPPER ,
ARM_R_GRIPPER ,
- HEAD_YAW ,
- HEAD_PITCH ,
- HEAD_LASER_PITCH ,
HEAD_PTZ_L_PAN ,
HEAD_PTZ_L_TILT ,
HEAD_PTZ_R_PAN ,
@@ -121,6 +121,8 @@
};
enum PR2_BODY_ID{
+ HEAD_PAN_BASE ,
+ HEAD_TILT_BASE ,
CASTER_FL_WHEEL_L ,
CASTER_FL_WHEEL_R ,
CASTER_FL_BODY ,
@@ -153,8 +155,6 @@
ARM_R_HAND ,
ARM_R_FINGER_1 ,
ARM_R_FINGER_2 ,
- HEAD_PAN_BASE ,
- HEAD_TILT_BASE ,
STEREO_BLOCK ,
TILT_LASER_BLOCK ,
BASE_LASER_BLOCK ,
@@ -287,9 +287,9 @@
};
// JointStart/JointEnd corresponds to the PR2_MODEL_ID, start and end id for each model
- enum PR2_MODEL_ID {PR2_BASE ,PR2_SPINE ,PR2_LEFT_ARM ,PR2_RIGHT_ARM ,PR2_LEFT_GRIPPER ,PR2_RIGHT_GRIPPER ,HEAD , MAX_MODELS };
- const int JointStart[MAX_MODELS] = {CASTER_FL_STEER ,SPINE_ELEVATOR ,ARM_L_PAN ,ARM_R_PAN ,ARM_L_GRIPPER ,ARM_R_GRIPPER ,HEAD_YAW };
- const int JointEnd[MAX_MODELS] = {CASTER_RR_DRIVE_R ,SPINE_ELEVATOR ,ARM_L_WRIST_ROLL ,ARM_R_WRIST_ROLL ,ARM_L_GRIPPER ,ARM_R_GRIPPER ,HEAD_LASER_PITCH };
+ enum PR2_MODEL_ID {PR2_BASE ,PR2_SPINE ,PR2_LEFT_ARM ,PR2_RIGHT_ARM ,PR2_LEFT_GRIPPER ,PR2_RIGHT_GRIPPER ,HEAD ,PR2_LEFT_PTZ ,PR2_RIGHT_PTZ , MAX_MODELS };
+ const int JointStart[MAX_MODELS] = {CASTER_FL_STEER ,SPINE_ELEVATOR ,ARM_L_PAN ,ARM_R_PAN ,ARM_L_GRIPPER ,ARM_R_GRIPPER ,HEAD_YAW ,PR2_LEFT_PTZ ,PR2_RIGHT_PTZ };
+ const int JointEnd[MAX_MODELS] = {CASTER_RR_DRIVE_R ,SPINE_ELEVATOR ,ARM_L_WRIST_ROLL ,ARM_R_WRIST_ROLL ,ARM_L_GRIPPER ,ARM_R_GRIPPER ,HEAD_LASER_PITCH ,PR2_LEFT_PTZ ,PR2_RIGHT_PTZ };
// Geometric description for the base
Modified: pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Misc.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Misc.h 2008-07-23 19:24:37 UTC (rev 1984)
+++ pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Misc.h 2008-07-23 19:25:45 UTC (rev 1985)
@@ -54,6 +54,20 @@
return false;
}
+ inline bool IsPTZLeft(PR2_MODEL_ID id)
+ {
+ if(id == PR2_LEFT_PTZ)
+ return true;
+ return false;
+ }
+
+ inline bool IsPTZRight(PR2_MODEL_ID id)
+ {
+ if(id == PR2_RIGHT_PTZ)
+ return true;
+ return false;
+ }
+
inline bool IsHead(PR2_JOINT_ID id)
{
if(id >= JointStart[HEAD] && id <= JointEnd[HEAD])
@@ -74,6 +88,20 @@
return false;
}
+ inline bool IsPTZRight(PR2_JOINT_ID id)
+ {
+ if (id >= JointStart[PR2_RIGHT_PTZ] && id <= JointEnd[PR2_RIGHT_PTZ])
+ return true;
+ return false;
+ }
+ inline bool IsPTZLeft(PR2_JOINT_ID id)
+ {
+ if (id >= JointStart[PR2_LEFT_PTZ] && id <= JointEnd[PR2_LEFT_PTZ])
+ return true;
+ return false;
+ }
+
+
inline double GetMagnitude(double xl[], int num)
{
int ii;
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model 2008-07-23 19:24:37 UTC (rev 1984)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model 2008-07-23 19:25:45 UTC (rev 1985)
@@ -14,8 +14,6 @@
<xyz>0.0 0.0 0.02</xyz>
<rpy>0.0 0.0 0.0 </rpy>
- <canonicalBody>base_body</canonicalBody>
-
<body:box name="base_body">
<xyz>0.175 0.0 0.15 </xyz> <!-- base bottom is h/2 + some height for wheel clearance-->
<rpy>0.0 0.0 0.0 </rpy>
@@ -52,127 +50,86 @@
</visual>
</geom:box>
- </body:box>
+ <!-- Hokuyo laser body -->
+ <geom:box name="base_laser_box">
+ <xyz>0.275 0.0 0.14</xyz>
+ <rpy>0 0 0</rpy>
+ <size>0.05 0.05 0.041</size>
+ <mass>0.12</mass>
+ <visual>
+ <scale>0.05 0.05 0.041</scale>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Blue</material>
+ </visual>
- <!-- Hokuyo laser body -->
- <model:physical name="base_laser_model">
- <canonicalBody>base_laser_body</canonicalBody>
- <body:box name="base_laser_body">
- <xyz>0.45 0.0 0.27</xyz> <!-- from the robot coordinate system (center bottom of base box)-->
- <rpy>0.0 0.0 0.0</rpy>
-
- <geom:box name="base_laser_box">
- <xyz>0.0 0.0 0.02</xyz>
+ </geom:box>
+ <geom:cylinder name="base_laser_cylinder1">
+ <xyz>0.275 0.0 0.161</xyz>
+ <rpy>0 0 0</rpy>
+ <size>0.02 0.013</size>
+ <mass>0.02</mass>
+
+ <visual>
<rpy>0 0 0</rpy>
- <size>0.05 0.05 0.041</size>
- <mass>0.12</mass>
+ <scale>0.04 0.04 0.013</scale>
+ <mesh>unit_cylinder</mesh>
+ <material>Gazebo/Black</material>
+ </visual>
- <visual>
- <scale>0.05 0.05 0.041</scale>
- <mesh>unit_box</mesh>
- <material>Gazebo/Blue</material>
- </visual>
+ </geom:cylinder>
- </geom:box>
- <geom:cylinder name="base_laser_cylinder1">
- <xyz>0.0 0.0 0.041</xyz>
+ <geom:cylinder name="base_laser_cylinder2">
+ <xyz>0.275 0.0 0.174</xyz>
+ <rpy>0 0 0</rpy>
+ <size>0.018 0.009</size>
+ <mass>0.02</mass>
+
+ <visual>
<rpy>0 0 0</rpy>
- <size>0.02 0.013</size>
- <mass>0.02</mass>
-
- <visual>
- <rpy>0 0 0</rpy>
- <scale>0.04 0.04 0.013</scale>
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Black</material>
- </visual>
+ <size>0.036 0.036 0.009</size>
+ <mesh>unit_cylinder</mesh>
+ <material>Gazebo/Black</material>
+ </visual>
- </geom:cylinder>
+ </geom:cylinder>
- <geom:cylinder name="base_laser_cylinder2">
- <xyz>0.0 0.0 0.054</xyz>
+ <geom:cylinder name="base_laser_cylinder3">
+ <xyz>0.275 0.0 0.183</xyz>
+ <rpy>0 0 0</rpy>
+ <size>0.0175 0.008</size>
+ <mass>0.02</mass>
+
+ <visual>
<rpy>0 0 0</rpy>
- <size>0.018 0.009</size>
- <mass>0.02</mass>
-
- <visual>
- <rpy>0 0 0</rpy>
- <size>0.036 0.036 0.009</size>
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Black</material>
- </visual>
+ <size>0.035 0.035 0.008</size>
+ <mesh>unit_cylinder</mesh>
+ <material>Gazebo/Shell</material>
+ </visual>
+ </geom:cylinder>
- </geom:cylinder>
+ <sensor:ray name="base_laser_1">
+ <origin>0.275 0.0 0.17</origin> <!-- from the robot coordinate system (center bottom of base box)-->
+ <rayCount>68</rayCount>
+ <rangeCount>683</rangeCount>
+ <laserCount>1</laserCount>
- <geom:cylinder name="base_laser_cylinder3">
- <xyz>0.0 0.0 0.063</xyz>
- <rpy>0 0 0</rpy>
- <size>0.0175 0.008</size>
- <mass>0.02</mass>
-
- <visual>
- <rpy>0 0 0</rpy>
- <size>0.035 0.035 0.008</size>
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Shell</material>
- </visual>
+ <minAngle>-135</minAngle>
+ <maxAngle>135</maxAngle>
- </geom:cylinder>
+ <minRange>0.05</minRange>
+ <maxRange>10.0</maxRange>
+ <updateRate>10.0</updateRate>
+ <controller:sicklms200_laser name="base_laser_controller_1">
+ <interface:laser name="base_laser_iface_1"/>
+ <updateRate>10</updateRate>
+ </controller:sicklms200_laser>
+ </sensor:ray>
+ </body:box>
- <sensor:ray name="base_laser_1">
- <rayCount>68</rayCount>
- <rangeCount>683</rangeCount>
- <laserCount>1</laserCount>
- <origin>0.0 0.0 0.05</origin>
- <minAngle>-135</minAngle>
- <maxAngle>135</maxAngle>
-
- <minRange>0.05</minRange>
- <maxRange>10.0</maxRange>
- <updateRate>10.0</updateRate>
- <controller:sicklms200_laser name="base_laser_controller_1">
- <interface:laser name="base_laser_iface_1"/>
- <updateRate>10</updateRate>
- </controller:sicklms200_laser>
- </sensor:ray>
- <!--
- -->
- <!--
- <sensor:ray2 name="laser_1">
- <rayCount>683</rayCount>
- <rangeCount>683</rangeCount>
- <laserCount>1</laserCount>
- <origin>0.0 0.0 0.05</origin>
-
- <minAngle>-135</minAngle>
- <maxAngle>135</maxAngle>
-
- <minRange>0.05</minRange>
- <maxRange>10.0</maxRange>
- <controller:sick2lms200_laser name="laser_controller_1">
- <interface:laser name="laser_iface_1"/>
- <updateRate>10</updateRate>
- </controller:sick2lms200_laser>
- </sensor:ray2>
- -->
-
- </body:box>
-
- <!-- attach hokuyo to torso -->
- <attach>
- <parentBody>base_body</parentBody>
- <myBody>base_laser_body</myBody>
- </attach>
- </model:physical>
-
-
-
<!-- ========== spine and shoulder =========== -->
- <canonicalBody>torso_body</canonicalBody>
-
<body:box name="torso_body">
<xyz>0.056 0.0 0.45</xyz>
<rpy>0.0 0.0 0.0</rpy>
@@ -851,469 +808,399 @@
<!-- neck body -->
- <model:physical name="neck_model">
- <body:box name="neck_body">
- <xyz>0.10 0.0 1.03</xyz>
- <rpy>0.0 0.0 0.0</rpy>
- <!--xyz>0.0 0.0 0.0</xyz>
- <rpy>0.0 0.0 0.0</rpy-->
- <geom:box name="neck_geom">
+ <body:box name="neck_body">
+ <xyz>0.10 0.0 1.03</xyz>
+ <rpy>0.0 0.0 0.0</rpy>
+ <!--xyz>0.0 0.0 0.0</xyz>
+ <rpy>0.0 0.0 0.0</rpy-->
+ <geom:box name="neck_geom">
+ <!--size>.20 .38 .14</size-->
+ <size>0.01 0.01 0.01</size>
+ <mass>0.01</mass>
+ <visual>
+ <mesh>unit_box</mesh>
<!--size>.20 .38 .14</size-->
<size>0.01 0.01 0.01</size>
- <mass>0.01</mass>
- <visual>
- <mesh>unit_box</mesh>
- <!--size>.20 .38 .14</size-->
- <size>0.01 0.01 0.01</size>
- <material>Gazebo/Black</material>
- </visual>
- </geom:box>
- </body:box>
+ <material>Gazebo/Black</material>
+ </visual>
+ </geom:box>
+ </body:box>
- <!-- attach neck to torso -->
- <attach>
- <parentBody>torso_body</parentBody>
- <myBody>neck_body</myBody>
- </attach>
+ <!-- attach neck to torso -->
+ <joint:hinge name="neck_torso_attach_joint">
+ <body1>torso_body</body1>
+ <body2>neck_body</body2>
+ <anchor>neck_body</anchor>
+ <axis> 1.0 0.0 0.0 </axis>
+ <lowStop> 0.0 </lowStop>
+ <highStop> 0.0 </highStop>
+ </joint:hinge>
- <canonicalBody>head_base_body</canonicalBody>
- <body:box name="head_base_body">
- <xyz>0.10 0.0 1.13 </xyz>
- <rpy>0.0 0.0 0.0 </rpy>
- <!-- report when this body collides with static bodies -->
- <reportStaticCollision>true</reportStaticCollision>
- <geom:box name="head_base_geom">
- <xyz> 0.0 0.00 0.000</xyz>
+
+ <body:box name="head_base_body">
+ <xyz>0.10 0.0 1.13 </xyz>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <!-- report when this body collides with static bodies -->
+ <reportStaticCollision>true</reportStaticCollision>
+ <geom:box name="head_base_geom">
+ <xyz> 0.0 0.00 0.000</xyz>
+ <size>0.165 0.21 0.003</size>
+ <mass>0.1</mass>
+ <mul>1</mul>
+ <!--color> 0.0 1.0 0.0</color-->
+ <visual>
<size>0.165 0.21 0.003</size>
- <mass>0.1</mass>
- <mul>1</mul>
- <!--color> 0.0 1.0 0.0</color-->
- <visual>
- <size>0.165 0.21 0.003</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/PioneerBody</material>
- </visual>
- </geom:box>
- </body:box>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/PioneerBody</material>
+ </visual>
+ </geom:box>
+ </body:box>
- <canonicalBody>head_tilt_body</canonicalBody>
- <body:box name="head_tilt_body">
- <xyz>0.10 0.0 1.07 </xyz>
- <rpy>0.0 0.0 0.0 </rpy>
- <!-- report when this body collides with static bodies -->
- <reportStaticCollision>true</reportStaticCollision>
- <geom:box name="head_tilt_geom">
- <xyz> 0.0 0.00 0.150</xyz>
+ <body:box name="head_tilt_body">
+ <xyz>0.10 0.0 1.07 </xyz>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <!-- report when this body collides with static bodies -->
+ <reportStaticCollision>true</reportStaticCollision>
+ <geom:box name="head_tilt_geom">
+ <xyz> 0.0 0.00 0.150</xyz>
+ <size>0.165 0.21 0.003</size>
+ <mass>0.1</mass>
+ <mul>1</mul>
+ <!--color> 0.0 1.0 0.0</color-->
+ <visual>
<size>0.165 0.21 0.003</size>
- <mass>0.1</mass>
- <mul>1</mul>
- <!--color> 0.0 1.0 0.0</color-->
- <visual>
- <size>0.165 0.21 0.003</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/Black</material>
- </visual>
- </geom:box>
- <geom:box name="head_tilt_left_geom">
- <xyz> 0.0 -0.105 0.10</xyz>
- <size>0.165 0.003 0.10</size>
- <mass>0.1</mass>
- <mul>1</mul>
- <!--color> 0.0 1.0 0.0</color-->
- <visual>
- <size>0.165 0.003 0.10</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/Shell</material>
- </visual>
- </geom:box>
- <geom:box name="head_tilt_right_geom">
- <xyz> 0.0 0.105 0.10</xyz>
- <size>0.165 0.003 0.10</size>
- <mass>0.1</mass>
- <mul>1</mul>
- <!--color> 0.0 1.0 0.0</color-->
- <visual>
- <size>0.165 0.003 0.10</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/Shell</material>
- </visual>
- </geom:box>
- </body:box>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Black</material>
+ </visual>
+ </geom:box>
+ <geom:box name="head_tilt_left_geom">
+ <xyz> 0.0 -0.105 0.10</xyz>
+ <size>0.165 0.003 0.10</size>
+ <mass>0.1</mass>
+ <mul>1</mul>
+ <!--color> 0.0 1.0 0.0</color-->
+ <visual>
+ <size>0.165 0.003 0.10</size>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Shell</material>
+ </visual>
+ </geom:box>
+ <geom:box name="head_tilt_right_geom">
+ <xyz> 0.0 0.105 0.10</xyz>
+ <size>0.165 0.003 0.10</size>
+ <mass>0.1</mass>
+ <mul>1</mul>
+ <!--color> 0.0 1.0 0.0</color-->
+ <visual>
+ <size>0.165 0.003 0.10</size>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Shell</material>
+ </visual>
+ </geom:box>
+ </body:box>
- <!-- attach head_base to head_tilt -->
- <joint:hinge name="head_tilt_joint">
- <body2>head_tilt_body</body2>
- <body1>head_base_body</body1>
- <anchor>head_tilt_body</anchor>
- <axis> 0.0 1.0 0.0 </axis>
- </joint:hinge>
+ <!-- attach head_base to head_tilt -->
+ <joint:hinge name="head_tilt_joint">
+ <body2>head_tilt_body</body2>
+ <body1>head_base_body</body1>
+ <anchor>head_tilt_body</anchor>
+ <axis> 0.0 1.0 0.0 </axis>
+ </joint:hinge>
- <!-- attach head to neck -->
- <joint:hinge name="neck_yaw_joint">
- <body1>neck_body</body1>
- <body2>head_base_body</body2>
- <anchor>head_base_body</anchor>
- <axis> 0.0 0.0 1.0 </axis>
- </joint:hinge>
+ <!-- attach head to neck -->
+ <joint:hinge name="neck_yaw_joint">
+ <body1>neck_body</body1>
+ <body2>head_base_body</body2>
+ <anchor>head_base_body</anchor>
+ <axis> 0.0 0.0 1.0 </axis>
+ </joint:hinge>
- <!-- left camera -->
- <body:box name="head_cam_left_body">
- <xyz> 0.15 0.23 1.07 </xyz>
- <rpy> 0.00 0.0 0.00 </rpy>
- <sensor:camera name="head_cam_left_sensor">
- <imageSize>640 480</imageSize>
- <hfov>60</hfov>
- <nearClip>0.1</nearClip>
- <farClip>100</farClip>
+ <!-- left camera -->
+ <body:box name="ptz_cam_left_body">
+ <xyz> 0.13 0.30 1.04 </xyz>
+ <rpy> 0.00 0.0 0.00 </rpy>
+ <sensor:camera name="ptz_cam_left_sensor">
+ <imageSize>640 480</imageSize>
+ <hfov>60</hfov>
+ <nearClip>0.1</nearClip>
+ <farClip>100</farClip>
+ <updateRate>15.0</updateRate>
+ <controller:generic_camera name="ptz_cam_left_controller">
<updateRate>15.0</updateRate>
- <controller:generic_camera name="head_cam_left_controller">
- <updateRate>15.0</updateRate>
- <interface:camera name="head_cam_left_iface_0" />
- </controll...
[truncated message content] |
|
From: <hsu...@us...> - 2008-07-23 21:35:17
|
Revision: 1996
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1996&view=rev
Author: hsujohnhsu
Date: 2008-07-23 21:35:25 +0000 (Wed, 23 Jul 2008)
Log Message:
-----------
added forearm cameras.
broadcast all cameras on ros.
added time detect and repeat prevention for camera data sending.
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h
pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp
pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2-test.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_torque.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_torque_control.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_torque_position.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_tray1.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_trimesh.model
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_wg.world
pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h
pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
Modified: pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h 2008-07-23 21:21:46 UTC (rev 1995)
+++ pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h 2008-07-23 21:35:25 UTC (rev 1996)
@@ -362,6 +362,13 @@
private: double lastTiltLaserTime;
private: double lastBaseLaserTime;
+ private: double lastCameraGlobalTime;
+ private: double lastPTZCameraLeftTime;
+ private: double lastPTZCameraRightTime;
+ private: double lastWristCameraLeftTime;
+ private: double lastWristCameraRightTime;
+ private: double lastForearmCameraLeftTime;
+ private: double lastForearmCameraRightTime;
};
Modified: pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp 2008-07-23 21:21:46 UTC (rev 1995)
+++ pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp 2008-07-23 21:35:25 UTC (rev 1996)
@@ -38,7 +38,6 @@
gazebo::PR2GripperIface *pr2GripperRightIface;
gazebo::LaserIface *pr2LaserIface;
gazebo::LaserIface *pr2BaseLaserIface;
-gazebo::CameraIface *pr2CameraIface;
gazebo::CameraIface *pr2CameraGlobalIface;
gazebo::CameraIface *pr2PTZCameraLeftIface;
gazebo::CameraIface *pr2PTZCameraRightIface;
@@ -47,6 +46,10 @@
gazebo::PositionIface *pr2LeftWristIface;
gazebo::PositionIface *pr2RightWristIface;
gazebo::PositionIface *pr2BaseIface;
+gazebo::CameraIface *pr2WristCameraLeftIface;
+gazebo::CameraIface *pr2WristCameraRightIface;
+gazebo::CameraIface *pr2ForearmCameraLeftIface;
+gazebo::CameraIface *pr2ForearmCameraRightIface;
////////////////////////////////////////////////////////////////////
// //
@@ -84,6 +87,10 @@
pr2PTZCameraRightIface = new gazebo::CameraIface();
pr2PTZLeftIface = new gazebo::PTZIface();
pr2PTZRightIface = new gazebo::PTZIface();
+ pr2WristCameraLeftIface = new gazebo::CameraIface();
+ pr2WristCameraRightIface = new gazebo::CameraIface();
+ pr2ForearmCameraLeftIface = new gazebo::CameraIface();
+ pr2ForearmCameraRightIface = new gazebo::CameraIface();
pr2LeftWristIface = new gazebo::PositionIface();
pr2RightWristIface = new gazebo::PositionIface();
@@ -144,6 +151,47 @@
<< e << "\n";
}
+ /// Open the wrist and forearm cameras
+ try
+ {
+ pr2WristCameraLeftIface->Open(client, "wrist_cam_left_iface");
+ }
+ catch (std::string e)
+ {
+ std::cout << "Gazebo error: Unable to connect to the pr2 Left wrist camera interface\n"
+ << e << "\n";
+ }
+
+ try
+ {
+ pr2WristCameraRightIface->Open(client, "wrist_cam_right_iface");
+ }
+ catch (std::string e)
+ {
+ std::cout << "Gazebo error: Unable to connect to the pr2 Right wrist camera interface\n"
+ << e << "\n";
+ }
+
+ try
+ {
+ pr2ForearmCameraLeftIface->Open(client, "forearm_cam_left_iface");
+ }
+ catch (std::string e)
+ {
+ std::cout << "Gazebo error: Unable to connect to the pr2 Left forearm camera interface\n"
+ << e << "\n";
+ }
+
+ try
+ {
+ pr2ForearmCameraRightIface->Open(client, "forearm_cam_right_iface");
+ }
+ catch (std::string e)
+ {
+ std::cout << "Gazebo error: Unable to connect to the pr2 Right forearm camera interface\n"
+ << e << "\n";
+ }
+
/// Open the Position interface for gripper left
try
{
@@ -307,6 +355,13 @@
GetSimTime(&(this->lastTiltLaserTime));
GetSimTime(&(this->lastBaseLaserTime));
+ GetSimTime(&(this->lastCameraGlobalTime ));
+ GetSimTime(&(this->lastPTZCameraLeftTime ));
+ GetSimTime(&(this->lastPTZCameraRightTime ));
+ GetSimTime(&(this->lastWristCameraLeftTime ));
+ GetSimTime(&(this->lastWristCameraRightTime ));
+ GetSimTime(&(this->lastForearmCameraLeftTime ));
+ GetSimTime(&(this->lastForearmCameraRightTime ));
return PR2_ALL_OK;
};
@@ -671,28 +726,153 @@
uint32_t* data_size ,void* buf )
{
+ gazebo::CameraIface *tmpCameraIface;
+
switch(id)
{
case CAMERA_GLOBAL:
- pr2CameraIface = pr2CameraGlobalIface;
+ if (pr2CameraGlobalIface && pr2CameraGlobalIface->Lock(1))
+ {
+ if (pr2CameraGlobalIface->data->head.time == this->lastCameraGlobalTime)
+ {
+ tmpCameraIface = NULL;
+ }
+ else
+ {
+ tmpCameraIface = pr2CameraGlobalIface;
+ this->lastCameraGlobalTime = pr2CameraGlobalIface->data->head.time;
+ }
+ pr2CameraGlobalIface->Unlock();
+ }
+ else
+ tmpCameraIface = NULL;
+
break;
case CAMERA_HEAD_LEFT:
- pr2CameraIface = pr2PTZCameraLeftIface;
+ if (pr2PTZCameraLeftIface && pr2PTZCameraLeftIface->Lock(1))
+ {
+ if (pr2PTZCameraLeftIface->data->head.time == this->lastPTZCameraLeftTime)
+ {
+ tmpCameraIface = NULL;
+ }
+ else
+ {
+ tmpCameraIface = pr2PTZCameraLeftIface;
+ this->lastPTZCameraLeftTime = pr2PTZCameraLeftIface->data->head.time;
+ }
+ pr2PTZCameraLeftIface->Unlock();
+ }
+ else
+ tmpCameraIface = NULL;
+
break;
case CAMERA_HEAD_RIGHT:
- pr2CameraIface = pr2PTZCameraRightIface;
+ if (pr2PTZCameraRightIface && pr2PTZCameraRightIface->Lock(1))
+ {
+ if (pr2PTZCameraRightIface->data->head.time == this->lastPTZCameraRightTime)
+ {
+ tmpCameraIface = NULL;
+ }
+ else
+ {
+ tmpCameraIface = pr2PTZCameraRightIface;
+ this->lastPTZCameraRightTime = pr2PTZCameraRightIface->data->head.time;
+ }
+ pr2PTZCameraRightIface->Unlock();
+ }
+ else
+ tmpCameraIface = NULL;
+
break;
+ case CAMERA_WRIST_LEFT:
+ if (pr2WristCameraLeftIface && pr2WristCameraLeftIface->Lock(1))
+ {
+ if (pr2WristCameraLeftIface->data->head.time == this->lastWristCameraLeftTime)
+ {
+ tmpCameraIface = NULL;
+ }
+ else
+ {
+ tmpCameraIface = pr2WristCameraLeftIface;
+ this->lastWristCameraLeftTime = pr2WristCameraLeftIface->data->head.time;
+ }
+ pr2WristCameraLeftIface->Unlock();
+ }
+ else
+ tmpCameraIface = NULL;
+
+ break;
+ case CAMERA_WRIST_RIGHT:
+ if (pr2WristCameraRightIface && pr2WristCameraRightIface->Lock(1))
+ {
+ if (pr2WristCameraRightIface->data->head.time == this->lastWristCameraRightTime)
+ {
+ tmpCameraIface = NULL;
+ }
+ else
+ {
+ tmpCameraIface = pr2WristCameraRightIface;
+ this->lastWristCameraRightTime = pr2WristCameraRightIface->data->head.time;
+ }
+ pr2WristCameraRightIface->Unlock();
+ }
+ else
+ tmpCameraIface = NULL;
+
+ break;
+ case CAMERA_FOREARM_LEFT:
+ if (pr2ForearmCameraLeftIface && pr2ForearmCameraLeftIface->Lock(1))
+ {
+ if (pr2ForearmCameraLeftIface->data->head.time == this->lastForearmCameraLeftTime)
+ {
+ tmpCameraIface = NULL;
+ }
+ else
+ {
+ tmpCameraIface = pr2ForearmCameraLeftIface;
+ this->lastForearmCameraLeftTime = pr2ForearmCameraLeftIface->data->head.time;
+ }
+ pr2ForearmCameraLeftIface->Unlock();
+ }
+ else
+ tmpCameraIface = NULL;
+
+ break;
+ case CAMERA_FOREARM_RIGHT:
+ if (pr2ForearmCameraRightIface && pr2ForearmCameraRightIface->Lock(1))
+ {
+ if (pr2ForearmCameraRightIface->data->head.time == this->lastForearmCameraRightTime)
+ {
+ tmpCameraIface = NULL;
+ }
+ else
+ {
+ tmpCameraIface = pr2ForearmCameraRightIface;
+ this->lastForearmCameraRightTime = pr2ForearmCameraRightIface->data->head.time;
+ }
+ pr2ForearmCameraRightIface->Unlock();
+ }
+ else
+ tmpCameraIface = NULL;
+
+ break;
default:
- pr2CameraIface = pr2CameraGlobalIface;
+ tmpCameraIface = NULL;
break;
}
- pr2CameraIface->Lock(1);
- *width = (uint32_t)pr2CameraIface->data->width;
- *height = (uint32_t)pr2CameraIface->data->height;
+ if (tmpCameraIface == NULL)
+ {
+ return PR2_ERROR;
+ }
+ else
+ {
+ tmpCameraIface->Lock(1);
+ *width = (uint32_t)tmpCameraIface->data->width;
+ *height = (uint32_t)tmpCameraIface->data->height;
*compression = "raw";
*colorspace = "rgb24"; //"mono";
- *data_size = pr2CameraIface->data->image_size;
+ *data_size = tmpCameraIface->data->image_size;
// on first pass, the sensor does not update after cameraIface is opened.
if (*data_size > 0)
@@ -703,22 +883,23 @@
// copy the image into local buffer
#if 1
- //buf = (void*)(pr2CameraIface->data->image);
- memcpy(buf,pr2CameraIface->data->image,buf_size);
+ //buf = (void*)(tmpCameraIface->data->image);
+ memcpy(buf,tmpCameraIface->data->image,buf_size);
#else
for (uint32_t i = 0; i < buf_size ; i=i+3)
{
// flip red and blue
- ((unsigned char*)buf)[i ] = pr2CameraIface->data->image[i+2];
- ((unsigned char*)buf)[i+1] = pr2CameraIface->data->image[i+1];
- ((unsigned char*)buf)[i+2] = pr2CameraIface->data->image[i ];
- //printf("%d %d\n",i,pr2CameraIface->data->image[i]);
+ ((unsigned char*)buf)[i ] = tmpCameraIface->data->image[i+2];
+ ((unsigned char*)buf)[i+1] = tmpCameraIface->data->image[i+1];
+ ((unsigned char*)buf)[i+2] = tmpCameraIface->data->image[i ];
+ //printf("%d %d\n",i,tmpCameraIface->data->image[i]);
}
#endif
}
- pr2CameraIface->Unlock();
+ tmpCameraIface->Unlock();
return PR2_ALL_OK;
+ }
};
PR2_ERROR_CODE PR2HW::GetWristPoseGroundTruth(PR2_MODEL_ID id, double *x, double *y, double *z, double *roll, double *pitch, double *yaw)
Modified: pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h 2008-07-23 21:21:46 UTC (rev 1995)
+++ pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h 2008-07-23 21:35:25 UTC (rev 1996)
@@ -111,8 +111,10 @@
CAMERA_STEREO_RIGHT ,
CAMERA_HEAD_LEFT ,
CAMERA_HEAD_RIGHT ,
- CAMERA_ARM_LEFT ,
- CAMERA_ARM_RIGHT ,
+ CAMERA_FOREARM_LEFT ,
+ CAMERA_FOREARM_RIGHT ,
+ CAMERA_WRIST_LEFT ,
+ CAMERA_WRIST_RIGHT ,
LASER_HEAD ,
LASER_BASE ,
TACTILE_FINGER_LEFT ,
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2-test.model
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2-test.model 2008-07-23 21:21:46 UTC (rev 1995)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2-test.model 2008-07-23 21:35:25 UTC (rev 1996)
@@ -54,116 +54,118 @@
<!-- Hokuyo laser body -->
- <model:physical name="base_laser_model">
- <body:box name="base_laser_body">
- <xyz>0.45 0.0 0.27</xyz> <!-- from the robot coordinate system (center bottom of base box)-->
- <rpy>0.0 0.0 0.0</rpy>
-
- <geom:box name="base_laser_box">
- <xyz>0.0 0.0 0.02</xyz>
- <rpy>0 0 0</rpy>
- <size>0.05 0.05 0.041</size>
- <mass>0.12</mass>
+ <body:box name="base_laser_body">
+ <xyz>0.45 0.0 0.27</xyz> <!-- from the robot coordinate system (center bottom of base box)-->
+ <rpy>0.0 0.0 0.0</rpy>
+
+ <geom:box name="base_laser_box">
+ <xyz>0.0 0.0 0.02</xyz>
+ <rpy>0 0 0</rpy>
+ <size>0.05 0.05 0.041</size>
+ <mass>0.12</mass>
- <visual>
- <scale>0.05 0.05 0.041</scale>
- <mesh>unit_box</mesh>
- <material>Gazebo/Blue</material>
- </visual>
+ <visual>
+ <scale>0.05 0.05 0.041</scale>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Blue</material>
+ </visual>
- </geom:box>
- <geom:cylinder name="base_laser_cylinder1">
- <xyz>0.0 0.0 0.041</xyz>
+ </geom:box>
+ <geom:cylinder name="base_laser_cylinder1">
+ <xyz>0.0 0.0 0.041</xyz>
+ <rpy>0 0 0</rpy>
+ <size>0.02 0.013</size>
+ <mass>0.02</mass>
+
+ <visual>
<rpy>0 0 0</rpy>
- <size>0.02 0.013</size>
- <mass>0.02</mass>
-
- <visual>
- <rpy>0 0 0</rpy>
- <scale>0.04 0.04 0.013</scale>
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Black</material>
- </visual>
+ <scale>0.04 0.04 0.013</scale>
+ <mesh>unit_cylinder</mesh>
+ <material>Gazebo/Black</material>
+ </visual>
- </geom:cylinder>
+ </geom:cylinder>
- <geom:cylinder name="base_laser_cylinder2">
- <xyz>0.0 0.0 0.054</xyz>
+ <geom:cylinder name="base_laser_cylinder2">
+ <xyz>0.0 0.0 0.054</xyz>
+ <rpy>0 0 0</rpy>
+ <size>0.018 0.009</size>
+ <mass>0.02</mass>
+
+ <visual>
<rpy>0 0 0</rpy>
- <size>0.018 0.009</size>
- <mass>0.02</mass>
-
- <visual>
- <rpy>0 0 0</rpy>
- <size>0.036 0.036 0.009</size>
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Black</material>
- </visual>
+ <size>0.036 0.036 0.009</size>
+ <mesh>unit_cylinder</mesh>
+ <material>Gazebo/Black</material>
+ </visual>
- </geom:cylinder>
+ </geom:cylinder>
- <geom:cylinder name="base_laser_cylinder3">
- <xyz>0.0 0.0 0.063</xyz>
+ <geom:cylinder name="base_laser_cylinder3">
+ <xyz>0.0 0.0 0.063</xyz>
+ <rpy>0 0 0</rpy>
+ <size>0.0175 0.008</size>
+ <mass>0.02</mass>
+
+ <visual>
<rpy>0 0 0</rpy>
- <size>0.0175 0.008</size>
- <mass>0.02</mass>
-
- <visual>
- <rpy>0 0 0</rpy>
- <size>0.035 0.035 0.008</size>
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Shell</material>
- </visual>
+ <size>0.035 0.035 0.008</size>
+ <mesh>unit_cylinder</mesh>
+ <material>Gazebo/Shell</material>
+ </visual>
- </geom:cylinder>
+ </geom:cylinder>
- <sensor:ray name="base_laser_1">
- <rayCount>68</rayCount>
- <rangeCount>683</rangeCount>
- <laserCount>1</laserCount>
- <origin>0.0 0.0 0.05</origin>
+ <sensor:ray name="base_laser_1">
+ <rayCount>68</rayCount>
+ <rangeCount>683</rangeCount>
+ <laserCount>1</laserCount>
+ <origin>0.0 0.0 0.05</origin>
- <minAngle>-135</minAngle>
- <maxAngle>135</maxAngle>
+ <minAngle>-135</minAngle>
+ <maxAngle>135</maxAngle>
- <minRange>0.05</minRange>
- <maxRange>10.0</maxRange>
- <updateRate>10.0</updateRate>
- <controller:sicklms200_laser name="base_laser_controller_1">
- <interface:laser name="base_laser_iface_1"/>
- <updateRate>10</updateRate>
- </controller:sicklms200_laser>
- </sensor:ray>
- <!--
- -->
- <!--
- <sensor:ray2 name="laser_1">
- <rayCount>683</rayCount>
- <rangeCount>683</rangeCount>
- <laserCount>1</laserCount>
- <origin>0.0 0.0 0.05</origin>
+ <minRange>0.05</minRange>
+ <maxRange>10.0</maxRange>
+ <updateRate>10.0</updateRate>
+ <controller:sicklms200_laser name="base_laser_controller_1">
+ <interface:laser name="base_laser_iface_1"/>
+ <updateRate>10</updateRate>
+ </controller:sicklms200_laser>
+ </sensor:ray>
+ <!--
+ -->
+ <!--
+ <sensor:ray2 name="laser_1">
+ <rayCount>683</rayCount>
+ <rangeCount>683</rangeCount>
+ <laserCount>1</laserCount>
+ <origin>0.0 0.0 0.05</origin>
- <minAngle>-135</minAngle>
- <maxAngle>135</maxAngle>
+ <minAngle>-135</minAngle>
+ <maxAngle>135</maxAngle>
- <minRange>0.05</minRange>
- <maxRange>10.0</maxRange>
- <controller:sick2lms200_laser name="laser_controller_1">
- <interface:laser name="laser_iface_1"/>
- <updateRate>10</updateRate>
- </controller:sick2lms200_laser>
- </sensor:ray2>
- -->
+ <minRange>0.05</minRange>
+ <maxRange>10.0</maxRange>
+ <controller:sick2lms200_laser name="laser_controller_1">
+ <interface:laser name="laser_iface_1"/>
+ <updateRate>10</updateRate>
+ </controller:sick2lms200_laser>
+ </sensor:ray2>
+ -->
+ </body:box>
- </body:box>
+ <!-- attach hokuyo to torso -->
+ <joint:hinge name="tiltlaser_torso_attach_joint">
+ <body1>base_body</body1>
+ <body2>base_laser_body</body2>
+ <anchor>base_laser_body</anchor>
+ <axis> 1.0 0.0 0.0 </axis>
+ <lowStop> 0.0 </lowStop>
+ <highStop> 0.0 </highStop>
+ </joint:hinge>
- <!-- attach hokuyo to torso -->
- <attach>
- <parentBody>base_body</parentBody>
- <myBody>base_laser_body</myBody>
- </attach>
- </model:physical>
@@ -847,461 +849,392 @@
<!-- neck body -->
- <model:physical name="neck_model">
- <body:box name="neck_body">
- <xyz>0.10 0.0 1.03</xyz>
- <rpy>0.0 0.0 0.0</rpy>
- <!--xyz>0.0 0.0 0.0</xyz>
- <rpy>0.0 0.0 0.0</rpy-->
- <geom:box name="neck_geom">
+ <body:box name="neck_body">
+ <xyz>0.10 0.0 1.03</xyz>
+ <rpy>0.0 0.0 0.0</rpy>
+ <!--xyz>0.0 0.0 0.0</xyz>
+ <rpy>0.0 0.0 0.0</rpy-->
+ <geom:box name="neck_geom">
+ <!--size>.20 .38 .14</size-->
+ <size>0.01 0.01 0.01</size>
+ <mass>0.01</mass>
+ <visual>
+ <mesh>unit_box</mesh>
<!--size>.20 .38 .14</size-->
<size>0.01 0.01 0.01</size>
- <mass>0.01</mass>
- <visual>
- <mesh>unit_box</mesh>
- <!--size>.20 .38 .14</size-->
- <size>0.01 0.01 0.01</size>
- <material>Gazebo/Black</material>
- </visual>
- </geom:box>
- </body:box>
+ <material>Gazebo/Black</material>
+ </visual>
+ </geom:box>
+ </body:box>
- <!-- attach neck to torso -->
- <attach>
- <parentBody>torso_body</parentBody>
- <myBody>neck_body</myBody>
- </attach>
+ <!-- attach neck to torso -->
+ <joint:hinge name="neck_torso_attach_joint">
+ <body1>torso_body</body1>
+ <body2>neck_body</body2>
+ <anchor>neck_body</anchor>
+ <axis> 1.0 0.0 0.0 </axis>
+ <lowStop> 0.0 </lowStop>
+ <highStop> 0.0 </highStop>
+ </joint:hinge>
- <body:box name="head_base_body">
- <xyz>0.10 0.0 1.13 </xyz>
- <rpy>0.0 0.0 0.0 </rpy>
- <!-- report when this body collides with static bodies -->
- <reportStaticCollision>true</reportStaticCollision>
- <geom:box name="head_base_geom">
- <xyz> 0.0 0.00 0.000</xyz>
+ <body:box name="head_base_body">
+ <xyz>0.10 0.0 1.13 </xyz>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <!-- report when this body collides with static bodies -->
+ <reportStaticCollision>true</reportStaticCollision>
+ <geom:box name="head_base_geom">
+ <xyz> 0.0 0.00 0.000</xyz>
+ <size>0.165 0.21 0.003</size>
+ <mass>0.1</mass>
+ <mul>1</mul>
+ <!--color> 0.0 1.0 0.0</color-->
+ <visual>
<size>0.165 0.21 0.003</size>
- <mass>0.1</mass>
- <mul>1</mul>
- <!--color> 0.0 1.0 0.0</color-->
- <visual>
- <size>0.165 0.21 0.003</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/PioneerBody</material>
- </visual>
- </geom:box>
- </body:box>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/PioneerBody</material>
+ </visual>
+ </geom:box>
+ </body:box>
- <body:box name="head_tilt_body">
- <xyz>0.10 0.0 1.07 </xyz>
- <rpy>0.0 0.0 0.0 </rpy>
- <!-- report when this body collides with static bodies -->
- <reportStaticCollision>true</reportStaticCollision>
- <geom:box name="head_tilt_geom">
- <xyz> 0.0 0.00 0.150</xyz>
+ <body:box name="head_tilt_body">
+ <xyz>0.10 0.0 1.07 </xyz>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <!-- report when this body collides with static bodies -->
+ <reportStaticCollision>true</reportStaticCollision>
+ <geom:box name="head_tilt_geom">
+ <xyz> 0.0 0.00 0.150</xyz>
+ <size>0.165 0.21 0.003</size>
+ <mass>0.1</mass>
+ <mul>1</mul>
+ <!--color> 0.0 1.0 0.0</color-->
+ <visual>
<size>0.165 0.21 0.003</size>
- <mass>0.1</mass>
- <mul>1</mul>
- <!--color> 0.0 1.0 0.0</color-->
- <visual>
- <size>0.165 0.21 0.003</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/Black</material>
- </visual>
- </geom:box>
- <geom:box name="head_tilt_left_geom">
- <xyz> 0.0 -0.105 0.10</xyz>
- <size>0.165 0.003 0.10</size>
- <mass>0.1</mass>
- <mul>1</mul>
- <!--color> 0.0 1.0 0.0</color-->
- <visual>
- <size>0.165 0.003 0.10</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/Shell</material>
- </visual>
- </geom:box>
- <geom:box name="head_tilt_right_geom">
- <xyz> 0.0 0.105 0.10</xyz>
- <size>0.165 0.003 0.10</size>
- <mass>0.1</mass>
- <mul>1</mul>
- <!--color> 0.0 1.0 0.0</color-->
- <visual>
- <size>0.165 0.003 0.10</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/Shell</material>
- </visual>
- </geom:box>
- </body:box>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Black</material>
+ </visual>
+ </geom:box>
+ <geom:box name="head_tilt_left_geom">
+ <xyz> 0.0 -0.105 0.10</xyz>
+ <size>0.165 0.003 0.10</size>
+ <mass>0.1</mass>
+ <mul>1</mul>
+ <!--color> 0.0 1.0 0.0</color-->
+ <visual>
+ <size>0.165 0.003 0.10</size>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Shell</material>
+ </visual>
+ </geom:box>
+ <geom:box name="head_tilt_right_geom">
+ <xyz> 0.0 0.105 0.10</xyz>
+ <size>0.165 0.003 0.10</size>
+ <mass>0.1</mass>
+ <mul>1</mul>
+ <!--color> 0.0 1.0 0.0</color-->
+ <visual>
+ <size>0.165 0.003 0.10</size>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Shell</material>
+ </visual>
+ </geom:box>
+ </body:box>
- <!-- attach head_base to head_tilt -->
- <joint:hinge name="head_tilt_joint">
- <body2>head_tilt_body</body2>
- <body1>head_base_body</body1>
- <anchor>head_tilt_body</anchor>
- <axis> 0.0 1.0 0.0 </axis>
- </joint:hinge>
+ <!-- attach head_base to head_tilt -->
+ <joint:hinge name="head_tilt_joint">
+ <body2>head_tilt_body</body2>
+ <body1>head_base_body</body1>
+ <anchor>head_tilt_body</anchor>
+ <axis> 0.0 1.0 0.0 </axis>
+ </joint:hinge>
- <!-- attach head to neck -->
- <joint:hinge name="neck_yaw_joint">
- <body1>neck_body</body1>
- <body2>head_base_body</body2>
- <anchor>head_base_body</anchor>
- <axis> 0.0 0.0 1.0 </axis>
- </joint:hinge>
+ <!-- attach head to neck -->
+ <joint:hinge name="neck_yaw_joint">
+ <body1>neck_body</body1>
+ <body2>head_base_body</body2>
+ <anchor>head_base_body</anchor>
+ <axis> 0.0 0.0 1.0 </axis>
+ </joint:hinge>
- <!-- left camera -->
- <body:box name="head_cam_left_body">
- <xyz> 0.15 0.23 1.07 </xyz>
- <rpy> 0.00 0.0 0.00 </rpy>
- <sensor:camera name="head_cam_left_sensor">
- <imageSize>640 480</imageSize>
- <hfov>60</hfov>
- <nearClip>0.1</nearClip>
- <farClip>100</farClip>
+ <!-- left camera -->
+ <body:box name="ptz_cam_left_body">
+ <xyz> 0.15 0.23 1.07 </xyz>
+ <rpy> 0.00 0.0 0.00 </rpy>
+ <sensor:camera name="ptz_cam_left_sensor">
+ <imageSize>640 480</imageSize>
+ <hfov>60</hfov>
+ <nearClip>0.1</nearClip>
+ <farClip>100</farClip>
+ <updateRate>15.0</updateRate>
+ <controller:generic_camera name="ptz_cam_left_controller">
<updateRate>15.0</updateRate>
- <controller:generic_camera name="head_cam_left_controller">
- <updateRate>15.0</updateRate>
- <interface:camera name="head_cam_left_iface_0" />
- </controller:generic_camera>
- </sensor:camera>
- <geom:box name="head_cam_left_geom">
- <xyz> 0.00 0.00 0.00</xyz>
+ <interface:camera name="ptz_cam_left_iface" />
+ </controller:generic_camera>
+ </sensor:camera>
+ <geom:box name="ptz_cam_left_geom">
+ <xyz> 0.00 0.00 0.00</xyz>
+ <size>0.05 0.03 0.04</size>
+ <mass>0.1</mass>
+ <!--color> 0.0 1.0 0.0</color-->
+ <visual>
<size>0.05 0.03 0.04</size>
- <mass>0.1</mass>
- <!--color> 0.0 1.0 0.0</color-->
- <visual>
- <size>0.05 0.03 0.04</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/PioneerBody</material>
- </visual>
- </geom:box>
- </body:box>
- <body:box name="head_cam_base_left_body">
- <xyz> 0.15 0.20 1.07 </xyz>
- <rpy> 0.00 0.0 0.00 </rpy>
- <geom:box name="head_cam_base_left_geom">
- <xyz> 0.00 0.00 0.00</xyz>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/PioneerBody</material>
+ </visual>
+ </geom:box>
+ </body:box>
+ <body:box name="ptz_cam_base_left_body">
+ <xyz> 0.15 0.20 1.07 </xyz>
+ <rpy> 0.00 0.0 0.00 </rpy>
+ <geom:box name="ptz_cam_base_left_geom">
+ <xyz> 0.00 0.00 0.00</xyz>
+ <size>0.05 0.02 0.02</size>
+ <mass>0.1</mass>
+ <!--color> 0.0 1.0 0.0</color-->
+ <visual>
<size>0.05 0.02 0.02</size>
- <mass>0.1</mass>
- <!--color> 0.0 1.0 0.0</color-->
- <visual>
- <size>0.05 0.02 0.02</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/Shell</material>
- </visual>
- </geom:box>
- </body:box>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Shell</material>
+ </visual>
+ </geom:box>
+ </body:box>
- <!-- right camera -->
- <body:empty name="head_cam_right_body">
- <xyz> 0.15 -0.23 1.07 </xyz>
- <rpy> 0.00 0.0 0.00 </rpy>
- <sensor:camera name="head_cam_right_sensor">
- <imageSize>640 480</imageSize>
- <hfov>60</hfov>
- <nearClip>0.1</nearClip>
- <farClip>100</farClip>
+ <!-- right camera -->
+ <body:empty name="ptz_cam_right_body">
+ <xyz> 0.15 -0.23 1.07 </xyz>
+ <rpy> 0.00 0.0 0.00 </rpy>
+ <sensor:camera name="ptz_cam_right_sensor">
+ <imageSize>640 480</imageSize>
+ <hfov>60</hfov>
+ <nearClip>0.1</nearClip>
+ <farClip>100</farClip>
+ <updateRate>15.0</updateRate>
+ <controller:generic_camera name="ptz_cam_right_controller">
<updateRate>15.0</updateRate>
- <controller:generic_camera name="head_cam_right_controller">
- <updateRate>15.0</updateRate>
- <interface:camera name="head_cam_right_iface_0" />
- </controller:generic_camera>
- </sensor:camera>
- <geom:box name="head_cam_right_geom">
- <xyz> 0.00 0.00 0.00</xyz>
+ <interface:camera name="ptz_cam_right_iface" />
+ </controller:generic_camera>
+ </sensor:camera>
+ <geom:box name="ptz_cam_right_geom">
+ <xyz> 0.00 0.00 0.00</xyz>
+ <size>0.05 0.03 0.04</size>
+ <mass>0.1</mass>
+ <!--color> 0.0 1.0 0.0</color-->
+ ...
[truncated message content] |
|
From: <MP...@us...> - 2008-07-23 22:10:47
|
Revision: 1998
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1998&view=rev
Author: MPPics
Date: 2008-07-23 22:10:54 +0000 (Wed, 23 Jul 2008)
Log Message:
-----------
Camera viewer changes and gui-simulator interaction fixes.
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model
pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
pkg/trunk/visualization/pr2_gui/pr2_gui.fbp
pkg/trunk/visualization/pr2_gui/src/Vis3d.cpp
pkg/trunk/visualization/pr2_gui/src/launcherimpl.cpp
pkg/trunk/visualization/pr2_gui/src/launcherimpl.h
pkg/trunk/visualization/pr2_gui/src/pr2_gui.cpp
pkg/trunk/visualization/pr2_gui/src/pr2_gui.h
pkg/trunk/visualization/wx_ros/wx_camera_panel/src/wx_camera_panel/CameraPanel.cpp
pkg/trunk/visualization/wx_ros/wx_camera_panel/src/wx_camera_panel/CameraPanel.h
Modified: pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h 2008-07-23 21:46:59 UTC (rev 1997)
+++ pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h 2008-07-23 22:10:54 UTC (rev 1998)
@@ -5,46 +5,47 @@
namespace PR2
{
+ static const int PR2_FRAMEID_COUNT = 37; //TODO: replace with the xml file
- enum PR2_FRAMEID{
- FRAMEID_CASTER_FL_WHEEL_L = 101 , // FIXME: this is a hack until we have a frameid server
- FRAMEID_CASTER_FL_WHEEL_R ,
- FRAMEID_CASTER_FL_BODY ,
- FRAMEID_CASTER_FR_WHEEL_L ,
- FRAMEID_CASTER_FR_WHEEL_R ,
- FRAMEID_CASTER_FR_BODY ,
- FRAMEID_CASTER_RL_WHEEL_L ,
- FRAMEID_CASTER_RL_WHEEL_R ,
- FRAMEID_CASTER_RL_BODY ,
- FRAMEID_CASTER_RR_WHEEL_L ,
- FRAMEID_CASTER_RR_WHEEL_R ,
- FRAMEID_CASTER_RR_BODY ,
- FRAMEID_BASE ,
- FRAMEID_TORSO ,
- FRAMEID_ARM_L_TURRET ,
- FRAMEID_ARM_L_SHOULDER ,
- FRAMEID_ARM_L_UPPERARM ,
- FRAMEID_ARM_L_ELBOW ,
- FRAMEID_ARM_L_FOREARM ,
- FRAMEID_ARM_L_WRIST ,
- FRAMEID_ARM_L_HAND ,
- FRAMEID_ARM_L_FINGER_1 ,
- FRAMEID_ARM_L_FINGER_2 ,
- FRAMEID_ARM_R_TURRET ,
- FRAMEID_ARM_R_SHOULDER ,
- FRAMEID_ARM_R_UPPERARM ,
- FRAMEID_ARM_R_ELBOW ,
- FRAMEID_ARM_R_FOREARM ,
- FRAMEID_ARM_R_WRIST ,
- FRAMEID_ARM_R_HAND ,
- FRAMEID_ARM_R_FINGER_1 ,
- FRAMEID_ARM_R_FINGER_2 ,
- FRAMEID_HEAD_PAN_BASE ,
- FRAMEID_HEAD_TILT_BASE ,
- FRAMEID_STEREO_BLOCK ,
- FRAMEID_TILT_LASER_BLOCK ,
- FRAMEID_BASE_LASER_BLOCK ,
- MAX_FRAMEIDS
+ static const char *PR2_FRAMEID[] = { //TODO: replace with the xml file
+ "FRAMEID_CASTER_FL_WHEEL_L" ,//0
+ "FRAMEID_CASTER_FL_WHEEL_R" ,//1
+ "FRAMEID_CASTER_FL_BODY" ,//2
+ "FRAMEID_CASTER_FR_WHEEL_L" ,//3
+ "FRAMEID_CASTER_FR_WHEEL_R" ,//4
+ "FRAMEID_CASTER_FR_BODY" ,//5
+ "FRAMEID_CASTER_RL_WHEEL_L" ,//6
+ "FRAMEID_CASTER_RL_WHEEL_R" ,//7
+ "FRAMEID_CASTER_RL_BODY" ,//8
+ "FRAMEID_CASTER_RR_WHEEL_L" ,//9
+ "FRAMEID_CASTER_RR_WHEEL_R" ,//10
+ "FRAMEID_CASTER_RR_BODY" ,//11
+ "FRAMEID_BASE" ,//12
+ "FRAMEID_TORSO" ,//13
+ "FRAMEID_ARM_L_TURRET" ,//14
+ "FRAMEID_ARM_L_SHOULDER" ,//15
+ "FRAMEID_ARM_L_UPPERARM" ,//16
+ "FRAMEID_ARM_L_ELBOW" ,//17
+ "FRAMEID_ARM_L_FOREARM" ,//18
+ "FRAMEID_ARM_L_WRIST" ,//19
+ "FRAMEID_ARM_L_HAND" ,//20
+ "FRAMEID_ARM_L_FINGER_1" ,//21
+ "FRAMEID_ARM_L_FINGER_2" ,//22
+ "FRAMEID_ARM_R_TURRET" ,//23
+ "FRAMEID_ARM_R_SHOULDER" ,//24
+ "FRAMEID_ARM_R_UPPERARM" ,//25
+ "FRAMEID_ARM_R_ELBOW" ,//26
+ "FRAMEID_ARM_R_FOREARM" ,//27
+ "FRAMEID_ARM_R_WRIST" ,//28
+ "FRAMEID_ARM_R_HAND" ,//29
+ "FRAMEID_ARM_R_FINGER_1" ,//30
+ "FRAMEID_ARM_R_FINGER_2" ,//31
+ "FRAMEID_HEAD_PAN_BASE" ,//32
+ "FRAMEID_HEAD_TILT_BASE" ,//33
+ "FRAMEID_STEREO_BLOCK" ,//34
+ "FRAMEID_TILT_LASER_BLOCK" ,//35
+ "FRAMEID_BASE_LASER_BLOCK" ,//36
+ "END"
};
enum PR2_JOINT_CONTROL_MODE{
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model 2008-07-23 21:46:59 UTC (rev 1997)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model 2008-07-23 22:10:54 UTC (rev 1998)
@@ -1198,8 +1198,8 @@
<laserCount>1</laserCount>
<origin>0.0 0.0 0.05</origin>
- <minAngle>-135</minAngle>
- <maxAngle> 135</maxAngle>
+ <minAngle>-45</minAngle>
+ <maxAngle> 45</maxAngle>
<minRange>0.05</minRange>
<maxRange>10.0</maxRange>
Modified: pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
===================================================================
--- pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp 2008-07-23 21:46:59 UTC (rev 1997)
+++ pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp 2008-07-23 22:10:54 UTC (rev 1998)
@@ -289,7 +289,7 @@
//advertise<std_msgs::LaserScan>("laser");
advertise<std_msgs::LaserScan>("scan");
advertise<std_msgs::RobotBase2DOdom>("odom");
- advertise<std_msgs::Image>("image");
+ advertise<std_msgs::Image>("PTZR_image");
advertise<std_msgs::Image>("image_ptz_right");
advertise<std_msgs::Image>("image_ptz_left");
advertise<std_msgs::Image>("image_wrist_right");
Modified: pkg/trunk/visualization/pr2_gui/pr2_gui.fbp
===================================================================
--- pkg/trunk/visualization/pr2_gui/pr2_gui.fbp 2008-07-23 21:46:59 UTC (rev 1997)
+++ pkg/trunk/visualization/pr2_gui/pr2_gui.fbp 2008-07-23 22:10:54 UTC (rev 1998)
@@ -32,7 +32,7 @@
<property name="minimum_size">452,273</property>
<property name="name">launcher</property>
<property name="pos"></property>
- <property name="size">953,406</property>
+ <property name="size">1474,523</property>
<property name="style">wxDEFAULT_FRAME_STYLE</property>
<property name="subclass"></property>
<property name="title">Launcher</property>
@@ -864,7 +864,7 @@
<event name="OnPaint"></event>
<event name="OnRightDClick"></event>
<event name="OnRightDown"></event>
- <event name="OnRightUp"></event>
+ <event name="OnRightUp">PTZL_click</event>
<event name="OnSetFocus"></event>
<event name="OnSize"></event>
<event name="OnUpdateUI"></event>
@@ -1034,7 +1034,7 @@
<property name="border">5</property>
<property name="flag">wxEXPAND|wxALIGN_CENTER_HORIZONTAL</property>
<property name="proportion">1</property>
- <object class="wxFlexGridSizer" expanded="0">
+ <object class="wxFlexGridSizer" expanded="1">
<property name="cols">1</property>
<property name="flexible_direction">wxBOTH</property>
<property name="growablecols">0</property>
@@ -1059,7 +1059,7 @@
<property name="border">5</property>
<property name="flag">wxEXPAND|wxALL</property>
<property name="proportion">1</property>
- <object class="wxStaticBoxSizer" expanded="0">
+ <object class="wxStaticBoxSizer" expanded="1">
<property name="id">wxID_ANY</property>
<property name="label">Controls</property>
<property name="minimum_size"></property>
@@ -1888,7 +1888,7 @@
<event name="OnPaint"></event>
<event name="OnRightDClick"></event>
<event name="OnRightDown"></event>
- <event name="OnRightUp"></event>
+ <event name="OnRightUp">PTZR_click</event>
<event name="OnSetFocus"></event>
<event name="OnSize"></event>
<event name="OnUpdateUI"></event>
Modified: pkg/trunk/visualization/pr2_gui/src/Vis3d.cpp
===================================================================
--- pkg/trunk/visualization/pr2_gui/src/Vis3d.cpp 2008-07-23 21:46:59 UTC (rev 1997)
+++ pkg/trunk/visualization/pr2_gui/src/Vis3d.cpp 2008-07-23 22:10:54 UTC (rev 1998)
@@ -207,9 +207,9 @@
//static const char *modelPaths[] = {"","","pr2_models/caster1000r2.3DS","","","pr2_models/caster1000r2.3DS","","","pr2_models/caster1000r2.3DS","","","pr2_models/caster1000r2.3DS","pr2_models/base1000r.3DS","pr2_models/body1000r.3DS","pr2_models/sh-pan1000.3DS","pr2_models/sh-pitch1000.3DS","pr2_models/sh-roll1000.3DS","","","","","","","pr2_models/sh-pan1000.3DS","pr2_models/sh-pitch1000.3DS","pr2_models/sh-roll1000.3DS","","","","","","","pr2_models/head-pan1000.3DS","pr2_models/head-tilt1000.3DS","","",""};
static const char *modelPaths[] = {"../pr2_models/wheel.3DS","../pr2_models/wheel.3DS","../pr2_models/caster.3DS","../pr2_models/wheel.3DS","../pr2_models/wheel.3DS","../pr2_models/caster.3DS","../pr2_models/wheel.3DS","../pr2_models/wheel.3DS","../pr2_models/caster.3DS","../pr2_models/wheel.3DS","../pr2_models/wheel.3DS","../pr2_models/caster.3DS","../pr2_models/base.3DS","../pr2_models/body.3DS","../pr2_models/sh-pan.3DS","../pr2_models/sh-pitch.3DS","../pr2_models/sh-roll.3DS","../pr2_models/el-pitch.3DS","../pr2_models/fa-roll.3DS","../pr2_models/wr-pitch.3DS","../pr2_models/wr-roll.3DS","","","../pr2_models/sh-pan.3DS","../pr2_models/sh-pitch.3DS","../pr2_models/sh-roll.3DS","../pr2_models/el-pitch.3DS","../pr2_models/fa-roll.3DS","../pr2_models/wr-pitch.3DS","../pr2_models/wr-roll.3DS","","","../pr2_models/head-pan.3DS","../pr2_models/head-tilt.3DS","","",""};
pLocalRenderer->lock();
- for(int i = 0; i < PR2::MAX_FRAMEIDS-PR2::FRAMEID_CASTER_FL_WHEEL_L; i++)
+ for(int i = 0; i < PR2::PR2_FRAMEID_COUNT; i++)
{
- if (i != 21 && i != 22 && i != 30 && i != 31 && i < 31){
+ //if (i != 21 && i != 22 && i != 30 && i != 31 && i < 31){
libTF::TFPose aPose;
aPose.x = 0;
aPose.y = 0;
@@ -218,7 +218,7 @@
aPose.pitch = 0;
aPose.yaw = 0;
aPose.time = 0;
- aPose.frame = this->tfClient.lookup("FRAMEID_CASTER_FL_WHEEL_L");
+ aPose.frame = this->tfClient.lookup(PR2::PR2_FRAMEID[i]);// + i
libTF::TFPose inBaseFrame;
try
{
@@ -236,13 +236,16 @@
inBaseFrame.frame = this->tfClient.lookup("FRAMEID_BASE");
}
std::cout << "Coordinates for : " << i << "; "<<inBaseFrame.x << ", " << inBaseFrame.y << ", " << inBaseFrame.z << "; "<<inBaseFrame.roll << ", " << inBaseFrame.pitch << ", " << inBaseFrame.yaw <<std::endl;
- ILModel *tempModel = new ILModel(pLocalRenderer->manager(), intermediate, (irr::c8*)modelPaths[i], this->tfClient.lookup("FRAMEID_CASTER_FL_WHEEL_L"), (float)inBaseFrame.x,(float)inBaseFrame.y, (float)inBaseFrame.z, (float)inBaseFrame.roll,(float)(inBaseFrame.pitch), (float)(inBaseFrame.yaw));
- tempModel->getNode()->getMaterial(0).AmbientColor.set(255,100+int(155.0*rand()/(RAND_MAX + 1.0)),100+int(155.0*rand()/(RAND_MAX + 1.0)),100+int(155.0*rand()/(RAND_MAX + 1.0)));
- model.push_back(tempModel);
+ if(modelPaths[i] != "")
+ {
+ ILModel *tempModel = new ILModel(pLocalRenderer->manager(), intermediate, (irr::c8*)modelPaths[i], this->tfClient.lookup(PR2::PR2_FRAMEID[i]), (float)inBaseFrame.x,(float)inBaseFrame.y, (float)inBaseFrame.z, (float)inBaseFrame.roll,(float)(inBaseFrame.pitch), (float)(inBaseFrame.yaw));
+ tempModel->getNode()->getMaterial(0).AmbientColor.set(255,100+int(155.0*rand()/(RAND_MAX + 1.0)),100+int(155.0*rand()/(RAND_MAX + 1.0)),100+int(155.0*rand()/(RAND_MAX + 1.0)));
+ model.push_back(tempModel);
}
else
model.push_back(NULL);
}
+ std::cout << "Loaded all models\n";
myNode->subscribe("transform",transform, &Vis3d::newTransform,this);
pLocalRenderer->unlock();
}
@@ -635,9 +638,11 @@
void Vis3d::newTransform()
{
- for(int i = 0; i < PR2::MAX_FRAMEIDS-PR2::FRAMEID_CASTER_FL_WHEEL_L; i++)
+ //std::cout << "New Transform!\n";
+ for(int i = 0; i < PR2::PR2_FRAMEID_COUNT; i++)
{
- if (i != 21 && i != 22 && i != 30 && i != 31 && i < 31){
+ //std::cout << "i = " << i << std::endl;
+ //if (i != 21 && i != 22 && i != 30 && i != 31 && i < 31){
libTF::TFPose aPose;
aPose.x = 0;
aPose.y = 0;
@@ -646,7 +651,7 @@
aPose.pitch = 0;
aPose.yaw = 0;
aPose.time = 0;
- aPose.frame = this->tfClient.lookup("FRAMEID_CASTER_FL_WHEEL_L");
+ aPose.frame = this->tfClient.lookup(PR2::PR2_FRAMEID[i]);
libTF::TFPose inBaseFrame;
try
{
@@ -664,9 +669,12 @@
inBaseFrame.time = 0;
inBaseFrame.frame = this->tfClient.lookup("FRAMEID_BASE");
}
- model[i]->setPosition((float)inBaseFrame.x,(float)inBaseFrame.y, (float)inBaseFrame.z);
- model[i]->setRotation((float)inBaseFrame.roll,(float)(inBaseFrame.pitch), (float)(inBaseFrame.yaw));
- }
+ if(model[i] != NULL)
+ {
+ model[i]->setPosition((float)inBaseFrame.x,(float)inBaseFrame.y, (float)inBaseFrame.z);
+ model[i]->setRotation((float)inBaseFrame.roll,(float)(inBaseFrame.pitch), (float)(inBaseFrame.yaw));
+ }
+ //}
}
}
Modified: pkg/trunk/visualization/pr2_gui/src/launcherimpl.cpp
===================================================================
--- pkg/trunk/visualization/pr2_gui/src/launcherimpl.cpp 2008-07-23 21:46:59 UTC (rev 1997)
+++ pkg/trunk/visualization/pr2_gui/src/launcherimpl.cpp 2008-07-23 22:10:54 UTC (rev 1998)
@@ -5,6 +5,9 @@
launcher( parent )
{
PTZLCodec = new ImageCodec<std_msgs::Image>(&PTZLImage);
+ PTZRCodec = new ImageCodec<std_msgs::Image>(&PTZRImage);
+ WristLCodec = new ImageCodec<std_msgs::Image>(&WristLImage);
+ WristRCodec = new ImageCodec<std_msgs::Image>(&WristRImage);
wxInitAllImageHandlers();
LeftDock_FGS->Hide(HeadLaser_RB,true);
@@ -260,7 +263,7 @@
tiltPTZL_S->Enable(true);
zoomPTZL_S->Enable(true);
PTZL_B->Enable(true);
- myNode->subscribe("image", PTZLImage, &LauncherImpl::incomingPTZLImageConn,this);
+ myNode->subscribe("PTZL_image", PTZLImage, &LauncherImpl::incomingPTZLImageConn,this);
myNode->subscribe("PTZL_state", PTZL_state, &LauncherImpl::incomingPTZLState,this);
myNode->advertise<std_msgs::PTZActuatorCmd>("PTZL_cmd");
@@ -283,6 +286,21 @@
}
}
+void LauncherImpl::PTZL_click( wxMouseEvent& event)
+{
+ ptz_cmd.pan.valid = 1;
+ float h_mid = ((float)PTZL_B->GetSize().GetWidth())/2.0f;
+ float v_mid = ((float)PTZL_B->GetSize().GetHeight())/2.0f;
+ float delH = (event.m_x - h_mid)/h_mid*(21.0f-((float)zoomPTZL_S->GetValue())/500.0f);
+ float delV = (event.m_y - v_mid)/v_mid*(15.0f-((float)zoomPTZL_S->GetValue())/700.0f);
+ ptz_cmd.pan.cmd = panPTZL + delH;
+ ptz_cmd.tilt.valid = 1;
+ ptz_cmd.tilt.cmd = tiltPTZL - delV;
+ ptz_cmd.zoom.valid = 0;
+ //std::cout << "Click: " << ptz_cmd.pan.cmd << ", " << ptz_cmd.tilt.cmd << std::endl << "Diff: " << delH << ", " << delV << std::endl << "At: " << panPTZR << ", " << tiltPTZR << std::endl;
+ myNode->publish("PTZL_cmd",ptz_cmd);
+}
+
void LauncherImpl::incomingPTZLState()
{
//std::cout << "receiving position L\n";
@@ -375,20 +393,42 @@
}
}
+void LauncherImpl::PTZR_click( wxMouseEvent& event)
+{
+ ptz_cmd.pan.valid = 1;
+ float h_mid = ((float)PTZR_B->GetSize().GetWidth())/2.0f;
+ float v_mid = ((float)PTZR_B->GetSize().GetHeight())/2.0f;
+ float delH = (event.m_x - h_mid)/h_mid*(21.0f-((float)zoomPTZR_S->GetValue())/500.0f);
+ float delV = (event.m_y - v_mid)/v_mid*(15.0f-((float)zoomPTZR_S->GetValue())/700.0f);
+ ptz_cmd.pan.cmd = panPTZR + delH;
+ ptz_cmd.tilt.valid = 1;
+ ptz_cmd.tilt.cmd = tiltPTZR - delV;
+ ptz_cmd.zoom.valid = 0;
+ //std::cout << "Click: " << ptz_cmd.pan.cmd << ", " << ptz_cmd.tilt.cmd << std::endl << "Diff: " << delH << ", " << delV << std::endl << "At: " << panPTZR << ", " << tiltPTZR << std::endl;
+ myNode->publish("PTZR_cmd",ptz_cmd);
+}
+
void LauncherImpl::incomingPTZRState()
{
//std::cout << "receiving position R\n";
if(PTZR_state.zoom.pos_valid)
zoomPTZR_S->SetValue(round(PTZR_state.zoom.pos));
if(PTZR_state.tilt.pos_valid)
+ {
+ tiltPTZR = PTZR_state.tilt.pos;
tiltPTZR_S->SetValue(round(PTZR_state.tilt.pos));
+ }
if(PTZR_state.pan.pos_valid)
+ {
+ panPTZR = PTZR_state.pan.pos;
panPTZR_S->SetValue(round(PTZR_state.pan.pos));
+ }
//std::cout << "getting pos " << PTZR_state.pan.pos << " " << PTZR_state.tilt.pos << " " << PTZR_state.zoom.pos << endl;
}
void LauncherImpl::incomingPTZRImageConn()
{
+ //std::cout << "image\n";
if(PTZR_GET_NEW_IMAGE)
{
PTZR_GET_NEW_IMAGE = false;
@@ -600,3 +640,11 @@
{
// TODO: Implement EmergencyStop
}
+
+template <class T> const T& max ( const T& a, const T& b ) {
+ return (b<a)?a:b;
+}
+
+template <class T> const T& min ( const T& a, const T& b ) {
+ return (a<b)?a:b;
+}
Modified: pkg/trunk/visualization/pr2_gui/src/launcherimpl.h
===================================================================
--- pkg/trunk/visualization/pr2_gui/src/launcherimpl.h 2008-07-23 21:46:59 UTC (rev 1997)
+++ pkg/trunk/visualization/pr2_gui/src/launcherimpl.h 2008-07-23 22:10:54 UTC (rev 1998)
@@ -139,6 +139,10 @@
void PTZR_ptzChanged(wxScrollEvent& event);
///(Publisher) Sends a position command to the left PTZ
void PTZL_ptzChanged(wxScrollEvent& event);
+ ///Centers the right PTZ on a clicked location
+ void PTZR_click( wxMouseEvent& event);
+ ///Centers the left PTZ on a clicked location
+ void PTZL_click( wxMouseEvent& event);
public:
//Variables
@@ -178,6 +182,8 @@
bool WristR_GET_NEW_IMAGE;
///Flag stating a new frame can be displayed for the left wrist
bool WristL_GET_NEW_IMAGE;
+ ///Floats to keep track of PTZs
+ float panPTZR, tiltPTZR, panPTZL, tiltPTZL;
///wx bitmap object necessary for displaying camera/pictures
wxBitmap *PTZL_bmp;
///wx bitmap object necessary for displaying camera/pictures
Modified: pkg/trunk/visualization/pr2_gui/src/pr2_gui.cpp
===================================================================
--- pkg/trunk/visualization/pr2_gui/src/pr2_gui.cpp 2008-07-23 21:46:59 UTC (rev 1997)
+++ pkg/trunk/visualization/pr2_gui/src/pr2_gui.cpp 2008-07-23 22:10:54 UTC (rev 1998)
@@ -344,6 +344,7 @@
HeadLaser_RB->Connect( wxEVT_COMMAND_RADIOBOX_SELECTED, wxCommandEventHandler( launcher::HeadLaserChanged ), NULL, this );
panPTZL_S->Connect( wxEVT_SCROLL_CHANGED, wxScrollEventHandler( launcher::PTZL_ptzChanged ), NULL, this );
zoomPTZL_S->Connect( wxEVT_SCROLL_CHANGED, wxScrollEventHandler( launcher::PTZL_ptzChanged ), NULL, this );
+ PTZL_B->Connect( wxEVT_RIGHT_UP, wxMouseEventHandler( launcher::PTZL_click ), NULL, this );
tiltPTZL_S->Connect( wxEVT_SCROLL_CHANGED, wxScrollEventHandler( launcher::PTZL_ptzChanged ), NULL, this );
Visualization_CB->Connect( wxEVT_COMMAND_CHECKBOX_CLICKED, wxCommandEventHandler( launcher::startStop_Visualization ), NULL, this );
Topdown_CB->Connect( wxEVT_COMMAND_CHECKBOX_CLICKED, wxCommandEventHandler( launcher::startStopTopdown ), NULL, this );
@@ -354,6 +355,7 @@
EmStop_B->Connect( wxEVT_COMMAND_BUTTON_CLICKED, wxCommandEventHandler( launcher::EmergencyStop ), NULL, this );
panPTZR_S->Connect( wxEVT_SCROLL_CHANGED, wxScrollEventHandler( launcher::PTZR_ptzChanged ), NULL, this );
zoomPTZR_S->Connect( wxEVT_SCROLL_CHANGED, wxScrollEventHandler( launcher::PTZR_ptzChanged ), NULL, this );
+ PTZR_B->Connect( wxEVT_RIGHT_UP, wxMouseEventHandler( launcher::PTZR_click ), NULL, this );
tiltPTZR_S->Connect( wxEVT_SCROLL_CHANGED, wxScrollEventHandler( launcher::PTZR_ptzChanged ), NULL, this );
}
@@ -371,6 +373,7 @@
HeadLaser_RB->Disconnect( wxEVT_COMMAND_RADIOBOX_SELECTED, wxCommandEventHandler( launcher::HeadLaserChanged ), NULL, this );
panPTZL_S->Disconnect( wxEVT_SCROLL_CHANGED, wxScrollEventHandler( launcher::PTZL_ptzChanged ), NULL, this );
zoomPTZL_S->Disconnect( wxEVT_SCROLL_CHANGED, wxScrollEventHandler( launcher::PTZL_ptzChanged ), NULL, this );
+ PTZL_B->Disconnect( wxEVT_RIGHT_UP, wxMouseEventHandler( launcher::PTZL_click ), NULL, this );
tiltPTZL_S->Disconnect( wxEVT_SCROLL_CHANGED, wxScrollEventHandler( launcher::PTZL_ptzChanged ), NULL, this );
Visualization_CB->Disconnect( wxEVT_COMMAND_CHECKBOX_CLICKED, wxCommandEventHandler( launcher::startStop_Visualization ), NULL, this );
Topdown_CB->Disconnect( wxEVT_COMMAND_CHECKBOX_CLICKED, wxCommandEventHandler( launcher::startStopTopdown ), NULL, this );
@@ -381,5 +384,6 @@
EmStop_B->Disconnect( wxEVT_COMMAND_BUTTON_CLICKED, wxCommandEventHandler( launcher::EmergencyStop ), NULL, this );
panPTZR_S->Disconnect( wxEVT_SCROLL_CHANGED, wxScrollEventHandler( launcher::PTZR_ptzChanged ), NULL, this );
zoomPTZR_S->Disconnect( wxEVT_SCROLL_CHANGED, wxScrollEventHandler( launcher::PTZR_ptzChanged ), NULL, this );
+ PTZR_B->Disconnect( wxEVT_RIGHT_UP, wxMouseEventHandler( launcher::PTZR_click ), NULL, this );
tiltPTZR_S->Disconnect( wxEVT_SCROLL_CHANGED, wxScrollEventHandler( launcher::PTZR_ptzChanged ), NULL, this );
}
Modified: pkg/trunk/visualization/pr2_gui/src/pr2_gui.h
===================================================================
--- pkg/trunk/visualization/pr2_gui/src/pr2_gui.h 2008-07-23 21:46:59 UTC (rev 1997)
+++ pkg/trunk/visualization/pr2_gui/src/pr2_gui.h 2008-07-23 22:10:54 UTC (rev 1998)
@@ -85,6 +85,7 @@
virtual void viewChanged( wxCommandEvent& event ){ event.Skip(); }
virtual void HeadLaserChanged( wxCommandEvent& event ){ event.Skip(); }
virtual void PTZL_ptzChanged( wxScrollEvent& event ){ event.Skip(); }
+ virtual void PTZL_click( wxMouseEvent& event ){ event.Skip(); }
virtual void startStop_Visualization( wxCommandEvent& event ){ event.Skip(); }
virtual void startStopTopdown( wxCommandEvent& event ){ event.Skip(); }
virtual void startStop_PTZL( wxCommandEvent& event ){ event.Skip(); }
@@ -93,10 +94,11 @@
virtual void startStop_WristR( wxCommandEvent& event ){ event.Skip(); }
virtual void EmergencyStop( wxCommandEvent& event ){ event.Skip(); }
virtual void PTZR_ptzChanged( wxScrollEvent& event ){ event.Skip(); }
+ virtual void PTZR_click( wxMouseEvent& event ){ event.Skip(); }
public:
- launcher( wxWindow* parent, wxWindowID id = wxID_ANY, const wxString& title = wxT("Launcher"), const wxPoint& pos = wxDefaultPosition, const wxSize& size = wxSize( 953,406 ), long style = wxDEFAULT_FRAME_STYLE|wxTAB_TRAVERSAL, const wxString& name = wxT("launcher") );
+ launcher( wxWindow* parent, wxWindowID id = wxID_ANY, const wxString& title = wxT("Launcher"), const wxPoint& pos = wxDefaultPosition, const wxSize& size = wxSize( 1474,523 ), long style = wxDEFAULT_FRAME_STYLE|wxTAB_TRAVERSAL, const wxString& name = wxT("launcher") );
~launcher();
};
Modified: pkg/trunk/visualization/wx_ros/wx_camera_panel/src/wx_camera_panel/CameraPanel.cpp
===================================================================
--- pkg/trunk/visualization/wx_ros/wx_camera_panel/src/wx_camera_panel/CameraPanel.cpp 2008-07-23 21:46:59 UTC (rev 1997)
+++ pkg/trunk/visualization/wx_ros/wx_camera_panel/src/wx_camera_panel/CameraPanel.cpp 2008-07-23 22:10:54 UTC (rev 1998)
@@ -52,8 +52,8 @@
#define TILT_MAX (90.0f)
#define ZOOM_SCALE (10.0f)
-#define ZOOM_MIN (0.0f)
-#define ZOOM_MAX (10000.0F)
+#define ZOOM_MIN (1.0f)
+#define ZOOM_MAX (9999.0F)
#define POSITION_TICK_LENGTH (20)
#define POSITION_TICK_WIDTH (5)
@@ -592,6 +592,25 @@
m_ImagePanel->Refresh();
}
+void CameraPanel::OnMiddleMouseUp( wxMouseEvent& event )
+{
+ wxSize scale = m_ImagePanel->GetSize();
+
+ float h_mid = ((float)scale.GetWidth())/2.0f;
+ float v_mid = ((float)scale.GetHeight())/2.0f;
+
+ float delH = (event.m_x - h_mid)/h_mid*(21.0f-(m_CurrentZoom)/500.0f);
+ float delV = (event.m_y - v_mid)/v_mid*(15.0f-(m_CurrentZoom)/700.0f);
+
+ m_PTZControlMessage.pan.valid = 1;
+ m_PTZControlMessage.pan.cmd = std::min(std::max(m_CurrentPan + delH, PAN_MIN), PAN_MAX);
+ m_PTZControlMessage.tilt.valid = 1;
+ m_PTZControlMessage.tilt.cmd = std::min(std::max(m_CurrentTilt - delV, TILT_MIN), TILT_MAX);
+ m_PTZControlMessage.zoom.valid = 0;
+
+ m_ROSNode->publish(m_PTZControlTopic, m_PTZControlMessage);
+}
+
void CameraPanel::OnRightMouseDown( wxMouseEvent& event )
{
if ( !IsPTZControlEnabled() )
Modified: pkg/trunk/visualization/wx_ros/wx_camera_panel/src/wx_camera_panel/CameraPanel.h
===================================================================
--- pkg/trunk/visualization/wx_ros/wx_camera_panel/src/wx_camera_panel/CameraPanel.h 2008-07-23 21:46:59 UTC (rev 1997)
+++ pkg/trunk/visualization/wx_ros/wx_camera_panel/src/wx_camera_panel/CameraPanel.h 2008-07-23 22:10:54 UTC (rev 1998)
@@ -121,7 +121,6 @@
void DrawNewZoomLocation( wxDC& dc );
/// Draws the current zoom location (latest data from the camera)
void DrawCurrentZoomLocation( wxDC& dc );
-
/// Draw a pan tickmark
void DrawPan( wxDC& dc, wxPen& pen, float pan );
/// Draw a tilt tickmark
@@ -150,6 +149,8 @@
void OnRightMouseDown( wxMouseEvent& event );
/// wx callback, called when the right mouse button is released on the image panel
void OnRightMouseUp( wxMouseEvent& event );
+ /// wx callback, called when the middle mouse button is released on the image panel
+ void OnMiddleMouseUp( wxMouseEvent& event );
/// wx callback, called when the left mouse button is pressed on the image panel
void OnLeftMouseDown( wxMouseEvent& event );
/// wx callback, called when the left mouse button is released on the image panel
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-07-23 23:15:49
|
Revision: 2008
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2008&view=rev
Author: isucan
Date: 2008-07-23 23:15:56 +0000 (Wed, 23 Jul 2008)
Log Message:
-----------
added robot and pr2 messages and servers. more from std_srvs and std_msgs should follow
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/manifest.xml
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/sbl_pr2arm_mpk/src/Makefile
Added Paths:
-----------
pkg/trunk/messages/
pkg/trunk/messages/pr2_msgs/
pkg/trunk/messages/pr2_msgs/Makefile
pkg/trunk/messages/pr2_msgs/manifest.xml
pkg/trunk/messages/pr2_msgs/msg/
pkg/trunk/messages/robot_msgs/
pkg/trunk/messages/robot_msgs/Makefile
pkg/trunk/messages/robot_msgs/manifest.xml
pkg/trunk/messages/robot_msgs/msg/
pkg/trunk/messages/robot_msgs/msg/KinematicPath.msg
pkg/trunk/messages/robot_msgs/msg/KinematicState.msg
pkg/trunk/services/
pkg/trunk/services/pr2_srvs/
pkg/trunk/services/pr2_srvs/Makefile
pkg/trunk/services/pr2_srvs/manifest.xml
pkg/trunk/services/pr2_srvs/srv/
pkg/trunk/services/robot_srvs/
pkg/trunk/services/robot_srvs/Makefile
pkg/trunk/services/robot_srvs/manifest.xml
pkg/trunk/services/robot_srvs/srv/
pkg/trunk/services/robot_srvs/srv/KinematicMotionPlan.srv
Added: pkg/trunk/messages/pr2_msgs/Makefile
===================================================================
--- pkg/trunk/messages/pr2_msgs/Makefile (rev 0)
+++ pkg/trunk/messages/pr2_msgs/Makefile 2008-07-23 23:15:56 UTC (rev 2008)
@@ -0,0 +1,4 @@
+all: gen_msgs
+clean: clean_msgs
+ -rm -rf src/std_msgs
+include $(shell rospack find mk)/msg.mk
Added: pkg/trunk/messages/pr2_msgs/manifest.xml
===================================================================
--- pkg/trunk/messages/pr2_msgs/manifest.xml (rev 0)
+++ pkg/trunk/messages/pr2_msgs/manifest.xml 2008-07-23 23:15:56 UTC (rev 2008)
@@ -0,0 +1,16 @@
+<package>
+ <description brief="Common ROS Messages">
+
+ Messages that are specific to PR2
+
+ </description>
+ <author>Morgan Quigley/mqu...@cs..., Ken Conley/kw...@wi...</author>
+ <license>BSD</license>
+ <url>http://pr.willowgarage.com/wiki/std_msgs</url>
+ <logo>http://www.willowgarage.com/files/willowgarage/robot10.jpg</logo>
+ <depend package="robot_msgs"/>
+ <export>
+ <cpp cflags="-I${prefix}/msg/cpp"/>
+ </export>
+</package>
+
Added: pkg/trunk/messages/robot_msgs/Makefile
===================================================================
--- pkg/trunk/messages/robot_msgs/Makefile (rev 0)
+++ pkg/trunk/messages/robot_msgs/Makefile 2008-07-23 23:15:56 UTC (rev 2008)
@@ -0,0 +1,4 @@
+all: gen_msgs
+clean: clean_msgs
+ -rm -rf src/std_msgs
+include $(shell rospack find mk)/msg.mk
Added: pkg/trunk/messages/robot_msgs/manifest.xml
===================================================================
--- pkg/trunk/messages/robot_msgs/manifest.xml (rev 0)
+++ pkg/trunk/messages/robot_msgs/manifest.xml 2008-07-23 23:15:56 UTC (rev 2008)
@@ -0,0 +1,16 @@
+<package>
+ <description brief="Common ROS Messages">
+
+ Service definitions that are common for different robots
+
+ </description>
+ <author>Morgan Quigley/mqu...@cs..., Ken Conley/kw...@wi...</author>
+ <license>BSD</license>
+ <url>http://pr.willowgarage.com/wiki/std_msgs</url>
+ <logo>http://www.willowgarage.com/files/willowgarage/robot10.jpg</logo>
+ <depend package="std_msgs"/>
+ <export>
+ <cpp cflags="-I${prefix}/msg/cpp"/>
+ </export>
+</package>
+
Added: pkg/trunk/messages/robot_msgs/msg/KinematicPath.msg
===================================================================
--- pkg/trunk/messages/robot_msgs/msg/KinematicPath.msg (rev 0)
+++ pkg/trunk/messages/robot_msgs/msg/KinematicPath.msg 2008-07-23 23:15:56 UTC (rev 2008)
@@ -0,0 +1 @@
+KinematicState[] states
Added: pkg/trunk/messages/robot_msgs/msg/KinematicState.msg
===================================================================
--- pkg/trunk/messages/robot_msgs/msg/KinematicState.msg (rev 0)
+++ pkg/trunk/messages/robot_msgs/msg/KinematicState.msg 2008-07-23 23:15:56 UTC (rev 2008)
@@ -0,0 +1 @@
+float64[] vals
Modified: pkg/trunk/motion_planning/kinematic_planning/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/manifest.xml 2008-07-23 23:11:16 UTC (rev 2007)
+++ pkg/trunk/motion_planning/kinematic_planning/manifest.xml 2008-07-23 23:15:56 UTC (rev 2008)
@@ -3,8 +3,8 @@
<author>Ioan A. Sucan</author>
<license>BSD</license>
<depend package="roscpp" />
- <depend package="std_msgs" />
- <depend package="std_srvs" />
+ <depend package="robot_msgs" />
+ <depend package="robot_srvs" />
<depend package="xmlparam" />
<depend package="ompl" />
<depend package="collision_space" />
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-07-23 23:11:16 UTC (rev 2007)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-07-23 23:15:56 UTC (rev 2008)
@@ -83,8 +83,8 @@
#include <ros/node.h>
#include <std_msgs/PointCloudFloat32.h>
-#include <std_msgs/KinematicPath.h>
-#include <std_srvs/KinematicMotionPlan.h>
+#include <robot_msgs/KinematicPath.h>
+#include <robot_srvs/KinematicMotionPlan.h>
#include <urdf/URDF.h>
#include <collision_space/environmentODE.h>
@@ -155,7 +155,7 @@
}
- bool plan(std_srvs::KinematicMotionPlan::request &req, std_srvs::KinematicMotionPlan::response &res)
+ bool plan(robot_srvs::KinematicMotionPlan::request &req, robot_srvs::KinematicMotionPlan::response &res)
{
Model *m = m_models[req.model_id];
Planner &p = m->planners[0];
@@ -163,11 +163,11 @@
const int dim = req.start_state.vals_size;
if ((int)p.si->getStateDimension() != dim)
return false;
- /*
+
libTF::Pose3D &tf = m->collisionSpace->models[m->collisionSpaceID]->rootTransform;
tf.setPosition(req.transform.xt, req.transform.yt, req.transform.zt);
tf.setQuaternion(req.transform.xr, req.transform.yr, req.transform.zr, req.transform.w);
- */
+
// set the 3D space bounding box for planning.
// if not specified in the request, infer it based on start + goal positions
@@ -188,10 +188,10 @@
goal->state->values[i] = req.goal_state.vals[i];
p.si->setGoal(goal);
- // bool ok = p.mp->solve(req.allowed_time);
+ bool ok = p.mp->solve(req.allowed_time);
/* copy the solution to the result */
- // if (ok)
+ if (ok)
{
ompl::SpaceInformationKinematic::PathKinematic_t path = static_cast<ompl::SpaceInformationKinematic::PathKinematic_t>(goal->getSolutionPath());
res.path.set_states_size(path->states.size());
@@ -258,7 +258,7 @@
struct Planner
{
- ompl::MotionPlanner_t mp;
+ ompl::Planner_t mp;
ompl::SpaceInformationKinematic_t si;
int type;
};
Modified: pkg/trunk/motion_planning/sbl_pr2arm_mpk/src/Makefile
===================================================================
--- pkg/trunk/motion_planning/sbl_pr2arm_mpk/src/Makefile 2008-07-23 23:11:16 UTC (rev 2007)
+++ pkg/trunk/motion_planning/sbl_pr2arm_mpk/src/Makefile 2008-07-23 23:15:56 UTC (rev 2008)
@@ -1,4 +1,4 @@
SRC = kinematic_planning.cpp
OUT = ../bin/kinematic_planning
-PKG = kinematic_planning
+PKG = sbl_pr2arm_mpk
include $(shell rospack find mk)/node.mk
Added: pkg/trunk/services/pr2_srvs/Makefile
===================================================================
--- pkg/trunk/services/pr2_srvs/Makefile (rev 0)
+++ pkg/trunk/services/pr2_srvs/Makefile 2008-07-23 23:15:56 UTC (rev 2008)
@@ -0,0 +1,4 @@
+all:
+ `rospack find rostools`/scripts/gensrv srv/*.srv
+clean:
+ -rm -rf srv/cpp src/std_srvs
Added: pkg/trunk/services/pr2_srvs/manifest.xml
===================================================================
--- pkg/trunk/services/pr2_srvs/manifest.xml (rev 0)
+++ pkg/trunk/services/pr2_srvs/manifest.xml 2008-07-23 23:15:56 UTC (rev 2008)
@@ -0,0 +1,16 @@
+<package>
+ <description brief="Common ROS Services">
+
+ Service definitions that are specific to PR2
+
+ </description>
+ <author>Morgan Quigley/mqu...@cs...</author>
+ <license>BSD</license>
+ <url>http://pr.willowgarage.com/wiki/std_srvs</url>
+ <logo>http://www.willowgarage.com/files/willowgarage/robot10.jpg</logo>
+ <depend package="robot_srvs"/>
+ <export>
+ <cpp cflags="-I${prefix}/srv/cpp"/>
+ </export>
+</package>
+
Added: pkg/trunk/services/robot_srvs/Makefile
===================================================================
--- pkg/trunk/services/robot_srvs/Makefile (rev 0)
+++ pkg/trunk/services/robot_srvs/Makefile 2008-07-23 23:15:56 UTC (rev 2008)
@@ -0,0 +1,4 @@
+all:
+ `rospack find rostools`/scripts/gensrv srv/*.srv
+clean:
+ -rm -rf srv/cpp src/std_srvs
Added: pkg/trunk/services/robot_srvs/manifest.xml
===================================================================
--- pkg/trunk/services/robot_srvs/manifest.xml (rev 0)
+++ pkg/trunk/services/robot_srvs/manifest.xml 2008-07-23 23:15:56 UTC (rev 2008)
@@ -0,0 +1,16 @@
+<package>
+ <description brief="Common ROS Services">
+
+ Service definitions that are common for different robots
+
+ </description>
+ <author>Morgan Quigley/mqu...@cs...</author>
+ <license>BSD</license>
+ <url>http://pr.willowgarage.com/wiki/std_srvs</url>
+ <logo>http://www.willowgarage.com/files/willowgarage/robot10.jpg</logo>
+ <depend package="std_srvs"/>
+ <export>
+ <cpp cflags="-I${prefix}/srv/cpp"/>
+ </export>
+</package>
+
Added: pkg/trunk/services/robot_srvs/srv/KinematicMotionPlan.srv
===================================================================
--- pkg/trunk/services/robot_srvs/srv/KinematicMotionPlan.srv (rev 0)
+++ pkg/trunk/services/robot_srvs/srv/KinematicMotionPlan.srv 2008-07-23 23:15:56 UTC (rev 2008)
@@ -0,0 +1,8 @@
+string model_id
+robot_msgs/KinematicState start_state
+robot_msgs/KinematicState goal_state
+float64 allowed_time
+std_msgs/TransformQuaternion transform
+std_msgs/Point3DFloat64 volume
+---
+robot_msgs/KinematicPath path
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <egi...@us...> - 2008-07-23 23:37:57
|
Revision: 2011
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2011&view=rev
Author: egiljones
Date: 2008-07-23 23:38:04 +0000 (Wed, 23 Jul 2008)
Log Message:
-----------
Moving EndEffectorState.msg from rosgazebo to messages/pr2_msgs and making requisite changes to users
Modified Paths:
--------------
pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h
pkg/trunk/simulators/rosgazebo/manifest.xml
pkg/trunk/util/kinematics/interpolated_kinematic_controller/manifest.xml
pkg/trunk/util/kinematics/interpolated_kinematic_controller/src/interpolated_kinematic_controller.cc
pkg/trunk/util/kinematics/interpolated_kinematic_controller/test/test_inter_kin_con.cc
Added Paths:
-----------
pkg/trunk/messages/pr2_msgs/msg/EndEffectorState.msg
Removed Paths:
-------------
pkg/trunk/simulators/rosgazebo/msg/EndEffectorState.msg
Copied: pkg/trunk/messages/pr2_msgs/msg/EndEffectorState.msg (from rev 2010, pkg/trunk/simulators/rosgazebo/msg/EndEffectorState.msg)
===================================================================
--- pkg/trunk/messages/pr2_msgs/msg/EndEffectorState.msg (rev 0)
+++ pkg/trunk/messages/pr2_msgs/msg/EndEffectorState.msg 2008-07-23 23:38:04 UTC (rev 2011)
@@ -0,0 +1,2 @@
+float64[] rot
+float64[] trans
Modified: pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h
===================================================================
--- pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h 2008-07-23 23:19:17 UTC (rev 2010)
+++ pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h 2008-07-23 23:38:04 UTC (rev 2011)
@@ -55,7 +55,7 @@
// roscpp - arm
#include <std_msgs/PR2Arm.h>
-#include "rosgazebo/EndEffectorState.h"
+#include <pr2_msgs/EndEffectorState.h>
// roscpp - camera
#include <std_msgs/Image.h>
@@ -167,8 +167,8 @@
std_msgs::PR2Arm rightarm;
// end effector cmds
- rosgazebo::EndEffectorState cmd_leftarmcartesian;
- rosgazebo::EndEffectorState cmd_rightarmcartesian;
+ pr2_msgs::EndEffectorState cmd_leftarmcartesian;
+ pr2_msgs::EndEffectorState cmd_rightarmcartesian;
//Flags to indicate that a new message has arrived
bool newRightArmPos;
Modified: pkg/trunk/simulators/rosgazebo/manifest.xml
===================================================================
--- pkg/trunk/simulators/rosgazebo/manifest.xml 2008-07-23 23:19:17 UTC (rev 2010)
+++ pkg/trunk/simulators/rosgazebo/manifest.xml 2008-07-23 23:38:04 UTC (rev 2011)
@@ -11,6 +11,7 @@
<depend package="libpr2HW" />
<depend package="pr2Core" />
<depend package="std_msgs" />
+ <depend package="pr2_msgs" />
<depend package="rosthread" />
<depend package="rosTF" />
<depend package="gazebo_plugin" />
Deleted: pkg/trunk/simulators/rosgazebo/msg/EndEffectorState.msg
===================================================================
--- pkg/trunk/simulators/rosgazebo/msg/EndEffectorState.msg 2008-07-23 23:19:17 UTC (rev 2010)
+++ pkg/trunk/simulators/rosgazebo/msg/EndEffectorState.msg 2008-07-23 23:38:04 UTC (rev 2011)
@@ -1,2 +0,0 @@
-float64[] rot
-float64[] trans
Modified: pkg/trunk/util/kinematics/interpolated_kinematic_controller/manifest.xml
===================================================================
--- pkg/trunk/util/kinematics/interpolated_kinematic_controller/manifest.xml 2008-07-23 23:19:17 UTC (rev 2010)
+++ pkg/trunk/util/kinematics/interpolated_kinematic_controller/manifest.xml 2008-07-23 23:38:04 UTC (rev 2011)
@@ -6,6 +6,7 @@
<url>http://pr.willowgarage.com</url>
<depend package="roscpp"/>
<depend package="std_msgs"/>
+<depend package="pr2_msgs"/>
<depend package="pr2Core"/>
<depend package="libpr2API"/>
<depend package="libKDL"/>
Modified: pkg/trunk/util/kinematics/interpolated_kinematic_controller/src/interpolated_kinematic_controller.cc
===================================================================
--- pkg/trunk/util/kinematics/interpolated_kinematic_controller/src/interpolated_kinematic_controller.cc 2008-07-23 23:19:17 UTC (rev 2010)
+++ pkg/trunk/util/kinematics/interpolated_kinematic_controller/src/interpolated_kinematic_controller.cc 2008-07-23 23:38:04 UTC (rev 2011)
@@ -2,7 +2,7 @@
#include <rosthread/mutex.h>
#include <std_msgs/PR2Arm.h>
-#include <rosgazebo/EndEffectorState.h>
+#include <pr2_msgs/EndEffectorState.h>
#include <libpr2API/pr2API.h>
@@ -18,8 +18,8 @@
public:
InterpolatedKinematicController(void) : ros::node("interpolated_kinematic_controller") {
- advertise<rosgazebo::EndEffectorState>("cmd_leftarm_cartesian");
- advertise<rosgazebo::EndEffectorState>("cmd_rightarm_cartesian");
+ advertise<pr2_msgs::EndEffectorState>("cmd_leftarm_cartesian");
+ advertise<pr2_msgs::EndEffectorState>("cmd_rightarm_cartesian");
subscribe("right_pr2arm_set_end_effector", _rightEndEffectorGoal, &InterpolatedKinematicController::setRightEndEffector);
subscribe("left_pr2arm_set_end_effector", _leftEndEffectorGoal, &InterpolatedKinematicController::setLeftEndEffector);
subscribe("left_pr2arm_pos", leftArmPosMsg, &InterpolatedKinematicController::currentLeftArmPos);
@@ -58,15 +58,20 @@
}
void publishFrame(bool isRightArm, const Frame& f) {
- rosgazebo::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;
if(isRightArm) {
publish("cmd_rightarm_cartesian",efs);
} else {
@@ -93,6 +98,8 @@
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);
@@ -105,6 +112,7 @@
for(int i=0;i<nSteps;i++) {
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);
usleep(1000000);
}
@@ -115,8 +123,8 @@
private:
- rosgazebo::EndEffectorState _leftEndEffectorGoal;
- rosgazebo::EndEffectorState _rightEndEffectorGoal;
+ pr2_msgs::EndEffectorState _leftEndEffectorGoal;
+ pr2_msgs::EndEffectorState _rightEndEffectorGoal;
std_msgs::PR2Arm leftArmPosMsg, rightArmPosMsg;
};
Modified: pkg/trunk/util/kinematics/interpolated_kinematic_controller/test/test_inter_kin_con.cc
===================================================================
--- pkg/trunk/util/kinematics/interpolated_kinematic_controller/test/test_inter_kin_con.cc 2008-07-23 23:19:17 UTC (rev 2010)
+++ pkg/trunk/util/kinematics/interpolated_kinematic_controller/test/test_inter_kin_con.cc 2008-07-23 23:38:04 UTC (rev 2011)
@@ -1,6 +1,6 @@
#include <ros/node.h>
#include <libpr2API/pr2API.h>
-#include <rosgazebo/EndEffectorState.h>
+#include <pr2_msgs/EndEffectorState.h>
using namespace KDL;
@@ -9,7 +9,7 @@
ros::init(argc, argv);
ros::node mynode("test_inter+kin_con");
- mynode.advertise<rosgazebo::EndEffectorState>("right_pr2arm_set_end_effector");
+ mynode.advertise<pr2_msgs::EndEffectorState>("right_pr2arm_set_end_effector");
//In shoulder frame x 0.562689
//In shoulder frame y -0.367447
@@ -17,9 +17,20 @@
Rotation r = Rotation::RotZ(DTOR(0));
Vector v(.562689,-.367447,-.369594);
+ std::cout << " rot: " << std::endl;
+ std::cout << r.data[0] << std::endl;
+ std::cout << r.data[1] << std::endl;
+ std::cout << r.data[2] << std::endl;
+ std::cout << r.data[3] << std::endl;
+ std::cout << r.data[4] << std::endl;
+ std::cout << r.data[5] << std::endl;
+ std::cout << r.data[6] << std::endl;
+ std::cout << r.data[7] << std::endl;
+ std::cout << r.data[8] << std::endl;
+
sleep(1);
- rosgazebo::EndEffectorState efs;
+ pr2_msgs::EndEffectorState efs;
efs.set_rot_size(9);
efs.set_trans_size(3);
for(int i = 0; i < 9; i++) {
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