[Personalrobots-commit] SF.net SVN: personalrobots:[16777] pkg/trunk/motion_planning/ planning_environment

[<is...@us...>]

Revision: 16777
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=16777&view=rev
Author:   isucan
Date:     2009-06-05 19:59:14 +0000 (Fri, 05 Jun 2009)

Log Message:
-----------
added runtime test

Modified Paths:
--------------
    pkg/trunk/motion_planning/planning_environment/CMakeLists.txt
    pkg/trunk/motion_planning/planning_environment/test/test_robot_models.cpp

Modified: pkg/trunk/motion_planning/planning_environment/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/planning_environment/CMakeLists.txt	2009-06-05 19:59:01 UTC (rev 16776)
+++ pkg/trunk/motion_planning/planning_environment/CMakeLists.txt	2009-06-05 19:59:14 UTC (rev 16777)
@@ -3,6 +3,8 @@
 
 rospack(planning_environment)
 
+set(ROS_BUILD_TYPE Release)
+
 rospack_add_library(planning_environment src/robot_models.cpp
 					 src/collision_models.cpp)
 

Modified: pkg/trunk/motion_planning/planning_environment/test/test_robot_models.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/test/test_robot_models.cpp	2009-06-05 19:59:01 UTC (rev 16776)
+++ pkg/trunk/motion_planning/planning_environment/test/test_robot_models.cpp	2009-06-05 19:59:14 UTC (rev 16777)
@@ -35,18 +35,39 @@
 /** \author Ioan Sucan */
 
 #include <planning_environment/robot_models.h>
+#include <ros/time.h>
 #include <gtest/gtest.h>
 #include <iostream>
 #include <sstream>
 
-TEST(Loading, EmptyRobot)
+TEST(Loading, Simple)
 {
     planning_environment::RobotModels m("robotdesc/pr2");
 
     EXPECT_TRUE(m.getKinematicModel().get() != NULL);
 }
 
+TEST(ForwardKinematics, RuntimeArm)
+{
+    planning_environment::RobotModels m("robotdesc/pr2");
+    planning_models::KinematicModel* kmodel = m.getKinematicModel().get();
+    
+    int gid = kmodel->getGroupID("pr2::right_arm");
+    unsigned int dim = kmodel->getGroupDimension(gid);
+    double params[dim];
+    for (unsigned int i = 0 ; i < dim ; ++i)
+	params[dim] = 0.1;
 
+    ros::WallTime tm = ros::WallTime::now();
+    const unsigned int NT = 100000;  
+    for (unsigned int i = 0 ; i < NT ; ++i)
+	kmodel->computeTransformsGroup(params, gid);
+    double fps = (double)NT / (ros::WallTime::now() - tm).toSec();
+    ROS_ERROR("%f forward kinematics steps per second", fps);
+    
+    EXPECT_TRUE(fps > 10000.0);
+}
+
 int main(int argc, char **argv)
 {
     testing::InitGoogleTest(&argc, argv);


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.