Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[9536] pkg/trunk
|
From: <rdi...@us...> - 2009-01-16 09:10:30
|
Revision: 9536
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9536&view=rev
Author: rdiankov
Date: 2009-01-16 09:10:25 +0000 (Fri, 16 Jan 2009)
Log Message:
-----------
moved PhaseSpace messages to robot_msgs and prefixed them with Mocap, made several x86-64 compat fixes, added better detection for optimization options in collision_map, fixed bug in rosarmik
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/calibration/kinematic_calibration/msg/SensorKinematics.msg
pkg/trunk/calibration/kinematic_calibration/src/arm_phasespace_grabber.cpp
pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp
pkg/trunk/drivers/cam/dc1394_cam/CMakeLists.txt
pkg/trunk/drivers/phase_space/CMakeLists.txt
pkg/trunk/drivers/phase_space/manifest.xml
pkg/trunk/drivers/phase_space/phase_space_node.cpp
pkg/trunk/drivers/phase_space/phase_space_node.h
pkg/trunk/drivers/robot/pr2/pr2_phase_space/manifest.xml
pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_fake_localization.cpp
pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_odometry.cpp
pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_pan_tilt.cpp
pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_pose_stamped.cpp
pkg/trunk/mapping/collision_map/CMakeLists.txt
pkg/trunk/mapping/collision_map/src/collision_map.cpp
pkg/trunk/openrave_planning/openraveros/src/openraveros.cpp
pkg/trunk/openrave_planning/openraveros/src/session.h
pkg/trunk/openrave_planning/ormanipulation/data/pr2table.env.xml
pkg/trunk/openrave_planning/ormanipulation/octave/runmanip.m
pkg/trunk/openrave_planning/ormanipulation/octave/runmanip_sim.m
pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m
pkg/trunk/openrave_planning/orrosplanning/manifest.xml
pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h
pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp
pkg/trunk/openrave_planning/orrosplanning/src/plugindefs.h
pkg/trunk/openrave_planning/orrosplanning/src/rosarmik.cpp
pkg/trunk/openrave_planning/orrosplanning/src/rosplanningproblem.h
pkg/trunk/openrave_planning/orrosplanning/src/simplesensorsystem.h
pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp
pkg/trunk/util/robot_self_filter/src/robotlinks_filter_node.cpp
pkg/trunk/vision/checkerboard_detector/checkerboard_detector.cpp
Added Paths:
-----------
pkg/trunk/openrave_planning/orrosplanning/src/mocapsystem.h
pkg/trunk/robot_msgs/msg/MocapBody.msg
pkg/trunk/robot_msgs/msg/MocapMarker.msg
pkg/trunk/robot_msgs/msg/MocapSnapshot.msg
Removed Paths:
-------------
pkg/trunk/drivers/phase_space/msg/
pkg/trunk/openrave_planning/orrosplanning/src/phasespacesystem.h
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/3rdparty/openrave/Makefile 2009-01-16 09:10:25 UTC (rev 9536)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 606
+SVN_REVISION = -r 609
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/calibration/kinematic_calibration/msg/SensorKinematics.msg
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/msg/SensorKinematics.msg 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/calibration/kinematic_calibration/msg/SensorKinematics.msg 2009-01-16 09:10:25 UTC (rev 9536)
@@ -19,4 +19,4 @@
std_msgs/PointCloud laser_cloud
# Data that we captured from phasespace
-phase_space/PhaseSpaceSnapshot phase_space_snapshot
+robot_msgs/MocapSnapshot phase_space_snapshot
Modified: pkg/trunk/calibration/kinematic_calibration/src/arm_phasespace_grabber.cpp
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/src/arm_phasespace_grabber.cpp 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/calibration/kinematic_calibration/src/arm_phasespace_grabber.cpp 2009-01-16 09:10:25 UTC (rev 9536)
@@ -37,7 +37,7 @@
#include "ros/node.h"
#include "rosthread/mutex.h"
#include "robot_msgs/MechanismState.h"
-#include "phase_space/PhaseSpaceSnapshot.h"
+#include "robot_msgs/MocapSnapshot.h"
#include "robot_kinematics/robot_kinematics.h"
#include "kdl/chain.hpp"
@@ -111,7 +111,7 @@
case ' ':
{
printf("Capturing...\n") ;
- phase_space::PhaseSpaceMarker cur_marker ;
+ robot_msgs::MocapMarker cur_marker ;
//GetMarker(cur_marker, marker_id_) ;
@@ -138,7 +138,7 @@
robot_kinematics::RobotKinematics robot_kinematics ;
string robot_desc ;
param("robotdesc/pr2", robot_desc, string("")) ;
- printf("RobotDesc.length() = %u\n", robot_desc.length()) ;
+ printf("RobotDesc.length() = %u\n", (unsigned int)robot_desc.length()) ;
robot_kinematics.loadString(robot_desc.c_str()) ;
@@ -238,7 +238,7 @@
return true ;
}
- void GetMarker(phase_space::PhaseSpaceMarker& marker, int id)
+ void GetMarker(robot_msgs::MocapMarker& marker, int id)
{
bool marker_found = false ;
@@ -304,10 +304,10 @@
}
private:
- phase_space::PhaseSpaceSnapshot snapshot_ ;
+ robot_msgs::MocapSnapshot snapshot_ ;
robot_msgs::MechanismState mech_state_ ;
- phase_space::PhaseSpaceSnapshot safe_snapshot_ ;
+ robot_msgs::MocapSnapshot safe_snapshot_ ;
int marker_id_ ;
robot_msgs::MechanismState safe_mech_state_ ;
Modified: pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp 2009-01-16 09:10:25 UTC (rev 9536)
@@ -37,7 +37,7 @@
#include "ros/node.h"
#include "rosthread/mutex.h"
#include "robot_msgs/MechanismState.h"
-#include "phase_space/PhaseSpaceSnapshot.h"
+#include "robot_msgs/MocapSnapshot.h"
#include "std_msgs/PointCloud.h"
Modified: pkg/trunk/drivers/cam/dc1394_cam/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/cam/dc1394_cam/CMakeLists.txt 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/drivers/cam/dc1394_cam/CMakeLists.txt 2009-01-16 09:10:25 UTC (rev 9536)
@@ -13,6 +13,8 @@
link_directories(${SDL_LIBRARY_DIRS})
rospack_add_library(dc1394cam src/dc1394_cam/dc1394_cam.cpp)
+rospack_add_compile_flags(dc1394cam "-fPIC")
+
# librt is needed for clock_gettime(); not sure how this interacts with
# real-time....
target_link_libraries(dc1394cam rt)
Modified: pkg/trunk/drivers/phase_space/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/phase_space/CMakeLists.txt 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/drivers/phase_space/CMakeLists.txt 2009-01-16 09:10:25 UTC (rev 9536)
@@ -1,7 +1,6 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
rospack(phase_space)
-genmsg()
link_directories(${PROJECT_SOURCE_DIR}/lib)
rospack_add_executable(phase_space_node phase_space_node.cpp)
#rospack_add_executable(phase_space_benchmark phase_space_benchmark.cpp)
Modified: pkg/trunk/drivers/phase_space/manifest.xml
===================================================================
--- pkg/trunk/drivers/phase_space/manifest.xml 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/drivers/phase_space/manifest.xml 2009-01-16 09:10:25 UTC (rev 9536)
@@ -5,6 +5,7 @@
<review status="unreviewed" notes=""/>
<depend package="roscpp" />
<depend package="std_msgs" />
+ <depend package="robot_msgs" />
<depend package="rospy" />
<sysdepend os="ubuntu" version="8.04-hardy" package="libstdc++5"/>
Modified: pkg/trunk/drivers/phase_space/phase_space_node.cpp
===================================================================
--- pkg/trunk/drivers/phase_space/phase_space_node.cpp 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/drivers/phase_space/phase_space_node.cpp 2009-01-16 09:10:25 UTC (rev 9536)
@@ -57,7 +57,7 @@
PhaseSpaceNode::PhaseSpaceNode() : ros::node("phase_space")
{
- advertise<PhaseSpaceSnapshot>("phase_space_snapshot", 48) ;
+ advertise<robot_msgs::MocapSnapshot>("phase_space_snapshot", 48) ;
}
PhaseSpaceNode::~PhaseSpaceNode()
@@ -88,7 +88,7 @@
while (ok()) // While the node has not been shutdown
{
usleep(1) ;
- PhaseSpaceSnapshot snapshot ;
+ robot_msgs::MocapSnapshot snapshot ;
snapshot.header.frame_id = "phase_space" ;
@@ -112,7 +112,7 @@
return true ;
}
-int PhaseSpaceNode::grabMarkers(PhaseSpaceSnapshot& snapshot)
+int PhaseSpaceNode::grabMarkers(robot_msgs::MocapSnapshot& snapshot)
{
OWLMarker markers[MAX_NUM_MARKERS] ;
@@ -147,7 +147,7 @@
return n ;
}
-int PhaseSpaceNode::grabBodies(PhaseSpaceSnapshot& snapshot)
+int PhaseSpaceNode::grabBodies(robot_msgs::MocapSnapshot& snapshot)
{
OWLRigid bodies[MAX_NUM_BODIES] ;
int n = owlGetRigids(bodies, MAX_NUM_BODIES) ;
@@ -173,7 +173,7 @@
return n ;
}
-void PhaseSpaceNode::grabTime(PhaseSpaceSnapshot& snapshot)
+void PhaseSpaceNode::grabTime(robot_msgs::MocapSnapshot& snapshot)
{
int timeVal[3] ;
timeVal[0] = 0 ;
@@ -193,7 +193,7 @@
owlDone() ; // OWL API-call to perform system cleanup before client termination
}
-void PhaseSpaceNode::copyMarkerToMessage(const OWLMarker& owl_marker, PhaseSpaceMarker& msg_marker)
+void PhaseSpaceNode::copyMarkerToMessage(const OWLMarker& owl_marker, robot_msgs::MocapMarker& msg_marker)
{
msg_marker.id = owl_marker.id ;
@@ -204,7 +204,7 @@
msg_marker.condition = owl_marker.cond ;
}
-void PhaseSpaceNode::copyBodyToMessage(const OWLRigid& owl_body, PhaseSpaceBody& msg_body)
+void PhaseSpaceNode::copyBodyToMessage(const OWLRigid& owl_body, robot_msgs::MocapBody& msg_body)
{
msg_body.id = owl_body.id ;
@@ -236,7 +236,7 @@
printf("%s: 0x%x\n", s, n);
}
-void PhaseSpaceNode::dispSnapshot(const PhaseSpaceSnapshot& s)
+void PhaseSpaceNode::dispSnapshot(const robot_msgs::MocapSnapshot& s)
{
printf("rosTF_frame: %s Frame #%u\n", s.header.frame_id.c_str(), s.frameNum) ;
printf(" Time: %lf\n", s.header.stamp.to_double()) ;
Modified: pkg/trunk/drivers/phase_space/phase_space_node.h
===================================================================
--- pkg/trunk/drivers/phase_space/phase_space_node.h 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/drivers/phase_space/phase_space_node.h 2009-01-16 09:10:25 UTC (rev 9536)
@@ -48,9 +48,9 @@
#include "owl.h"
// Messages
-#include "phase_space/PhaseSpaceSnapshot.h"
-#include "phase_space/PhaseSpaceMarker.h"
-#include "phase_space/PhaseSpaceBody.h"
+#include "robot_msgs/MocapSnapshot.h"
+#include "robot_msgs/MocapMarker.h"
+#include "robot_msgs/MocapBody.h"
namespace phase_space
{
@@ -68,13 +68,13 @@
bool spin() ;
private :
- int grabMarkers(PhaseSpaceSnapshot& snapshot) ;
- int grabBodies (PhaseSpaceSnapshot& snapshot) ;
- void grabTime (PhaseSpaceSnapshot& snapshot) ;
- void copyMarkerToMessage(const OWLMarker& owl_marker, PhaseSpaceMarker& msg_marker) ;
- void copyBodyToMessage(const OWLRigid& owl_body, PhaseSpaceBody& msg_body) ;
+ int grabMarkers(robot_msgs::MocapSnapshot& snapshot) ;
+ int grabBodies (robot_msgs::MocapSnapshot& snapshot) ;
+ void grabTime (robot_msgs::MocapSnapshot& snapshot) ;
+ void copyMarkerToMessage(const OWLMarker& owl_marker, robot_msgs::MocapMarker& msg_marker) ;
+ void copyBodyToMessage(const OWLRigid& owl_body, robot_msgs::MocapBody& msg_body) ;
void owlPrintError(const char *s, int n) ;
- void dispSnapshot(const PhaseSpaceSnapshot& snapshot) ;
+ void dispSnapshot(const robot_msgs::MocapSnapshot& snapshot) ;
} ;
}
Modified: pkg/trunk/drivers/robot/pr2/pr2_phase_space/manifest.xml
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2_phase_space/manifest.xml 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/drivers/robot/pr2/pr2_phase_space/manifest.xml 2009-01-16 09:10:25 UTC (rev 9536)
@@ -5,6 +5,7 @@
<review status="unreviewed" notes=""/>
<depend package="roscpp" />
<depend package="std_msgs" />
+ <depend package="robot_msgs" />
<depend package="phase_space" />
<depend package="tf" />
<depend package="pr2_mechanism_controllers" />
Modified: pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_fake_localization.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_fake_localization.cpp 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_fake_localization.cpp 2009-01-16 09:10:25 UTC (rev 9536)
@@ -35,15 +35,15 @@
//! \author Sachin Chitta
/****
- * This node takes the PhaseSpaceSnapshot packet and repackages into a form that can be used with the odometry
+ * This node takes the MocapSnapshot packet and repackages into a form that can be used with the odometry
*/
#include "ros/node.h"
// Messages
-#include "phase_space/PhaseSpaceSnapshot.h"
-#include "phase_space/PhaseSpaceMarker.h"
-#include "phase_space/PhaseSpaceBody.h"
+#include "robot_msgs/MocapSnapshot.h"
+#include "robot_msgs/MocapMarker.h"
+#include "robot_msgs/MocapBody.h"
#include "std_msgs/Transform.h"
#include "std_msgs/RobotBase2DOdom.h"
@@ -101,7 +101,7 @@
{
if (snapshot_.bodies[i].id == base_id_) // Check if we found the robot base in the list of rigid bodies
{
- const phase_space::PhaseSpaceBody& body = snapshot_.bodies[0] ;
+ const robot_msgs::MocapBody& body = snapshot_.bodies[0] ;
// Build Transform Message
tf::Transform mytf;
@@ -150,7 +150,7 @@
private:
- phase_space::PhaseSpaceSnapshot snapshot_;
+ robot_msgs::MocapSnapshot snapshot_;
std_msgs::RobotBase2DOdom m_currentPos;
tf::TransformBroadcaster *m_tfServer;
unsigned int publish_success_count_ ;
Modified: pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_odometry.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_odometry.cpp 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_odometry.cpp 2009-01-16 09:10:25 UTC (rev 9536)
@@ -35,15 +35,15 @@
//! \author Vijay Pradeep
/****
- * This node takes the PhaseSpaceSnapshot packet and repackages into a form that can be used with the odometry
+ * This node takes the MocapSnapshot packet and repackages into a form that can be used with the odometry
*/
#include "ros/node.h"
// Messages
-#include "phase_space/PhaseSpaceSnapshot.h"
-#include "phase_space/PhaseSpaceMarker.h"
-#include "phase_space/PhaseSpaceBody.h"
+#include "robot_msgs/MocapSnapshot.h"
+#include "robot_msgs/MocapMarker.h"
+#include "robot_msgs/MocapBody.h"
#include "std_msgs/TransformWithRateStamped.h"
@@ -84,7 +84,7 @@
private :
- phase_space::PhaseSpaceSnapshot snapshot_ ;
+ robot_msgs::MocapSnapshot snapshot_ ;
int publish_count_ ;
Modified: pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_pan_tilt.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_pan_tilt.cpp 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_pan_tilt.cpp 2009-01-16 09:10:25 UTC (rev 9536)
@@ -35,7 +35,7 @@
//! \author Vijay Pradeep
/****
- * This node takes the PhaseSpaceSnapshot packet and repackages to work with the pan_tilt tracker
+ * This node takes the MocapSnapshot packet and repackages to work with the pan_tilt tracker
*/
#include <string>
@@ -43,9 +43,9 @@
#include "ros/node.h"
// Messages
-#include "phase_space/PhaseSpaceSnapshot.h"
-#include "phase_space/PhaseSpaceMarker.h"
-#include "phase_space/PhaseSpaceBody.h"
+#include "robot_msgs/MocapSnapshot.h"
+#include "robot_msgs/MocapMarker.h"
+#include "robot_msgs/MocapBody.h"
#include "std_msgs/PointStamped.h"
@@ -79,7 +79,7 @@
if (snapshot_.get_markers_size() == 0)
return ;
- phase_space::PhaseSpaceMarker& cur_marker = snapshot_.markers[0] ;
+ robot_msgs::MocapMarker& cur_marker = snapshot_.markers[0] ;
std_msgs::PointStamped target_point ;
target_point.header = snapshot_.header ;
@@ -93,7 +93,7 @@
private :
const string topic ;
- phase_space::PhaseSpaceSnapshot snapshot_ ;
+ robot_msgs::MocapSnapshot snapshot_ ;
int publish_count_ ;
Modified: pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_pose_stamped.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_pose_stamped.cpp 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/drivers/robot/pr2/pr2_phase_space/src/phase_space_pose_stamped.cpp 2009-01-16 09:10:25 UTC (rev 9536)
@@ -35,7 +35,7 @@
//! \author Vijay Pradeep
/****
- * This node takes the PhaseSpaceSnapshot packet and repackages a marker as a point_stamped message
+ * This node takes the MocapSnapshot packet and repackages a marker as a point_stamped message
*/
#include <string>
@@ -43,9 +43,9 @@
#include "ros/node.h"
// Messages
-#include "phase_space/PhaseSpaceSnapshot.h"
-#include "phase_space/PhaseSpaceMarker.h"
-#include "phase_space/PhaseSpaceBody.h"
+#include "robot_msgs/MocapSnapshot.h"
+#include "robot_msgs/MocapMarker.h"
+#include "robot_msgs/MocapBody.h"
#include "std_msgs/PoseStamped.h"
@@ -57,12 +57,12 @@
{
/**
- * \brief Listens for a PhaseSpaceSnapshot message and then publishes it as a PoseStamped message, based on a
+ * \brief Listens for a MocapSnapshot message and then publishes it as a PoseStamped message, based on a
* series a parameters.
* @section topic ROS topics
* Subscribes to (name [type]):
- * - @b "phase_space_snapshot" [phase_space/PhaseSpaceSnapshot] : The current state of the phasespace system,
- * which is normally published by phase_space::PhaseSpaceNode
+ * - @b "phase_space_snapshot" [phase_space/MocapSnapshot] : The current state of the phasespace system,
+ * which is normally published by robot_msgs::MocapSnapshot
*
* Publishes to (name [type]):
* - @b "cmd" [std_msgs/PoseStamped] : The commanded pose, with an associated timestamp and frame_id. You will
@@ -134,7 +134,7 @@
{
if (snapshot_.bodies[i].id == body_id_) // Did we find our body?
{
- const phase_space::PhaseSpaceBody& cur_body = snapshot_.bodies[i] ;
+ const robot_msgs::MocapBody& cur_body = snapshot_.bodies[i] ;
// Define our starting frame
tf::Quaternion rot_phasespace ;
@@ -166,7 +166,7 @@
}
private :
- phase_space::PhaseSpaceSnapshot snapshot_ ;
+ robot_msgs::MocapSnapshot snapshot_ ;
tf::Transform transform_ ;
Modified: pkg/trunk/mapping/collision_map/CMakeLists.txt
===================================================================
--- pkg/trunk/mapping/collision_map/CMakeLists.txt 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/mapping/collision_map/CMakeLists.txt 2009-01-16 09:10:25 UTC (rev 9536)
@@ -1,9 +1,83 @@
cmake_minimum_required(VERSION 2.4.6)
+set( CMAKE_ALLOW_LOOSE_LOOP_CONSTRUCTS TRUE )
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
set(ROS_BUILD_TYPE RelWithDebInfo)
rospack(collision_map)
-add_definitions(-Wall -O3 -DNDEBUG -march=pentium4 -mfpmath=sse -mmmx -msse -msse2 -msse3)
+#add_definitions(-Wall -O3 -DNDEBUG -march=pentium4 -mfpmath=sse -mmmx -msse -msse2 -msse3)
+add_definitions(-Wall -O3 -DNDEBUG)
+
+include(CheckIncludeFile)
+include(CheckLibraryExists)
+include(CheckCXXSourceRuns)
+include(CheckCXXCompilerFlag)
+
+# check for SSE extensions
+if( CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX )
+ set(SSE_FLAGS)
+
+ set(CMAKE_REQUIRED_FLAGS "-msse2")
+ check_cxx_source_runs("
+ #include <emmintrin.h>
+
+ int main()
+ {
+ __m128d a, b;
+ double vals[2] = {0};
+ a = _mm_loadu_pd(vals);
+ b = _mm_add_pd(a,a);
+ _mm_storeu_pd(vals,b);
+ return 0;
+ }"
+ HAS_SSE2_EXTENSIONS)
+
+ set(CMAKE_REQUIRED_FLAGS "-msse")
+ check_cxx_source_runs("
+ #include <xmmintrin.h>
+ int main()
+ {
+ __m128 a, b;
+ float vals[4] = {0};
+ a = _mm_loadu_ps(vals);
+ b = a;
+ b = _mm_add_ps(a,b);
+ _mm_storeu_ps(vals,b);
+ return 0;
+ }"
+ HAS_SSE_EXTENSIONS)
+
+ set(CMAKE_REQUIRED_FLAGS)
+
+ if(HAS_SSE2_EXTENSIONS)
+ message(STATUS "Using SSE2 extensions")
+ set(SSE_FLAGS "-msse2 -mfpmath=sse")
+ elseif(HAS_SSE_EXTENSIONS)
+ message(STATUS "Using SSE extensions")
+ set(SSE_FLAGS "-msse -mfpmath=sse")
+ endif()
+
+ add_definitions(${SSE_FLAGS})
+elseif(MSVC)
+ check_cxx_source_runs("
+ #include <emmintrin.h>
+
+ int main()
+ {
+ __m128d a, b;
+ double vals[2] = {0};
+ a = _mm_loadu_pd(vals);
+ b = _mm_add_pd(a,a);
+ _mm_storeu_pd(vals,b);
+ return 0;
+ }"
+ HAS_SSE2_EXTENSIONS)
+ if( HAS_SSE2_EXTENSIONS )
+ message(STATUS "Using SSE2 extensions")
+ add_definitions( "/arch:SSE2 /fp:fast -D__SSE__ -D__SSE2__" )
+ endif()
+endif()
+
+
genmsg()
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
Modified: pkg/trunk/mapping/collision_map/src/collision_map.cpp
===================================================================
--- pkg/trunk/mapping/collision_map/src/collision_map.cpp 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/mapping/collision_map/src/collision_map.cpp 2009-01-16 09:10:25 UTC (rev 9536)
@@ -172,7 +172,7 @@
ROS_ERROR ("TF not running or wrong TF frame specified! Defaulting to 0,0,0.");
torso_lift_origin = base_origin;
}
- ROS_INFO ("Received %d data points.", cloud_.pts.size ());
+ ROS_INFO ("Received %u data points.", (unsigned int)cloud_.pts.size ());
timeval t1, t2;
gettimeofday (&t1, NULL);
@@ -271,7 +271,7 @@
gettimeofday (&t2, NULL);
double time_spent = t2.tv_sec + (double)t2.tv_usec / 1000000.0 - (t1.tv_sec + (double)t1.tv_usec / 1000000.0);
- ROS_INFO ("Collision map computed in %g seconds. Number of boxes: %d.", time_spent, c_map_.boxes.size ());
+ ROS_INFO ("Collision map computed in %g seconds. Number of boxes: %u.", time_spent, (unsigned int)c_map_.boxes.size ());
publish ("collision_map", c_map_);
}
Modified: pkg/trunk/openrave_planning/openraveros/src/openraveros.cpp
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/openraveros.cpp 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/openrave_planning/openraveros/src/openraveros.cpp 2009-01-16 09:10:25 UTC (rev 9536)
@@ -146,6 +146,6 @@
void sigint_handler(int)
{
- s_sessionserver->selfDestruct();
- s_pmasternode->selfDestruct();
+ s_sessionserver->shutdown();
+ s_pmasternode->shutdown();
}
Modified: pkg/trunk/openrave_planning/openraveros/src/session.h
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/session.h 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/openrave_planning/openraveros/src/session.h 2009-01-16 09:10:25 UTC (rev 9536)
@@ -175,7 +175,7 @@
void Destroy()
{
- selfDestruct();
+ shutdown();
node* pnode = node::instance();
if( pnode == NULL )
@@ -228,7 +228,7 @@
};
}
- virtual void selfDestruct()
+ virtual void shutdown()
{
_ok = false;
boost::mutex::scoped_lock lockcreate(_mutexViewer);
Modified: pkg/trunk/openrave_planning/ormanipulation/data/pr2table.env.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/data/pr2table.env.xml 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/openrave_planning/ormanipulation/data/pr2table.env.xml 2009-01-16 09:10:25 UTC (rev 9536)
@@ -18,9 +18,7 @@
</Body>
</Kinbody>
- <Kinbody file="willow_table.kinbody.xml">
- <translation>2 0 0</translation>
- </Kinbody>
+ <Kinbody file="willow_table.kinbody.xml"/>
<Kinbody name="ricebox0" file="ricebox.kinbody.xml">
<rotationmat>0 0 -1 0 1 0 1 0 0</rotationmat>
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/runmanip.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/runmanip.m 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/runmanip.m 2009-01-16 09:10:25 UTC (rev 9536)
@@ -1,4 +1,7 @@
#!/usr/bin/env octave
+[status,rosoctpath] = system(['rospack find rosoct']);
+rosoctpath = strtrim(rosoctpath);
+addpath(fullfile(rosoctpath, 'octave'));
cd(fullfile(rosoct_findpackage('ormanipulation'),'octave'));
global updir probs
@@ -26,7 +29,7 @@
end
%% phase space system
-out = orProblemSendCommand('createsystem PhaseSpace phase_space_snapshot',probs.task);
+out = orProblemSendCommand('createsystem ROSMocap phase_space_snapshot',probs.task);
if( isempty(out) )
error('failed to create phasespace');
end
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/runmanip_sim.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/runmanip_sim.m 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/runmanip_sim.m 2009-01-16 09:10:25 UTC (rev 9536)
@@ -1,5 +1,8 @@
#!/usr/bin/env octave
global updir probs
+[status,rosoctpath] = system(['rospack find rosoct']);
+rosoctpath = strtrim(rosoctpath);
+addpath(fullfile(rosoctpath, 'octave'));
cd(fullfile(rosoct_findpackage('ormanipulation'),'octave'));
startup;
Modified: pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/openrave_planning/ormanipulation/octave/runperception.m 2009-01-16 09:10:25 UTC (rev 9536)
@@ -1,4 +1,7 @@
#!/usr/bin/env octave
+[status,rosoctpath] = system(['rospack find rosoct']);
+rosoctpath = strtrim(rosoctpath);
+addpath(fullfile(rosoctpath, 'octave'));
cd(fullfile(rosoct_findpackage('ormanipulation'),'octave'));
global updir probs
@@ -26,7 +29,7 @@
end
%% phase space system
-out = orProblemSendCommand('createsystem PhaseSpace phase_space_snapshot',probs.task);
+out = orProblemSendCommand('createsystem ROSMocap phase_space_snapshot',probs.task);
if( isempty(out) )
error('failed to create phasespace');
end
Modified: pkg/trunk/openrave_planning/orrosplanning/manifest.xml
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/manifest.xml 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/openrave_planning/orrosplanning/manifest.xml 2009-01-16 09:10:25 UTC (rev 9536)
@@ -6,7 +6,6 @@
<license>BSD</license>
<depend package="roscpp"/>
<depend package="openrave"/>
- <depend package="phase_space"/>
<depend package="openrave_robot_description"/>
<depend package="libKinematics"/>
<depend package="boost"/>
Modified: pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/openrave_planning/orrosplanning/src/collisionmapsystem.h 2009-01-16 09:10:25 UTC (rev 9536)
@@ -207,7 +207,7 @@
GetEnv()->LockPhysics(false);
- MocapData* pdata = new MocapData();
+ XMLData* pdata = new XMLData();
pdata->strOffsetLink = pbody->GetLinks().front()->GetName();
BODY* b = AddKinBody(pbody, pdata);
if( b == NULL ) {
Copied: pkg/trunk/openrave_planning/orrosplanning/src/mocapsystem.h (from rev 9530, pkg/trunk/openrave_planning/orrosplanning/src/phasespacesystem.h)
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/mocapsystem.h (rev 0)
+++ pkg/trunk/openrave_planning/orrosplanning/src/mocapsystem.h 2009-01-16 09:10:25 UTC (rev 9536)
@@ -0,0 +1,98 @@
+// Software License Agreement (BSD License)
+// Copyright (c) 2008, Rosen Diankov
+// 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.
+// * The name of the author may not 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: Rosen Diankov
+#ifndef PHASESPACE_MOCAP_SYSTEM
+#define PHASESPACE_MOCAP_SYSTEM
+
+#include "rossensorsystem.h"
+#include "robot_msgs/MocapSnapshot.h"
+
+// used to update objects through a mocap system
+class MocapXMLID
+{
+public:
+ static const char* GetXMLId() { return "phasespace"; }
+};
+
+class ROSMocapSystem : public ROSSensorSystem<robot_msgs::MocapSnapshot, MocapXMLID>
+{
+public:
+ ROSMocapSystem(EnvironmentBase* penv)
+ : ROSSensorSystem<robot_msgs::MocapSnapshot, MocapXMLID>(penv)
+ {
+ }
+
+ virtual bool Init(istream& sinput)
+ {
+ _topic = "robot_msgs_snapshot";
+ return ROSSensorSystem<robot_msgs::MocapSnapshot, MocapXMLID>::Init(sinput);
+ }
+
+private:
+ void newdatacb()
+ {
+ list< SNAPSHOT > listbodies;
+ list< const robot_msgs::MocapBody* > listnewbodies;
+
+ {
+ boost::mutex::scoped_lock lock(_mutex);
+
+ for (unsigned int i=0; i<_topicmsg.get_bodies_size(); i++) {
+ const robot_msgs::MocapBody& psbody = _topicmsg.bodies[i];
+
+ boost::shared_ptr<BODY> b;
+ Transform tnew = GetTransform(psbody.pose);
+
+ FOREACH(it, _mapbodies) {
+ if( it->second->_initdata->id == psbody.id ) {
+ b = it->second;
+ break;
+ }
+ }
+
+ if( !b ) {
+ listnewbodies.push_back(&psbody);
+ }
+ else {
+ if( !b->IsEnabled() )
+ continue;
+
+ listbodies.push_back(SNAPSHOT(b, tnew));
+ }
+ }
+
+ UpdateBodies(listbodies);
+ }
+
+ // try to add the left-over objects
+ }
+
+ Transform GetTransform(const std_msgs::Transform& pose)
+ {
+ return Transform(Vector(pose.rotation.w, pose.rotation.x, pose.rotation.y, pose.rotation.z), Vector(pose.translation.x, pose.translation.y, pose.translation.z));
+ }
+};
+
+#endif
Modified: pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/openrave_planning/orrosplanning/src/orrosplanningmain.cpp 2009-01-16 09:10:25 UTC (rev 9536)
@@ -26,7 +26,7 @@
#include "plugindefs.h"
#include "rosarmik.h"
-#include "phasespacesystem.h"
+#include "mocapsystem.h"
#include "objecttransformsystem.h"
#include "collisionmapsystem.h"
#include "rosrobotcontroller.h"
@@ -67,8 +67,8 @@
return new ROSPlanningProblem(penv);
break;
case PT_SensorSystem:
- if( wcsicmp(name, L"PhaseSpace") == 0 )
- return new PhaseSpaceMocapClient(penv);
+ if( wcsicmp(name, L"ROSMocap") == 0 )
+ return new ROSMocapSystem(penv);
else if( wcsicmp(name, L"ObjectTransform") == 0 )
return new ObjectTransformSystem(penv);
else if( wcsicmp(name, L"CollisionMap") == 0 )
@@ -85,16 +85,16 @@
{
if( pinfo == NULL ) return false;
if( size != sizeof(PLUGININFO) ) {
- printf("bad plugin info sizes %d != %d\n", size, sizeof(PLUGININFO));
+ RAVELOG_ERRORA("bad plugin info sizes %d != %d\n", size, sizeof(PLUGININFO));
return false;
}
pinfo->controllers.push_back(L"ROSRobot");
pinfo->iksolvers.push_back(L"ROSArmIK");
- pinfo->sensorsystems.push_back(L"PhaseSpace");
+ pinfo->problems.push_back(L"ROSPlanning");
+ pinfo->sensorsystems.push_back(L"ROSMocap");
pinfo->sensorsystems.push_back(L"ObjectTransform");
pinfo->sensorsystems.push_back(L"CollisionMap");
- pinfo->problems.push_back(L"ROSPlanning");
return true;
}
Deleted: pkg/trunk/openrave_planning/orrosplanning/src/phasespacesystem.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/phasespacesystem.h 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/openrave_planning/orrosplanning/src/phasespacesystem.h 2009-01-16 09:10:25 UTC (rev 9536)
@@ -1,98 +0,0 @@
-// Software License Agreement (BSD License)
-// Copyright (c) 2008, Rosen Diankov
-// 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.
-// * The name of the author may not 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: Rosen Diankov
-#ifndef PHASESPACE_MOCAP_SYSTEM
-#define PHASESPACE_MOCAP_SYSTEM
-
-#include "rossensorsystem.h"
-#include "phase_space/PhaseSpaceSnapshot.h"
-
-// used to update objects through a mocap system
-class PhaseSpaceXMLID
-{
-public:
- static const char* GetXMLId() { return "phasespace"; }
-};
-
-class PhaseSpaceMocapClient : public ROSSensorSystem<phase_space::PhaseSpaceSnapshot, PhaseSpaceXMLID>
-{
-public:
- PhaseSpaceMocapClient(EnvironmentBase* penv)
- : ROSSensorSystem<phase_space::PhaseSpaceSnapshot, PhaseSpaceXMLID>(penv)
- {
- }
-
- virtual bool Init(istream& sinput)
- {
- _topic = "phase_space_snapshot";
- return ROSSensorSystem<phase_space::PhaseSpaceSnapshot, PhaseSpaceXMLID>::Init(sinput);
- }
-
-private:
- void newdatacb()
- {
- list< SNAPSHOT > listbodies;
- list< const phase_space::PhaseSpaceBody* > listnewbodies;
-
- {
- boost::mutex::scoped_lock lock(_mutex);
-
- for (unsigned int i=0; i<_topicmsg.get_bodies_size(); i++) {
- const phase_space::PhaseSpaceBody& psbody = _topicmsg.bodies[i];
-
- boost::shared_ptr<BODY> b;
- Transform tnew = GetTransform(psbody.pose);
-
- FOREACH(it, _mapbodies) {
- if( it->second->_initdata->id == psbody.id ) {
- b = it->second;
- break;
- }
- }
-
- if( !b ) {
- listnewbodies.push_back(&psbody);
- }
- else {
- if( !b->IsEnabled() )
- continue;
-
- listbodies.push_back(SNAPSHOT(b, tnew));
- }
- }
-
- UpdateBodies(listbodies);
- }
-
- // try to add the left-over objects
- }
-
- Transform GetTransform(const std_msgs::Transform& pose)
- {
- return Transform(Vector(pose.rotation.w, pose.rotation.x, pose.rotation.y, pose.rotation.z), Vector(pose.translation.x, pose.translation.y, pose.translation.z));
- }
-};
-
-#endif
Modified: pkg/trunk/openrave_planning/orrosplanning/src/plugindefs.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/plugindefs.h 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/openrave_planning/orrosplanning/src/plugindefs.h 2009-01-16 09:10:25 UTC (rev 9536)
@@ -179,7 +179,7 @@
// start roscpp
ros::node* pnode = ros::node::instance();
- if( pnode && !pnode->check_master() ) {
+ if( pnode && !pnode->checkMaster() ) {
ros::fini();
delete pnode;
return NULL;
@@ -195,7 +195,7 @@
pnode = new ros::node(strname, ros::node::DONT_HANDLE_SIGINT|ros::node::ANONYMOUS_NAME|ros::node::DONT_ADD_ROSOUT_APPENDER);
- bool bCheckMaster = pnode->check_master();
+ bool bCheckMaster = pnode->checkMaster();
ros::fini();
delete pnode;
@@ -215,7 +215,7 @@
inline ros::node* check_roscpp_nocreate()
{
ros::node* pnode = ros::node::instance();
- return (pnode && pnode->check_master()) ? pnode : NULL;
+ return (pnode && pnode->checkMaster()) ? pnode : NULL;
}
#endif
Modified: pkg/trunk/openrave_planning/orrosplanning/src/rosarmik.cpp
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/rosarmik.cpp 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/openrave_planning/orrosplanning/src/rosarmik.cpp 2009-01-16 09:10:25 UTC (rev 9536)
@@ -365,7 +365,7 @@
// find the first valid solutino that satisfies joint constraints and collisions
if( !checkjointangles(vravesol) )
continue;
-
+
// check for self collisions (does WAM ever self-collide?)
_probot->SetActiveDOFValues(NULL, &vravesol[0]);
@@ -467,12 +467,12 @@
const RobotBase::Manipulator* pmanip = _probot->GetActiveManipulator();
if( pmanip == NULL )
return false;
+ assert( _qlower.size() > 2 && _qupper.size() > 2 );
assert( pmanip->_vecarmjoints.size() > 0 && pmanip->_vecarmjoints[2] < _probot->GetDOF());
- assert( _qlower.size() > 0 && _qupper.size() > 0 );
-
+
dReal values[3];
_probot->GetJointFromDOFIndex(pmanip->_vecarmjoints[2])->GetValues(values);
- pFreeParameters[0] = (values[2]-_qlower[2])*fiFreeParam;
+ pFreeParameters[0] = (values[0]-_qlower[2])*fiFreeParam;
return true;
}
Modified: pkg/trunk/openrave_planning/orrosplanning/src/rosplanningproblem.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/rosplanningproblem.h 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/openrave_planning/orrosplanning/src/rosplanningproblem.h 2009-01-16 09:10:25 UTC (rev 9536)
@@ -33,7 +33,7 @@
public:
ROSPlanningProblem(EnvironmentBase* penv) : CmdProblemInstance(penv)
{
- PhaseSpaceMocapClient::RegisterXMLReader(GetEnv());
+ ROSMocapSystem::RegisterXMLReader(GetEnv());
}
virtual ~ROSPlanningProblem() {
Destroy();
Modified: pkg/trunk/openrave_planning/orrosplanning/src/simplesensorsystem.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/simplesensorsystem.h 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/openrave_planning/orrosplanning/src/simplesensorsystem.h 2009-01-16 09:10:25 UTC (rev 9536)
@@ -33,13 +33,13 @@
class SimpleSensorSystem : public SensorSystemBase
{
public:
- class MocapData : public XMLReadable
+ class XMLData : public XMLReadable
{
public:
- MocapData() {}
+ XMLData() {}
virtual const char* GetXMLId() { return XMLID::GetXMLId(); }
- virtual void copy(const MocapData* pdata) {
+ virtual void copy(const XMLData* pdata) {
assert( pdata != NULL );
*this = *pdata;
}
@@ -53,7 +53,7 @@
class BODY : public BODYBASE
{
public:
- BODY(KinBody* pbody, MocapData* pdata) : bPresent(false), bEnabled(true), bLock(false)
+ BODY(KinBody* pbody, XMLData* pdata) : bPresent(false), bEnabled(true), bLock(false)
{
assert( pbody != NULL && pdata != NULL);
_initdata.reset(pdata);
@@ -104,7 +104,7 @@
ros::Time lastupdated;
Transform tnew; ///< most recent transform that is was set
- boost::shared_ptr<MocapData> _initdata;
+ boost::shared_ptr<XMLData> _initdata;
private:
bool bPresent;
@@ -120,14 +120,14 @@
class SimpleXMLReader : public BaseXMLReader
{
public:
- SimpleXMLReader(MocapData* pMocap, const char **atts) {
+ SimpleXMLReader(XMLData* pMocap, const char **atts) {
_pMocap = pMocap;
if( _pMocap == NULL )
- _pMocap = new MocapData();
+ _pMocap = new XMLData();
}
virtual ~SimpleXMLReader() { delete _pMocap; }
- void* Release() { MocapData* temp = _pMocap; _pMocap = NULL; return temp; }
+ void* Release() { XMLData* temp = _pMocap; _pMocap = NULL; return temp; }
virtual void startElement(void *ctx, const char *name, const char **atts) {}
virtual bool endElement(void *ctx, const char *name)
@@ -192,7 +192,7 @@
}
protected:
- MocapData* _pMocap;
+ XMLData* _pMocap;
stringstream ss;
};
@@ -231,7 +231,7 @@
{
// go through all bodies in the environment and check for mocap data
FOREACHC(itbody, vbodies) {
- MocapData* pmocapdata = (MocapData*)((*itbody)->GetExtraInterface(XMLID::GetXMLId()));
+ XMLData* pmocapdata = (XMLData*)((*itbody)->GetExtraInterface(XMLID::GetXMLId()));
if( pmocapdata != NULL ) {
BODY* p = AddKinBody(*itbody, pmocapdata);
if( p != NULL ) {
@@ -248,9 +248,9 @@
return false;
assert( pbody->GetEnv() == GetEnv() );
- const MocapData* pdata = (const MocapData*)_pdata;
+ const XMLData* pdata = (const XMLData*)_pdata;
if( pdata == NULL ) {
- pdata = (const MocapData*)(pbody->GetExtraInterface(XMLID::GetXMLId()));
+ pdata = (const XMLData*)(pbody->GetExtraInterface(XMLID::GetXMLId()));
if( pdata == NULL ) {
RAVELOG_ERRORA("failed to find mocap data for body %S\n", pbody->GetName());
return NULL;
@@ -263,7 +263,7 @@
return NULL;
}
- BODY* b = CreateBODY(pbody, (const MocapData*)pdata);
+ BODY* b = CreateBODY(pbody, (const XMLData*)pdata);
_mapbodies[pbody->GetNetworkId()].reset(b);
RAVELOG_DEBUGA("system adding body %S, total: %d\n", pbody->GetName(), _mapbodies.size());
return b;
@@ -343,9 +343,9 @@
}
protected:
- virtual BODY* CreateBODY(KinBody* pbody, const MocapData* pdata)
+ virtual BODY* CreateBODY(KinBody* pbody, const XMLData* pdata)
{
- MocapData* pnewdata = new MocapData();
+ XMLData* pnewdata = new XMLData();
pnewdata->copy(pdata);
BODY* b = new BODY(pbody, pnewdata);
return b;
@@ -422,7 +422,7 @@
#ifdef RAVE_REGISTER_BOOST
#include BOOST_TYPEOF_INCREMENT_REGISTRATION_GROUP()
-BOOST_TYPEOF_REGISTER_TEMPLATE(SimpleSensorSystem::MocapData, 1)
+BOOST_TYPEOF_REGISTER_TEMPLATE(SimpleSensorSystem::XMLData, 1)
BOOST_TYPEOF_REGISTER_TEMPLATE(SimpleSensorSystem::BODY, 1)
BOOST_TYPEOF_REGISTER_TEMPLATE(SimpleSensorSystem::SimpleXMLReader, 1)
#endif
Modified: pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp
===================================================================
--- pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/robot_descriptions/openrave_robot_description/src/urdf2openrave.cpp 2009-01-16 09:10:25 UTC (rev 9536)
@@ -131,6 +131,7 @@
TiXmlElement *body = new TiXmlElement("body");
body->SetAttribute("name", link->name);
+ printf("name: %s\n", link->name.c_str());
// compute global transform
btTransform localTransform;
Added: pkg/trunk/robot_msgs/msg/MocapBody.msg
===================================================================
--- pkg/trunk/robot_msgs/msg/MocapBody.msg (rev 0)
+++ pkg/trunk/robot_msgs/msg/MocapBody.msg 2009-01-16 09:10:25 UTC (rev 9536)
@@ -0,0 +1,3 @@
+int32 id
+std_msgs/Transform pose
+float32 condition
\ No newline at end of file
Added: pkg/trunk/robot_msgs/msg/MocapMarker.msg
===================================================================
--- pkg/trunk/robot_msgs/msg/MocapMarker.msg (rev 0)
+++ pkg/trunk/robot_msgs/msg/MocapMarker.msg 2009-01-16 09:10:25 UTC (rev 9536)
@@ -0,0 +1,3 @@
+int32 id
+std_msgs/Point location
+float32 condition
\ No newline at end of file
Added: pkg/trunk/robot_msgs/msg/MocapSnapshot.msg
===================================================================
--- pkg/trunk/robot_msgs/msg/MocapSnapshot.msg (rev 0)
+++ pkg/trunk/robot_msgs/msg/MocapSnapshot.msg 2009-01-16 09:10:25 UTC (rev 9536)
@@ -0,0 +1,4 @@
+Header header
+MocapMarker[] markers
+MocapBody[] bodies
+int32 frameNum
\ No newline at end of file
Modified: pkg/trunk/util/robot_self_filter/src/robotlinks_filter_node.cpp
===================================================================
--- pkg/trunk/util/robot_self_filter/src/robotlinks_filter_node.cpp 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/util/robot_self_filter/src/robotlinks_filter_node.cpp 2009-01-16 09:10:25 UTC (rev 9536)
@@ -151,7 +151,7 @@
}
RobotBase* probot = penv->GetRobots().front();
- ROS_INFO("generating convex hulls for robot %S, num links: %d", probot->GetName(), probot->GetLinks().size());
+ ROS_INFO("generating convex hulls for robot %S, num links: %lu", probot->GetName(), probot->GetLinks().size());
ros::Time starthull = ros::Time::now();
_vLinkHulls.resize(probot->GetLinks().size());
@@ -161,7 +161,7 @@
// compute convex hull
if( compute_convex_hull((*itlink)->GetCollisionData().vertices, ithull->vconvexhull) ) {
totalplanes += ithull->vconvexhull.size();
- ROS_DEBUG("link %S convex hull has %d planes", (*itlink)->GetName(), ithull->vconvexhull.size());
+ ROS_DEBUG("link %S convex hull has %lu planes", (*itlink)->GetName(), ithull->vconvexhull.size());
}
else
ROS_ERROR("failed to compute convex hull for link %S", (*itlink)->GetName());
@@ -170,7 +170,7 @@
++ithull;
}
- ROS_INFO("total convex planes: %d, time: %fs", totalplanes, (ros::Time::now()-starthull).toSec());
+ ROS_INFO("total convex planes: %lu, time: %fs", totalplanes, (ros::Time::now()-starthull).toSec());
return true;
}
@@ -459,7 +459,7 @@
ros::init(argc,argv);
s_pmasternode.reset(new ros::node("robobtlinks_filter"));
- if( !s_pmasternode->check_master() )
+ if( !s_pmasternode->checkMaster() )
return -1;
boost::shared_ptr<RobotLinksFilter> plinksfilter(new RobotLinksFilter(robotname, padding, bAccurateTiming));
Modified: pkg/trunk/vision/checkerboard_detector/checkerboard_detector.cpp
===================================================================
--- pkg/trunk/vision/checkerboard_detector/checkerboard_detector.cpp 2009-01-16 08:58:06 UTC (rev 9535)
+++ pkg/trunk/vision/checkerboard_detector/checkerboard_detector.cpp 2009-01-16 09:10:25 UTC (rev 9536)
@@ -81,23 +81,23 @@
string type;
sprintf(str,"grid%d_size_x",index);
- if( !get_param(str,dimx) )
+ if( !getParam(str,dimx) )
break;
sprintf(str,"grid%d_size_y",index);
- if( !get_param(str,dimy) )
+ if( !getParam(str,dimy) )
break;
sprintf(str,"rect%d_size_x",index);
- if( !get_param(str,fRectSize[0]) )
+ if( !getParam(str,fRectSize[0]) )
break;
sprintf(str,"rect%d_size_y",index);
- if( !get_param(str,fRectSize[1]) )
+ if( !getParam(str,fRectSize[1]) )
break;
sprintf(str,"type%d",index);
- if( !get_param(str,type) ) {
+ if( !getParam(str,type) ) {
sprintf(str,"checker%dx%d", dimx, dimy);
type = str;
}
@@ -170,7 +170,7 @@
IplImage *pimggray = _cvbridge.toIpl();
if( display ) {
// copy the raw image
- if( frame != NULL && (frame->width != _caminfomsg.width || frame->height != _caminfomsg.height) ) {
+ if( frame != NULL && (frame->width != (int)_caminfomsg.width || frame->height != (int)_caminfomsg.height) ) {
cvReleaseImage(&frame);
frame = NULL;
}
@@ -183,7 +183,7 @@
vector<checkerboard_detector::Object6DPose> vobjects;
#pragma omp parallel for schedule(dynamic,1)
- for(size_t i = 0; i < vcheckers.size(); ++i) {
+ for(int i = 0; i < (int)vcheckers.size(); ++i) {
CHECKERBOARD& cb = vcheckers[i];
int ncorners;
checkerboard_detector::Object6DPose objpose;
@@ -251,8 +251,8 @@
_objdetmsg.header.frame_id = frame_id;
publish("ObjectDetection", _objdetmsg);
- ROS_INFO("checkerboard: image: %dx%d (size=%d), num: %d, total: %.3fs",_caminfomsg.width,_caminfomsg.height,
- _imagemsg.uint8_data.data.size(), _objdetmsg.get_objects_size(),
+ ROS_INFO("checkerboard: image: %ux%u (size=%u), num: %u, total: %.3fs",_caminfomsg.width,_caminfomsg.height,
+ (unsigned int)_imagemsg.uint8_data.data.size(), (unsigned int)_objdetmsg.get_objects_size(),
(float)(ros::Time::now()-lasttime).toSec());
lasttime = ros::Time::now();
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
