Menu
▾
▴
personalrobots-commit — Subversion commit messages
You can subscribe to this list here.
| 2008 |
Jan
|
Feb
|
Mar
(43) |
Apr
(196) |
May
(316) |
Jun
(518) |
Jul
(1293) |
Aug
(1446) |
Sep
(930) |
Oct
(1271) |
Nov
(1275) |
Dec
(1385) |
|---|---|---|---|---|---|---|---|---|---|---|---|---|
| 2009 |
Jan
(1622) |
Feb
(1546) |
Mar
(1236) |
Apr
(1512) |
May
(1782) |
Jun
(1551) |
Jul
(2300) |
Aug
(3088) |
Sep
(452) |
Oct
|
Nov
|
Dec
|
|
From: <mee...@us...> - 2009-09-04 15:42:41
|
Revision: 23831
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23831&view=rev
Author: meeussen
Date: 2009-09-04 15:42:33 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
update test
Modified Paths:
--------------
pkg/trunk/stacks/pr2_mechanism/pr2_mechanism_model/test/test_chain.cpp
Modified: pkg/trunk/stacks/pr2_mechanism/pr2_mechanism_model/test/test_chain.cpp
===================================================================
--- pkg/trunk/stacks/pr2_mechanism/pr2_mechanism_model/test/test_chain.cpp 2009-09-04 13:17:00 UTC (rev 23830)
+++ pkg/trunk/stacks/pr2_mechanism/pr2_mechanism_model/test/test_chain.cpp 2009-09-04 15:42:33 UTC (rev 23831)
@@ -61,7 +61,7 @@
HardwareInterface hw(0);
Robot model(&hw);
ASSERT_TRUE(model.initXml(root));
- RobotState state(&model, &hw);
+ RobotState state(&model);
// extract chain
Chain chain;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <bc...@us...> - 2009-09-04 13:17:10
|
Revision: 23830
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23830&view=rev
Author: bcohen1
Date: 2009-09-04 13:17:00 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
commented out some dead functions. fixed the dist problem
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_research/sbpl_arm_planner/include/sbpl_arm_planner/robarm3d/environment_robarm3d.h
pkg/trunk/motion_planning/planning_research/sbpl_arm_planner/params.cfg
pkg/trunk/motion_planning/planning_research/sbpl_arm_planner/src/discrete_space_information/robarm3d/environment_robarm3d.cpp
Added Paths:
-----------
pkg/trunk/motion_planning/planning_research/sbpl_arm_planner/ROS_BUILD_BLACKLIST
Added: pkg/trunk/motion_planning/planning_research/sbpl_arm_planner/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/motion_planning/planning_research/sbpl_arm_planner/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/motion_planning/planning_research/sbpl_arm_planner/ROS_BUILD_BLACKLIST 2009-09-04 13:17:00 UTC (rev 23830)
@@ -0,0 +1,16 @@
+make[3]: Entering directory `/home/rosbuild/.hudson/jobs/personalrobots-stable-update/workspace/personalrobots/motion_planning/planning_research/sbpl_arm_planner_node/build'
+[1100%] Building CXX object CMakeFiles/sbpl_arm_planner_node.dir/src/sbpl_arm_planner_node.o
+/home/rosbuild/.hudson/jobs/personalrobots-stable-update/workspace/personalrobots/motion_planning/planning_research/sbpl_arm_planner_node/src/sbpl_arm_planner_node.cpp: In member function 'bool
+sbpl_arm_planner_node::SBPLArmPlannerNode::setGoalPosition(const std::vector<motion_planning_msgs::PoseConstraint, std::allocator<motion_planning_msgs::PoseConstraint> >&)':
+/home/rosbuild/.hudson/jobs/personalrobots-stable-update/workspace/personalrobots/motion_planning/planning_research/sbpl_arm_planner_node/src/sbpl_arm_planner_node.cpp:538: error: 'const class
+motion_planning_msgs::PoseConstraint' has no member named 'position_distance'
+/home/rosbuild/.hudson/jobs/personalrobots-stable-update/workspace/personalrobots/motion_planning/planning_research/sbpl_arm_planner_node/src/sbpl_arm_planner_node.cpp:539: error: 'const class
+motion_planning_msgs::PoseConstraint' has no member named 'orientation_distance'
+/home/rosbuild/.hudson/jobs/personalrobots-stable-update/workspace/personalrobots/motion_planning/planning_research/sbpl_arm_planner_node/src/sbpl_arm_planner_node.cpp: In member function 'bool
+sbpl_arm_planner_node::SBPLArmPlannerNode::setGoalState(const std::vector<motion_planning_msgs::JointConstraint, std::allocator<motion_planning_msgs::JointConstraint> >&)':
+/home/rosbuild/.hudson/jobs/personalrobots-stable-update/workspace/personalrobots/motion_planning/planning_research/sbpl_arm_planner_node/src/sbpl_arm_planner_node.cpp:597: error: 'const class
+motion_planning_msgs::JointConstraint' has no member named 'toleranceAbove'
+/home/rosbuild/.hudson/jobs/personalrobots-stable-update/workspace/personalrobots/motion_planning/planning_research/sbpl_arm_planner_node/src/sbpl_arm_planner_node.cpp:598: error: 'const class
+motion_planning_msgs::JointConstraint' has no member named 'toleranceBelow'
+make[3]: *** [CMakeFiles/sbpl_arm_planner_node.dir/src/sbpl_arm_planner_node.o] Error 1
+
Modified: pkg/trunk/motion_planning/planning_research/sbpl_arm_planner/include/sbpl_arm_planner/robarm3d/environment_robarm3d.h
===================================================================
--- pkg/trunk/motion_planning/planning_research/sbpl_arm_planner/include/sbpl_arm_planner/robarm3d/environment_robarm3d.h 2009-09-04 13:09:33 UTC (rev 23829)
+++ pkg/trunk/motion_planning/planning_research/sbpl_arm_planner/include/sbpl_arm_planner/robarm3d/environment_robarm3d.h 2009-09-04 13:17:00 UTC (rev 23830)
@@ -495,13 +495,13 @@
bool isValidCell(const short unsigned int xyz[], const int &radius, unsigned char ***Grid, const boost::shared_ptr<Voxel3d> vGrid);
bool isValidCell(const int x, const int y, const int z, const int &radius, unsigned char ***Grid, const boost::shared_ptr<Voxel3d> vGrid);
- int IsValidCoord(short unsigned int coord[NUMOFLINKS], short unsigned int endeff_pos[3], short unsigned int wrist_pos[3], short unsigned int elbow_pos[3], double orientation[3][3]);
+// int IsValidCoord(short unsigned int coord[NUMOFLINKS], short unsigned int endeff_pos[3], short unsigned int wrist_pos[3], short unsigned int elbow_pos[3], double orientation[3][3]);
// int IsValidCoord(const short unsigned int coord[], const std::vector<std::vector<short unsigned int> > &joints, double orientation[3][3], unsigned char ***Grid, const short unsigned int grid_dims[]);
int IsValidCoord(short unsigned int coord[], short unsigned int endeff_pos[3], short unsigned int wrist_pos[3], short unsigned int elbow_pos[3], double orientation[3][3], unsigned char &dist);
- int IsValidLineSegment(short unsigned int x0, short unsigned int y0, short unsigned int z0, short unsigned int x1, short unsigned int y1, short unsigned int z1, unsigned char ***Grid3D, vector<CELLV>* pTestedCells);
- int IsValidLineSegment(const short unsigned int xyz0[], const short unsigned int xyz1[], const int &radius, unsigned char*** Grid3D, const int grid_dims[], vector<CELLV>* pTestedCells);
- int IsValidLineSegment(const short unsigned int a[], const short unsigned int b[], int radius, vector<CELLV>* pTestedCells, unsigned char *** Grid3D,const boost::shared_ptr<Voxel3d> vGrid);
+// int IsValidLineSegment(short unsigned int x0, short unsigned int y0, short unsigned int z0, short unsigned int x1, short unsigned int y1, short unsigned int z1, unsigned char ***Grid3D, vector<CELLV>* pTestedCells);
+ //int IsValidLineSegment(const short unsigned int xyz0[], const short unsigned int xyz1[], const int &radius, unsigned char*** Grid3D, const int grid_dims[], vector<CELLV>* pTestedCells);
+// int IsValidLineSegment(const short unsigned int a[], const short unsigned int b[], int radius, vector<CELLV>* pTestedCells, unsigned char *** Grid3D,const boost::shared_ptr<Voxel3d> vGrid);
int IsValidLineSegment(const short unsigned int a[],const short unsigned int b[],const unsigned int radius,vector<CELLV>* pTestedCells, unsigned char *** Grid3D, unsigned char &dist);
void UpdateEnvironment();
Modified: pkg/trunk/motion_planning/planning_research/sbpl_arm_planner/params.cfg
===================================================================
--- pkg/trunk/motion_planning/planning_research/sbpl_arm_planner/params.cfg 2009-09-04 13:09:33 UTC (rev 23829)
+++ pkg/trunk/motion_planning/planning_research/sbpl_arm_planner/params.cfg 2009-09-04 13:17:00 UTC (rev 23830)
@@ -1,4 +1,4 @@
-ARAPlanner_Epsilon: 30.0
+ARAPlanner_Epsilon: 1.0
Use_DH_for_FK: 0
PlanInJointSpace: 0
Enforce_Motor_Limits: 1
@@ -13,21 +13,24 @@
LowRes_Collision_Checking: 1
HighResActions_Distance_Threshold(cells): 4
HighResActions_Distance_Threshold(radians): 0.175
-Dist_from_goal_to_plan_for_RPY(meters): 0.8
+Dist_from_goal_to_plan_for_RPY(meters): 0.00
Use_standard_orientation_heuristic: 1
-Distance_from_goal_to_plan_for_r_p_y(meters): 0.3 0.6 0.8
+Distance_from_goal_to_plan_for_r_p_y(meters): 0.1 0.0 0.0
Use_selective_actions(js_planner_only): 0
Add_Direct_MotionPrimitive(js_only): 0
MultiRes_Successor_Actions: 1
-Actions: 15 8
-4 0 0 0 0 0 0
-0 4 0 0 0 0 0
+Actions: 18 7
+6 0 0 0 0 0 0
+0 6 0 0 0 0 0
0 0 6 0 0 0 0
-0 0 0 4 0 0 0
+0 0 0 6 0 0 0
0 0 0 0 6 0 0
0 0 0 0 0 6 0
0 0 0 0 0 0 6
-0 0 4 4 0 0 0
+0 0 6 -6 0 0 0
+0 0 -6 -6 0 0 0
+6 0 6 0 0 0 0
+6 0 0 -6 0 0 0
2 0 0 0 0 0 0
0 2 0 0 0 0 0
0 0 2 0 0 0 0
Modified: pkg/trunk/motion_planning/planning_research/sbpl_arm_planner/src/discrete_space_information/robarm3d/environment_robarm3d.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_research/sbpl_arm_planner/src/discrete_space_information/robarm3d/environment_robarm3d.cpp 2009-09-04 13:09:33 UTC (rev 23829)
+++ pkg/trunk/motion_planning/planning_research/sbpl_arm_planner/src/discrete_space_information/robarm3d/environment_robarm3d.cpp 2009-09-04 13:17:00 UTC (rev 23830)
@@ -40,9 +40,6 @@
// cost multiplier
#define COSTMULT 1000
-// distance from goal
-#define MAX_EUCL_DIJK_m .03
-
// discretization of joint angles
#define ANGLEDELTA 360.0
@@ -58,7 +55,6 @@
#define WRIST_JOINT 4
#define ENDEFF 6
#define GOAL_JOINT 9 //temp 6.22.09
-#define RADIUS 7;
// number of successors to each cell (for dyjkstra's heuristic)
#define DIRECTIONS 26
@@ -557,14 +553,10 @@
int EnvironmentROBARM3D::cost(short unsigned int state1coord[], short unsigned int state2coord[])
{
-// if(!IsValidCoord(state1coord) || !IsValidCoord(state2coord))
-// return INFINITECOST;
-
+ printf("calling cost(state1coord..)\n");
#if UNIFORM_COST
return 1*COSTMULT;
#else
-// printf("%i\n",1 * COSTMULT * (EnvROBARMCfg.Grid3D[HashEntry2->endeff[0]][HashEntry1->endeff[1]][HashEntry1->endeff[2]] + 1));
- //temporary
return 1*COSTMULT;
#endif
@@ -572,6 +564,9 @@
int EnvironmentROBARM3D::cost(EnvROBARMHashEntry_t* HashEntry1, EnvROBARMHashEntry_t* HashEntry2, bool bState2IsGoal)
{
+// if(bState2IsGoal)
+// return 0;
+
#if UNIFORM_COST
return 1*COSTMULT;
#else
@@ -580,7 +575,7 @@
else if(int(HashEntry2->dist) < 14)
return 1 * COSTMULT * 10;
else if(int(HashEntry2->dist) < 20)
- return 1 * COSTMULT * 5;
+ return 1 * COSTMULT * 4;
else
return 1 * COSTMULT;
#endif
@@ -1853,7 +1848,8 @@
double angles[NUMOFLINKS],orientation[3][3];
short unsigned int endeff[3],wrist[3],elbow[3];
int i, inc;
-
+ unsigned char dist;
+
if(fOut == NULL)
fOut = stdout;
@@ -1886,7 +1882,7 @@
ComputeEndEffectorPos(angles,endeff,wrist,elbow,orientation);
//skip invalid successors
- if(!IsValidCoord(succcoord,endeff,wrist,elbow,orientation))
+ if(!IsValidCoord(succcoord,endeff,wrist,elbow,orientation,dist))
continue;
if(endeff[0] == EnvROBARMCfg.EndEffGoals[0].xyz[0] && endeff[1] == EnvROBARMCfg.EndEffGoals[0].xyz[1] && endeff[2] == EnvROBARMCfg.EndEffGoals[0].xyz[2])
@@ -1923,10 +1919,6 @@
/** \brief Get successors when searching towards a cartesian goal. */
void EnvironmentROBARM3D::GetSuccs(int SourceStateID, vector<int>* SuccIDV, vector<int>* CostV)
{
-// #if DEBUG_HEURISTIC
-// GetFromToHeuristic(SourceStateID,EnvROBARM.goalHashEntry->stateID, fHeurExpansions);
-// #endif
-
if(EnvROBARMCfg.PlanInJointSpace)
{
GetJointSpaceSuccs(SourceStateID, SuccIDV, CostV);
@@ -1977,11 +1969,11 @@
}
}
- if(EnvROBARMCfg.use_jacobian_motion_prim)
- {
+ //if(EnvROBARMCfg.use_jacobian_motion_prim)
+ //{
//computeJacobian(s_angles, 0.104, jnt_vel);
//actions_i_max++;
- }
+ //}
#if DEBUG
fprintf(fDeb, "\nstate %d: %.2f %.2f %.2f %.2f %.2f %.2f %.2f endeff: %d %d %d\n",SourceStateID,
@@ -2112,6 +2104,7 @@
short unsigned int k, wrist[3], elbow[3], endeff[3];
double orientation [3][3];
int i, a;
+ unsigned char dist;
double angles[NUMOFLINKS], s_angles[NUMOFLINKS];
//to support two sets of succesor actions
@@ -2243,7 +2236,7 @@
}
//do collision checking
- if(!IsValidCoord(succcoord, endeff, wrist, elbow, orientation))
+ if(!IsValidCoord(succcoord, endeff, wrist, elbow, orientation, dist))
{
// printf("[GetJointSpaceSuccs] endeff (%u %u %u) successor is not valid.\n",endeff[0],endeff[1],endeff[2]);
continue;
@@ -3853,7 +3846,7 @@
temp = 0.0;
for (i = 0; i < NUMOFLINKS; i++)
{
- temp += ((EnvROBARMCfg.SuccActions[x][i]-EnvROBARMCfg.SuccActions[y][i])*(EnvROBARMCfg.SuccActions[x][i]-EnvROBARMCfg.SuccActions[y][i]));
+ temp += ((EnvROBARMCfg.SuccActions[x][i]-EnvROBARMCfg.SuccActions[y][i])*(EnvROBARMCfg.SuccActions[x][i]-EnvROBARMCfg.SuccActions[y][i]));
}
EnvROBARMCfg.ActiontoActionCosts[x][y] = temp * COSTMULT * EnvROBARMCfg.smoothing_weight * EnvROBARMCfg.use_smooth_actions;
}
@@ -3874,16 +3867,16 @@
else
gridcell_size = EnvROBARMCfg.GridCellWidth;
- //starting at zeroed angles, find end effector position after each action
+ //starting at zeroed angles, find end effector position after each action
ComputeEndEffectorPos(start_angles,start_endeff_m);
- //iterate through all possible actions and find the one with the minimum cost per cell
+ //iterate through all possible actions and find the one with the minimum cost per cell
for (int i = 0; i < EnvROBARMCfg.nSuccActions; i++)
{
for(int j = 0; j < EnvROBARMCfg.num_joints; j++)
angles[j] = DEG2RAD(EnvROBARMCfg.SuccActions[i][j]);// * (360.0/ANGLEDELTA));
- //starting at zeroed angles, find end effector position after each action
+ //starting at zeroed angles, find end effector position after each action
ComputeEndEffectorPos(angles,endeff_m);
eucl_dist = sqrt((start_endeff_m[0]-endeff_m[0])*(start_endeff_m[0]-endeff_m[0]) +
@@ -3903,7 +3896,7 @@
EnvROBARMCfg.cost_sqrt3_move = sqrt(3.0)*EnvROBARMCfg.cost_per_cell;
EnvROBARMCfg.cost_per_mm = EnvROBARMCfg.cost_per_cell / (gridcell_size * 1000);
- //compute cost per radian for the joint space planner (this was moved here in a hackish way)
+ //compute cost per radian for the joint space planner (this was moved here in a hackish way)
double max_angle = 0;
for (int i = 0; i < EnvROBARMCfg.nSuccActions; i++)
{
@@ -4293,14 +4286,6 @@
std::vector<std::vector<double> >* EnvironmentROBARM3D::getCollisionMap()
{
-// for(unsigned int i=0; i < EnvROBARMCfg.cubes.size(); i++)
-// {
-// printf("[getCollisionMap] cube %i: ",i);
-// for(unsigned int k=0; k < EnvROBARMCfg.cubes[i].size(); k++)
-// printf("%.3f ",EnvROBARMCfg.cubes[i][k]);
-// printf("\n");
-// }
-
if(EnvROBARMCfg.mCopyingGrid.try_lock())
{
EnvROBARMCfg.sbpl_cubes = EnvROBARMCfg.cubes;
@@ -4337,9 +4322,6 @@
for (z = 0; z < EnvROBARMCfg.EnvDepth_c; ++z)
{
EnvROBARMCfg.Grid3D[x][y][z] = EnvROBARMCfg.Grid3D_temp[x][y][z];
-
- if(EnvROBARMCfg.Grid3D[x][y][z] > 0)
- c++;
}
}
}
@@ -4353,16 +4335,10 @@
for (z = 0; z < EnvROBARMCfg.LowResEnvDepth_c; ++z)
{
EnvROBARMCfg.LowResGrid3D[x][y][z] = EnvROBARMCfg.LowResGrid3D_temp[x][y][z];
-
- if(EnvROBARMCfg.LowResGrid3D[x][y][z] > 0)
- b++;
}
}
}
}
- // printf("[UpdateEnvironment] There are %d obstacle cells in the low resolution planning grid.\n",b);
- // printf("[UpdateEnvironment] There are %d obstacle cells in the high resolution planning grid.\n",c);
-
}
/* //add obstacles to grid (right now just supports cubes)
@@ -4741,6 +4717,7 @@
/**------------------------------------------------------------------------*/
//if pTestedCells is NULL, then the tested points are not saved and it is more
//efficient as it returns as soon as it sees first invalid point
+/*
int EnvironmentROBARM3D::IsValidLineSegment(short unsigned int x0, short unsigned int y0, short unsigned int z0, short unsigned int x1, short unsigned int y1, short unsigned int z1, unsigned char*** Grid3D, vector<CELLV>* pTestedCells)
{
printf("entering old isValidLineSeg\n");
@@ -4834,6 +4811,7 @@
// printf("exiting isValidLineSeg\n");
return retvalue;
}
+*/
int EnvironmentROBARM3D::IsValidLineSegment(const short unsigned int a[],const short unsigned int b[],const unsigned int radius,vector<CELLV>* pTestedCells, unsigned char *** Grid3D, unsigned char &dist)
{
@@ -4862,7 +4840,7 @@
//insert the tested point
if(pTestedCells)
{
- if(cell_val <= radius)
+ if((unsigned int)cell_val <= radius)
tempcell.bIsObstacle = true;
else
tempcell.bIsObstacle = false;
@@ -4873,9 +4851,12 @@
}
} while (get_next_point3d(¶ms));
+ dist = min_dist;
+
return retvalue;
}
+/*
int EnvironmentROBARM3D::IsValidLineSegment(const short unsigned int xyz0[], const short unsigned int xyz1[], const int &radius, unsigned char*** Grid3D, const int grid_dims[], vector<CELLV>* pTestedCells)
{
bresenham_param_t params;
@@ -4916,7 +4897,9 @@
return retvalue;
}
+*/
+/*
int EnvironmentROBARM3D::IsValidCoord(short unsigned int coord[], short unsigned int endeff_pos[3], short unsigned int wrist_pos[3], short unsigned int elbow_pos[3], double orientation[3][3])
{
int grid_dims[3] = {EnvROBARMCfg.EnvWidth_c, EnvROBARMCfg.EnvHeight_c, EnvROBARMCfg.EnvDepth_c};
@@ -4994,31 +4977,6 @@
// if(!isValidCell(endeff,6,Grid3D,vGrid) || !isValidCell(wrist,6,Grid3D,vGrid) || !isValidCell(elbow,6,Grid3D,vGrid))
// return 0;
- //check the validity of the corresponding arm links (line segments)
-/* if(!IsValidLineSegment(shoulder, elbow, EnvROBARMCfg.upper_arm_radius, pTestedCells, Grid3D, vGrid) ||
- !IsValidLineSegment(elbow,wrist, EnvROBARMCfg.forearm_radius, pTestedCells, Grid3D, vGrid) ||
- !IsValidLineSegment(wrist,endeff, EnvROBARMCfg.forearm_radius, pTestedCells, Grid3D, vGrid))
- {
- if(pTestedCells == NULL)
- {
- #if DEBUG_VALID_COORD
- printf("[IsValidCoord] shoulder: (%i %i %i) --> elbow: (%i %i %i)\n",shoulder[0],shoulder[1],shoulder[2],elbow[0],elbow[1],elbow[2]);
- printf("[IsValidCoord] wrist: (%i %i %i)\n",wrist[0],wrist[1],wrist[2]);
- printf("[IsValidCoord] endeff: (%i %i %i)\n",endeff[0],endeff[1],endeff[2]);
- printf("[IsValidCoord] Arm link collision checking failed.\n");
- #endif
- return 0;
- }
- else
- {
- #if DEBUG_VALID_COORD
- printf("[IsValidCoord] Arm link collision checking failed..\n");
- #endif
- retvalue = 0;
- }
- }
-*/
-
if(!IsValidLineSegment(shoulder, elbow, EnvROBARMCfg.upper_arm_radius, pTestedCells, Grid3D, vGrid))
{
if(pTestedCells == NULL)
@@ -5127,6 +5085,7 @@
return retvalue;
}
+*/
int EnvironmentROBARM3D::IsValidCoord(short unsigned int coord[], short unsigned int endeff_pos[3], short unsigned int wrist_pos[3], short unsigned int elbow_pos[3], double orientation[3][3], unsigned char &dist)
{
@@ -5306,17 +5265,15 @@
int num = 0;
if(GetDistToClosestGoal(endeff_pos, &num) <= .3/EnvROBARMCfg.GridCellWidth)
{
-// printf("planning monitor: distance of endeff(%u %u %u) to goal #%i is %i\n", endeff_pos[0],endeff_pos[1],endeff_pos[2], num, GetDistToClosestGoal(endeff_pos, &num));
+ // printf("planning monitor: distance of endeff(%u %u %u) to goal #%i is %i\n", endeff_pos[0],endeff_pos[1],endeff_pos[2], num, GetDistToClosestGoal(endeff_pos, &num));
// printf("checking %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n",angles[0],angles[1],angles[2],angles[3],angles[4],angles[5],angles[6]);
-// pm_start = clock();
if(!planning_monitor->areLinksValid(angles))
return 0;
-// printf("planning monitor took %lf seconds to check the gripper for collisions\n",(clock() - pm_start) / (double)CLOCKS_PER_SEC);
+ // printf("planning monitor took %lf seconds to check the gripper for collisions\n",(clock() - pm_start) / (double)CLOCKS_PER_SEC);
}
else
{
-// printf("basic check: distance of endeff(%u %u %u) to goal #%i is %i\n", endeff_pos[0],endeff_pos[1],endeff_pos[2], num, GetDistToClosestGoal(endeff_pos, &num));
- //get position of gripper in torso_lift_link frame
+ //get position of gripper in torso_lift_link frame
gripper[0] = (endeff[0] + (orientation[0][0]*EnvROBARMCfg.gripper_m[0] + orientation[0][1]*EnvROBARMCfg.gripper_m[1] + orientation[0][2]*EnvROBARMCfg.gripper_m[2]) / grid_cell_m) + 0.5;
gripper[1] = (endeff[1] + (orientation[1][0]*EnvROBARMCfg.gripper_m[0] + orientation[1][1]*EnvROBARMCfg.gripper_m[1] + orientation[1][2]*EnvROBARMCfg.gripper_m[2]) / grid_cell_m) + 0.5;
gripper[2] = (endeff[2] + (orientation[2][0]*EnvROBARMCfg.gripper_m[0] + orientation[2][1]*EnvROBARMCfg.gripper_m[1] + orientation[2][2]*EnvROBARMCfg.gripper_m[2]) / grid_cell_m) + 0.5;;
@@ -5334,22 +5291,6 @@
}
}
-// if(!EnvROBARMCfg.object_in_gripper)
-// {
-// //get position of gripper in torso_lift_link frame
-// object[0] = (endeff[0]+(0.18/grid_cell_m) + (orientation[0][0]*EnvROBARMCfg.object_length_m[0] + orientation[0][1]*EnvROBARMCfg.object_length_m[1] + orientation[0][2]*EnvROBARMCfg.object_length_m[2]) / grid_cell_m) + 0.5;
-// object[1] = (endeff[1] + (orientation[1][0]*EnvROBARMCfg.object_length_m[0] + orientation[1][1]*EnvROBARMCfg.object_length_m[1] + orientation[1][2]*EnvROBARMCfg.object_length_m[2]) / grid_cell_m) + 0.5;
-// object[2] = (endeff[2] + (orientation[2][0]*EnvROBARMCfg.object_length_m[0] + orientation[2][1]*EnvROBARMCfg.object_length_m[1] + orientation[2][2]*EnvROBARMCfg.object_length_m[2]) / grid_cell_m) + 0.5;;
-//
-// if(object[0] >= grid_dims[0] || object[1] >= grid_dims[1] || object[2] >= grid_dims[2] || !isValidCell(object,EnvROBARMCfg.object_radius,Grid3D,vGrid))
-// return 0;
-//
-// pTestedCells = NULL;
-//
-// if(!IsValidLineSegment(endeff, object, EnvROBARMCfg.object_radius, pTestedCells, Grid3D, dist_temp))
-// return 0;
-// }
-
if(!EnvROBARMCfg.object_in_gripper)
{
short unsigned int object[3], gripper_tip[3] = {endeff[0]+(0.18/grid_cell_m), endeff[1], endeff[2]};
@@ -5367,6 +5308,9 @@
if(!IsValidLineSegment(gripper_tip, object, object_radius, pTestedCells, Grid3D, dist_temp))
return 0;
+
+ if(dist_temp < dist)
+ dist = dist_temp;
}
return retvalue;
@@ -5392,6 +5336,7 @@
bool EnvironmentROBARM3D::isPathValid(double** path, int num_waypoints)
{
+ unsigned char dist;
double angles[NUMOFLINKS], orientation[3][3];
short unsigned int endeff[3], wrist[3], elbow[3], coord[NUMOFLINKS];
@@ -5413,7 +5358,7 @@
ComputeCoord(angles, coord);
//check if valid
- if(!IsValidCoord(coord,endeff, wrist, elbow, orientation))
+ if(!IsValidCoord(coord,endeff, wrist, elbow, orientation, dist))
{
printf("[isPathValid] Path is invalid.\n");
return false;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jue...@us...> - 2009-09-04 13:09:44
|
Revision: 23829
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23829&view=rev
Author: juergensturm
Date: 2009-09-04 13:09:33 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
added parameters
Modified Paths:
--------------
pkg/trunk/sandbox/planar_objects/narrow_box_detector.launch
pkg/trunk/sandbox/planar_objects/run_experiment.sh
pkg/trunk/sandbox/planar_objects/src/box_tracker.cpp
pkg/trunk/sandbox/planar_objects/src/box_tracker.h
pkg/trunk/sandbox/planar_objects/src/mocap_eval.cpp
pkg/trunk/sandbox/planar_objects/src/mocap_eval.h
Added Paths:
-----------
pkg/trunk/sandbox/planar_objects/highdoor-simple.launch
Added: pkg/trunk/sandbox/planar_objects/highdoor-simple.launch
===================================================================
--- pkg/trunk/sandbox/planar_objects/highdoor-simple.launch (rev 0)
+++ pkg/trunk/sandbox/planar_objects/highdoor-simple.launch 2009-09-04 13:09:33 UTC (rev 23829)
@@ -0,0 +1,18 @@
+<launch>
+
+<include file="$(find stereo_image_proc)/narrow_stereoproc.launch" />
+
+<node pkg="planar_objects" type="box_tracker" name="box_tracker" output="screen">
+</node>
+
+<node pkg="planar_objects" type="track_visualizer" name="track_visualizer" output="screen">
+</node>
+
+<node pkg="planar_objects" type="articulation_learner" name="articulation_learner" output="screen">
+</node>
+
+<node pkg="rosrecord" type="rosplay" name="rosplay" args="-r 10 /home/lollypop/sturm/bags/output/highdoor-simple_2009-08-28-23-11-29-topic.bag"/>
+
+<node pkg="rviz" type="rviz"/>
+
+</launch>
Modified: pkg/trunk/sandbox/planar_objects/narrow_box_detector.launch
===================================================================
--- pkg/trunk/sandbox/planar_objects/narrow_box_detector.launch 2009-09-04 09:53:41 UTC (rev 23828)
+++ pkg/trunk/sandbox/planar_objects/narrow_box_detector.launch 2009-09-04 13:09:33 UTC (rev 23829)
@@ -23,8 +23,8 @@
<param name="max_lines" type="int" value="200"/>
<param name="max_corners" type="int" value="500"/>
- <param name="min_precision" type="double" value="0.8"/>
- <param name="min_recall" type="double" value="0.8"/>
+ <param name="min_precision" type="double" value="0.0"/>
+ <param name="min_recall" type="double" value="0.0"/>
<param name="rect_min_size" type="double" value="0.05"/>
<param name="rect_max_size" type="double" value="1.50"/>
Modified: pkg/trunk/sandbox/planar_objects/run_experiment.sh
===================================================================
--- pkg/trunk/sandbox/planar_objects/run_experiment.sh 2009-09-04 09:53:41 UTC (rev 23828)
+++ pkg/trunk/sandbox/planar_objects/run_experiment.sh 2009-09-04 13:09:33 UTC (rev 23829)
@@ -1,9 +1,13 @@
#!/bin/bash
-for bagfile in "$@"
-do
+#for bagfile in "$@"
+#do
-#bagfile=$1
+export ROS_MASTER_URI="http://localhost:$1/"
+shift
+
+bagfile=$@
+
time_per_frame="30.00"
save_to="$HOME/bags/output"
record_topics="/box_detector/observations"
@@ -84,9 +88,9 @@
sleep 1
-start 80x6-0-800 "roslaunch $HOME/ros/ros-pkg/sandbox/planar_objects/narrow_mocap_eval$launchsuffix.launch"
+#start 80x6-0-800 "roslaunch $HOME/ros/ros-pkg/sandbox/planar_objects/narrow_mocap_eval$launchsuffix.launch"
-sleep 1
+#sleep 1
rosplay -r $rate $bagfile
@@ -117,4 +121,4 @@
echo "Done"
-done
+#done
Modified: pkg/trunk/sandbox/planar_objects/src/box_tracker.cpp
===================================================================
--- pkg/trunk/sandbox/planar_objects/src/box_tracker.cpp 2009-09-04 09:53:41 UTC (rev 23828)
+++ pkg/trunk/sandbox/planar_objects/src/box_tracker.cpp 2009-09-04 13:09:33 UTC (rev 23829)
@@ -45,6 +45,10 @@
nh.param("~rotation_tolerance", params.rotation_tolerance, M_PI/8);
nh.param("~size_tolerance", params.size_tolerance, 0.03);
+ nh.param("~min_precision", min_precision, 0.7);
+ nh.param("~min_recall", min_recall, 0.7);
+ nh.param("~plane_limit", plane_limit, 0);
+
newLines = 0;
oldLines = 0;
track_ids = 0;
@@ -71,7 +75,16 @@
observations.clear();
for(size_t i=0; i<observations_msg->obs.size(); i++) {
- observations.push_back(btBoxObservation(observations_msg->obs[i], observations_msg->header.stamp));
+ btBoxObservation obs = btBoxObservation(observations_msg->obs[i], observations_msg->header.stamp);
+
+ if(obs.recall<min_recall)
+ continue;
+ if(obs.precision<min_precision)
+ continue;
+ if(plane_limit>0 && obs.plane_id>=plane_limit)
+ continue;
+
+ observations.push_back(obs);
}
// if(observations.size()==0) return;
Modified: pkg/trunk/sandbox/planar_objects/src/box_tracker.h
===================================================================
--- pkg/trunk/sandbox/planar_objects/src/box_tracker.h 2009-09-04 09:53:41 UTC (rev 23828)
+++ pkg/trunk/sandbox/planar_objects/src/box_tracker.h 2009-09-04 13:09:33 UTC (rev 23829)
@@ -34,6 +34,12 @@
int newLines;
int track_ids;
+ double min_precision;
+ double min_recall;
+ int plane_limit; // 0: don't limit visible planes
+ // >0: only consider first n planes
+
+
// PARAMETERS
bool show_obs;
bool show_tracks;
Modified: pkg/trunk/sandbox/planar_objects/src/mocap_eval.cpp
===================================================================
--- pkg/trunk/sandbox/planar_objects/src/mocap_eval.cpp 2009-09-04 09:53:41 UTC (rev 23828)
+++ pkg/trunk/sandbox/planar_objects/src/mocap_eval.cpp 2009-09-04 13:09:33 UTC (rev 23829)
@@ -61,6 +61,11 @@
newLines = 0;
oldLines = 0;
+ nh.param("~min_precision", min_precision, 0.7);
+ nh.param("~min_recall", min_recall, 0.7);
+ nh.param("~plane_limit", plane_limit, 0);
+ nh.param("~calibration_type", calibration_type, 0);
+
// subscribe to topics
mocap_sub = nh.subscribe("/phase_space_snapshot", 1,
&MocapEval::mocapCallback, this);
@@ -352,6 +357,12 @@
continue;
if (obs.h > mocap_obs.h * 1.5 || obs.h < mocap_obs.h * 0.5)
continue;
+ if(obs.recall<min_recall)
+ continue;
+ if(obs.precision<min_precision)
+ continue;
+ if(plane_limit>0 && obs.plane_id>=plane_limit)
+ continue;
observations.push_back(obs);
}
@@ -383,15 +394,20 @@
evaluateData(mocap_obs2, observations);
+ if(calibration_type>0)
+ findBestDelta();
-// findBestDelta();
-
}
void MocapEval::findBestDelta() {
if (delta.size() == 0)
return;
+ if(calibration_type==1) {
+ bestDelta = delta.back();
+ return;
+ }
+
double currentError = deltaError(delta, bestDelta);
// strategy 1: find delta that yields smallest error in list
@@ -525,7 +541,7 @@
double h = visual_obs[i].h;
stringstream s;
- s <<dist_to_camera<<" "<<(angle_to_camera/M_PI*180.)<<" "<<err_trans<<" "<<(err_rot/M_PI*180.0)<<" " <<w<<" "<<h<<endl;
+ s <<dist_to_camera<<" "<<(angle_to_camera/M_PI*180.)<<" "<<err_trans<<" "<<(err_rot/M_PI*180.0)<<" " <<w<<" "<<h<<" "<<visual_obs[i].plane_id<<endl;
obs_file << s.str();
cout <<s.str();
Modified: pkg/trunk/sandbox/planar_objects/src/mocap_eval.h
===================================================================
--- pkg/trunk/sandbox/planar_objects/src/mocap_eval.h 2009-09-04 09:53:41 UTC (rev 23828)
+++ pkg/trunk/sandbox/planar_objects/src/mocap_eval.h 2009-09-04 13:09:33 UTC (rev 23829)
@@ -45,6 +45,15 @@
int oldLines;
int newLines;
+ double min_precision;
+ double min_recall;
+ int plane_limit; // 0: don't limit visible planes
+ // >0: only consider first n planes
+
+ int calibration_type; // 0: calibration off
+ //1 : differencing
+ //2: least-squares optimization
+
roslib::Header header;
// MESSAGES - INCOMING
ros::Subscriber observations_sub;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <eth...@us...> - 2009-09-04 09:53:56
|
Revision: 23828
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23828&view=rev
Author: ethandreyfuss
Date: 2009-09-04 09:53:41 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
Adding the narrow stereo pair to pre.launch.
Modified Paths:
--------------
pkg/trunk/stacks/pr2/pr2_alpha/pre.launch
Modified: pkg/trunk/stacks/pr2/pr2_alpha/pre.launch
===================================================================
--- pkg/trunk/stacks/pr2/pr2_alpha/pre.launch 2009-09-04 08:29:41 UTC (rev 23827)
+++ pkg/trunk/stacks/pr2/pr2_alpha/pre.launch 2009-09-04 09:53:41 UTC (rev 23828)
@@ -62,6 +62,34 @@
<param name="imu/angular_velocity_stdev" type="double" value="0.00017" />
<node machine="realtime" pkg="imu_node" type="imu_node" name="imu_node" output="screen"/>
+<!--
+ <group ns="wide_stereo">
+ <node machine="two" pkg="dcam" type="stereodcam" name="stereodcam" respawn="false" >
+ <param name="videre_mode" type="str" value="none"/>
+ <param name="fps" type="double" value="15.0"/>
+ <param name="frame_id" type="string" value="wide_stereo_optical_frame"/>
+ <param name="exposure_auto" type="bool" value="true"/>
+ <param name="brightness_auto" type="bool" value="true"/>
+ <param name="gain_auto" type="bool" value="true"/>
+ <param name="guid" type="string" value="FILL_ME_IN"/>
+ </node>
+ </group>
+-->
+
+ <group ns="narrow_stereo">
+ <node machine="two" pkg="dcam" type="stereodcam" name="stereodcam" respawn="false" >
+ <param name="videre_mode" type="str" value="none"/>
+ <param name="fps" type="double" value="15.0"/>
+ <param name="frame_id" type="string" value="narrow_stereo_optical_frame"/>
+ <param name="exposure" type="int" value="6"/>
+ <param name="exposure_auto" type="bool" value="true"/>
+ <param name="brightness_auto" type="bool" value="true"/>
+ <param name="gain_auto" type="bool" value="true"/>
+ <param name="guid" type="string" value="5505040008ffff"/>
+ </node>
+ </group>
+
+
<!-- Forearm Cameras -->
<node machine="two" name="forearm_camera_r" pkg="wge100_camera" type="wge100_camera_node" respawn="false" output="screen">
<param name="camera_url" type="str" value="name://forearm_r"/>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jue...@us...> - 2009-09-04 08:29:49
|
Revision: 23827
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23827&view=rev
Author: juergensturm
Date: 2009-09-04 08:29:41 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
eval saver
Modified Paths:
--------------
pkg/trunk/sandbox/planar_objects/CMakeLists.txt
pkg/trunk/sandbox/planar_objects/run_experiment.sh
pkg/trunk/sandbox/planar_objects/src/box_detector.cpp
Added Paths:
-----------
pkg/trunk/sandbox/planar_objects/src/eval_saver.cpp
pkg/trunk/sandbox/planar_objects/src/eval_saver.h
Modified: pkg/trunk/sandbox/planar_objects/CMakeLists.txt
===================================================================
--- pkg/trunk/sandbox/planar_objects/CMakeLists.txt 2009-09-04 08:22:17 UTC (rev 23826)
+++ pkg/trunk/sandbox/planar_objects/CMakeLists.txt 2009-09-04 08:29:41 UTC (rev 23827)
@@ -36,6 +36,8 @@
rospack_add_executable(mocap_eval src/mocap_eval.cpp src/vis_utils.cpp src/track_utils.cpp)
+rospack_add_executable(eval_saver src/eval_saver.cpp)
+
#rospack_add_executable(stereo_throttle src/stereo_throttle)
rospack_add_executable(box_tracker src/box_tracker.cpp src/vis_utils.cpp src/track_utils.cpp)
Modified: pkg/trunk/sandbox/planar_objects/run_experiment.sh
===================================================================
--- pkg/trunk/sandbox/planar_objects/run_experiment.sh 2009-09-04 08:22:17 UTC (rev 23826)
+++ pkg/trunk/sandbox/planar_objects/run_experiment.sh 2009-09-04 08:29:41 UTC (rev 23827)
@@ -4,7 +4,7 @@
do
#bagfile=$1
-time_per_frame="40.00"
+time_per_frame="30.00"
save_to="$HOME/bags/output"
record_topics="/box_detector/observations"
@@ -64,8 +64,8 @@
# gnome-terminal --geometry $1 --command "bash -i -c \". $HOME/.bashrc; pwd; echo $2; $2; echo Exiting..; sleep 3\""
- $2 &> /dev/null &
-# $2 &
+# $2 &> /dev/null &
+ $2 &
}
start 80x6-0-0 "roscore"
Modified: pkg/trunk/sandbox/planar_objects/src/box_detector.cpp
===================================================================
--- pkg/trunk/sandbox/planar_objects/src/box_detector.cpp 2009-09-04 08:22:17 UTC (rev 23826)
+++ pkg/trunk/sandbox/planar_objects/src/box_detector.cpp 2009-09-04 08:29:41 UTC (rev 23827)
@@ -238,7 +238,7 @@
Time timeStamp = Time::now();
currentTime = Time::now();
- if (lastTime + lastDuration * 1.1 > currentTime)
+ if (lastTime + lastDuration * 0.1 > currentTime)
{
ROS_INFO("skipping frame..");
return;
Added: pkg/trunk/sandbox/planar_objects/src/eval_saver.cpp
===================================================================
--- pkg/trunk/sandbox/planar_objects/src/eval_saver.cpp (rev 0)
+++ pkg/trunk/sandbox/planar_objects/src/eval_saver.cpp 2009-09-04 08:29:41 UTC (rev 23827)
@@ -0,0 +1,73 @@
+/*
+ * box_tracker.cpp
+ *
+ * Created on: Jul 28, 2009
+ * Author: sturm
+ */
+
+#include "assert.h"
+#include "eval_saver.h"
+#include "ros/node.h"
+
+#include <fstream>
+
+#define PRINTVEC(a) a.x()<<" "<<a.y()<<" "<<a.z()<<" "
+#define SQR(a) ((a)*(a))
+#define sqr(a) ((a)*(a))
+#define CVWINDOW(a) cvNamedWindow(a,CV_WINDOW_AUTOSIZE); cvMoveWindow(a,(cvWindows /3 )* 500,(cvWindows%3)*500); cvWindows++;
+
+#define PRINTTF(tf) \
+ "btTransform(btQuaternion(" <<\
+ " "<<(tf).getRotation().x()<< \
+ ","<<(tf).getRotation().y()<< \
+ ","<<(tf).getRotation().z()<< \
+ ","<<(tf).getRotation().w() << \
+ "),btVector3("<<(tf).getOrigin().x()<< \
+ ","<<(tf).getOrigin().y()<< \
+ ","<<(tf).getOrigin().z()<< "))"
+
+using namespace std;
+
+int main(int argc, char **argv) {
+ ros::init(argc, argv, "eval_saver");
+
+ planar_objects::EvalSaver node;
+ ros::spin();
+
+ return 0;
+}
+
+namespace planar_objects {
+
+// Constructor
+EvalSaver::EvalSaver() {
+ // subscribe to topics
+ eval_sub = nh.subscribe("mocap_eval/eval", 0,&EvalSaver::evalCallback, this);
+}
+
+void EvalSaver::evalCallback(const MocapEvalObservations::ConstPtr& msg) {
+ eval_msg = msg;
+
+ ofstream obs_file ( "eval-obs.txt", ios::app );
+ ofstream rec_file ( "eval-rec.txt", ios::app );
+
+ for (size_t i = 0; i < msg->obs.size(); i++) {
+ stringstream s;
+ s <<msg->dist_to_camera<<" "<<(msg->angle_to_camera/M_PI*180.)<<" "<<
+ msg->obs[i].err_trans<<" "<<(msg->obs[i].err_rot/M_PI*180.0)<<" " <<
+ msg->obs[i].w<<" "<<msg->obs[i].h<<" "<<
+ msg->obs[i].plane_id<<" "<<
+ endl;
+
+ obs_file << s.str();
+ cout <<s.str();
+ }
+
+ stringstream s;
+ s <<msg->dist_to_camera<<" "<<(msg->angle_to_camera/M_PI*180.)<<" "<<msg->obs.size()<<endl;
+
+ rec_file << s.str();
+ cout <<s.str();
+}
+
+}
Added: pkg/trunk/sandbox/planar_objects/src/eval_saver.h
===================================================================
--- pkg/trunk/sandbox/planar_objects/src/eval_saver.h (rev 0)
+++ pkg/trunk/sandbox/planar_objects/src/eval_saver.h 2009-09-04 08:29:41 UTC (rev 23827)
@@ -0,0 +1,37 @@
+/*
+ * box_tracker.h
+ *
+ * Created on: Jul 28, 2009
+ * Author: sturm
+ */
+
+#ifndef EVAL_SAVER_H_
+#define EVAL_SAVER_H_
+
+#include "ros/ros.h"
+#include "planar_objects/MocapEvalObservations.h"
+
+namespace planar_objects
+{
+
+class EvalSaver
+{
+public:
+ ros::NodeHandle nh;
+
+ // MESSAGES - INCOMING
+ ros::Subscriber eval_sub;
+ MocapEvalObservationsConstPtr eval_msg;
+
+ // Constructor
+ EvalSaver();
+
+ // Callbacks
+ void evalCallback(const MocapEvalObservations::ConstPtr& observations);
+};
+
+}
+
+int main(int argc, char** argv);
+
+#endif /* BOX_TRACKER_H_ */
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ale...@us...> - 2009-09-04 08:22:30
|
Revision: 23826
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23826&view=rev
Author: alexeylatyshev
Date: 2009-09-04 08:22:17 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
Detection test now writes average detection time into statistics files
Modified Paths:
--------------
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/include/outlet_detection/outlet_detector_test.h
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/detection_test.cpp
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/outlet_detector_test.cpp
Modified: pkg/trunk/stacks/visual_feature_detectors/outlet_detection/include/outlet_detection/outlet_detector_test.h
===================================================================
--- pkg/trunk/stacks/visual_feature_detectors/outlet_detection/include/outlet_detection/outlet_detector_test.h 2009-09-04 07:56:06 UTC (rev 23825)
+++ pkg/trunk/stacks/visual_feature_detectors/outlet_detection/include/outlet_detection/outlet_detector_test.h 2009-09-04 08:22:17 UTC (rev 23826)
@@ -55,7 +55,7 @@
void setRealOutlet(outlet_test_elem& test_elem, CvMat* intrinsic_matrix = 0, CvMat* distortion_params = 0);
// Writes the file with test resultes
-int writeTestResults(char* filename, const vector<outlet_test_elem>& test_data, TestResult* result = 0);
+int writeTestResults(char* filename, const vector<outlet_test_elem>& test_data, TestResult* result = 0, float avg_time = 0);
int compareOutlets(outlet_test_elem& test_elem, int accuracy = 3);
int compareAllOutlets(vector<outlet_test_elem>& test_data, int accuracy = 3);
Modified: pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/detection_test.cpp
===================================================================
--- pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/detection_test.cpp 2009-09-04 07:56:06 UTC (rev 23825)
+++ pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/detection_test.cpp 2009-09-04 08:22:17 UTC (rev 23826)
@@ -34,9 +34,10 @@
//---------------
int run_detection_test(int detector_id, char* mode, char* test_path, char* config_filename,
char* camera_filename, char* train_config, char* output_path,
- char* new_test_config = 0, int accuracy = 5, TestResult* result = 0)
+ char* new_test_config = 0, int accuracy = 5, TestResult* result = 0, float* avg_time = 0)
{
char output_test_config[1024];
+ float time = 0;
if (result)
@@ -185,7 +186,6 @@
#endif //_VERBOSE
-
//Running the test
if (strcmp(mode,"modify")==0)
{
@@ -194,6 +194,7 @@
}
else
{
+ int64 time_start,time_end;
switch (detector_id)
{
case ONE_WAY_DETECTOR:
@@ -201,29 +202,42 @@
{
outlet_template.load(train_config);
}
+ time_start = cvGetTickCount();
runOneWayOutletDetectorTest(intrinsic_matrix, distortion_params, outlet_template, test_data, output_path);
+ time_end = cvGetTickCount();
break;
case ONE_WAY_POSES_DETECTOR:
if (train_config)
{
outlet_template.load(train_config);
}
+ time_start = cvGetTickCount();
runOneWayPosesOutletDetectorTest(intrinsic_matrix, distortion_params, outlet_template, test_data, output_path);
+ time_end = cvGetTickCount();
break;
case L_DETECTOR:
+ time_start = cvGetTickCount();
runLOutletDetectorTest(intrinsic_matrix, distortion_params, train_config, test_data, output_path);
+ time_end = cvGetTickCount();
break;
case FERNS_L_DETECTOR:
+ time_start = cvGetTickCount();
runFernsLOutletDetectorTest(intrinsic_matrix, distortion_params, train_config, test_data, output_path);
+ time_end = cvGetTickCount();
break;
case FERNS_ONE_WAY_DETECTOR:
if (train_config)
{
outlet_template.load(train_config);
}
+ time_start = cvGetTickCount();
runFernsOneWayOutletDetectorTest(intrinsic_matrix, distortion_params, outlet_template, train_config, test_data, output_path);
+ time_end = cvGetTickCount();
break;
}
+ time = float(time_end - time_start)/cvGetTickFrequency()*1e-6;
+ if ((int)test_data.size() > 0)
+ time/=(int)test_data.size();
}
@@ -244,8 +258,12 @@
sprintf(filename,"%s/results.txt",output_path);
else
sprintf(filename,"results.txt",output_path);
- writeTestResults(filename,test_data,result);
+ writeTestResults(filename,test_data,result,time);
+ printf("-------------\n");
+ printf ("Average detection time: %f\n", time);
printf ("Test results were successfully written into %s\n", filename);
+ printf("=============\n");
+ *avg_time = time;
}
@@ -271,7 +289,37 @@
int detector_id = -1;
+ //float time_det = 0;
+ //float time_match = 0;
+ //float time_total = 0;
+ //float t;
+ //int nn = 0;
+ //FILE* f = fopen("d:\\detect.txt","r");
+ //char str[1024];
+ //while (fscanf(f,"%f\n",&t)!=EOF)
+ //{
+ // time_det+=t;
+ // nn++;
+ //}
+ //printf("%f\n",time_det/nn);
+ //fclose(f);
+ //f = fopen("d:/match.txt","r");
+ //while (fscanf(f,"%f\n",&t)!=EOF)
+ //{
+ // time_match+=t;
+ //}
+ //printf("%f\n",time_match/nn);
+ //fclose(f);
+ //f = fopen("d:/total.txt","r");
+ //while (fscanf(f,"%f\n",&t)!=EOF)
+ //{
+ // time_total+=t;
+ //}
+ //printf("%f\n",time_total/nn);
+ //fclose(f);
+
+
if(argc != 8 && argc != 9 && argc!=4 && argc!=5 && argc!=6)
{
printf("Usage: detection_test <detector_name> <mode[test|modify|generate]> <images_path> <test_config_filename> <camera_config> <output_path> <accuracy|output_test_config_filename|output_test_config_filename>\n");
@@ -341,6 +389,7 @@
char image_dir[1024];
vector<TestResult*> test_results;
vector<char*> dataset_names;
+ vector<float> avg_times;
bool doWork = true;
while (fscanf(f,"%s\n",image_dir) !=EOF)
{
@@ -413,7 +462,8 @@
//strcpy(command_line[8],argv[4]);
TestResult* res = new TestResult();
- if (/*run(9,command_line,res)*/run_detection_test(detector_id,mode,images_path,test_config_filename,camera_config,train_config,output_path,0,accuracy,res) < 0 )
+ float avg_time = 0;
+ if (run_detection_test(detector_id,mode,images_path,test_config_filename,camera_config,train_config,output_path,0,accuracy,res,&avg_time) < 0 )
{
printf("Unable to %s dataset %s\n",mode,image_dir);
}
@@ -423,6 +473,7 @@
char* name = new char[strlen(image_dir)];
strcpy(name,image_dir);
dataset_names.push_back(name);
+ avg_times.push_back(avg_time);
}
}
}
@@ -449,8 +500,8 @@
nCorrect += res->correct;
nIncorrect += res->incorrect;
nSkipped += res->skipped;
- fprintf(fres,"%s (correct/total): %d/%d\n",dataset_names[i],res->correct,res->correct+res->incorrect);
- printf("%s (correct/total): %d/%d\n",dataset_names[i],res->correct,res->correct+res->incorrect);
+ fprintf(fres,"%s (correct/total): %d/%d | Average time: %f\n",dataset_names[i],res->correct,res->correct+res->incorrect,avg_times[i]);
+ printf("%s (correct/total): %d/%d | Average time: %f\n",dataset_names[i],res->correct,res->correct+res->incorrect,avg_times[i]);
}
fprintf(fres,"---------------\n");
Modified: pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/outlet_detector_test.cpp
===================================================================
--- pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/outlet_detector_test.cpp 2009-09-04 07:56:06 UTC (rev 23825)
+++ pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/outlet_detector_test.cpp 2009-09-04 08:22:17 UTC (rev 23826)
@@ -515,7 +515,7 @@
}
}
//--------------------------------
-int writeTestResults(char* filename, const vector<outlet_test_elem>& test_data, TestResult* result)
+int writeTestResults(char* filename, const vector<outlet_test_elem>& test_data, TestResult* result, float avg_time)
{
FILE* f = fopen(filename,"w");
if (f)
@@ -551,7 +551,9 @@
}
if (nCorrect+nNA > 0)
fprintf(f,"---------------------\n");
- fprintf(f,"Total images:%d\nSkipped:%d\nCorrect:%d\nIncorrect:%d\nNA:%d",nTotal-nSkipped,nSkipped,nCorrect,(int)test_data.size()-nCorrect-nNA,nNA);
+ fprintf(f,"Total images:%d\nSkipped:%d\nCorrect:%d\nIncorrect:%d\nNA:%d\n",nTotal-nSkipped,nSkipped,nCorrect,(int)test_data.size()-nCorrect-nNA,nNA);
+ if (avg_time > 0)
+ fprintf(f,"---------------------Average detection time: %f",avg_time);
fclose(f);
if (result)
{
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2009-09-04 07:56:13
|
Revision: 23825
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23825&view=rev
Author: gerkey
Date: 2009-09-04 07:56:06 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
switched ~ to /u/gerkey, in an effort to avoid creating the symlink in the package directory, which happens on some Hudson machines
Modified Paths:
--------------
pkg/trunk/stacks/trex/trex_core/make_trex
Modified: pkg/trunk/stacks/trex/trex_core/make_trex
===================================================================
--- pkg/trunk/stacks/trex/trex_core/make_trex 2009-09-04 07:47:36 UTC (rev 23824)
+++ pkg/trunk/stacks/trex/trex_core/make_trex 2009-09-04 07:56:06 UTC (rev 23825)
@@ -9,8 +9,8 @@
# Set up a build directory with suitable soft link & environment variable bindings. This is set
# at the root of the user tree to avoid problems with very long paths to build files which
# cause problems with jam, which has a hard limit on the command line length
-ln -s `rospack find trex_core` ~/trex_core_build
-cd ~/trex_core_build
+ln -s `rospack find trex_core` $HOME/trex_core_build
+cd $HOME/trex_core_build
source ./TREX/devConfig
cd TREX
@@ -29,7 +29,7 @@
if [ $? -ne 0 ] ; then
echo "Jam of $1 failed."
- rm -rf ~/trex_core_build
+ rm -rf $HOME/trex_core_build
exit $3
fi
}
@@ -38,13 +38,13 @@
if [ "$1" = "test" ] ; then
if [ $? -ne 0 ] ; then
echo "TREX test file creation failed."
- rm -rf ~/trex_core_build
+ rm -rf $HOME/trex_core_build
exit 1
fi
jam run-agent-module-tests
if [ $? -ne 0 ] ; then
echo "TREX tests failed."
- rm -rf ~/trex_core_build
+ rm -rf $HOME/trex_core_build
exit 1
fi
@@ -54,6 +54,6 @@
fi
jam nddl-reader
run_jam "libTREX_o" "-sVARIANTS=OPTIMIZED -sLIBRARIES=SHARED" 2
- rm -rf ~/trex_core_build
+ rm -rf $HOME/trex_core_build
fi
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <wgh...@us...> - 2009-09-04 07:47:45
|
Revision: 23824
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23824&view=rev
Author: wghassan
Date: 2009-09-04 07:47:36 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
fixes for the app bar
Modified Paths:
--------------
pkg/trunk/sandbox/web/map_tiler/src/map_tiler/handler.py
pkg/trunk/sandbox/web/navigation_application/src/navigation_application/jslib/map_viewer.js
pkg/trunk/sandbox/web/navigation_application/src/navigation_application/templates/index.cs
pkg/trunk/sandbox/web/rosweb2/src/rosweb.py
pkg/trunk/sandbox/web/sample_application/src/sample_application/cgibin/sample_application_index.py
pkg/trunk/sandbox/web/sample_application/src/sample_application/templates/index.cs
pkg/trunk/sandbox/web/webui/src/webui/mod/webui/jslib/ros.js
pkg/trunk/sandbox/web/webui/src/webui/mod/webui/templates/header.cs
pkg/trunk/sandbox/web/webui/src/webui/mod/webui/templates/nodes.cs
pkg/trunk/sandbox/web/webui/src/webui/mod/webui/templates/powerboard.cs
pkg/trunk/sandbox/web/webui/src/webui/mod/webui/templates/status.cs
pkg/trunk/sandbox/web/webui/src/webui/mod/webui/templates/topic.cs
pkg/trunk/sandbox/web/webui/src/webui/mod/webui/templates/topics.cs
Added Paths:
-----------
pkg/trunk/sandbox/web/webui/src/webui/mod/webui/templates/status_header.cs
Modified: pkg/trunk/sandbox/web/map_tiler/src/map_tiler/handler.py
===================================================================
--- pkg/trunk/sandbox/web/map_tiler/src/map_tiler/handler.py 2009-09-04 07:45:26 UTC (rev 23823)
+++ pkg/trunk/sandbox/web/map_tiler/src/map_tiler/handler.py 2009-09-04 07:47:36 UTC (rev 23824)
@@ -14,6 +14,7 @@
from numpy import float64
from simplify import Simplifier
+import rosweb
_map_cache = {}
_scale_cache = {}
Modified: pkg/trunk/sandbox/web/navigation_application/src/navigation_application/jslib/map_viewer.js
===================================================================
--- pkg/trunk/sandbox/web/navigation_application/src/navigation_application/jslib/map_viewer.js 2009-09-04 07:45:26 UTC (rev 23823)
+++ pkg/trunk/sandbox/web/navigation_application/src/navigation_application/jslib/map_viewer.js 2009-09-04 07:47:36 UTC (rev 23824)
@@ -45,8 +45,8 @@
var MapViewer = Class.create({
initialize: function(domobj) {
this.viewer = domobj;
- //this.topics = ['/robot_pose_visualization', '/move_base/NavfnROS/plan:simplified', '/base_pose_ground_truth'];
- this.topics = ['/robot_pose_visualization', '/move_base/NavfnROS/plan:simplified'];
+ this.topics = ['/robot_pose_visualization', '/move_base/NavfnROS/plan:simplified', '/base_pose_ground_truth'];
+ //this.topics = ['/robot_pose_visualization', '/move_base/NavfnROS/plan:simplified'];
},
buttons: [
Modified: pkg/trunk/sandbox/web/navigation_application/src/navigation_application/templates/index.cs
===================================================================
--- pkg/trunk/sandbox/web/navigation_application/src/navigation_application/templates/index.cs 2009-09-04 07:45:26 UTC (rev 23823)
+++ pkg/trunk/sandbox/web/navigation_application/src/navigation_application/templates/index.cs 2009-09-04 07:47:36 UTC (rev 23824)
@@ -11,8 +11,7 @@
<table align=right>
<td>
-<input class=app_button type=button value="" objtype="LaunchButtonWidget2" topic="/app_update" taskid="<?cs var:CGI.cur.app.taskid ?>">
-<div class=app_status objtype=TextWidget topic="/app_update" key=status selector="taskid" selectorValue="<?cs var:CGI.cur.app.taskid?>"> </div>
+<div class=app_button objtype="LaunchButtonWidget2" taskid="navigation_application/navigation_application.app">
</td>
</table>
Modified: pkg/trunk/sandbox/web/rosweb2/src/rosweb.py
===================================================================
--- pkg/trunk/sandbox/web/rosweb2/src/rosweb.py 2009-09-04 07:45:26 UTC (rev 23823)
+++ pkg/trunk/sandbox/web/rosweb2/src/rosweb.py 2009-09-04 07:47:36 UTC (rev 23824)
@@ -235,7 +235,7 @@
self.messages.append((self.factory.counter, newmsg))
self.factory.counter = self.factory.counter + 1
- self.newmsg = self.messages[-20:]
+ self.messages = self.messages[-20:]
Modified: pkg/trunk/sandbox/web/sample_application/src/sample_application/cgibin/sample_application_index.py
===================================================================
--- pkg/trunk/sandbox/web/sample_application/src/sample_application/cgibin/sample_application_index.py 2009-09-04 07:45:26 UTC (rev 23823)
+++ pkg/trunk/sandbox/web/sample_application/src/sample_application/cgibin/sample_application_index.py 2009-09-04 07:47:36 UTC (rev 23824)
@@ -11,12 +11,14 @@
from webui import MBPage
+import db_webui
+
class MyPage(MBPage.MBPage):
def setup(self, hdf):
pass
def display(self, hdf):
- pass
+ db_webui.grabTopics(hdf, ['/chatter:more'])
def run(context):
Modified: pkg/trunk/sandbox/web/sample_application/src/sample_application/templates/index.cs
===================================================================
--- pkg/trunk/sandbox/web/sample_application/src/sample_application/templates/index.cs 2009-09-04 07:45:26 UTC (rev 23823)
+++ pkg/trunk/sandbox/web/sample_application/src/sample_application/templates/index.cs 2009-09-04 07:47:36 UTC (rev 23824)
@@ -9,7 +9,7 @@
<table align=right>
<td>
-<div class=app_button objtype="LaunchButtonWidget2" taskid="<?cs var:CGI.cur.app.taskid ?>">
+<div class=app_button objtype="LaunchButtonWidget2" taskid="sample_application/sample_app.app">
</td>
</table>
@@ -22,5 +22,6 @@
<div id=ErrorDiv></div>
+<?cs include:"rosfooter.cs"?>
</body>
</html>
Modified: pkg/trunk/sandbox/web/webui/src/webui/mod/webui/jslib/ros.js
===================================================================
--- pkg/trunk/sandbox/web/webui/src/webui/mod/webui/jslib/ros.js 2009-09-04 07:45:26 UTC (rev 23823)
+++ pkg/trunk/sandbox/web/webui/src/webui/mod/webui/jslib/ros.js 2009-09-04 07:47:36 UTC (rev 23824)
@@ -392,8 +392,10 @@
new_message: function($super, msg) {
if(msg[this.key] != null) {
var d = document.createElement("div");
- d.innerHTML = msg[this.key] + "<br>";
+ d.appendChild(document.createTextNode(msg[this.key]));
+ d.innerHTML = msg[this.key];
this.textdiv.appendChild(d);
+
}
}
});
Modified: pkg/trunk/sandbox/web/webui/src/webui/mod/webui/templates/header.cs
===================================================================
--- pkg/trunk/sandbox/web/webui/src/webui/mod/webui/templates/header.cs 2009-09-04 07:45:26 UTC (rev 23823)
+++ pkg/trunk/sandbox/web/webui/src/webui/mod/webui/templates/header.cs 2009-09-04 07:47:36 UTC (rev 23824)
@@ -20,13 +20,9 @@
</tr>
</table>
-<table class=head_buttons width=100%>
+<table class=head_buttons width=80%>
<tr>
<td class=head_buttons width=1% onclick="javascript:location.href='<?cs var:CGI.ScriptName?>/webui/apps.py'">Apps</a></td>
-<!--<td class=head_buttons width=1% onclick="javascript:location.href='<?cs var:CGI.ScriptName?>/webui/move.py'">Move</td> -->
-<td class=head_buttons width=1% onclick="javascript:location.href='<?cs var:CGI.ScriptName?>/webui/topics.py'">Topics</a></td>
-<td class=head_buttons width=1% onclick="javascript:location.href='<?cs var:CGI.ScriptName?>/webui/nodes.py'">Nodes</td>
-<td class=head_buttons width=1% onclick="javascript:location.href='<?cs var:CGI.ScriptName?>/webui/powerboard.py'">Power</td>
<td class=head_buttons width=1% onclick="javascript:location.href='<?cs var:CGI.ScriptName?>/webui/status.py'">Status</a></td>
<td class=head_buttons width=1% onclick="javascript:location.href='<?cs var:CGI.ScriptName?>/webui/admin.py'">Admin</a></td>
Modified: pkg/trunk/sandbox/web/webui/src/webui/mod/webui/templates/nodes.cs
===================================================================
--- pkg/trunk/sandbox/web/webui/src/webui/mod/webui/templates/nodes.cs 2009-09-04 07:45:26 UTC (rev 23823)
+++ pkg/trunk/sandbox/web/webui/src/webui/mod/webui/templates/nodes.cs 2009-09-04 07:47:36 UTC (rev 23824)
@@ -6,6 +6,7 @@
<body onload="ros_handleOnLoad('/ros')">
<?cs include:"header.cs" ?>
<br>
+<?cs include:"status_header.cs" ?>
<h3>Nodes:</h3>
<ul objtype="ListWidget" topic="/topics" key="nodes">
Modified: pkg/trunk/sandbox/web/webui/src/webui/mod/webui/templates/powerboard.cs
===================================================================
--- pkg/trunk/sandbox/web/webui/src/webui/mod/webui/templates/powerboard.cs 2009-09-04 07:45:26 UTC (rev 23823)
+++ pkg/trunk/sandbox/web/webui/src/webui/mod/webui/templates/powerboard.cs 2009-09-04 07:47:36 UTC (rev 23824)
@@ -11,6 +11,7 @@
<body onload="ros_handleOnLoad('/ros')">
<?cs include:"header.cs" ?>
<br>
+<?cs include:"status_header.cs" ?>
<br>
Modified: pkg/trunk/sandbox/web/webui/src/webui/mod/webui/templates/status.cs
===================================================================
--- pkg/trunk/sandbox/web/webui/src/webui/mod/webui/templates/status.cs 2009-09-04 07:45:26 UTC (rev 23823)
+++ pkg/trunk/sandbox/web/webui/src/webui/mod/webui/templates/status.cs 2009-09-04 07:47:36 UTC (rev 23824)
@@ -6,7 +6,7 @@
<body onload="ros_handleOnLoad('/ros')">
<?cs include:"header.cs" ?>
<br>
-<br>
+<?cs include:"status_header.cs" ?>
<table style="border: 1px solid black; width: 150px; float: right">
<tr>
Added: pkg/trunk/sandbox/web/webui/src/webui/mod/webui/templates/status_header.cs
===================================================================
--- pkg/trunk/sandbox/web/webui/src/webui/mod/webui/templates/status_header.cs (rev 0)
+++ pkg/trunk/sandbox/web/webui/src/webui/mod/webui/templates/status_header.cs 2009-09-04 07:47:36 UTC (rev 23824)
@@ -0,0 +1,8 @@
+<table class=head_buttons width=30%>
+<tr>
+<td class=head_buttons width=1% onclick="javascript:location.href='<?cs var:CGI.ScriptName?>/webui/topics.py'">Topics</a></td>
+<td class=head_buttons width=1% onclick="javascript:location.href='<?cs var:CGI.ScriptName?>/webui/nodes.py'">Nodes</td>
+<td class=head_buttons width=1% onclick="javascript:location.href='<?cs var:CGI.ScriptName?>/webui/powerboard.py'">Power</td>
+
+</tr>
+</table>
Modified: pkg/trunk/sandbox/web/webui/src/webui/mod/webui/templates/topic.cs
===================================================================
--- pkg/trunk/sandbox/web/webui/src/webui/mod/webui/templates/topic.cs 2009-09-04 07:45:26 UTC (rev 23823)
+++ pkg/trunk/sandbox/web/webui/src/webui/mod/webui/templates/topic.cs 2009-09-04 07:47:36 UTC (rev 23824)
@@ -6,6 +6,9 @@
<body onload="ros_handleOnLoad('/ros')">
<?cs include:"header.cs" ?>
+<br>
+<?cs include:"status_header.cs" ?>
+
<h3>Topic: <?cs var:CGI.cur.topic?></h3>
<div style="font-size: 10pt; " objtype=MessageWidget topic="<?cs var:CGI.cur.topic ?>"></div><br>
Modified: pkg/trunk/sandbox/web/webui/src/webui/mod/webui/templates/topics.cs
===================================================================
--- pkg/trunk/sandbox/web/webui/src/webui/mod/webui/templates/topics.cs 2009-09-04 07:45:26 UTC (rev 23823)
+++ pkg/trunk/sandbox/web/webui/src/webui/mod/webui/templates/topics.cs 2009-09-04 07:47:36 UTC (rev 23824)
@@ -6,6 +6,7 @@
<body onload="ros_handleOnLoad('/ros')">
<?cs include:"header.cs" ?>
<br>
+<?cs include:"status_header.cs" ?>
<h3>Published topics:</h3>
<ul objtype="ListWidget" topic="/topics" key="pubtopics">
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2009-09-04 07:45:33
|
Revision: 23823
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23823&view=rev
Author: gerkey
Date: 2009-09-04 07:45:26 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
blacklisted
Added Paths:
-----------
pkg/trunk/sandbox/dynamics_estimation/ROS_BUILD_BLACKLIST
Added: pkg/trunk/sandbox/dynamics_estimation/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/sandbox/dynamics_estimation/ROS_BUILD_BLACKLIST (rev 0)
+++ pkg/trunk/sandbox/dynamics_estimation/ROS_BUILD_BLACKLIST 2009-09-04 07:45:26 UTC (rev 23823)
@@ -0,0 +1,21 @@
+[ 25%] Building CXX object
+CMakeFiles/dynamics_estimation.dir/src/dynamics_estimation_node.o
+/u/gerkey/code/personalrobots/sandbox/dynamics_estimation/src/dynamics_estimation_node.cpp:
+In member function ‘void
+dynamics_estimation::DynamicsEstimationNode::mechanismStateCallback(const
+boost::shared_ptr<const pr2_mechanism_msgs::MechanismState>&)’:
+/u/gerkey/code/personalrobots/sandbox/dynamics_estimation/src/dynamics_estimation_node.cpp:173:
+error: ‘const class pr2_mechanism_msgs::MechanismState’ has no member named
+‘time’
+make[3]: ***
+[CMakeFiles/dynamics_estimation.dir/src/dynamics_estimation_node.o] Error 1
+make[3]: Leaving directory
+`/wg/stor6a/gerkey/code/personalrobots/sandbox/dynamics_estimation/build'
+make[2]: *** [CMakeFiles/dynamics_estimation.dir/all] Error 2
+make[2]: Leaving directory
+`/wg/stor6a/gerkey/code/personalrobots/sandbox/dynamics_estimation/build'
+make[1]: *** [all] Error 2
+make[1]: Leaving directory
+`/wg/stor6a/gerkey/code/personalrobots/sandbox/dynamics_estimation/build'
+make: *** [all] Error 2
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2009-09-04 07:38:54
|
Revision: 23822
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23822&view=rev
Author: gerkey
Date: 2009-09-04 07:38:43 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
Removed numpy dependency
Modified Paths:
--------------
pkg/trunk/stacks/joystick_drivers/wiimote/manifest.xml
Modified: pkg/trunk/stacks/joystick_drivers/wiimote/manifest.xml
===================================================================
--- pkg/trunk/stacks/joystick_drivers/wiimote/manifest.xml 2009-09-04 07:05:40 UTC (rev 23821)
+++ pkg/trunk/stacks/joystick_drivers/wiimote/manifest.xml 2009-09-04 07:38:43 UTC (rev 23822)
@@ -21,7 +21,6 @@
<url>http://pr.willowgarage.com/wiki/wiimote</url>
<depend package="rospy"/>
<depend package="std_msgs"/>
- <depend package="numpy"/>
</package>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <wa...@us...> - 2009-09-04 07:05:50
|
Revision: 23821
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23821&view=rev
Author: wattsk
Date: 2009-09-04 07:05:40 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
Changes to PRG launch files for new servers
Modified Paths:
--------------
pkg/trunk/stacks/pr2/pr2_alpha/prg.launch
pkg/trunk/stacks/pr2/pr2_alpha/prg.machine
pkg/trunk/stacks/pr2/pr2_alpha/prg_ethercat_reset.launch
Modified: pkg/trunk/stacks/pr2/pr2_alpha/prg.launch
===================================================================
--- pkg/trunk/stacks/pr2/pr2_alpha/prg.launch 2009-09-04 07:03:58 UTC (rev 23820)
+++ pkg/trunk/stacks/pr2/pr2_alpha/prg.launch 2009-09-04 07:05:40 UTC (rev 23821)
@@ -21,7 +21,7 @@
</node>
<!-- Battery Monitor -->
- <node machine="two" pkg="ocean_battery_driver" type="ocean_server" respawn="true">
+<!-- <node machine="two" pkg="ocean_battery_driver" type="ocean_server" respawn="true">
<param name="number_of_ports" type="int" value="4" />
<param name="port0" type="string" value="/dev/ttyUSB0" />
<param name="port1" type="string" value="/dev/ttyUSB1" />
@@ -29,6 +29,7 @@
<param name="port3" type="string" value="/dev/ttyUSB3" />
<param name="debug_level" type="int" value="0" />
</node>
+-->
<node pkg="power_monitor" type="power_monitor" respawn="true"/>
@@ -67,7 +68,7 @@
<param name="acquisition_mode" type="str" value="Triggered"/>
<param name="ip_address" type="str" value="10.68.7.20"/>
</group>
- <node machine="three" name="prosilica" pkg="prosilica_camera" type="prosilica_node" output="screen" respawn="true"/-->
+ <node machine="two" name="prosilica" pkg="prosilica_camera" type="prosilica_node" output="screen" respawn="true"/-->
<!-- Watts: Need to get driver working for stereo on servers -->
<!-- Double stereo setup -->
@@ -102,7 +103,7 @@
<!-- Forearm Camera -->
- <node machine="three" name="forearm_camera_r" pkg="wge100_camera" type="wge100_camera_node" respawn="true" output="screen">
+ <node machine="two" name="forearm_camera_r" pkg="wge100_camera" type="wge100_camera_node" respawn="true" output="screen">
<param name="camera_url" type="str" value="name://forearm_r"/>
<param name="video_mode" type="str" value="640x480x30" />
<param name="auto_exposure" type="bool" value="True" />
Modified: pkg/trunk/stacks/pr2/pr2_alpha/prg.machine
===================================================================
--- pkg/trunk/stacks/pr2/pr2_alpha/prg.machine 2009-09-04 07:03:58 UTC (rev 23820)
+++ pkg/trunk/stacks/pr2/pr2_alpha/prg.machine 2009-09-04 07:05:40 UTC (rev 23821)
@@ -1,7 +1,6 @@
<launch>
<machine name="realtime_root" user="root" address="prg1" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" default="never" />
-
<machine name="realtime" address="prg1" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" />
<machine name="two" address="prg2" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" default="true"/>
</launch>
Modified: pkg/trunk/stacks/pr2/pr2_alpha/prg_ethercat_reset.launch
===================================================================
--- pkg/trunk/stacks/pr2/pr2_alpha/prg_ethercat_reset.launch 2009-09-04 07:03:58 UTC (rev 23820)
+++ pkg/trunk/stacks/pr2/pr2_alpha/prg_ethercat_reset.launch 2009-09-04 07:05:40 UTC (rev 23821)
@@ -9,7 +9,7 @@
<include file="$(find pr2_alpha)/prg.machine" />
<!-- pr2_etherCAT -->
- <node machine="realtime_root" pkg="pr2_etherCAT" type="pr2_etherCAT" args="-i eth0 -x robot_description"/>
+ <node machine="realtime_root" pkg="pr2_etherCAT" type="pr2_etherCAT" args="-i ecat0 -x robot_description"/>
<!-- PR2 Calibration -->
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2009-09-04 07:04:11
|
Revision: 23820
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23820&view=rev
Author: hsujohnhsu
Date: 2009-09-04 07:03:58 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
test world
Added Paths:
-----------
pkg/trunk/stacks/simulator_gazebo/gazebo_worlds/worlds/test_friction.world
Added: pkg/trunk/stacks/simulator_gazebo/gazebo_worlds/worlds/test_friction.world
===================================================================
--- pkg/trunk/stacks/simulator_gazebo/gazebo_worlds/worlds/test_friction.world (rev 0)
+++ pkg/trunk/stacks/simulator_gazebo/gazebo_worlds/worlds/test_friction.world 2009-09-04 07:03:58 UTC (rev 23820)
@@ -0,0 +1,348 @@
+<?xml version="1.0"?>
+
+<gazebo:world
+ xmlns:xi="http://www.w3.org/2001/XInclude"
+ xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
+ xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
+ xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+ xmlns:window="http://playerstage.sourceforge.net/gazebo/xmlschema/#window"
+ xmlns:param="http://playerstage.sourceforge.net/gazebo/xmlschema/#param"
+ xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
+ xmlns:geo="http://willowgarage.com/xmlschema/#geo"
+ xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
+ xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
+ xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
+ xmlns:ui="http://playerstage.sourceforge.net/gazebo/xmlschema/#ui"
+ xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
+ xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
+ xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+ xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics" >
+
+ <verbosity>5</verbosity>
+
+ <!-- cfm is 1e-5 for single precision -->
+ <!-- erp is typically .1-.8 -->
+ <!-- here's the global contact cfm/erp -->
+ <physics:ode>
+ <stepTime>0.001</stepTime>
+ <gravity>0 0 -9.8</gravity>
+ <cfm>0.0000000001</cfm>
+ <erp>0.2</erp>
+ <quickStep>true</quickStep>
+ <quickStepIters>100</quickStepIters>
+ <quickStepW>1.3</quickStepW>
+ <contactMaxCorrectingVel>100.0</contactMaxCorrectingVel>
+ <contactSurfaceLayer>0.001</contactSurfaceLayer>
+ </physics:ode>
+
+
+ <geo:origin>
+ <lat>37.4270909558</lat><lon>-122.077919338</lon>
+ </geo:origin>
+
+ <rendering:gui>
+ <type>fltk</type>
+ <size>480 320</size>
+ <pos>0 0</pos>
+ <frames>
+ <row height="100%">
+ <camera width="100%">
+ <xyz>-22.4 1.72 17.21</xyz>
+ <rpy>0 20.90 -0.9</rpy>
+ </camera>
+ </row>
+ </frames>
+ </rendering:gui>
+
+
+ <rendering:ogre>
+ <ambient>1.0 1.0 1.0 1.0</ambient>
+ <sky>
+ <material>Gazebo/CloudySky</material>
+ </sky>
+ <gazeboPath>media</gazeboPath>
+ <grid>false</grid>
+ <maxUpdateRate>30.0</maxUpdateRate>
+ </rendering:ogre>
+
+
+
+ <model:physical name="gplane">
+ <xyz>0 0 0</xyz>
+ <rpy>0 0 0</rpy>
+ <static>true</static>
+
+ <body:plane name="plane">
+ <geom:plane name="plane">
+ <laserRetro>2000.0</laserRetro>
+ <mu1>50.0</mu1>
+ <mu2>50.0</mu2>
+ <kp>1000000000.0</kp>
+ <kd>1.0</kd>
+ <normal>0 0 1</normal>
+ <size>51.3 51.3</size>
+ <segments>10 10</segments>
+ <uvTile>100 100</uvTile>
+ <material>Gazebo/GrayGrid</material>
+ </geom:plane>
+ </body:plane>
+ </model:physical>
+
+
+ <!-- This sphere is for debugging the target
+ <model:physical name="debug_model">
+ <xyz> -3.0 0.5 2.6</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <body:cylinder name="debug_body">
+ <geom:cylinder name="debug_geom">
+ <kp>100000.0</kp>
+ <kd>1.0</kd>
+ <mesh>default</mesh>
+ <size> 0 0 0</size>
+ <mass> 1.0</mass>
+ <visual>
+ <size> 8 8 1</size>
+ <material>Gazebo/Brick</material>
+ <mesh>unit_cylinder</mesh>
+ </visual>
+ </geom:cylinder>
+ </body:cylinder>
+ </model:physical>-->
+
+
+
+ <!-- The Slide-->
+ <model:physical name="slide1_model">
+ <static>true</static>
+ <xyz> 0.0 0.0 0.00</xyz>
+ <rpy> 0.0 0.0 0.00</rpy>
+ <body:box name="slide1_legs_body">
+ <geom:box name="slide_base_geom">
+ <kp>1000000000.0</kp>
+ <kd>1.0</kd>
+ <mu1>10.0</mu1>
+ <mu2>10.0</mu2>
+ <xyz> 0.0 5.0 13</xyz>
+ <rpy> 45.0 0.0 0.00</rpy>
+ <size> 50.0 20.0 1.0</size>
+ <mass> 1000.0</mass>
+ <visual>
+ <size>50.0 20.0 1.0</size>
+ <material>PR2/Red</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+
+ <!-- boxes -->
+ <model:physical name="box_01_model">
+ <xyz> 2.0 8.0 17.42</xyz>
+ <rpy> 45.0 0.0 0.0</rpy>
+ <body:box name="box_01_body">
+ <massMatrix>true</massMatrix>
+ <mass>1.0</mass>
+ <ixx>10.0</ixx>
+ <ixy>0.0</ixy>
+ <ixz>0.0</ixz>
+ <iyy>10.0</iyy>
+ <iyz>0.0</iyz>
+ <izz>10.0</izz>
+ <cx>0.0</cx>
+ <cy>0.0</cy>
+ <cz>-0.5</cz>
+
+ <geom:box name="box_01_geom">
+ <kp>100000.0</kp>
+ <kd>1.0</kd>
+ <mu1>0.98</mu1>
+ <mu2>0.98</mu2>
+ <mesh>default</mesh>
+ <size>1 1 1</size>
+ <visual>
+ <size>1 1 1</size>
+ <material>Gazebo/Rocky</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+ <model:physical name="box_02_model">
+ <xyz> 4.0 8.0 17.42</xyz>
+ <rpy> 45.0 0.0 0.0</rpy>
+ <body:box name="box_02_body">
+ <massMatrix>true</massMatrix>
+ <mass>1.0</mass>
+ <ixx>10.0</ixx>
+ <ixy>0.0</ixy>
+ <ixz>0.0</ixz>
+ <iyy>10.0</iyy>
+ <iyz>0.0</iyz>
+ <izz>10.0</izz>
+ <cx>0.0</cx>
+ <cy>0.0</cy>
+ <cz>-0.5</cz>
+
+ <geom:box name="box_02_geom">
+ <kp>100000.0</kp>
+ <kd>1.0</kd>
+ <mu1>0.99</mu1>
+ <mu2>0.99</mu2>
+ <mesh>default</mesh>
+ <size>1 1 1</size>
+ <visual>
+ <size>1 1 1</size>
+ <material>Gazebo/Rocky</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+ <model:physical name="box_03_model">
+ <xyz> 6.0 8.0 17.42</xyz>
+ <rpy> 45.0 0.0 0.0</rpy>
+ <body:box name="box_03_body">
+ <massMatrix>true</massMatrix>
+ <mass>1.0</mass>
+ <ixx>10.0</ixx>
+ <ixy>0.0</ixy>
+ <ixz>0.0</ixz>
+ <iyy>10.0</iyy>
+ <iyz>0.0</iyz>
+ <izz>10.0</izz>
+ <cx>0.0</cx>
+ <cy>0.0</cy>
+ <cz>-0.5</cz>
+
+ <geom:box name="box_03_geom">
+ <kp>100000.0</kp>
+ <kd>1.0</kd>
+ <mu1>0.999</mu1>
+ <mu2>0.999</mu2>
+ <mesh>default</mesh>
+ <size>1 1 1</size>
+ <visual>
+ <size>1 1 1</size>
+ <material>Gazebo/Rocky</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+ <model:physical name="box_04_model">
+ <xyz> 8.0 8.0 17.42</xyz>
+ <rpy> 45.0 0.0 0.0</rpy>
+ <body:box name="box_04_body">
+ <massMatrix>true</massMatrix>
+ <mass>1.0</mass>
+ <ixx>10.0</ixx>
+ <ixy>0.0</ixy>
+ <ixz>0.0</ixz>
+ <iyy>10.0</iyy>
+ <iyz>0.0</iyz>
+ <izz>10.0</izz>
+ <cx>0.0</cx>
+ <cy>0.0</cy>
+ <cz>-0.5</cz>
+
+ <geom:box name="box_04_geom">
+ <kp>100000.0</kp>
+ <kd>1.0</kd>
+ <mu1>0.9999</mu1>
+ <mu2>0.9999</mu2>
+ <mesh>default</mesh>
+ <size>1 1 1</size>
+ <visual>
+ <size>1 1 1</size>
+ <material>Gazebo/Rocky</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+ <model:physical name="box_05_model">
+ <xyz> 10.0 8.0 17.42</xyz>
+ <rpy> 45.0 0.0 0.0</rpy>
+ <body:box name="box_05_body">
+ <massMatrix>true</massMatrix>
+ <mass>1.0</mass>
+ <ixx>10.0</ixx>
+ <ixy>0.0</ixy>
+ <ixz>0.0</ixz>
+ <iyy>10.0</iyy>
+ <iyz>0.0</iyz>
+ <izz>10.0</izz>
+ <cx>0.0</cx>
+ <cy>0.0</cy>
+ <cz>-0.5</cz>
+
+ <geom:box name="box_05_geom">
+ <kp>100000.0</kp>
+ <kd>1.0</kd>
+ <mu1>1.00</mu1>
+ <mu2>1.00</mu2>
+ <mesh>default</mesh>
+ <size>1 1 1</size>
+ <visual>
+ <size>1 1 1</size>
+ <material>Gazebo/Rocky</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+ <model:physical name="box_06_model">
+ <xyz> 12.0 8.0 17.42</xyz>
+ <rpy> 45.0 0.0 0.0</rpy>
+ <body:box name="box_06_body">
+ <massMatrix>true</massMatrix>
+ <mass>1.0</mass>
+ <ixx>10.0</ixx>
+ <ixy>0.0</ixy>
+ <ixz>0.0</ixz>
+ <iyy>10.0</iyy>
+ <iyz>0.0</iyz>
+ <izz>10.0</izz>
+ <cx>0.0</cx>
+ <cy>0.0</cy>
+ <cz>-0.5</cz>
+
+ <geom:box name="box_06_geom">
+ <kp>100000.0</kp>
+ <kd>1.0</kd>
+ <mu1>1.01</mu1>
+ <mu2>1.01</mu2>
+ <mesh>default</mesh>
+ <size>1 1 1</size>
+ <visual>
+ <size>1 1 1</size>
+ <material>Gazebo/Rocky</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+
+
+ <!-- White Directional light -->
+ <!--
+ <model:renderable name="directional_white">
+ <light>
+ <type>directional</type>
+ <direction>0 -0.5 -0.5</direction>
+ <diffuseColor>0.4 0.4 0.4</diffuseColor>
+ <specularColor>0.0 0.0 0.0</specularColor>
+ <attenuation>1 0.0 1.0 0.4</attenuation>
+ </light>
+ </model:renderable>
+ -->
+
+</gazebo:world>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <wa...@us...> - 2009-09-04 06:36:48
|
Revision: 23819
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23819&view=rev
Author: wattsk
Date: 2009-09-04 06:36:31 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
Auto power start, use bays, GUI changes to Test Manager
Modified Paths:
--------------
pkg/trunk/pr2/life_test/bin/gui
pkg/trunk/pr2/life_test/manifest.xml
pkg/trunk/pr2/life_test/scripts/ethercat_test_monitor.py
pkg/trunk/pr2/life_test/simple_test/fake_test.py
pkg/trunk/pr2/life_test/simple_test/test.launch
pkg/trunk/pr2/life_test/src/life_test/life_test.py
pkg/trunk/pr2/life_test/src/life_test/test_param.py
pkg/trunk/pr2/life_test/src/life_test/ui.py
pkg/trunk/pr2/life_test/tests.xml
pkg/trunk/pr2/life_test/xrc/gui.fbp
pkg/trunk/pr2/life_test/xrc/gui.xrc
Added Paths:
-----------
pkg/trunk/pr2/life_test/head_test/head_trans.csv
pkg/trunk/pr2/life_test/scripts/test_monitor.py
pkg/trunk/pr2/life_test/simple_test/fake_listeners.yaml
pkg/trunk/pr2/life_test/src/life_test/ethercat_listener.py
pkg/trunk/pr2/life_test/src/life_test/test_bay.py
pkg/trunk/pr2/life_test/src/life_test/trans_listener.py
pkg/trunk/pr2/life_test/wg_test_rooms.xml
Removed Paths:
-------------
pkg/trunk/pr2/life_test/src/life_test/trans_monitor.py
Modified: pkg/trunk/pr2/life_test/bin/gui
===================================================================
--- pkg/trunk/pr2/life_test/bin/gui 2009-09-04 06:18:52 UTC (rev 23818)
+++ pkg/trunk/pr2/life_test/bin/gui 2009-09-04 06:36:31 UTC (rev 23819)
@@ -46,4 +46,4 @@
except Exception, e:
print "Caught exception in TestManagerApp Main Loop"
import traceback
- trackback.print_exc()
+ traceback.print_exc()
Added: pkg/trunk/pr2/life_test/head_test/head_trans.csv
===================================================================
--- pkg/trunk/pr2/life_test/head_test/head_trans.csv (rev 0)
+++ pkg/trunk/pr2/life_test/head_test/head_trans.csv 2009-09-04 06:36:31 UTC (rev 23819)
@@ -0,0 +1 @@
+head_pan_motor,head_pan_joint,0.0,0.1,0,1
\ No newline at end of file
Modified: pkg/trunk/pr2/life_test/manifest.xml
===================================================================
--- pkg/trunk/pr2/life_test/manifest.xml 2009-09-04 06:18:52 UTC (rev 23818)
+++ pkg/trunk/pr2/life_test/manifest.xml 2009-09-04 06:36:31 UTC (rev 23819)
@@ -2,18 +2,11 @@
<description brief="Runs life/impact tests on test carts and robots">
- This package contains the scripts needed to run a life and impact tests on
-robots and test carts.
+ This package contains the scripts needed to run burn in and life tests on PR2 components.
-To launch on test cart, go to {part}_test/ and 'roslaunch test_cart.launch'
-To launch on robot, go to {part}_test and 'roslaunch life_test.launch'
-
-If the part also has an impact test, go to:
-{part}_test/(life or impact)_test/
-and launch the tests as above.
-
-When launching on robot, part must be calibrated with power on.
-
+ Use Test Manager to run tests:
+ roscd life_test
+ bin/gui
</description>
<author>Kevin Watts</author>
<review status="na" notes=""/>
Modified: pkg/trunk/pr2/life_test/scripts/ethercat_test_monitor.py
===================================================================
--- pkg/trunk/pr2/life_test/scripts/ethercat_test_monitor.py 2009-09-04 06:18:52 UTC (rev 23818)
+++ pkg/trunk/pr2/life_test/scripts/ethercat_test_monitor.py 2009-09-04 06:36:31 UTC (rev 23819)
@@ -58,7 +58,7 @@
class EtherCATTestMonitorNode():
def __init__(self):
- rospy.init_node('test_monitor', anonymous = True)
+ rospy.init_node('test_monitor')
self._trans_monitors = []
if len(rospy.myargv()) > 1:
@@ -102,7 +102,6 @@
def create_trans_monitors(self, csv_filename):
-
trans_csv = csv.reader(open(csv_filename, 'rb'))
for row in trans_csv:
actuator = row[0].lstrip().rstrip()
Added: pkg/trunk/pr2/life_test/scripts/test_monitor.py
===================================================================
--- pkg/trunk/pr2/life_test/scripts/test_monitor.py (rev 0)
+++ pkg/trunk/pr2/life_test/scripts/test_monitor.py 2009-09-04 06:36:31 UTC (rev 23819)
@@ -0,0 +1,150 @@
+#!/usr/bin/env python
+#
+# 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 Willow Garage, Inc. 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.
+#
+
+# Author: Kevin Watts
+
+PKG = 'life_test'
+
+import roslib
+roslib.load_manifest(PKG)
+
+import rospy
+
+from life_test.msg import TestStatus
+
+from diagnostic_msgs.msg import DiagnosticArray
+from std_srvs.srv import *
+
+import traceback
+import sys
+
+def create_listener(params, listeners):
+ if not (params.has_key('type') and params.has_key('file')):
+ rospy.logerr('Params "type" and "file" weren\'t found!')
+ return False
+
+ file = params['file']
+ type = params['type']
+
+ try:
+ import_str = '%s.%s' % (PKG, file)
+ __import__(import_str)
+ pypkg = sys.modules[import_str]
+ listener_type = getattr(pypkg, type)
+ except:
+ rospy.logerr('Couldn\'t load listener %s from %s. Exception: %s' % (type, file, traceback.format_exc()))
+ return False
+
+ try:
+ listener = listener_type()
+ except:
+ rospy.logerr('Listener %s failed to construct.\nException: %s' % (type, traceback.format_exc()))
+ return False
+
+ if not listener.create(params):
+ return False
+
+ listeners.append(listener)
+ return True
+
+# Listeners need: reset, halt, init, check_ok
+class TestMonitor:
+ def __init__(self):
+ rospy.init_node('test_monitor')
+
+ self._listeners = []
+
+ my_params = rospy.get_param("~")
+
+ for ns, listener_param in my_params.iteritems():
+ if not create_listener(listener_param, self._listeners):
+ rospy.logfatal('Listener failed to initialize. Exiting')
+ sys.exit()
+
+ self._status_pub = rospy.Publisher('test_status', TestStatus)
+ self._diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray)
+
+ self.reset_srv = rospy.Service('reset_test', Empty, self.reset_test)
+ self.halt_srv = rospy.Service('halt_test', Empty, self.halt_test)
+
+ def reset_test(self, srv):
+ for listener in self._listeners:
+ listener.reset()
+ return EmptyResponse()
+
+ def halt_test(self, srv):
+ for listener in self._listeners:
+ listener.halt()
+ return EmptyResponse()
+
+ def publish_status(self):
+ status = 0
+ messages = []
+
+ array = DiagnosticArray()
+ for listener in self._listeners:
+ stat, msg, diags = listener.check_ok()
+ status = max(status, stat)
+ if msg != '':
+ messages.append(msg)
+ if diags:
+ array.status.extend(diags)
+
+ if len(self._listeners) == 0:
+ status = 3
+ messages = [ 'No listeners' ]
+
+ if len(array.status) > 0:
+ self._diag_pub.publish(array)
+
+ test_stat = TestStatus()
+ test_stat.test_ok = int(status)
+ test_stat.message = ', '.join(messages)
+ if test_stat.test_ok == 0:
+ test_stat.message = 'OK'
+
+ self._status_pub.publish(test_stat)
+
+if __name__ == '__main__':
+ try:
+ tm = TestMonitor()
+
+ rate = rospy.Rate(1.0)
+ while not rospy.is_shutdown():
+ rate.sleep()
+ tm.publish_status()
+ except:
+ traceback.print_exc()
+ rospy.logerr(traceback.format_exc())
Added: pkg/trunk/pr2/life_test/simple_test/fake_listeners.yaml
===================================================================
--- pkg/trunk/pr2/life_test/simple_test/fake_listeners.yaml (rev 0)
+++ pkg/trunk/pr2/life_test/simple_test/fake_listeners.yaml 2009-09-04 06:36:31 UTC (rev 23819)
@@ -0,0 +1,20 @@
+ethercat:
+ type: EthercatListener
+ file: ethercat_listener
+trans:
+ type: TransmissionListener
+ file: trans_listener
+ linear_trans:
+ joint: fake_joint
+ actuator: fake_motor
+ max: 2
+ min: -2
+ up_ref: 0
+ deadband: 0.1
+ cont_trans:
+ joint: cont_joint
+ actuator: cont_motor
+ up_ref: 0
+ down_ref: 3.14
+ wrap: 6.28
+ deadband: 0.1
\ No newline at end of file
Modified: pkg/trunk/pr2/life_test/simple_test/fake_test.py
===================================================================
--- pkg/trunk/pr2/life_test/simple_test/fake_test.py 2009-09-04 06:18:52 UTC (rev 23818)
+++ pkg/trunk/pr2/life_test/simple_test/fake_test.py 2009-09-04 06:36:31 UTC (rev 23819)
@@ -41,14 +41,14 @@
from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
from std_srvs.srv import *
-from mechanism_msgs.msg import MechanismState, JointState, ActuatorState
+from pr2_mechanism_msgs.msg import MechanismState, JointState, ActuatorState
import os
import time
import rospy
-import threading
+from time import sleep
from wx import xrc
import math
@@ -110,6 +110,19 @@
jnt_st.applied_effort = float(-2 * sine)
jnt_st.commanded_effort = float(-2 * sine)
jnt_st.is_calibrated = 1
+
+ cont_st = JointState()
+ cont_st.name = 'cont_joint'
+ cont_st.position = 5 * float(0.5 * sine)
+ cont_st.velocity = 2.5 * float(0.5 * cosine)
+ cont_st.is_calibrated = 1
+
+ cont_act_st = ActuatorState()
+ cont_act_st.name = 'cont_motor'
+ cont_act_st.calibration_reading = 14
+ wrapped_position = (cont_st.position % 6.28)
+ if wrapped_position > 3.14:
+ cont_act_st.calibration_reading = 13
# Use same position as above, with 100:1 reduction -> Ampltitude 200
act_st = ActuatorState()
@@ -122,9 +135,9 @@
act_st.velocity = float(200 * cosine)
# Only one that makes a difference
- act_st.calibration_reading = 14
+ act_st.calibration_reading = 13
if sine > 0.0 and self._cal_box.IsChecked():
- act_st.calibration_reading = 13
+ act_st.calibration_reading = 14
act_st.calibration_rising_edge_valid = 1
act_st.calibration_falling_edge_valid = 1
@@ -143,8 +156,8 @@
mech_st = MechanismState()
mech_st.time = rospy.get_time()
- mech_st.actuator_states = [ act_st ]
- mech_st.joint_states = [ jnt_st ]
+ mech_st.actuator_states = [ act_st, cont_act_st ]
+ mech_st.joint_states = [ jnt_st, cont_st ]
self.mech_pub.publish(mech_st)
@@ -170,6 +183,7 @@
self.range_param_ctrl.SetValue(range_param)
def on_timer(self, event = None):
+ sleep(0.1)
if not rospy.is_shutdown():
self._diag_timer.Start(1000, True)
Modified: pkg/trunk/pr2/life_test/simple_test/test.launch
===================================================================
--- pkg/trunk/pr2/life_test/simple_test/test.launch 2009-09-04 06:18:52 UTC (rev 23818)
+++ pkg/trunk/pr2/life_test/simple_test/test.launch 2009-09-04 06:36:31 UTC (rev 23819)
@@ -1,5 +1,7 @@
<launch>
<node pkg="life_test" type="fake_test.py" />
- <node pkg="life_test" type="ethercat_test_monitor.py" args="$(find life_test)/simple_test/fake_trans.csv" />
-</launch>
\ No newline at end of file
+ <node pkg="life_test" type="test_monitor.py" name="monitor" >
+ <rosparam command="load" file="$(find life_test)/simple_test/fake_listeners.yaml" />
+ </node>
+</launch>
Added: pkg/trunk/pr2/life_test/src/life_test/ethercat_listener.py
===================================================================
--- pkg/trunk/pr2/life_test/src/life_test/ethercat_listener.py (rev 0)
+++ pkg/trunk/pr2/life_test/src/life_test/ethercat_listener.py 2009-09-04 06:36:31 UTC (rev 23819)
@@ -0,0 +1,94 @@
+#!/usr/bin/env python
+#
+# 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 Willow Garage, Inc. 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.
+#
+
+# Author: Kevin Watts
+PKG = 'life_test'
+
+import roslib
+roslib.load_manifest(PKG)
+
+from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
+from std_srvs.srv import *
+
+import rospy
+
+import threading
+
+class EthercatListener:
+ def __init__(self):
+ self._diag_sub = rospy.Subscriber('/diagnostics', DiagnosticArray, self._diag_callback)
+ self._mutex = threading.Lock()
+
+ self._ok = True
+ self._update_time = 0
+ self._reset_motors = rospy.ServiceProxy('reset_motors', Empty)
+ self._halt_motors = rospy.ServiceProxy('halt_motors', Empty)
+
+ # Doesn't do anything
+ def create(self, params):
+ return True
+
+ def halt(self):
+ self._halt_motors()
+
+ def reset(self):
+ self._reset_motors()
+
+ def _diag_callback(self, msg):
+ self._mutex.acquire()
+ for stat in msg.status:
+ if stat.name == 'EtherCAT Master':
+ self._ok = (stat.level == 0)
+ self._update_time = rospy.get_time()
+ break
+ self._mutex.release()
+
+ def check_ok(self):
+ self._mutex.acquire()
+ msg = ''
+ stat = 0
+ if not self._ok:
+ stat = 2
+ msg = 'EtherCAT Error'
+
+ if rospy.get_time() - self._update_time > 3:
+ stat = 3
+ msg = 'EtherCAT Stale'
+ if self._update_time == 0:
+ msg = 'No EtherCAT Data'
+
+ self._mutex.release()
+ return stat, msg, None
+
Modified: pkg/trunk/pr2/life_test/src/life_test/life_test.py
===================================================================
--- pkg/trunk/pr2/life_test/src/life_test/life_test.py 2009-09-04 06:18:52 UTC (rev 23818)
+++ pkg/trunk/pr2/life_test/src/life_test/life_test.py 2009-09-04 06:36:31 UTC (rev 23819)
@@ -2,7 +2,7 @@
#
# Software License Agreement (BSD License)
#
-# Copyright (c) 2008, Willow Garage, Inc.
+# Copyright (c) 2009, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
@@ -66,7 +66,7 @@
from email.mime.base import MIMEBase
from email import Encoders
-
+import roslaunch
from roslaunch_caller import roslaunch_caller
@@ -94,9 +94,10 @@
self._test_desc.SetValue(self._test._desc)
self._launch_button = xrc.XRCCTRL(self._panel, 'launch_test_button')
- self._launch_button.Bind(wx.EVT_BUTTON, self.launch_test)
+ self._launch_button.Bind(wx.EVT_BUTTON, self.start_stop_test)
- self._test_machine_ctrl = xrc.XRCCTRL(self._panel, 'test_machine_ctrl')
+ self._test_bay_ctrl = xrc.XRCCTRL(self._panel, 'test_machine_ctrl')
+ self._test_bay_ctrl.SetItems(self._manager.room.get_bay_names(test.needs_power()))
self._end_cond_type = xrc.XRCCTRL(self._panel, 'end_cond_type')
self._end_cond_type.SetStringSelection('Continuous')
@@ -117,9 +118,6 @@
self._halt_button = xrc.XRCCTRL(self._panel, 'halt_motors_button')
self._halt_button.Bind(wx.EVT_BUTTON, self.on_halt_motors)
- self._stop_button = xrc.XRCCTRL(self._panel, 'stop_test_button')
- self._stop_button.Bind(wx.EVT_BUTTON, self.stop_test)
-
self._user_log = xrc.XRCCTRL(self._panel, 'user_log_input')
self._user_submit = xrc.XRCCTRL(self._panel, 'user_submit_button')
self._user_submit.Bind(wx.EVT_BUTTON, self.on_user_entry)
@@ -135,6 +133,31 @@
self._send_log_button = xrc.XRCCTRL(self._panel, 'send_test_log_button')
self._send_log_button.Bind(wx.EVT_BUTTON, self.on_send_test_log)
+ # Power control
+ self._power_board_text = xrc.XRCCTRL(self._panel, 'power_board_text')
+ self._power_board_text.SetBackgroundColour("White")
+ self._power_board_text.SetValue("Test Not Running")
+
+ self._power_run_button = xrc.XRCCTRL(self._panel, 'power_run_button')
+ self._power_run_button.Bind(wx.EVT_BUTTON, self.on_power_run)
+
+ self._power_standby_button = xrc.XRCCTRL(self._panel, 'power_standby_button')
+ self._power_standby_button.Bind(wx.EVT_BUTTON, self.on_power_standby)
+
+
+ self._power_reset_button = xrc.XRCCTRL(self._panel, 'power_reset_button')
+ self._power_reset_button.Bind(wx.EVT_BUTTON, self.on_power_reset)
+
+
+ self._power_disable_button = xrc.XRCCTRL(self._panel, 'power_disable_button')
+ self._power_disable_button.Bind(wx.EVT_BUTTON, self.on_power_disable)
+
+
+ # Bay data
+ self._power_sn_text = xrc.XRCCTRL(self._panel, 'power_sn_text')
+ self._power_breaker_text = xrc.XRCCTRL(self._panel, 'power_breaker_text')
+ self._machine_text = xrc.XRCCTRL(self._panel, 'machine_text')
+
# Add runtime to the panel...
self._notebook = xrc.XRCCTRL(self._panel, 'test_data_notebook')
wx.CallAfter(self.create_monitor)
@@ -144,7 +167,7 @@
self.SetSizer(self._sizer)
self.Layout()
- self._machine = None
+ self._bay = None
self._current_log = {}
self._diag_msgs = {}
@@ -175,18 +198,17 @@
self.update_controls()
self.on_end_choice()
-
+
+
def create_monitor(self):
self._monitor_panel = MonitorPanel(self._notebook)
self._monitor_panel.SetSize(wx.Size(400, 500))
self._notebook.AddPage(self._monitor_panel, "Runtime Monitor")
-
def __del__(self):
- # Somehow calling log function in destructor
self.stop_test()
- if self._status_sub:
+ if self._status_sub is not None:
self._status_sub.unregister()
def is_launched(self):
@@ -236,11 +258,19 @@
self.notify_operator(3, 'Log Requested.', string.join(names, ','))
def on_close(self, event):
- self.update_test_record('Closing down test.')
- self.update_invent()
- self.record_test_log()
- self.notify_operator(1, 'Closing.')
+ try:
+ #bi = wx.BusyInfo("Closing test and logging, please wait.")
+ #wx.Yield()
+ self.update_test_record('Closing down test.')
+ self.update_invent()
+ self.record_test_log()
+ self.notify_operator(1, 'Closing.')
+ #bi.Destroy()
+ except:
+ rospy.logerr('Exception on close: %s' % traceback.format_exc())
+
self._manager.close_tab(self._serial)
+
def update_test_record(self, note = ''):
@@ -251,7 +281,10 @@
if alert > 0 or note != '':
self._current_log[rospy.get_time()] = message
- self._manager.log_test_entry(self._test._name, self._machine, message)
+ if self._bay is not None:
+ self._manager.log_test_entry(self._test._name, self._bay.name, message)
+ else:
+ self._manager.log_test_entry(self._test._name, 'None', message)
self.display_logs()
if alert > 0:
@@ -331,7 +364,7 @@
interval = rospy.get_time() - self.last_message_time
- if interval > self.timeout_interval: # or interval < 0:
+ if interval > self.timeout_interval:
# Make EtherCAT status stale
self._is_running = False
self._is_stale = True
@@ -344,6 +377,29 @@
self._mutex.release()
+ def on_power_run(self):
+ self._manager.power_run(self._bay)
+
+ def on_power_standby(self):
+ self._manager.power_standby(self._bay)
+
+ def on_power_reset(self):
+ self._manager.power_reset(self._bay)
+
+ def on_power_disable(self):
+ self._manager.power_disable(self._bay)
+
+ def update_board(self, value):
+ if value == "Standby":
+ self._power_board_text.SetBackgroundColour("Orange")
+ self._power_board_text.SetValue("Standby")
+ elif value == "On":
+ self._power_board_text.SetBackgroundColour("Light Green")
+ self._power_board_text.SetValue("Running")
+ else:
+ self._power_board_text.SetBackgroundColour("Red")
+ self._power_board_text.SetValue("Disabled")
+
def update_controls(self, level = 4, msg = 'None'):
# Assumes it has the lock
if not self.is_launched():
@@ -367,7 +423,6 @@
self._reset_button.Enable(self.is_launched())
self._halt_button.Enable(self.is_launched())
- self._stop_button.Enable(self.is_launched())
# FIX
remaining = self.calc_remaining()
@@ -385,10 +440,18 @@
self._active_time_ctrl.SetValue(self._record.get_active_str())
self._elapsed_time_ctrl.SetValue(self._record.get_elapsed_str())
- self._test_machine_ctrl.Enable(not self.is_launched())
- self._launch_button.Enable(not self.is_launched())
+ self._test_bay_ctrl.Enable(not self.is_launched())
+ #self._launch_button.Enable(not self.is_launched())
self._close_button.Enable(not self.is_launched())
+
+ # Power buttons
+ self._power_run_button.Enable(self.is_launched() and self._bay.board is not None)
+ self._power_standby_button.Enable(self.is_launched() and self._bay.board is not None)
+ self._power_reset_button.Enable(self.is_launched() and self._bay.board is not None)
+ self._power_disable_button.Enable(self.is_launched() and self._bay.board is not None)
+
+
def display_logs(self):
kys = dict.keys(self._current_log)
kys.sort()
@@ -403,6 +466,8 @@
def stop_if_done(self):
remain = self.calc_remaining()
+
+ self._stop_count = 0
# Make sure we've had five consecutive seconds of
# negative time before we shutdown
@@ -411,25 +476,6 @@
self._test_complete = True
self.stop_test()
- def stop_test(self, event = None):
- if self.is_launched():
- print 'Shutting down test'
- self.on_halt_motors(None)
- self._test_launcher.shutdown()
- self._manager.test_stop(self._machine)
- self.update_test_record('Stopping test launch')
- self._test_launcher = None
-
- if self._status_sub:
- self._status_sub.unregister()
- self._status_sub = None
-
- self._is_running = False
-
- if event is not None: # In GUI thread
- self.update_controls()
-
-
def status_callback(self, msg):
self._mutex.acquire()
self._status_msg = msg
@@ -453,20 +499,21 @@
def make_launch_script(self, local_diag_topic):
launch = '<launch>\n'
- launch += '<group ns="%s">' % self._machine
+ launch += '<group ns="%s">' % self._bay.name
# Remap
launch += '<remap from="/diagnostics" to="%s" />' % local_diag_topic
# Init machine
- launch += '<machine name="test_host_root" user="root" address="%s" ' % self._machine
+ launch += '<machine name="test_host_root" user="root" address="%s" ' % self._bay.machine
launch += 'ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" default="never"/>'
- launch += '<machine name="test_host" address="%s" ' % self._machine
+ launch += '<machine name="test_host" address="%s" ' % self._bay.machine
launch += 'ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" />'
# Include our launch file
launch += '<include file="$(find life_test)/%s" />' % self._test._launch_script
+ # Will set bag name to be serial
launch += ' <node pkg="rosrecord" type="rosrecord" args="-f /hwlog/test_runtime_automatic /diagnostics" />'
launch += '</group>\n</launch>'
@@ -477,39 +524,112 @@
# Put in master file
# Add subscriber to diagnostics
# Launch file, subscribe diagnostics
- def launch_test(self, event):
+ def start_stop_test(self, event):
+ if self.is_launched():
+ self.stop_test_user()
+ else:
+ self.launch_test()
+
+ self.update_controls()
+
+ def stop_test_user(self):
+ dialog = wx.MessageDialog(self, 'Are you sure you want to stop this test?', 'Confirm Stop Test', wx.OK|wx.CANCEL)
+ if dialog.ShowModal() != wx.ID_OK:
+ return
+
+ self.stop_test()
+
+ def stop_test(self):
+ if self.is_launched():
+ self._launch_button.Enable(False)
+ if self._bay.board is not None:
+ self._manager.power_disable(self._bay)
+ self._power_board_text.SetBackgroundColour("White")
+ self._power_board_text.SetValue("Test Not Running")
+
+ # Machine, board stats
+ self._machine_text.SetValue("Not running")
+ self._power_sn_text.SetValue("Not running")
+ self._power_breaker_text.SetValue("Not running")
+ #bi = wx.BusyInfo('Stopping test, please wait')
+ #wx.Yield()
+ print 'Shutting down test'
+ self.on_halt_motors(None)
+ self._test_launcher.shutdown()
+ self._manager.test_stop(self._bay)
+ self.update_test_record('Stopping test launch')
+ self._test_launcher = None
+ #bi.Destroy()
+ self._launch_button.Enable(True)
+ self._launch_button.SetLabel("Launch")
+
+ if self._status_sub:
+ self._status_sub.unregister()
+ self._status_sub = None
+
+ self._is_running = False
+
+ def launch_test(self):
dialog = wx.MessageDialog(self, 'Are you sure you want to launch?', 'Confirm Launch', wx.OK|wx.CANCEL)
if dialog.ShowModal() != wx.ID_OK:
return
+
+ self._launch_button.Enable(False)
+ #bi = wx.BusyInfo("Launching test, please wait")
+ #wx.Yield()
# Get machine, end condition, etc
- machine = self._test_machine_ctrl.GetStringSelection()
- if not self._manager.test_start_check(machine):
- wx.MessageBox('Machine in use, select again!', 'Machine in use', wx.OK|wx.ICON_ERROR, self)
+ bay_name = self._test_bay_ctrl.GetStringSelection()
+ bay = self._manager.room.get_bay(bay_name)
+ if bay is None:
+ wx.MessageBox('Select test bay', 'Select Bay', wx.OK|wx.ICON_ERROR, self)
+ self._launch_button.Enable(True)
return
+
+ if not self._manager.test_start_check(bay, self._serial):
+ wx.MessageBox('Test bay in use, select again!', 'Test bay in use', wx.OK|wx.ICON_ERROR, self)
+ self._launch_button.Enable(True)
+ return
- self._machine = machine
- local_diag = '/' + self._machine + '/diagnostics'
+ self._bay = bay
- self.update_test_record('Launching test %s on machine %s.' % (self._test._name, self._machine))
+ if self._bay.board is not None:
+ self._manager.power_run(self._bay)
- self._test.set_params(self._machine)
+ # Machine, board stats
+ self._machine_text.SetValue(self._bay.machine)
+ if self._bay.board is not None:
+ self._power_sn_text.SetValue(self._bay.board)
+ self._power_breaker_text.SetValue(self._bay.breaker)
+ self._power_board_text.SetValue("No data")
+ else:
+ self._power_sn_text.SetValue("No board")
+ self._power_breaker_text.SetValue("No breaker")
+ self._power_board_text.SetValue("No board")
+
+ local_diag = '/' + self._bay.name + '/diagnostics'
+
+ self.update_test_record('Launching test %s on bay %s, machine %s.' % (self._test._name, self._bay.name, self._bay.machine))
+
+ self._test.set_params(self._bay.name)
self._test_launcher = roslaunch_caller.ScriptRoslaunch(
self.make_launch_script(local_diag))
try:
self._test_launcher.start()
except roslaunch.RLException, e:
traceback.print_exc()
- self._manager.test_stop(self._machine)
- self._machine = None
+ self._manager.test_stop(self._bay.name)
+ self._bay = None
self.update_test_record('Failed to launch script')
self.update_test_record(traceback.format_exc())
self._test_launcher.shutdown()
self._test_launcher = None
+ self.launch_button.Enable(True)
+ #bi.Destroy()
return None
- local_status = '/' + self._machine + '/test_status'
+ local_status = '/' + self._bay.name + '/test_status'
self._is_running = True
self.update_invent()
self._monitor_panel.change_diagnostic_topic(local_diag)
@@ -517,13 +637,16 @@
self.update_controls()
self._status_sub = rospy.Subscriber(local_status, TestStatus, self.status_callback)
-
+ #bi.Destroy()
+ self._launch_button.Enable(True)
+ self._launch_button.SetLabel("Stop")
+
# Changed from halt_motors to halt_test for test monitor
def on_halt_motors(self, event = None):
try:
self.update_test_record('Halting motors.')
# Call halt motors service on NAME_SPACE/pr2_etherCAT
- halt_srv = rospy.ServiceProxy(self._machine + '/halt_test', Empty)
+ halt_srv = rospy.ServiceProxy(self._bay.name + '/halt_test', Empty)
halt_srv()
except Exception, e:
@@ -532,7 +655,7 @@
def on_reset_motors(self, event = None):
try:
self.update_test_record('Reseting motors')
- reset = rospy.ServiceProxy(self._machine + '/reset_test', Empty)
+ reset = rospy.ServiceProxy(self._bay.name + '/reset_test', Empty)
reset()
except:
@@ -543,17 +666,17 @@
# Notifier class needs test record, that's it
#
def get_test_team(self):
- # Don't email everyone it's debugging on NSF
+ # HACK!!! Don't email everyone it's debugging on NSF
if os.environ['USER'] == 'watts' and gethostname() == 'nsf':
return 'wa...@wi...'
return 'tes...@li...'
def line_summary(self, msg):
- machine_str = self._machine
- if not self._machine:
+ machine_str = self._bay.name
+ if not self._bay:
machine_str = 'NONE'
- return "Test %s on machine %s. MSG: %s" % (self._test.get_title(self._serial), machine_str, msg)
+ return "Test %s on bay %s. MSG: %s" % (self._test.get_title(self._serial), machine_str, msg)
def notify_operator(self, level, alert_msg, recipient = None):
# Don't notify if we haven't done anything
@@ -652,7 +775,7 @@
else:
html += '<H3>Test Status: Shutdown</H3>\n'
- # Table of test machine, etc
+ # Table of test bay, etc
html += '<hr size="3">\n'
html += '<H4>Test Info</H4>\n'
html += '<p>Description: %s</p>\n<br>' % self._test._desc
@@ -681,8 +804,12 @@
def make_test_info_table(self):
html = '<table border="1" cellpadding="2" cellspacing="0">\n'
html += self.make_table_row(['Test Name', self._test._name])
- if self._machine:
- html += self.make_table_row(['Machine', self._machine])
+ if self._bay:
+ html += self.make_table_row(['Test Bay', self._bay])
+ html += self.make_table_row(['Machine', self._bay.machine])
+ html += self.make_table_row(['Powerboard', self._bay.board])
+ html += self.make_table_row(['Breaker', self._bay.breaker])
+
html += self.make_table_row(['Serial', self._serial])
html += self.make_table_row(['Test Type', self._test._test_type])
Added: pkg/trunk/pr2/life_test/src/life_test/test_bay.py
===================================================================
--- pkg/trunk/pr2/life_test/src/life_test/test_bay.py (rev 0)
+++ pkg/trunk/pr2/life_test/src/life_test/test_bay.py 2009-09-04 06:36:31 UTC (rev 23819)
@@ -0,0 +1,89 @@
+#!/usr/bin/env python
+#
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2009, 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.
+
+# Author: Kevin Watts
+
+import roslib
+roslib.load_manifest('life_test')
+
+
+class FailedLoadError(Exception): pass
+class BayNameExistsError(Exception): pass
+
+##\brief Collection of test bays
+class TestRoom:
+ def __init__(self, hostname):
+ self.hostname = hostname
+ self._bays = {}
+
+ def add_bay(self, bay):
+ if self._bays.has_key(bay.name):
+ raise BayNameExistsError
+ self._bays[bay.name] = bay
+
+ def get_bay_names(self, need_power):
+ if not need_power:
+ return self._bays.keys()
+
+ names = []
+ for name in self._bays.keys():
+ if self._bays[name].power:
+ names.append(name)
+ return names
+
+ def get_bay(self, name):
+ if not self._bays.has_key(name):
+ return None
+ return self._bays[name]
+
+##\brief Computer, powerboard and breaker to run test
+class TestBay:
+ def __init__(self, xml_doc):
+ self.name = xml_doc.attributes['name'].value
+ self.machine = xml_doc.attributes['machine'].value
+ if xml_doc.attributes.has_key('board'):
+ self.board = xml_doc.attributes['board'].value
+ self.breaker = int(xml_doc.attributes['breaker'].value)
+ if self.breaker not in [0, 1, 2]:
+ raise FailedLoadError
+ else:
+ self.board = None
+ self.breaker = None
+
+
+
+
+
+
+
Modified: pkg/trunk/pr2/life_test/src/life_test/test_param.py
===================================================================
--- pkg/trunk/pr2/life_test/src/life_test/test_param.py 2009-09-04 06:18:52 UTC (rev 23818)
+++ pkg/trunk/pr2/life_test/src/life_test/test_param.py 2009-09-04 06:36:31 UTC (rev 23819)
@@ -41,7 +41,8 @@
# Should include parameters as a list here
class LifeTest:
- def __init__(self, short_serial, test_name, short_name, trac, desc, test_type, launch_file, params):
+ def __init__(self, short_serial, test_name, short_name,
+ trac, desc, test_type, launch_file, need_power, params):
self._short_serial = short_serial
self._name = test_name
self._short = short_name
@@ -52,6 +53,8 @@
self._params = params
+ self.need_power = need_power
+
def set_params(self, namespace):
for param in self._params:
param.set_namespace(namespace)
@@ -63,6 +66,9 @@
len(serial)])
# Or just return the short name and the trac ticket
return "%s #%s" % (self._short, self._trac)
+
+ def needs_power(self):
+ return self.need_power
## Stores parameter data for each test
Copied: pkg/trunk/pr2/life_test/src/life_test/trans_listener.py (from rev 23104, pkg/trunk/pr2/life_test/src/life_test/trans_monitor.py)
===================================================================
--- pkg/trunk/pr2/life_test/src/life_test/trans_listener.py (rev 0)
+++ pkg/trunk/pr2/life_test/src/life_test/trans_listener.py 2009-09-04 06:36:31 UTC (rev 23819)
@@ -0,0 +1,330 @@
+# 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 Willow Garage, Inc. 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.
+#
+
+# Author: Kevin Watts
+# Shamelessly copied from joint_calibration_monitor/generic_joint_monitor by Vijay Pradeep
+
+PKG = 'life_test'
+
+import roslib
+roslib.load_manifest(PKG)
+
+from pr2_mechanism_msgs.msg import MechanismState
+from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
+from std_srvs.srv import *
+
+import math
+
+import rospy
+
+import threading
+
+class JointTransmissionListener():
+ def __init__(self):
+ self._ok = True
+ self._num_errors = 0
+ self._num_errors_since_reset = 0
+ self._rx_count = 0
+ self._max_position = -10000000
+ self._min_position = 100000000
+
+ ## Mandatory params: actuator, joint, deadband
+ def create(self, params):
+ if not params.has_key('deadband'):
+ rospy.logerr('Parameter "deadband" not found! Aborting.')
+ return False
+ self._deadband = params['deadband']
+
+ if not params.has_key('actuator'):
+ rospy.logerr('Parameter "actuator" not found! Aborting.')
+ return False
+ self._actuator = params['actuator']
+
+ if not params.has_key('joint'):
+ rospy.logerr('Parameter "joint" not found! Aborting.')
+ return False
+ self._joint = params['joint']
+
+ ## Calibration flag references
+ if not params.has_key('up_ref'):
+ self._up_ref = None
+ else:
+ self._up_ref = params['up_ref']
+
+ if not params.has_key('down_ref'):
+ self._down_ref = None
+ else:
+ self._down_ref = params['down_ref']
+
+ if self._up_ref is None and self._down_ref is None:
+ rospy.logerr('Neither up or down reference was given! Aborting.')
+ return False
+
+ # For continuous joints
+ if not params.has_key('wrap'):
+ self._wrap = None
+ else:
+ self._wrap = params['wrap']
+
+ if not params.has_key('max'):
+ self._max = None
+ else:
+ self._max = params['max']
+
+ if not params.has_key('min'):
+ self._min = None
+ else:
+ self._min = params['min']
+
+ return True
+
+ def reset(self):
+ self._ok = True
+ self._num_errors_since_reset = 0
+
+ def check_device(self, position, cal_reading, calibrated):
+ if self._up_ref is not None:
+ if abs(position - self._up_ref) < self._deadband:
+ return True # Don't know b/c in deadband
+ if self._down_ref is not None:
+ if abs(position - self._down_ref) < self._deadband:
+ return True # Don't know b/c in deadband
+
+ if not calibrated:
+ return True # Don't bother with uncalibrated joints
+
+ if self._wrap is not None and self._wrap > 0:
+ position = position % (self._wrap)
+
+ # Check limits
+ if self._max is not None and position > self._max:
+ return False
+ if self._min is not None and position < self._min:
+ return False
+
+ # Reverse for joints that have negative search velocities
+ cal_bool = cal_reading % 2 == 0
+
+ ## Has both up and down limit
+ if (self._up_ref is not None) and (self._down_ref is not None):
+ if self._up_ref > self._down_ref:
+ if position < self._down_ref and cal_bool:
+ return True
+ if (position > self._down_ref) and (position < self._up_ref) and not cal_bool:
+ return True
+ if position > self._up_ref and cal_bool:
+ return True
+ else:
+ return False
+ else: # Down > Up
+ if position < self._up_ref and cal_bool:
+ return True
+ if (position > self._up_ref) and (position < self._down_ref) and cal_bool:
+ return True
+ if (position > self._down_ref) and not cal_bool:
+ return True
+ else:
+ return False
+
+ ## Has only up limit
+ if self._up_ref is not None:
+ if position > self._up_ref and cal_bool:
+ return True
+ if position < self._up_ref and not cal_bool:
+ return True
+ else:
+ return False
+
+ ## Has only down limit
+ if self._down_ref is not None:
+ if position > self._down_ref and not cal_bool:
+ return True
+ if position < self._down_ref and cal_bool:
+ return True
+ else:
+ return False
+
+ return False
+
+
+ def update(self, mech_state):
+ self._rx_count += 1
+ diag = DiagnosticStatus()
+ diag.level = 0 # Default the level to 'OK'
+ diag.name = "Trans. Monitor: %s" % self._joint
+ diag.message = "OK"
+ diag.values = [ ]
+
+ diag.values.append(KeyValue("Joint", self._joint))
+ diag.values.append(KeyValue("Actuator", self._actuator))
+ diag.values.append(KeyValue("Up position", str(self._up_ref)))
+ diag.values.append(KeyValue("Down position", str(self._down_ref)))
+ diag.values.append(KeyValue("Wrap", str(self._wrap)))
+ diag.values.append(KeyValue("Max Limit", str(self._max)))
+ diag.values.append(KeyValue("Min Limit", str(self._min)))
+
+ diag.values.append(KeyValue("Deadband", str(self._deadband)))
+ diag.values.append(KeyValue("Mech State RX Count", str(self._rx_count)))
+
+
+ # Check if we can find both the joint and actuator
+ act_names = [x.name for x in mech_state.actuator_states]
+ act_exists = self._actuator in act_names ;
+ act_exists_msg = 'Error'
+
+ if act_exists :
+ cal_reading = mech_state.actuator_states[act_names.index(self._actuator)].calibration_reading
+
+ joint_names = [x.name for x in mech_state.joint_states]
+ joint_exists = self._joint in joint_names
+ jnt_exists_msg = 'Error'
+ if joint_exists :
+ position = mech_state.joint_states[joint_names.index(self._joint)].position
+ calibrated = (mech_state.joint_states[joint_names.index(self._joint)].is_calibrated == 1)
+
+
+ diag.values.append(KeyValue('Actuator Exists', str(act_exists)))
+ diag.values.append(KeyValue('Joint Exists', str(joint_exists)))
+
+ # First check existance of joint, actuator
+ if not (act_exists and joint_exists):
+ diag.level = 2
+ diag.message = 'Actuators, Joints missing'
+ self._ok = False
+ return diag, False
+
+ # Monitor current state
+ reading_msg = 'OK'
+ if not self.check_device(position, cal_reading, calibrated):
+ self._ok = False
+ self._num_errors += 1
+ self._num_errors_since_reset += 1
+ reading_msg = 'Broken'
+
+ # If we've had an error since the last reset, we're no good
+ if not self._ok:
+ diag.message = 'Broken'
+ diag.level = 2
+
+ diag.values.insert(0, KeyValue('Transmission Status', diag.message))
+ diag.values.insert(1, KeyValue('Current Reading', reading_msg))
+ diag.values.append(KeyValue('Is Calibrated', str(calibrated)))
+ diag.values.append(KeyValue('Calibration Reading',str(cal_reading)))
+ diag.values.append(KeyValue('Joint Position', str(position)))
+
+ diag.values.append(KeyValue('Total Errors', str(self._num_errors)))
+ diag.values.append(KeyValue('Errors Since Reset', str(self._num_errors_since_reset)))
+
+ self._max_position = max(self._max_position, position)
+ diag.values.append(KeyValue('Max Obs. Position', str(self._max_position)))
+
+ self._min_position = min(self._min_position, position)
+ diag.values.append(KeyValue('Min Obs. Position', str(self._min_position)))
+
+ return self._ok, diag
+
+# Loads individual joint listeners
+class TransmissionListener:
+ def __init__(self):
+ self._joint_monitors = []
+ self._mech_sub = rospy.Subscriber('mechanism_state', MechanismState, self._callback)
+ self._halt_motors = rospy.ServiceProxy('halt_motors', Empty)
+
+ self._mutex = threading.Lock()
+ self._diag_stats = []
+ self._ok = True
+ self._last_msg_time = 0
+
+ def create(self, params):
+ for joint, joint_param in params.iteritems():
+ # Ignore setup params
+ if joint == 'type' or joint == 'file':
+ continue
+
+ joint_mon = JointTransmissionListener()
+ if not joint_mon.create(joint_param):
+ return False
+ self._joint_monitors.append(joint_mon)
+ return True
+
+ def _callback(self, msg):
+ self._mutex.acquire()
+ self._last_msg_time = rospy.get_time()
+ self._diag_stats = []
+
+ was_ok = self._ok
+
+ for joint_mon in self._joint_monitors:
+ ok, stat = joint_mon.update(msg)
+ self._diag_stats.append(stat)
+ self._ok = ok and self._ok
+
+ self._mutex.release()
+
+ # Halt if broken
+ if not self._ok and was_ok:
+ rospy.logerr('Halting motors, broken transmission')
+ #self._halt_motors()
+
+
+ def reset(self):
+ self._mutex.acquire()
+ self._ok = True
+ for joint_mon in self._joint_monitors:
+ joint_mon.reset()
+ self._mutex.release()
+
+ def halt(self):
+ pass
+
+ def check_ok(self):
+ self._mutex.acquire()
+
+ if self._last_msg_time == 0:
+ self._mutex.release()
+ return 3, "No mech stat messages", None
+ if rospy.get_time() - self._last_msg_time > 3:
+ self._mutex.release()
+ return 3, "Mech state stale", None
+
+ if self._ok:
+ status = 0
+ msg = ''
+ else:
+ status = 2
+ msg = 'Transmission Broken'
+
+ diag_stats = self._diag_stats
+ self._mutex.release()
+
+ return status, msg, diag_stats
Property changes on: pkg/trunk/pr2/life_test/src/life_test/trans_listener.py
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/pr2/life_test/src/life_test/trans_monitor.py:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Deleted: pkg/trunk/pr2/life_test/src/life_test/trans_monitor.py
===================================================================
--- pkg/trunk/pr2/life_test/src/life_test/trans_monitor.py 2009-09-04 06:18:52 UTC (rev 23818)
+++ pkg/trunk/pr2/life_test/src/life_test/trans_monitor.py 2009-09-04 06:36:31 UTC (rev 23819)
@@ -1,165 +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 Willow Garage, Inc. 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.
-#
-
-# Author: Kevin Watts
-# Shamelessly copied from joint_calibration_monitor/generic_joint_monitor by Vijay Pradeep
-
-import roslib
-roslib.load_manifest('life_test')
-
-from mechanism_msgs.msg import *
-from diagnostic_msgs.msg import *
-
-import math
-
-import rospy
-
-
-class TransmissionMonitor():
- def __init__(self, actuator_name, joint_name, ref_position, deadband, continuous, positive):
- # Should probably strip these or something for no spaces
- self._actuator = actuator_name
- self._joint = joint_name
- self._ref_position = ref_position
- self._positive = positive
- self._continuous = continuous
- self._deadband = deadband
-
- self._ok = True
-
- self._num_errors = 0
- self._num_errors_since_reset = 0
- self._rx_count = 0
- self._max_position = -1000000
- self._min_position = 10000000
-
- def reset(self):
- self._ok = True
- self._num_errors_since_reset = 0
-
- def check_device(self, position, cal_reading, calibrated):
- if abs(position - self._ref_position) < self._deadband:
- return True # Don't know b/c in deadband
- if not calibrated:
- return True # Don't bother with uncalibrated joints
-
- if self._continuous: # Reset position to home value
- position = position % (2*math.pi) - math.pi # Between -pi, pi
-
- # Reverse for joints that have negative search velocities
- cal_bool = cal_reading % 2 == 0
- if not self._positive:
- cal_bool = not cal_bool
-
- if (position > self._ref_position and not cal_bool):
- return True
- if (position < self._ref_position and cal_bool):
- return True
- else:
- return False
-
- def update(self, mech_state):
- self._rx_count += 1
- diag = DiagnosticStatus()
- diag.level = 0 # Default the level to 'OK'
- diag.name = "Trans. Monitor: %s" % self._joint
- diag.message = "OK"
- diag.values = [ ]
-
- diag.values.append(KeyValue("Joint", self._joint))
- diag.values.append(KeyValue("Actuator", self._actuator))
- diag.values.append(KeyValue("Positive Joint", str(self._positive)))
- diag.values.append(KeyValue("Continuous Joint", str(self._continuous)))
- diag.values.append(KeyValue("Reference Position", str(self._ref_position)))
- diag.values.append(KeyValue("Deadband", str(self._deadband)))
- diag.values.append(KeyValue("Mech State RX Count", str(self._rx_count)))
-
-
- # Check if we can find both the joint and actuator
- act_names = [x.name for x in mech_state.actuator_states]
- act_exists = self._actuator in act_names ;
- act_exists_msg = 'Error'
-
- if act_exists :
- cal_reading = mech_state.actuator_states[act_names.index(self._actuator)].calibration_reading
-
- joint_names = [x.name for x in mech_state.joint_states]
- joint_exists = self._joint in joint_names
- jnt_exists_msg = 'Error'
- if joint_exists :
- position = mech_state.joint_states[joint_names.index(self._joint)].position
- calibrated = (mech_state.joint_states[joint_names.index(self._joint)].is_calibrated == 1)
-
-
- diag.values.append(KeyValue('Actuator Exists', str(act_exists)))
- diag.values.append(KeyValue('Joint Exists', str(joint_exists)))
-
-
-
- # First check existance of joint, actuator
- if not (act_exists and joint_exists):
- diag.level = 2
- diag.message = 'Actuators, Joints missing'
- self._ok = False
- return diag, False
-
- # Monitor current state
- reading_msg = 'OK'
- if not self.check_device(position, cal_reading, calibrated):
- self._ok = False
- self._num_errors += 1
- self._num_errors_since_reset += 1
- reading_msg = 'Broken'
-
- # If we've had an error since the last reset, we're no good
- if not self._ok:
- diag.message = 'Broken'
- diag.level = 2
-
- diag.values.insert(0, KeyValue('Transmission Status', diag.message))
- diag.values.insert(1, KeyValue('Current Reading', reading_msg))
- diag.values.append(KeyValue('Is Calibrated', str(calibrated)))
- diag.values.append(KeyValue('Calibration Reading',str(cal_reading)))
- diag.values.append(KeyValue('Joint Position', str(position)))
-
- diag.values.append(KeyValue('Total Errors', str(self._num_errors)))
- diag.values.append(KeyValue('Errors Since Reset', str(self._num_errors_since_reset)))
-
- self._max_position = max(self._max_position, position)
- diag.values.append(KeyValue('Max Obs. Position', str(self._max_position)))
-
- self._min_position = min(self._min_position, position)
- diag.values.append(KeyValue('Min Obs. Position', str(self._min_position)))
-
- return diag, self._ok
-
Modified: pkg/trunk/pr2/life_test/src/life_test/ui.py
===================================================================
--- pkg/trunk/pr2/life_test/src/life_test/ui.py 2009-09-04 06:18:52 UTC (rev 23818)
+++ pkg/trunk/pr2/life_test/src/life_test/ui.py 2009-09-04 06:36:31 UTC (rev 23819)
@@ -2,7 +2,7 @@
#
# Software License Agreement (BSD License)
#
-# Copyright (c) 2008, Willow Garage, Inc.
+# Copyright (c) 2009, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
@@ -32,16 +32,16 @@
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
-### Author: Kevin Watts
-
+## Author: Kevin Watts
+PKG = 'life_test'
import roslib
-import ...
[truncated message content] |
|
From: <wgh...@us...> - 2009-09-04 06:19:00
|
Revision: 23818
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23818&view=rev
Author: wghassan
Date: 2009-09-04 06:18:52 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
fixed talker app
Modified Paths:
--------------
pkg/trunk/sandbox/web/launchman/src/launchman.py
pkg/trunk/sandbox/web/sample_application/src/sample_application/listener.py
pkg/trunk/sandbox/web/sample_application/src/sample_application/talker.py
pkg/trunk/sandbox/web/sample_application/src/sample_application/templates/index.cs
Modified: pkg/trunk/sandbox/web/launchman/src/launchman.py
===================================================================
--- pkg/trunk/sandbox/web/launchman/src/launchman.py 2009-09-04 05:28:37 UTC (rev 23817)
+++ pkg/trunk/sandbox/web/launchman/src/launchman.py 2009-09-04 06:18:52 UTC (rev 23818)
@@ -110,7 +110,14 @@
print "ALL DONE!"
self.manager._stopTask(self)
else:
- print "too many procs", self.runner.pm.procs
+ if self.runner.pm.procs:
+ self.task.status = "error"
+ self.manager._send_status()
+
+ print "too many processes left:", len(self.runner.pm.procs)
+ for proc in self.runner.pm.procs:
+ proc.stop()
+
self.runner = None
self.manager._stopTask(self)
@@ -196,7 +203,6 @@
runner.stop()
runner.task.status = "stopped"
-# self.app_update.publish(runner.task)
self._send_status()
if runner.app.depends:
Modified: pkg/trunk/sandbox/web/sample_application/src/sample_application/listener.py
===================================================================
--- pkg/trunk/sandbox/web/sample_application/src/sample_application/listener.py 2009-09-04 05:28:37 UTC (rev 23817)
+++ pkg/trunk/sandbox/web/sample_application/src/sample_application/listener.py 2009-09-04 06:18:52 UTC (rev 23818)
@@ -36,7 +36,7 @@
## Simple talker demo that listens to std_msgs/Strings published
## to the 'chatter' topic
-PKG = 'rospy_tutorials' # this package name
+PKG = 'sample_application' # this package name
import roslib; roslib.load_manifest(PKG)
import rospy
@@ -52,7 +52,7 @@
# anonymous=True flag means that rospy will choose a unique
# name for our 'talker' node so that multiple talkers can
# run simultaenously.
- rospy.init_node('listener', anonymous=True)
+ rospy.init_node('app_listener')
rospy.Subscriber("chatter", String, callback)
Modified: pkg/trunk/sandbox/web/sample_application/src/sample_application/talker.py
===================================================================
--- pkg/trunk/sandbox/web/sample_application/src/sample_application/talker.py 2009-09-04 05:28:37 UTC (rev 23817)
+++ pkg/trunk/sandbox/web/sample_application/src/sample_application/talker.py 2009-09-04 06:18:52 UTC (rev 23818)
@@ -36,20 +36,23 @@
## Simple talker demo that published std_msgs/Strings messages
## to the 'chatter' topic
-import roslib; roslib.load_manifest('rospy_tutorials')
+import roslib; roslib.load_manifest('sample_application')
import rospy
from std_msgs.msg import String
+import os, time
def talker():
pub = rospy.Publisher('chatter', String)
- rospy.init_node('talker', anonymous=True)
- r = rospy.Rate(1) # 10hz
+# rospy.init_node('talker', anonymous=True)
+ rospy.init_node('app_talker')
+ t = 0
while not rospy.is_shutdown():
- str = "hello world %s"%rospy.get_time()
+ str = "hello world %s %s" % (os.getpid(), t)
rospy.loginfo(str)
pub.publish(str)
- r.sleep()
+ time.sleep(1)
+ t = t + 1
if __name__ == '__main__':
try:
Modified: pkg/trunk/sandbox/web/sample_application/src/sample_application/templates/index.cs
===================================================================
--- pkg/trunk/sandbox/web/sample_application/src/sample_application/templates/index.cs 2009-09-04 05:28:37 UTC (rev 23817)
+++ pkg/trunk/sandbox/web/sample_application/src/sample_application/templates/index.cs 2009-09-04 06:18:52 UTC (rev 23818)
@@ -9,8 +9,7 @@
<table align=right>
<td>
-<input class=app_button type=button value="" objtype="LaunchButtonWidget2" topic="/app_update" taskid="<?cs var:CGI.cur.app.taskid ?>">
-<div class=app_status objtype=TextWidget topic="/app_update" key=status selector="taskid" selectorValue="<?cs var:CGI.cur.app.taskid?>"> </div>
+<div class=app_button objtype="LaunchButtonWidget2" taskid="<?cs var:CGI.cur.app.taskid ?>">
</td>
</table>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <wgh...@us...> - 2009-09-04 05:28:43
|
Revision: 23817
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23817&view=rev
Author: wghassan
Date: 2009-09-04 05:28:37 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
removed an extra copy of webhandler
Removed Paths:
-------------
pkg/trunk/sandbox/web/sample_application/webhandler.py
Deleted: pkg/trunk/sandbox/web/sample_application/webhandler.py
===================================================================
--- pkg/trunk/sandbox/web/sample_application/webhandler.py 2009-09-04 05:24:42 UTC (rev 23816)
+++ pkg/trunk/sandbox/web/sample_application/webhandler.py 2009-09-04 05:28:37 UTC (rev 23817)
@@ -1,15 +0,0 @@
-#! /usr/bin/env python
-
-import rosweb
-from std_msgs.msg import String
-
-def config_plugin(context):
- context.register_subtopic("/chatter:more", UpperChatterSubtopic)
-
-class UpperChatterSubtopic(rosweb.ROSWebSubTopic):
- def __init__(self, topic, factory, main_rwt):
- rosweb.ROSWebSubTopic.__init__(self, topic, factory, main_rwt)
-
- def transform(self, msg):
- newmsg = String(msg.data.upper())
- return newmsg
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <wgh...@us...> - 2009-09-04 05:24:55
|
Revision: 23816
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23816&view=rev
Author: wghassan
Date: 2009-09-04 05:24:42 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
removed the app_update message to improve status updates
Modified Paths:
--------------
pkg/trunk/sandbox/web/launchman/src/launchman.py
pkg/trunk/sandbox/web/webui/src/webui/cgistarter.py
pkg/trunk/sandbox/web/webui/src/webui/mod/webui/cgibin/apps.py
pkg/trunk/sandbox/web/webui/src/webui/mod/webui/jslib/ros.js
pkg/trunk/sandbox/web/webui/src/webui/mod/webui/jslib/ros_toolbar.js
pkg/trunk/sandbox/web/webui/src/webui/mod/webui/templates/apps.cs
Added Paths:
-----------
pkg/trunk/sandbox/web/navigation_application/src/navigation_application/__init__.py
Modified: pkg/trunk/sandbox/web/launchman/src/launchman.py
===================================================================
--- pkg/trunk/sandbox/web/launchman/src/launchman.py 2009-09-04 04:33:45 UTC (rev 23815)
+++ pkg/trunk/sandbox/web/launchman/src/launchman.py 2009-09-04 05:24:42 UTC (rev 23816)
@@ -84,7 +84,9 @@
loader.load(fn, config)
except:
self.task.status = "error"
- self.manager.app_update.publish(self.task)
+ #self.manager.app_update.publish(self.task)
+ self.manager._send_status()
+
return
self.runner = ROSLaunchRunner(rospy.get_param("/run_id"), config, is_core=False)
@@ -167,12 +169,13 @@
runner.task.started = rospy.get_rostime()
runner.task.status = "starting"
- self.app_update.publish(runner.task)
+# self.app_update.publish(runner.task)
+ self._send_status()
runner.launch()
runner.task.status = "running"
- self.app_update.publish(runner.task)
+# self.app_update.publish(runner.task)
self._send_status()
return StartTaskResponse("done")
@@ -184,7 +187,8 @@
def _stopTask(self, runner):
runner.task.status = "stopping"
- self.app_update.publish(runner.task)
+# self.app_update.publish(runner.task)
+ self._send_status()
for cgroup in runner.childGroups[:]:
self._stopTask(cgroup)
@@ -192,7 +196,8 @@
runner.stop()
runner.task.status = "stopped"
- self.app_update.publish(runner.task)
+# self.app_update.publish(runner.task)
+ self._send_status()
if runner.app.depends:
pgroup = self._taskGroups.get(runner.app.depends, None)
@@ -221,8 +226,9 @@
return StopTaskResponse("done")
def status_update(self, req):
- for provides, runner in self._taskGroups.items():
- self.app_update.publish(runner.task)
+ self._send_status()
+# for provides, runner in self._taskGroups.items():
+# self.app_update.publish(runner.task)
return StatusUpdateResponse("done")
Modified: pkg/trunk/sandbox/web/webui/src/webui/cgistarter.py
===================================================================
--- pkg/trunk/sandbox/web/webui/src/webui/cgistarter.py 2009-09-04 04:33:45 UTC (rev 23815)
+++ pkg/trunk/sandbox/web/webui/src/webui/cgistarter.py 2009-09-04 05:24:42 UTC (rev 23816)
@@ -100,7 +100,7 @@
else:
n = 0
- debug("self.path", self.path)
+ #warn("self.path", self.path)
if len(self.path) > 1:
module = self.path[n]
@@ -113,13 +113,13 @@
app = module
n = n + 1
- modpath = getPackagePath(module)
+ modpath = os.path.join(getPackagePath(module), "src")
#roslib.load_manifest(module)
- fn = apply(os.path.join, [modpath] + self.path[n:])
+ fn = apply(os.path.join, [modpath] + [module] + self.path[n:])
moduleRootPath = modpath
- handlerRoot = apply(os.path.join, [modpath, "cgibin"])
- moduleTemplatePath = apply(os.path.join, [modpath, "templates"])
+ handlerRoot = apply(os.path.join, [modpath, module, "cgibin"])
+ moduleTemplatePath = apply(os.path.join, [modpath, module, "templates"])
else:
moduleRootPath = apply(os.path.join, ["mod", module])
handlerRoot = apply(os.path.join, ["mod", module, "cgibin"])
@@ -130,7 +130,7 @@
systemTemplatePath = apply(os.path.join, [cwd, "mod", "webui", "templates"])
systemJLIBPath = apply(os.path.join, [cwd, "mod", "webui", "jslib"])
- debug("fn", fn)
+ #warn("fn", fn)
## if requesting a file, then just output it to the browser.
if os.path.isfile(fn):
@@ -141,7 +141,7 @@
if modpath: sys.path.insert(0, os.path.abspath(modpath))
sys.path.insert(0, os.path.abspath(moduleRootPath))
- debug("sys.path", sys.path)
+ #debug("sys.path", sys.path)
handlerPath = ''
@@ -169,8 +169,9 @@
else:
modulePath, moduleFilename = os.path.split(handlerPath)
- debug(handlerPath, pathinfo)
- #warn("PATH", handlerPath, pathinfo, modulePath, moduleFilename)
+ #debug(handlerPath, pathinfo)
+ #warn("handlerRoot", handlerRoot)
+ #warn("handlerPath", handlerPath, "pathinfo", pathinfo, "modulePath", modulePath, "moduleFilename", moduleFilename)
#warn("PATH", self.path)
if not os.path.isfile(handlerPath):
@@ -183,7 +184,7 @@
#module = __import__(moduleName)
if app:
- module = __import__("cgibin.%s" % (moduleName, ), {}, {}, (None,))
+ module = __import__("%s.cgibin.%s" % (module, moduleName, ), {}, {}, (None,))
else:
module = __import__("mod.%s.cgibin.%s" % (module, moduleName), {}, {}, (None,))
Modified: pkg/trunk/sandbox/web/webui/src/webui/mod/webui/cgibin/apps.py
===================================================================
--- pkg/trunk/sandbox/web/webui/src/webui/mod/webui/cgibin/apps.py 2009-09-04 04:33:45 UTC (rev 23815)
+++ pkg/trunk/sandbox/web/webui/src/webui/mod/webui/cgibin/apps.py 2009-09-04 05:24:42 UTC (rev 23816)
@@ -20,6 +20,8 @@
self.db = db_webui.initSchema()
def display(self, hdf):
+ db_webui.grabTopics(hdf, [])
+
hdf.setValue("CGI.now", str(time.time()))
apps = self.db.apps.fetchAllRows()
Modified: pkg/trunk/sandbox/web/webui/src/webui/mod/webui/jslib/ros.js
===================================================================
--- pkg/trunk/sandbox/web/webui/src/webui/mod/webui/jslib/ros.js 2009-09-04 04:33:45 UTC (rev 23815)
+++ pkg/trunk/sandbox/web/webui/src/webui/mod/webui/jslib/ros.js 2009-09-04 05:24:42 UTC (rev 23816)
@@ -223,6 +223,7 @@
gPump.evalMessages(window.gMessage);
}
gPump.pump();
+ gPump.service_call("status_update", []);
}
// *******************************************
Modified: pkg/trunk/sandbox/web/webui/src/webui/mod/webui/jslib/ros_toolbar.js
===================================================================
--- pkg/trunk/sandbox/web/webui/src/webui/mod/webui/jslib/ros_toolbar.js 2009-09-04 04:33:45 UTC (rev 23815)
+++ pkg/trunk/sandbox/web/webui/src/webui/mod/webui/jslib/ros_toolbar.js 2009-09-04 05:24:42 UTC (rev 23816)
@@ -7,23 +7,40 @@
this.domobj = domobj;
this.taskid = domobj.getAttribute("taskid");
this.state = false;
- this.topics = [domobj.getAttribute("topic")];
+ this.topics = ["/app_update", "/app_status"];
},
init: function() {
var obj = this;
this.domobj.onmousedown = function() {obj.onmousedown();};
- if(this.state == true) {
- this.domobj.setAttribute("class", "buttonOn");
- } else {
- this.domobj.setAttribute("class", "buttonOff");
+ this.set_state();
+ },
+
+ set_state: function() {
+ var button = this.domobj;
+ if(button != null) {
+ this.domobj.style.border = "solid";
+ if(this.state == true) {
+ button.setAttribute("class", "buttonOn");
+ } else {
+ button.setAttribute("class", "buttonOff");
+ }
}
+ },
- //this.pump.service_call("status_update", []);
+ receive: function(topic, msg) {
+ if(topic == "/app_update") this.receive_app_update(msg);
+ else if(topic == "/app_status") this.receive_app_status(msg);
},
- receive: function(topic, msg) {
+ receive_app_status: function(msg) {
+ for(var i=0; i<msg.active.length; i++) {
+ this.receive_app_update(msg.active[i]);
+ }
+ },
+
+ receive_app_update: function(msg) {
if(msg.taskid != this.taskid) return;
var prev_state = this.state;
@@ -33,15 +50,7 @@
if(state == "stopped") this.state = false;
if(prev_state != this.state) {
- var button = this.domobj;
- if(button != null) {
- this.domobj.style.border = "solid";
- if(this.state == true) {
- button.setAttribute("class", "buttonOn");
- } else {
- button.setAttribute("class", "buttonOff");
- }
- }
+ this.set_state();
}
},
@@ -70,28 +79,57 @@
this.pump = null;
this.domobj = domobj;
this.taskid = domobj.getAttribute("taskid");
- this.state = false;
- this.topics = [domobj.getAttribute("topic")];
+ this.state = null;
+ this.topics = ["/app_status"];
+
+ this.button = null;
+ this.statusdiv = null;
},
init: function() {
var obj = this;
- this.domobj.onmousedown = function() {obj.onmousedown();};
+ this.domobj.innerHTML = '<input class=app_button type=button value="">\n<div class=app_status> </div>';
+ this.button = this.domobj.childNodes[0];
+ this.statusdiv = this.domobj.childNodes[2];
+
+ this.button.onmousedown = function() {obj.onmousedown();};
+
this.set_state();
},
receive: function(topic, msg) {
+ if(topic == "/app_update") this.receive_app_update(msg);
+ else if(topic == "/app_status") this.receive_app_status(msg);
+ },
+
+ receive_app_status: function(msg) {
+ var active = false;
+ for(var i=0; i<msg.active.length; i++) {
+ if(msg.active[i].taskid == this.taskid) {
+ this.receive_app_update(msg.active[i]);
+ active = true;
+ }
+ }
+ if(active == false) {
+ this.state = false;
+ this.set_state();
+ }
+ },
+
+ receive_app_update: function(msg) {
if(msg.taskid != this.taskid) return;
var prev_state = this.state;
+ this.statusdiv.innerHTML = msg.status;
+
var state = msg.status;
if(state == "running") this.state = true;
if(state == "stopped") this.state = false;
if(prev_state != this.state) {
- this.set_state(this.state);
+ this.set_state();
}
},
@@ -99,14 +137,14 @@
if(this.domobj == null) return;
if(this.state == true) {
- // this.domobj.disabled = 0;
- this.domobj.value = "Stop";
+ this.button.disabled = 0;
+ this.button.value = "Stop";
} else if (this.state == false) {
- // this.domobj.disabled = 0;
- this.domobj.value = "Launch";
+ this.button.disabled = 0;
+ this.button.value = "Launch";
} else {
- this.domobj.disabled = 1;
- this.domobj.value = "";
+ this.button.disabled = 1;
+ this.button.value = "";
}
},
Modified: pkg/trunk/sandbox/web/webui/src/webui/mod/webui/templates/apps.cs
===================================================================
--- pkg/trunk/sandbox/web/webui/src/webui/mod/webui/templates/apps.cs 2009-09-04 04:33:45 UTC (rev 23815)
+++ pkg/trunk/sandbox/web/webui/src/webui/mod/webui/templates/apps.cs 2009-09-04 05:24:42 UTC (rev 23816)
@@ -29,8 +29,7 @@
<div class=app_description><?cs var:_app.description ?></div>
</td>
<td valign=top>
-<input class=app_button type=button value="" objtype="LaunchButtonWidget2" topic="/app_update" taskid="<?cs var:_app.taskid ?>">
-<div class=app_status objtype=TextWidget topic="/app_update" key=status selector="taskid" selectorValue="<?cs var:_app.taskid?>"> </div>
+<div class=app_button objtype="LaunchButtonWidget2" taskid="<?cs var:_app.taskid ?>">
</td>
</tr>
</table>
@@ -43,5 +42,6 @@
<div id=ErrorDiv></div>
+<?cs include:"rosfooter.cs"?>
</body>
</html>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-09-04 04:33:54
|
Revision: 23815
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23815&view=rev
Author: isucan
Date: 2009-09-04 04:33:45 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
switching from message_notifier to message_filter
Modified Paths:
--------------
pkg/trunk/motion_planning/robot_self_filter/src/self_filter.cpp
Modified: pkg/trunk/motion_planning/robot_self_filter/src/self_filter.cpp
===================================================================
--- pkg/trunk/motion_planning/robot_self_filter/src/self_filter.cpp 2009-09-04 04:16:11 UTC (rev 23814)
+++ pkg/trunk/motion_planning/robot_self_filter/src/self_filter.cpp 2009-09-04 04:33:45 UTC (rev 23815)
@@ -36,7 +36,8 @@
#include <ros/ros.h>
#include "robot_self_filter/self_mask.h"
-#include <tf/message_notifier.h>
+#include <tf/message_filter.h>
+#include <message_filters/subscriber.h>
#include <sstream>
class SelfFilter
@@ -70,8 +71,10 @@
if (!sensor_frame_.empty())
frames.push_back(sensor_frame_);
- mn_ = new tf::MessageNotifier<sensor_msgs::PointCloud>(tf_, boost::bind(&SelfFilter::cloudCallback, this, _1), "cloud_in", "", 1);
- mn_->setTargetFrame(frames);
+ sub_ = new message_filters::Subscriber<sensor_msgs::PointCloud>(nh_, "cloud_in", 1);
+ mn_ = new tf::MessageFilter<sensor_msgs::PointCloud>(*sub_, tf_, "", 1);
+ mn_->setTargetFrames(frames);
+ mn_->registerCallback(boost::bind(&SelfFilter::cloudCallback, this, _1));
nh_.param<std::string>("~annotate", annotate_, std::string());
pointCloudPublisher_ = nh_.advertise<sensor_msgs::PointCloud>("cloud_out", 1);
@@ -85,6 +88,8 @@
{
if (mn_)
delete mn_;
+ if (sub_)
+ delete sub_;
if (sf_)
delete sf_;
}
@@ -177,15 +182,17 @@
}
- tf::TransformListener tf_;
- robot_self_filter::SelfMask *sf_;
- tf::MessageNotifier<sensor_msgs::PointCloud> *mn_;
- ros::Publisher pointCloudPublisher_;
- std::string sensor_frame_;
- ros::NodeHandle nh_;
- std::string annotate_;
- double min_sensor_dist_;
+ tf::TransformListener tf_;
+ robot_self_filter::SelfMask *sf_;
+ tf::MessageFilter<sensor_msgs::PointCloud> *mn_;
+ message_filters::Subscriber<sensor_msgs::PointCloud> *sub_;
+ ros::Publisher pointCloudPublisher_;
+ std::string sensor_frame_;
+ ros::NodeHandle nh_;
+ std::string annotate_;
+ double min_sensor_dist_;
+
};
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-09-04 04:16:19
|
Revision: 23814
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23814&view=rev
Author: isucan
Date: 2009-09-04 04:16:11 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
removing dependency of planning_models on ogre_tools
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_models/manifest.xml
pkg/trunk/motion_planning/planning_models/src/kinematic_model.cpp
Modified: pkg/trunk/motion_planning/planning_models/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/planning_models/manifest.xml 2009-09-04 04:11:58 UTC (rev 23813)
+++ pkg/trunk/motion_planning/planning_models/manifest.xml 2009-09-04 04:16:11 UTC (rev 23814)
@@ -12,7 +12,6 @@
<depend package="geometric_shapes"/>
<depend package="bullet"/>
<depend package="resource_retriever"/>
- <depend package="ogre_tools"/>
<export>
<cpp cflags="-I${prefix}/include `rosboost-cfg --cflags`" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lplanning_models `rosboost-cfg --lflags thread`"/>
Modified: pkg/trunk/motion_planning/planning_models/src/kinematic_model.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/kinematic_model.cpp 2009-09-04 04:11:58 UTC (rev 23813)
+++ pkg/trunk/motion_planning/planning_models/src/kinematic_model.cpp 2009-09-04 04:16:11 UTC (rev 23814)
@@ -36,7 +36,7 @@
#include <planning_models/kinematic_model.h>
#include <resource_retriever/retriever.h>
-#include <ogre_tools/stl_loader.h>
+#include <queue>
#include <ros/console.h>
#include <cmath>
@@ -363,23 +363,12 @@
if (ok)
{
if (res.size == 0)
- ROS_WARN("Retrieved empty mesh for resource [%s]", mesh->filename.c_str());
+ ROS_WARN("Retrieved empty mesh for resource '%s'", mesh->filename.c_str());
else
{
- ogre_tools::STLLoader loader;
- if (loader.load(res.data.get()))
- {
- std::vector<btVector3> triangles;
- for (unsigned int i = 0 ; i < loader.triangles_.size() ; ++i)
- {
- triangles.push_back(btVector3(loader.triangles_[i].vertices_[0].x, loader.triangles_[i].vertices_[0].y, loader.triangles_[i].vertices_[0].z));
- triangles.push_back(btVector3(loader.triangles_[i].vertices_[1].x, loader.triangles_[i].vertices_[1].y, loader.triangles_[i].vertices_[1].z));
- triangles.push_back(btVector3(loader.triangles_[i].vertices_[2].x, loader.triangles_[i].vertices_[2].y, loader.triangles_[i].vertices_[2].z));
- }
- result = shapes::createMeshFromVertices(triangles);
- }
- else
- ROS_ERROR("Failed to load mesh [%s]", mesh->filename.c_str());
+ result = shapes::createMeshFromBinaryStlData(reinterpret_cast<char*>(res.data.get()), res.size);
+ if (result == NULL)
+ ROS_ERROR("Failed to load mesh '%s'", mesh->filename.c_str());
}
}
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-09-04 04:12:09
|
Revision: 23813
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23813&view=rev
Author: isucan
Date: 2009-09-04 04:11:58 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
removing ogre dependency for robot self filter
Modified Paths:
--------------
pkg/trunk/motion_planning/robot_self_filter/manifest.xml
pkg/trunk/motion_planning/robot_self_filter/src/self_mask.cpp
pkg/trunk/stacks/manipulation_common/geometric_shapes/include/geometric_shapes/shapes.h
pkg/trunk/stacks/manipulation_common/geometric_shapes/src/load_mesh.cpp
Modified: pkg/trunk/motion_planning/robot_self_filter/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/robot_self_filter/manifest.xml 2009-09-04 03:17:59 UTC (rev 23812)
+++ pkg/trunk/motion_planning/robot_self_filter/manifest.xml 2009-09-04 04:11:58 UTC (rev 23813)
@@ -17,7 +17,6 @@
<depend package="geometric_shapes"/>
<depend package="urdf"/>
<depend package="resource_retriever"/>
- <depend package="ogre_tools"/>
<export>
<cpp cflags="-I${prefix}/include `rosboost-cfg --cflags`" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lrobot_self_filter" />
Modified: pkg/trunk/motion_planning/robot_self_filter/src/self_mask.cpp
===================================================================
--- pkg/trunk/motion_planning/robot_self_filter/src/self_mask.cpp 2009-09-04 03:17:59 UTC (rev 23812)
+++ pkg/trunk/motion_planning/robot_self_filter/src/self_mask.cpp 2009-09-04 04:11:58 UTC (rev 23813)
@@ -30,7 +30,6 @@
#include "robot_self_filter/self_mask.h"
#include <urdf/model.h>
#include <resource_retriever/retriever.h>
-#include <ogre_tools/stl_loader.h>
#include <ros/console.h>
#include <algorithm>
#include <sstream>
@@ -103,19 +102,8 @@
ROS_WARN("Retrieved empty mesh for resource '%s'", mesh->filename.c_str());
else
{
- ogre_tools::STLLoader loader;
- if (loader.load(res.data.get()))
- {
- std::vector<btVector3> triangles;
- for (unsigned int i = 0 ; i < loader.triangles_.size() ; ++i)
- {
- triangles.push_back(btVector3(loader.triangles_[i].vertices_[0].x, loader.triangles_[i].vertices_[0].y, loader.triangles_[i].vertices_[0].z));
- triangles.push_back(btVector3(loader.triangles_[i].vertices_[1].x, loader.triangles_[i].vertices_[1].y, loader.triangles_[i].vertices_[1].z));
- triangles.push_back(btVector3(loader.triangles_[i].vertices_[2].x, loader.triangles_[i].vertices_[2].y, loader.triangles_[i].vertices_[2].z));
- }
- result = shapes::createMeshFromVertices(triangles);
- }
- else
+ result = shapes::createMeshFromBinaryStlData(reinterpret_cast<char*>(res.data.get()), res.size);
+ if (result == NULL)
ROS_ERROR("Failed to load mesh '%s'", mesh->filename.c_str());
}
}
Modified: pkg/trunk/stacks/manipulation_common/geometric_shapes/include/geometric_shapes/shapes.h
===================================================================
--- pkg/trunk/stacks/manipulation_common/geometric_shapes/include/geometric_shapes/shapes.h 2009-09-04 03:17:59 UTC (rev 23812)
+++ pkg/trunk/stacks/manipulation_common/geometric_shapes/include/geometric_shapes/shapes.h 2009-09-04 04:11:58 UTC (rev 23813)
@@ -237,6 +237,10 @@
recomputed and repeating vertices are identified. */
Mesh* createMeshFromBinaryStl(const char *filename);
+ /** \brief Load a mesh from a binary STL stream. Normals are
+ recomputed and repeating vertices are identified. */
+ Mesh* createMeshFromBinaryStlData(const char *data, unsigned int size);
+
/** \brief Create a copy of a shape */
Shape* cloneShape(const Shape *shape);
Modified: pkg/trunk/stacks/manipulation_common/geometric_shapes/src/load_mesh.cpp
===================================================================
--- pkg/trunk/stacks/manipulation_common/geometric_shapes/src/load_mesh.cpp 2009-09-04 03:17:59 UTC (rev 23812)
+++ pkg/trunk/stacks/manipulation_common/geometric_shapes/src/load_mesh.cpp 2009-09-04 04:11:58 UTC (rev 23813)
@@ -187,10 +187,67 @@
return mesh;
}
+
+ shapes::Mesh* createMeshFromBinaryStlData(const char *data, unsigned int size)
+ {
+ const char* pos = data;
+ pos += 80; // skip the 80 byte header
+
+ unsigned int numTriangles = *(unsigned int*)pos;
+ pos += 4;
+
+ // make sure we have read enough data
+ if ((long)(50 * numTriangles + 84) <= size)
+ {
+ std::vector<btVector3> vertices;
+
+ for (unsigned int currentTriangle = 0 ; currentTriangle < numTriangles ; ++currentTriangle)
+ {
+ // skip the normal
+ pos += 12;
+
+ // read vertices
+ btVector3 v1(0,0,0);
+ btVector3 v2(0,0,0);
+ btVector3 v3(0,0,0);
+
+ v1.setX(*(float*)pos);
+ pos += 4;
+ v1.setY(*(float*)pos);
+ pos += 4;
+ v1.setZ(*(float*)pos);
+ pos += 4;
+
+ v2.setX(*(float*)pos);
+ pos += 4;
+ v2.setY(*(float*)pos);
+ pos += 4;
+ v2.setZ(*(float*)pos);
+ pos += 4;
+
+ v3.setX(*(float*)pos);
+ pos += 4;
+ v3.setY(*(float*)pos);
+ pos += 4;
+ v3.setZ(*(float*)pos);
+ pos += 4;
+
+ // skip attribute
+ pos += 2;
+
+ vertices.push_back(v1);
+ vertices.push_back(v2);
+ vertices.push_back(v3);
+ }
+
+ return createMeshFromVertices(vertices);
+ }
+
+ return NULL;
+ }
shapes::Mesh* createMeshFromBinaryStl(const char *filename)
{
-
FILE* input = fopen(filename, "r");
if (!input)
return NULL;
@@ -204,65 +261,14 @@
fclose(input);
+ shapes::Mesh *result = NULL;
+
if (rd == 1)
- {
- char* pos = buffer;
- pos += 80; // skip the 80 byte header
-
- unsigned int numTriangles = *(unsigned int*)pos;
- pos += 4;
-
- // make sure we have read enough data
- if ((long)(50 * numTriangles + 84) <= fileSize)
- {
- std::vector<btVector3> vertices;
-
- for (unsigned int currentTriangle = 0 ; currentTriangle < numTriangles ; ++currentTriangle)
- {
- // skip the normal
- pos += 12;
-
- // read vertices
- btVector3 v1(0,0,0);
- btVector3 v2(0,0,0);
- btVector3 v3(0,0,0);
-
- v1.setX(*(float*)pos);
- pos += 4;
- v1.setY(*(float*)pos);
- pos += 4;
- v1.setZ(*(float*)pos);
- pos += 4;
-
- v2.setX(*(float*)pos);
- pos += 4;
- v2.setY(*(float*)pos);
- pos += 4;
- v2.setZ(*(float*)pos);
- pos += 4;
-
- v3.setX(*(float*)pos);
- pos += 4;
- v3.setY(*(float*)pos);
- pos += 4;
- v3.setZ(*(float*)pos);
- pos += 4;
-
- // skip attribute
- pos += 2;
-
- vertices.push_back(v1);
- vertices.push_back(v2);
- vertices.push_back(v3);
- }
-
- delete[] buffer;
-
- return createMeshFromVertices(vertices);
- }
- }
+ result = createMeshFromBinaryStlData(buffer, fileSize);
- return NULL;
+ delete[] buffer;
+
+ return result;
}
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <goe...@us...> - 2009-09-04 03:18:08
|
Revision: 23812
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23812&view=rev
Author: goeckerd
Date: 2009-09-04 03:17:59 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
Updated documentation for wiki.
Modified Paths:
--------------
pkg/trunk/sandbox/texas/mainpage.dox
Modified: pkg/trunk/sandbox/texas/mainpage.dox
===================================================================
--- pkg/trunk/sandbox/texas/mainpage.dox 2009-09-04 03:13:07 UTC (rev 23811)
+++ pkg/trunk/sandbox/texas/mainpage.dox 2009-09-04 03:17:59 UTC (rev 23812)
@@ -2,20 +2,26 @@
\mainpage
\htmlinclude manifest.html
-\b texas contains the scripts necessary to launch and drive the DallasBot/SkypeBot.
+\b texas package contains the descriptions and launch files needed to operate the telepresence robot known as Texas.
\section codeapi Code API
-Launching the DallasBot:
+Launching Texas:
\verbatim
roslaunch texas.launch
+roslaunch ps3_drive.launch
\endverbatim
-Driving the Dallas Bot:
+Driving Texas using a PS3 joystick connected through USB to a local computer.
+From the local computer:
\verbatim
-roslaunch drive.launch
+sudo chmod 666 /dev/input/js0
+roscd joy
+./joy
\endverbatim
+The chmod command is only needed once after every time the joystick is plugged in.
+
<!--
Provide links to specific auto-generated API documentation within your
package that is of particular interest to a reader. Doxygen will
@@ -117,4 +123,4 @@
END: Command-Line Tools Section -->
-*/
\ No newline at end of file
+*/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <goe...@us...> - 2009-09-04 03:13:18
|
Revision: 23811
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23811&view=rev
Author: goeckerd
Date: 2009-09-04 03:13:07 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
Deleted unneeded files and directories.
Removed Paths:
-------------
pkg/trunk/sandbox/texas/CMakeLists.txt
pkg/trunk/sandbox/texas/Makefile
pkg/trunk/sandbox/texas/bin/
pkg/trunk/sandbox/texas/controllers.xml
pkg/trunk/sandbox/texas/lib/
pkg/trunk/sandbox/texas/msg/
pkg/trunk/sandbox/texas/src/
Deleted: pkg/trunk/sandbox/texas/CMakeLists.txt
===================================================================
--- pkg/trunk/sandbox/texas/CMakeLists.txt 2009-09-04 03:08:58 UTC (rev 23810)
+++ pkg/trunk/sandbox/texas/CMakeLists.txt 2009-09-04 03:13:07 UTC (rev 23811)
@@ -1,30 +0,0 @@
-cmake_minimum_required(VERSION 2.4.6)
-include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
-
-# Set the build type. Options are:
-# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
-# Debug : w/ debug symbols, w/o optimization
-# Release : w/o debug symbols, w/ optimization
-# RelWithDebInfo : w/ debug symbols, w/ optimization
-# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
-#set(ROS_BUILD_TYPE RelWithDebInfo)
-
-rospack(texas)
-
-#set the default path for built executables to the "bin" directory
-set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
-#set the default path for built libraries to the "lib" directory
-set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
-
-#uncomment if you have defined messages
-genmsg() # TexasCmd
-#uncomment if you have defined services
-#gensrv()
-
-#common commands for building c++ executables and libraries
-#rospack_add_library(${PROJECT_NAME} src/example.cpp)
-#target_link_libraries(${PROJECT_NAME} another_library)
-#rospack_add_boost_directories()
-#rospack_link_boost(${PROJECT_NAME} thread)
-#rospack_add_executable(teleop_texas_keyboard teleop_texas_keyboard.cpp)
-#target_link_libraries(example ${PROJECT_NAME})
Deleted: pkg/trunk/sandbox/texas/Makefile
===================================================================
--- pkg/trunk/sandbox/texas/Makefile 2009-09-04 03:08:58 UTC (rev 23810)
+++ pkg/trunk/sandbox/texas/Makefile 2009-09-04 03:13:07 UTC (rev 23811)
@@ -1 +0,0 @@
-include $(shell rospack find mk)/cmake.mk
\ No newline at end of file
Deleted: pkg/trunk/sandbox/texas/controllers.xml
===================================================================
--- pkg/trunk/sandbox/texas/controllers.xml 2009-09-04 03:08:58 UTC (rev 23810)
+++ pkg/trunk/sandbox/texas/controllers.xml 2009-09-04 03:13:07 UTC (rev 23811)
@@ -1,9 +0,0 @@
-<controllers>
- <controller name="caster" type="CasterControllerNode">
- <joints caster="bl_caster_rotation_joint"
- wheel_l="bl_caster_l_wheel_joint"
- wheel_r="bl_caster_r_wheel_joint" />
- <caster_pid p="5.0" i="0" d="0" iClamp="0" />
- <wheel_pid p="4" i="0" d="0" iClamp="0" />
- </controller>
-</controllers>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-09-04 03:09:05
|
Revision: 23810
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23810&view=rev
Author: isucan
Date: 2009-09-04 03:08:58 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
switching to message filters instead of message notifier
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h
pkg/trunk/motion_planning/planning_environment/manifest.xml
pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/tools/clear_known_objects.cpp
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h 2009-09-04 02:26:34 UTC (rev 23809)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h 2009-09-04 03:08:58 UTC (rev 23810)
@@ -69,12 +69,18 @@
virtual ~CollisionSpaceMonitor(void)
{
- if (objectInMapNotifier_)
- delete objectInMapNotifier_;
- if (collisionMapNotifier_)
- delete collisionMapNotifier_;
- if (collisionMapUpdateNotifier_)
- delete collisionMapUpdateNotifier_;
+ if (objectInMapFilter_)
+ delete objectInMapFilter_;
+ if (objectInMapSubscriber_)
+ delete objectInMapSubscriber_;
+ if (collisionMapFilter_)
+ delete collisionMapFilter_;
+ if (collisionMapSubscriber_)
+ delete collisionMapSubscriber_;
+ if (collisionMapUpdateFilter_)
+ delete collisionMapUpdateFilter_;
+ if (collisionMapUpdateSubscriber_)
+ delete collisionMapUpdateSubscriber_;
}
/** \brief Start the environment monitor. By default, the monitor is started after creation */
@@ -188,10 +194,14 @@
bool haveMap_;
ros::Time lastMapUpdate_;
- tf::MessageNotifier<mapping_msgs::CollisionMap> *collisionMapNotifier_;
- tf::MessageNotifier<mapping_msgs::CollisionMap> *collisionMapUpdateNotifier_;
- tf::MessageNotifier<mapping_msgs::ObjectInMap> *objectInMapNotifier_;
+ message_filters::Subscriber<mapping_msgs::CollisionMap> *collisionMapSubscriber_;
+ tf::MessageFilter<mapping_msgs::CollisionMap> *collisionMapFilter_;
+ message_filters::Subscriber<mapping_msgs::CollisionMap> *collisionMapUpdateSubscriber_;
+ tf::MessageFilter<mapping_msgs::CollisionMap> *collisionMapUpdateFilter_;
+ message_filters::Subscriber<mapping_msgs::ObjectInMap> *objectInMapSubscriber_;
+ tf::MessageFilter<mapping_msgs::ObjectInMap> *objectInMapFilter_;
+
boost::function<void(const mapping_msgs::CollisionMapConstPtr, bool)> onBeforeMapUpdate_;
boost::function<void(const mapping_msgs::CollisionMapConstPtr, bool)> onAfterMapUpdate_;
boost::function<void(const mapping_msgs::ObjectInMapConstPtr)> onObjectInMapUpdate_;
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h 2009-09-04 02:26:34 UTC (rev 23809)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h 2009-09-04 03:08:58 UTC (rev 23810)
@@ -41,7 +41,8 @@
#include "planning_models/kinematic_state.h"
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
-#include <tf/message_notifier.h>
+#include <tf/message_filter.h>
+#include <message_filters/subscriber.h>
#include <sensor_msgs/JointState.h>
#include <mapping_msgs/AttachedObject.h>
#include <boost/bind.hpp>
@@ -81,8 +82,10 @@
{
if (robotState_)
delete robotState_;
- if (attachedBodyNotifier_)
- delete attachedBodyNotifier_;
+ if (attachedBodyFilter_)
+ delete attachedBodyFilter_;
+ if (attachedBodySubscriber_)
+ delete attachedBodySubscriber_;
}
/** \brief Start the state monitor. By default, the monitor is started after creation */
@@ -208,8 +211,11 @@
ros::Subscriber jointStateSubscriber_;
tf::TransformListener *tf_;
- tf::MessageNotifier<mapping_msgs::AttachedObject>
- *attachedBodyNotifier_;
+ message_filters::Subscriber<mapping_msgs::AttachedObject>
+ *attachedBodySubscriber_;
+
+ tf::MessageFilter<mapping_msgs::AttachedObject>
+ *attachedBodyFilter_;
planning_models::KinematicState *robotState_;
double robotVelocity_;
Modified: pkg/trunk/motion_planning/planning_environment/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/planning_environment/manifest.xml 2009-09-04 02:26:34 UTC (rev 23809)
+++ pkg/trunk/motion_planning/planning_environment/manifest.xml 2009-09-04 03:08:58 UTC (rev 23810)
@@ -12,6 +12,7 @@
<depend package="roscpp" />
<depend package="rosconsole" />
<depend package="tf" />
+ <depend package="message_filters" />
<depend package="angles" />
<depend package="sensor_msgs" />
<depend package="motion_planning_msgs" />
Modified: pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp 2009-09-04 02:26:34 UTC (rev 23809)
+++ pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp 2009-09-04 03:08:58 UTC (rev 23810)
@@ -58,10 +58,14 @@
onAfterMapUpdate_ = NULL;
onObjectInMapUpdate_ = NULL;
- collisionMapNotifier_ = NULL;
- collisionMapUpdateNotifier_ = NULL;
- objectInMapNotifier_ = NULL;
+ collisionMapFilter_ = NULL;
+ collisionMapUpdateFilter_ = NULL;
+ objectInMapFilter_ = NULL;
+ collisionMapSubscriber_ = NULL;
+ collisionMapUpdateSubscriber_ = NULL;
+ objectInMapSubscriber_ = NULL;
+
haveMap_ = false;
collisionSpace_ = cm_->getODECollisionModel().get();
@@ -74,15 +78,21 @@
{
if (envMonitorStarted_)
return;
-
- collisionMapNotifier_ = new tf::MessageNotifier<mapping_msgs::CollisionMap>(*tf_, boost::bind(&CollisionSpaceMonitor::collisionMapCallback, this, _1), "collision_map", getFrameId(), 1);
- ROS_DEBUG("Listening to collision_map using message notifier with target frame %s", collisionMapNotifier_->getTargetFramesString().c_str());
- collisionMapUpdateNotifier_ = new tf::MessageNotifier<mapping_msgs::CollisionMap>(*tf_, boost::bind(&CollisionSpaceMonitor::collisionMapUpdateCallback, this, _1), "collision_map_update", getFrameId(), 1);
- ROS_DEBUG("Listening to collision_map_update using message notifier with target frame %s", collisionMapUpdateNotifier_->getTargetFramesString().c_str());
+ collisionMapSubscriber_ = new message_filters::Subscriber<mapping_msgs::CollisionMap>(nh_, "collision_map", 1);
+ collisionMapFilter_ = new tf::MessageFilter<mapping_msgs::CollisionMap>(*collisionMapSubscriber_, *tf_, getFrameId(), 1);
+ collisionMapFilter_->registerCallback(boost::bind(&CollisionSpaceMonitor::collisionMapCallback, this, _1));
+ ROS_DEBUG("Listening to collision_map using message notifier with target frame %s", collisionMapFilter_->getTargetFramesString().c_str());
- objectInMapNotifier_ = new tf::MessageNotifier<mapping_msgs::ObjectInMap>(*tf_, boost::bind(&CollisionSpaceMonitor::objectInMapCallback, this, _1), "object_in_map", getFrameId(), 1024);
- ROS_DEBUG("Listening to object_in_map using message notifier with target frame %s", collisionMapUpdateNotifier_->getTargetFramesString().c_str());
+ collisionMapUpdateSubscriber_ = new message_filters::Subscriber<mapping_msgs::CollisionMap>(nh_, "collision_map_update", 1);
+ collisionMapUpdateFilter_ = new tf::MessageFilter<mapping_msgs::CollisionMap>(*collisionMapUpdateSubscriber_, *tf_, getFrameId(), 1);
+ collisionMapUpdateFilter_->registerCallback(boost::bind(&CollisionSpaceMonitor::collisionMapUpdateCallback, this, _1));
+ ROS_DEBUG("Listening to collision_map_update using message notifier with target frame %s", collisionMapUpdateFilter_->getTargetFramesString().c_str());
+
+ objectInMapSubscriber_ = new message_filters::Subscriber<mapping_msgs::ObjectInMap>(nh_, "object_in_map", 1024);
+ objectInMapFilter_ = new tf::MessageFilter<mapping_msgs::ObjectInMap>(*objectInMapSubscriber_, *tf_, getFrameId(), 1024);
+ objectInMapFilter_->registerCallback(boost::bind(&CollisionSpaceMonitor::objectInMapCallback, this, _1));
+ ROS_DEBUG("Listening to object_in_map using message notifier with target frame %s", collisionMapUpdateFilter_->getTargetFramesString().c_str());
envMonitorStarted_ = true;
}
@@ -92,14 +102,20 @@
if (!envMonitorStarted_)
return;
- delete collisionMapUpdateNotifier_;
- collisionMapUpdateNotifier_ = NULL;
+ delete collisionMapUpdateFilter_;
+ collisionMapUpdateFilter_ = NULL;
+ delete collisionMapUpdateSubscriber_;
+ collisionMapUpdateSubscriber_ = NULL;
- delete collisionMapNotifier_;
- collisionMapNotifier_ = NULL;
+ delete collisionMapFilter_;
+ collisionMapFilter_ = NULL;
+ delete collisionMapSubscriber_;
+ collisionMapSubscriber_ = NULL;
- delete objectInMapNotifier_;
- objectInMapNotifier_ = NULL;
+ delete objectInMapFilter_;
+ objectInMapFilter_ = NULL;
+ delete objectInMapSubscriber_;
+ objectInMapSubscriber_ = NULL;
ROS_DEBUG("Environment state is no longer being monitored");
Modified: pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp 2009-09-04 02:26:34 UTC (rev 23809)
+++ pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp 2009-09-04 03:08:58 UTC (rev 23810)
@@ -47,7 +47,8 @@
robotState_ = NULL;
onStateUpdate_ = NULL;
onAfterAttachBody_ = NULL;
- attachedBodyNotifier_ = NULL;
+ attachedBodyFilter_ = NULL;
+ attachedBodySubscriber_ = NULL;
havePose_ = haveJointState_ = false;
robotVelocity_ = 0.0;
@@ -94,9 +95,12 @@
jointStateSubscriber_ = nh_.subscribe("joint_states", 1, &KinematicModelStateMonitor::jointStateCallback, this);
ROS_DEBUG("Listening to joint states");
- attachedBodyNotifier_ = new tf::MessageNotifier<mapping_msgs::AttachedObject>(*tf_, boost::bind(&KinematicModelStateMonitor::attachObjectCallback, this, _1), "attach_object", "", 1);
- attachedBodyNotifier_->setTargetFrame(rm_->getCollisionCheckLinks());
- ROS_DEBUG("Listening to attach_object using message notifier with target frame %s", attachedBodyNotifier_->getTargetFramesString().c_str());
+ attachedBodySubscriber_ = new message_filters::Subscriber<mapping_msgs::AttachedObject>(nh_, "attach_object", 1);
+ attachedBodyFilter_ = new tf::MessageFilter<mapping_msgs::AttachedObject>(*attachedBodySubscriber_, *tf_, "", 1);
+ attachedBodyFilter_->setTargetFrames(rm_->getCollisionCheckLinks());
+ attachedBodyFilter_->registerCallback(boost::bind(&KinematicModelStateMonitor::attachObjectCallback, this, _1));
+
+ ROS_DEBUG("Listening to attach_object using message notifier with target frame %s", attachedBodyFilter_->getTargetFramesString().c_str());
}
stateMonitorStarted_ = true;
}
@@ -106,8 +110,11 @@
if (!stateMonitorStarted_)
return;
- delete attachedBodyNotifier_;
- attachedBodyNotifier_ = NULL;
+ delete attachedBodyFilter_;
+ attachedBodyFilter_ = NULL;
+
+ delete attachedBodySubscriber_;
+ attachedBodySubscriber_ = NULL;
jointStateSubscriber_.shutdown();
Modified: pkg/trunk/motion_planning/planning_environment/src/tools/clear_known_objects.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/tools/clear_known_objects.cpp 2009-09-04 02:26:34 UTC (rev 23809)
+++ pkg/trunk/motion_planning/planning_environment/src/tools/clear_known_objects.cpp 2009-09-04 03:08:58 UTC (rev 23810)
@@ -46,7 +46,8 @@
#include "planning_environment/util/construct_object.h"
#include <geometric_shapes/bodies.h>
-#include <tf/message_notifier.h>
+#include <tf/message_filter.h>
+#include <message_filters/subscriber.h>
#include <sensor_msgs/PointCloud.h>
#include <geometry_msgs/PoseStamped.h>
#include <mapping_msgs/AttachedObject.h>
@@ -71,23 +72,35 @@
cloudPublisher_ = nh_.advertise<sensor_msgs::PointCloud>("cloud_out", 1);
kmsm_->setOnAfterAttachBodyCallback(boost::bind(&ClearKnownObjects::attachObjectEvent, this, _1));
- cloudNotifier_ = new tf::MessageNotifier<sensor_msgs::PointCloud>(tf_, boost::bind(&ClearKnownObjects::cloudCallback, this, _1), "cloud_in", fixed_frame_, 1);
- objectInMapNotifier_ = new tf::MessageNotifier<mapping_msgs::ObjectInMap>(tf_, boost::bind(&ClearKnownObjects::objectInMapCallback, this, _1), "object_in_map", fixed_frame_, 1024);
+
+ cloudSubscriber_ = new message_filters::Subscriber<sensor_msgs::PointCloud>(nh_, "cloud_in", 1);
+ cloudFilter_ = new tf::MessageFilter<sensor_msgs::PointCloud>(*cloudSubscriber_, tf_, fixed_frame_, 1);
+ cloudFilter_->registerCallback(boost::bind(&ClearKnownObjects::cloudCallback, this, _1));
+
+ objectInMapSubscriber_ = new message_filters::Subscriber<mapping_msgs::ObjectInMap>(nh_, "object_in_map", 1024);
+ objectInMapFilter_ = new tf::MessageFilter<mapping_msgs::ObjectInMap>(*objectInMapSubscriber_, tf_, fixed_frame_, 1024);
+ objectInMapFilter_->registerCallback(boost::bind(&ClearKnownObjects::objectInMapCallback, this, _1));
}
else
{
kmsm_ = NULL;
- cloudNotifier_ = NULL;
- objectInMapNotifier_ = NULL;
+ cloudFilter_ = NULL;
+ cloudSubscriber_ = NULL;
+ objectInMapFilter_ = NULL;
+ objectInMapSubscriber_ = NULL;
}
}
~ClearKnownObjects(void)
{
- if (cloudNotifier_)
- delete cloudNotifier_;
- if (objectInMapNotifier_)
- delete objectInMapNotifier_;
+ if (cloudFilter_)
+ delete cloudFilter_;
+ if (cloudSubscriber_)
+ delete cloudSubscriber_;
+ if (objectInMapFilter_)
+ delete objectInMapFilter_;
+ if (objectInMapSubscriber_)
+ delete objectInMapSubscriber_;
if (kmsm_)
delete kmsm_;
if (rm_)
@@ -361,8 +374,12 @@
tf::TransformListener tf_;
planning_environment::RobotModels *rm_;
planning_environment::KinematicModelStateMonitor *kmsm_;
- tf::MessageNotifier<sensor_msgs::PointCloud> *cloudNotifier_;
- tf::MessageNotifier<mapping_msgs::ObjectInMap> *objectInMapNotifier_;
+
+ message_filters::Subscriber<mapping_msgs::ObjectInMap> *objectInMapSubscriber_;
+ tf::MessageFilter<mapping_msgs::ObjectInMap> *objectInMapFilter_;
+ message_filters::Subscriber<sensor_msgs::PointCloud> *cloudSubscriber_;
+ tf::MessageFilter<sensor_msgs::PointCloud> *cloudFilter_;
+
std::string fixed_frame_;
boost::mutex updateObjects_;
ros::Publisher cloudPublisher_;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-09-04 02:26:41
|
Revision: 23809
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23809&view=rev
Author: isucan
Date: 2009-09-04 02:26:34 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
updating dependencies
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_environment/manifest.xml
Modified: pkg/trunk/motion_planning/planning_environment/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/planning_environment/manifest.xml 2009-09-04 02:15:28 UTC (rev 23808)
+++ pkg/trunk/motion_planning/planning_environment/manifest.xml 2009-09-04 02:26:34 UTC (rev 23809)
@@ -13,8 +13,6 @@
<depend package="rosconsole" />
<depend package="tf" />
<depend package="angles" />
- <depend package="pr2_defs" />
- <depend package="tabletop_msgs" />
<depend package="sensor_msgs" />
<depend package="motion_planning_msgs" />
<depend package="mapping_msgs" />
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-09-04 02:15:37
|
Revision: 23808
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23808&view=rev
Author: isucan
Date: 2009-09-04 02:15:28 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
robot self filter no longer depends on planning stack
Modified Paths:
--------------
pkg/trunk/motion_planning/robot_self_filter/include/robot_self_filter/self_mask.h
pkg/trunk/motion_planning/robot_self_filter/manifest.xml
pkg/trunk/motion_planning/robot_self_filter/src/self_mask.cpp
pkg/trunk/motion_planning/robot_self_filter/src/test_filter.cpp
Modified: pkg/trunk/motion_planning/robot_self_filter/include/robot_self_filter/self_mask.h
===================================================================
--- pkg/trunk/motion_planning/robot_self_filter/include/robot_self_filter/self_mask.h 2009-09-04 01:31:22 UTC (rev 23807)
+++ pkg/trunk/motion_planning/robot_self_filter/include/robot_self_filter/self_mask.h 2009-09-04 02:15:28 UTC (rev 23808)
@@ -31,7 +31,6 @@
#define ROBOT_SELF_FILTER_SELF_MASK_
#include <sensor_msgs/PointCloud.h>
-#include <planning_environment/models/robot_models.h>
#include <geometric_shapes/bodies.h>
#include <tf/transform_listener.h>
#include <boost/bind.hpp>
@@ -82,7 +81,7 @@
public:
/** \brief Construct the filter */
- SelfMask(tf::TransformListener &tf, const std::vector<std::string> &links, double scale, double padd) : rm_("robot_description"), tf_(tf)
+ SelfMask(tf::TransformListener &tf, const std::vector<std::string> &links, double scale, double padd) : tf_(tf)
{
configure(links, scale, padd);
}
@@ -170,7 +169,6 @@
/** \brief Perform the actual mask computation. */
void maskAuxIntersection(const sensor_msgs::PointCloud& data_in, std::vector<int> &mask, const boost::function<void(const btVector3&)> &callback);
- planning_environment::RobotModels rm_;
tf::TransformListener &tf_;
ros::NodeHandle nh_;
Modified: pkg/trunk/motion_planning/robot_self_filter/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/robot_self_filter/manifest.xml 2009-09-04 01:31:22 UTC (rev 23807)
+++ pkg/trunk/motion_planning/robot_self_filter/manifest.xml 2009-09-04 02:15:28 UTC (rev 23808)
@@ -15,7 +15,9 @@
<depend package="sensor_msgs"/>
<depend package="visualization_msgs"/>
<depend package="geometric_shapes"/>
- <depend package="planning_environment"/>
+ <depend package="urdf"/>
+ <depend package="resource_retriever"/>
+ <depend package="ogre_tools"/>
<export>
<cpp cflags="-I${prefix}/include `rosboost-cfg --cflags`" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lrobot_self_filter" />
Modified: pkg/trunk/motion_planning/robot_self_filter/src/self_mask.cpp
===================================================================
--- pkg/trunk/motion_planning/robot_self_filter/src/self_mask.cpp 2009-09-04 01:31:22 UTC (rev 23807)
+++ pkg/trunk/motion_planning/robot_self_filter/src/self_mask.cpp 2009-09-04 02:15:28 UTC (rev 23808)
@@ -28,9 +28,13 @@
*/
#include "robot_self_filter/self_mask.h"
+#include <urdf/model.h>
+#include <resource_retriever/retriever.h>
+#include <ogre_tools/stl_loader.h>
+#include <ros/console.h>
#include <algorithm>
+#include <sstream>
#include <climits>
-#include <ros/console.h>
void robot_self_filter::SelfMask::freeMemory(void)
{
@@ -45,23 +49,145 @@
bodies_.clear();
}
+
+namespace robot_self_filter
+{
+ static inline btTransform urdfPose2btTransform(const urdf::Pose &pose)
+ {
+ return btTransform(btQuaternion(pose.rotation.x, pose.rotation.y, pose.rotation.z, pose.rotation.w),
+ btVector3(pose.position.x, pose.position.y, pose.position.z));
+ }
+
+ static shapes::Shape* constructShape(const urdf::Geometry *geom)
+ {
+ ROS_ASSERT(geom);
+
+ shapes::Shape *result = NULL;
+ switch (geom->type)
+ {
+ case urdf::Geometry::SPHERE:
+ result = new shapes::Sphere(dynamic_cast<const urdf::Sphere*>(geom)->radius);
+ break;
+ case urdf::Geometry::BOX:
+ {
+ urdf::Vector3 dim = dynamic_cast<const urdf::Box*>(geom)->dim;
+ result = new shapes::Box(dim.x, dim.y, dim.z);
+ }
+ break;
+ case urdf::Geometry::CYLINDER:
+ result = new shapes::Cylinder(dynamic_cast<const urdf::Cylinder*>(geom)->radius,
+ dynamic_cast<const urdf::Cylinder*>(geom)->length);
+ break;
+ case urdf::Geometry::MESH:
+ {
+ const urdf::Mesh *mesh = dynamic_cast<const urdf::Mesh*>(geom);
+ if (!mesh->filename.empty())
+ {
+ resource_retriever::Retriever retriever;
+ resource_retriever::MemoryResource res;
+ bool ok = true;
+
+ try
+ {
+ res = retriever.get(mesh->filename);
+ }
+ catch (resource_retriever::Exception& e)
+ {
+ ROS_ERROR("%s", e.what());
+ ok = false;
+ }
+
+ if (ok)
+ {
+ if (res.size == 0)
+ ROS_WARN("Retrieved empty mesh for resource '%s'", mesh->filename.c_str());
+ else
+ {
+ ogre_tools::STLLoader loader;
+ if (loader.load(res.data.get()))
+ {
+ std::vector<btVector3> triangles;
+ for (unsigned int i = 0 ; i < loader.triangles_.size() ; ++i)
+ {
+ triangles.push_back(btVector3(loader.triangles_[i].vertices_[0].x, loader.triangles_[i].vertices_[0].y, loader.triangles_[i].vertices_[0].z));
+ triangles.push_back(btVector3(loader.triangles_[i].vertices_[1].x, loader.triangles_[i].vertices_[1].y, loader.triangles_[i].vertices_[1].z));
+ triangles.push_back(btVector3(loader.triangles_[i].vertices_[2].x, loader.triangles_[i].vertices_[2].y, loader.triangles_[i].vertices_[2].z));
+ }
+ result = shapes::createMeshFromVertices(triangles);
+ }
+ else
+ ROS_ERROR("Failed to load mesh '%s'", mesh->filename.c_str());
+ }
+ }
+ }
+ else
+ ROS_WARN("Empty mesh filename");
+ }
+
+ break;
+ default:
+ ROS_ERROR("Unknown geometry type: %d", (int)geom->type);
+ break;
+ }
+
+ return result;
+ }
+}
+
bool robot_self_filter::SelfMask::configure(const std::vector<std::string> &links, double scale, double padd)
{
// in case configure was called before, we free the memory
freeMemory();
sensor_pos_.setValue(0, 0, 0);
+ std::string content;
+ boost::shared_ptr<urdf::Model> urdfModel;
+
+ if (nh_.getParam("robot_description", content))
+ {
+ urdfModel = boost::shared_ptr<urdf::Model>(new urdf::Model());
+ if (!urdfModel->initString(content))
+ {
+ ROS_ERROR("Unable to parse URDF description!");
+ return false;
+ }
+ }
+ else
+ {
+ ROS_ERROR("Robot model not found! Did you remap 'robot_description'?");
+ return false;
+ }
+
+ std::stringstream missing;
+
// from the geometric model, find the shape of each link of interest
// and create a body from it, one that knows about poses and can
// check for point inclusion
for (unsigned int i = 0 ; i < links.size() ; ++i)
{
- planning_models::KinematicModel::Link *link = rm_.getKinematicModel()->getLink(links[i]);
+ const urdf::Link *link = urdfModel->getLink(links[i]).get();
if (!link)
+ {
+ missing << " " << links[i];
continue;
+ }
+ if (!(link->collision && link->collision->geometry))
+ {
+ ROS_WARN("No collision geometry specified for link '%s'", links[i].c_str());
+ continue;
+ }
+
+ shapes::Shape *shape = constructShape(link->collision->geometry.get());
+
+ if (!shape)
+ {
+ ROS_ERROR("Unable to construct collision shape for link '%s'", links[i].c_str());
+ continue;
+ }
+
SeeLink sl;
- sl.body = bodies::createBodyFromShape(link->shape);
+ sl.body = bodies::createBodyFromShape(shape);
if (sl.body)
{
@@ -69,19 +195,25 @@
// collision models may have an offset, in addition to what TF gives
// so we keep it around
- sl.constTransf = link->constGeomTrans;
+ sl.constTransf = urdfPose2btTransform(link->collision->origin);
+
sl.body->setScale(scale);
sl.body->setPadding(padd);
sl.volume = sl.body->computeVolume();
- sl.unscaledBody = bodies::createBodyFromShape(link->shape);
+ sl.unscaledBody = bodies::createBodyFromShape(shape);
bodies_.push_back(sl);
}
else
ROS_WARN("Unable to create point inclusion body for link '%s'", links[i].c_str());
+
+ delete shape;
}
+ if (missing.str().size() > 0)
+ ROS_WARN("Some links were included for self mask but they do not exist in the model:%s", missing.str().c_str());
+
if (bodies_.empty())
- ROS_WARN("No robot links will be checked for self collision");
+ ROS_WARN("No robot links will be checked for self mask");
// put larger volume bodies first -- higher chances of containing a point
std::sort(bodies_.begin(), bodies_.end(), SortBodies());
Modified: pkg/trunk/motion_planning/robot_self_filter/src/test_filter.cpp
===================================================================
--- pkg/trunk/motion_planning/robot_self_filter/src/test_filter.cpp 2009-09-04 01:31:22 UTC (rev 23807)
+++ pkg/trunk/motion_planning/robot_self_filter/src/test_filter.cpp 2009-09-04 02:15:28 UTC (rev 23808)
@@ -42,7 +42,7 @@
{
public:
- TestSelfFilter(void) : rm_("robot_description")
+ TestSelfFilter(void)
{
id_ = 1;
vmPub_ = nodeHandle_.advertise<visualization_msgs::Marker>("visualization_marker", 10240);
@@ -146,7 +146,6 @@
tf::TransformListener tf_;
robot_self_filter::SelfMask *sf_;
- planning_environment::RobotModels rm_;
ros::Publisher vmPub_;
ros::NodeHandle nodeHandle_;
int id_;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-09-04 01:31:29
|
Revision: 23807
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23807&view=rev
Author: isucan
Date: 2009-09-04 01:31:22 +0000 (Fri, 04 Sep 2009)
Log Message:
-----------
removing draw rays debug app
Modified Paths:
--------------
pkg/trunk/motion_planning/robot_self_filter/CMakeLists.txt
Removed Paths:
-------------
pkg/trunk/motion_planning/robot_self_filter/src/draw_rays.cpp
Modified: pkg/trunk/motion_planning/robot_self_filter/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/robot_self_filter/CMakeLists.txt 2009-09-04 01:16:47 UTC (rev 23806)
+++ pkg/trunk/motion_planning/robot_self_filter/CMakeLists.txt 2009-09-04 01:31:22 UTC (rev 23807)
@@ -24,7 +24,3 @@
rospack_add_executable(self_filter src/self_filter.cpp)
rospack_add_openmp_flags(self_filter)
target_link_libraries(self_filter robot_self_filter)
-
-rospack_add_executable(draw_rays src/draw_rays.cpp)
-rospack_add_openmp_flags(draw_rays)
-target_link_libraries(draw_rays robot_self_filter)
Deleted: pkg/trunk/motion_planning/robot_self_filter/src/draw_rays.cpp
===================================================================
--- pkg/trunk/motion_planning/robot_self_filter/src/draw_rays.cpp 2009-09-04 01:16:47 UTC (rev 23806)
+++ pkg/trunk/motion_planning/robot_self_filter/src/draw_rays.cpp 2009-09-04 01:31:22 UTC (rev 23807)
@@ -1,140 +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.
-*********************************************************************/
-
-/** \author Ioan Sucan */
-
-#include <ros/ros.h>
-#include <visualization_msgs/Marker.h>
-#include <sensor_msgs/PointCloud.h>
-#include <tf/message_notifier.h>
-#include <tf/transform_listener.h>
-
-class DrawRays
-{
-public:
-
- DrawRays(void) : cloudNotifier_(tf_, boost::bind(&DrawRays::cloudCallback, this, _1), "cloud_in", "laser_tilt_mount_link", 1)
- {
- id_ = 1;
- vmPub_ = nodeHandle_.advertise<visualization_msgs::Marker>("visualization_marker", 10240);
- nodeHandle_.param<std::string>("~sensor_frame", sensor_frame_, "laser_tilt_mount_link");
- }
-
- ~DrawRays(void)
- {
- }
-
- void cloudCallback(const sensor_msgs::PointCloudConstPtr &cloud)
- {
- // compute the origin of the sensor in the frame of the cloud
- btVector3 sensor_pos;
- try
- {
- tf::Stamped<btTransform> transf;
- tf_.lookupTransform(cloud->header.frame_id, sensor_frame_, cloud->header.stamp, transf);
- sensor_pos = transf.getOrigin();
- }
- catch(...)
- {
- sensor_pos.setValue(0, 0, 0);
- ROS_ERROR("Unable to lookup transform from %s to %s", sensor_frame_.c_str(), cloud->header.frame_id.c_str());
- }
- for (unsigned int i = 0 ; i < cloud->points.size() ; ++i)
- sendLine(cloud->header, cloud->points[i].x, cloud->points[i].y, cloud->points[i].z, sensor_pos.x(), sensor_pos.y(), sensor_pos.z());
- id_ = 1;
- }
-
- void sendLine(const roslib::Header &header, double x1, double y1, double z1, double x2, double y2, double z2)
- {
- visualization_msgs::Marker mk;
-
- mk.header = header;
-
- mk.ns = "draw_rays";
- mk.id = id_++;
- mk.type = visualization_msgs::Marker::ARROW;
- mk.action = visualization_msgs::Marker::ADD;
- mk.pose.position.x = 0;
- mk.pose.position.y = 0;
- mk.pose.position.z = 0;
- mk.pose.orientation.x = 0;
- mk.pose.orientation.y = 0;
- mk.pose.orientation.z = 0;
- mk.pose.orientation.w = 1.0;
-
- mk.scale.x = mk.scale.y = mk.scale.z = 0.01;
-
- mk.color.a = 1.0;
- mk.color.r = 0.7;
- mk.color.g = 0.4;
- mk.color.b = 0.4;
-
- mk.points.resize(2);
- mk.points[0].x = x1;
- mk.points[0].y = y1;
- mk.points[0].z = z1;
-
- mk.points[1].x = x2;
- mk.points[1].y = y2;
- mk.points[1].z = z2;
-
- mk.lifetime = ros::Duration(2);
-
- vmPub_.publish(mk);
- }
-
-protected:
-
-
- ros::NodeHandle nodeHandle_;
- tf::TransformListener tf_;
- tf::MessageNotifier<sensor_msgs::PointCloud> cloudNotifier_;
- std::string sensor_frame_;
-
- ros::Publisher vmPub_;
- int id_;
-};
-
-int main(int argc, char **argv)
-{
- ros::init(argc, argv, "draw_rays");
-
- DrawRays t;
- ros::spin();
-
- return 0;
-}
-
-
-
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
