Menu
▾
▴
personalrobots-commit
|
From: <mee...@us...> - 2009-01-21 00:15:49
|
Revision: 9828
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9828&view=rev
Author: meeussen
Date: 2009-01-21 00:15:40 +0000 (Wed, 21 Jan 2009)
Log Message:
-----------
patch to fix issue caused by previous rosrecord patch
Modified Paths:
--------------
pkg/trunk/util/logsetta/carmen/genlog_carmen/genlog_carmen.cpp
pkg/trunk/util/logsetta/imu/imu_extract.cpp
pkg/trunk/util/logsetta/localize_extract/localize_extract.cpp
pkg/trunk/util/logsetta/odom/odom_extract.cpp
pkg/trunk/vision/calib_converter/src/calib_converter.cpp
Modified: pkg/trunk/util/logsetta/carmen/genlog_carmen/genlog_carmen.cpp
===================================================================
--- pkg/trunk/util/logsetta/carmen/genlog_carmen/genlog_carmen.cpp 2009-01-21 00:15:13 UTC (rev 9827)
+++ pkg/trunk/util/logsetta/carmen/genlog_carmen/genlog_carmen.cpp 2009-01-21 00:15:40 UTC (rev 9828)
@@ -42,7 +42,7 @@
FILE *test_log = NULL;
double prev_x = 0, prev_y = 0, prev_th = 0, dumb_rv = 0, dumb_tv = 0, prev_time;
-void odom_callback(string name, std_msgs::RobotBase2DOdom* odom, ros::Time t, void* n)
+void odom_callback(string name, std_msgs::RobotBase2DOdom* odom, ros::Time t, ros::Time t_no_use, void* n)
{
double rel_time = t.to_double();
@@ -84,7 +84,7 @@
dumb_tv, dumb_rv, rel_time, rel_time);
}
-void scan_callback(string name, std_msgs::LaserScan* scan, ros::Time t, void* n)
+void scan_callback(string name, std_msgs::LaserScan* scan, ros::Time t, ros::Time t_no_use, void* n)
{
double rel_time = t.to_double();
Modified: pkg/trunk/util/logsetta/imu/imu_extract.cpp
===================================================================
--- pkg/trunk/util/logsetta/imu/imu_extract.cpp 2009-01-21 00:15:13 UTC (rev 9827)
+++ pkg/trunk/util/logsetta/imu/imu_extract.cpp 2009-01-21 00:15:40 UTC (rev 9828)
@@ -36,7 +36,7 @@
#include <string>
#include "rosrecord/Player.h"
-void imu_callback(std::string name, std_msgs::PoseWithRatesStamped* imu, ros::Time t, void* f)
+void imu_callback(std::string name, std_msgs::PoseWithRatesStamped* imu, ros::Time t, ros::Time t_no_use, void* f)
{
FILE* file = (FILE*)f;
Modified: pkg/trunk/util/logsetta/localize_extract/localize_extract.cpp
===================================================================
--- pkg/trunk/util/logsetta/localize_extract/localize_extract.cpp 2009-01-21 00:15:13 UTC (rev 9827)
+++ pkg/trunk/util/logsetta/localize_extract/localize_extract.cpp 2009-01-21 00:15:40 UTC (rev 9828)
@@ -36,7 +36,7 @@
#include <string>
#include "rosrecord/Player.h"
-void localize_callback(std::string name, std_msgs::RobotBase2DOdom* bL, ros::Time t, void* f)
+void localize_callback(std::string name, std_msgs::RobotBase2DOdom* bL, ros::Time t, ros::Time t_no_use, void* f)
{
FILE* file = (FILE*)f;
Modified: pkg/trunk/util/logsetta/odom/odom_extract.cpp
===================================================================
--- pkg/trunk/util/logsetta/odom/odom_extract.cpp 2009-01-21 00:15:13 UTC (rev 9827)
+++ pkg/trunk/util/logsetta/odom/odom_extract.cpp 2009-01-21 00:15:40 UTC (rev 9828)
@@ -36,7 +36,7 @@
#include <string>
#include "rosrecord/Player.h"
-void odom_callback(std::string name, std_msgs::RobotBase2DOdom* odom, ros::Time t, void* f)
+void odom_callback(std::string name, std_msgs::RobotBase2DOdom* odom, ros::Time t, ros::Time t_no_use, void* f)
{
FILE* file = (FILE*)f;
Modified: pkg/trunk/vision/calib_converter/src/calib_converter.cpp
===================================================================
--- pkg/trunk/vision/calib_converter/src/calib_converter.cpp 2009-01-21 00:15:13 UTC (rev 9827)
+++ pkg/trunk/vision/calib_converter/src/calib_converter.cpp 2009-01-21 00:15:40 UTC (rev 9828)
@@ -35,7 +35,7 @@
}
template <class T>
-void copyMsg(string name, ros::Message* m, ros::Time t, void* n)
+void copyMsg(string name, ros::Message* m, ros::Time t, ros::Time t_no_use, void* n)
{
if (m != 0) {
g_names.insert(name);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <pof...@us...> - 2009-01-21 23:31:30
|
Revision: 9920
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9920&view=rev
Author: poftwaresatent
Date: 2009-01-21 23:31:19 +0000 (Wed, 21 Jan 2009)
Log Message:
-----------
deprecate sbpl_util and mp_benchmarks (#879), use mpglue and mpbench instead
Added Paths:
-----------
pkg/trunk/deprecated/mp_benchmarks/
pkg/trunk/deprecated/sbpl_util/
Removed Paths:
-------------
pkg/trunk/motion_planning/mp_benchmarks/
pkg/trunk/motion_planning/sbpl_util/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2009-01-22 02:16:52
|
Revision: 9954
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9954&view=rev
Author: stuglaser
Date: 2009-01-22 02:16:46 +0000 (Thu, 22 Jan 2009)
Log Message:
-----------
Changed constraint controller to include the torso link and to ignore fixed joints.
Modified Paths:
--------------
pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp
pkg/trunk/demos/arm_gazebo/r_arm_constraint.launch
pkg/trunk/demos/arm_gazebo/r_arm_constraint_controller.xml
pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/r_arm_groups.xml
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp 2009-01-22 01:54:15 UTC (rev 9953)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp 2009-01-22 02:16:46 UTC (rev 9954)
@@ -88,10 +88,10 @@
// some parameters
node->param("constraint/wall_x" , wall_x , 0.6) ; /// location of the wall
- node->param("constraint/threshold_x" , threshold_x , 0.2 ) ; /// distance within the wall to apply constraint force
+ node->param("constraint/threshold_x" , threshold_x , 0.05 ) ; /// distance within the wall to apply constraint force
node->param("constraint/wall_r" , wall_r , 0.2 ) ; /// cylinder radius
node->param("constraint/threshold_r" , threshold_r , 0.05) ; /// radius over with constraint is applied
- node->param("constraint/f_x_max" , f_x_max , -20.0) ; /// max x force
+ node->param("constraint/f_x_max" , f_x_max , -10000.0) ; /// max x force
node->param("constraint/f_y_max" , f_r_max , -20.0) ; /// max r force
@@ -174,8 +174,13 @@
// get the joint positions
JntArray jnt_pos(num_joints_);
+ unsigned int j = 0;
for (unsigned int i=0; i<num_joints_; i++)
- jnt_pos(i) = joints_[i]->position_;
+ {
+ while (joints_[j]->joint_->type_ == mechanism::JOINT_FIXED)
+ ++j;
+ jnt_pos(i) = joints_[j++]->position_;
+ }
// get the chain jacobian
Jacobian jacobian(num_joints_, num_segments_);
@@ -189,6 +194,7 @@
constraint_wrench_ = constraint_jac_ * constraint_force_;
// convert the wrench into joint torques
+ j = 0;
JntArray constraint_torq(num_joints_);
for (unsigned int i=0; i<num_joints_; i++)
{
@@ -197,7 +203,10 @@
{
constraint_torq(i) += (jacobian(j,i) * constraint_wrench_(j));
}
- joints_[i]->commanded_effort_ = constraint_torq(i);
+
+ while (joints_[j]->joint_->type_ == mechanism::JOINT_FIXED)
+ ++j;
+ joints_[j++]->commanded_effort_ = constraint_torq(i);
}
}
@@ -220,7 +229,8 @@
//ROS_ERROR("x_dist_to_wall: %f m\n", (x_dist_to_wall));
if (x_dist_to_wall > 0)
{
- f_x = (exp(x_dist_to_wall)-1) * f_x_max; /// @todo: FIXME, replace with some exponential function
+ //f_x = (exp(x_dist_to_wall)-1) * f_x_max; /// @todo: FIXME, replace with some exponential function
+ f_x = x_dist_to_wall * f_x_max;
//ROS_ERROR("x_dist_to_wall: %f m f_x: %f N\n", x_dist_to_wall, f_x);
if((x_dist_to_wall-threshold_x) > 0)
{
Modified: pkg/trunk/demos/arm_gazebo/r_arm_constraint.launch
===================================================================
--- pkg/trunk/demos/arm_gazebo/r_arm_constraint.launch 2009-01-22 01:54:15 UTC (rev 9953)
+++ pkg/trunk/demos/arm_gazebo/r_arm_constraint.launch 2009-01-22 02:16:46 UTC (rev 9954)
@@ -8,6 +8,7 @@
-->
<!-- send pr2_r_arm.xml to param server -->
+<!--
<param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/r_arm.xacro.xml'" />
<!-- -g flag runs gazebo in gui-less mode -->
@@ -22,8 +23,9 @@
<node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen"/>
+-->
<!-- start arm controller -->
- <node pkg="mechanism_control" type="mech.py" args="sp $(find arm_gazebo)/r_arm_constraint_controller.xml" respawn="false" />
+ <node pkg="mechanism_control" type="spawner.py" args="$(find arm_gazebo)/r_arm_constraint_controller.xml" respawn="false" />
</group>
Modified: pkg/trunk/demos/arm_gazebo/r_arm_constraint_controller.xml
===================================================================
--- pkg/trunk/demos/arm_gazebo/r_arm_constraint_controller.xml 2009-01-22 01:54:15 UTC (rev 9953)
+++ pkg/trunk/demos/arm_gazebo/r_arm_constraint_controller.xml 2009-01-22 02:16:46 UTC (rev 9954)
@@ -1,6 +1,6 @@
<controllers>
<controller type="EndeffectorConstraintControllerNode" name="arm_constraint" topic="arm_constraint">
- <chain root="r_shoulder_pan_link" tip="r_wrist_roll_link" offset="0 0 0" />
+ <chain root="torso_lift_link" tip="r_wrist_roll_link" offset="0 0 0" />
</controller>
</controllers>
Modified: pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/r_arm_groups.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/r_arm_groups.xml 2009-01-22 01:54:15 UTC (rev 9953)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/r_arm_groups.xml 2009-01-22 02:16:46 UTC (rev 9954)
@@ -1,8 +1,9 @@
<?xml version="1.0"?>
<robot name="pr2"><!-- name of the robot-->
-
+
<group name="right_arm" flags="planning kinematic">
+ torso_lift_link
r_shoulder_pan_link
r_shoulder_lift_link
r_upper_arm_roll_link
@@ -14,10 +15,10 @@
<map name="RRT" flag="planning">
<elem key="range" value="0.75" />
</map>
-
+
<map name="LazyRRT" flag="planning">
<elem key="range" value="0.75" />
</map>
</group>
-
+
</robot>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-01-22 02:22:11
|
Revision: 9948
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9948&view=rev
Author: tfoote
Date: 2009-01-22 01:30:15 +0000 (Thu, 22 Jan 2009)
Log Message:
-----------
disambiguating ros::Time constructors
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp
pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/highlevel_controller.hh
pkg/trunk/mapping/collision_map/src/collision_map.cpp
pkg/trunk/nav/trajectory_rollout/src/governor_node.cpp
pkg/trunk/vision/people/src/face_detection.cpp
pkg/trunk/vision/people/src/train_leg_detector.cpp
pkg/trunk/world_models/costmap_2d/src/observation_buffer.cc
pkg/trunk/world_models/map_server/test/rtest.cpp
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp 2009-01-22 01:06:42 UTC (rev 9947)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp 2009-01-22 01:30:15 UTC (rev 9948)
@@ -495,7 +495,7 @@
{
if (publisher_->trylock())
{
- publisher_->msg_.header.stamp = ros::Time((uint64_t) (c_->getTime()*1000000000)) ;
+ publisher_->msg_.header.stamp = ros::Time().fromSec(c_->getTime()) ;
publisher_->msg_.signal = m_scanner_signal_.signal ;
publisher_->unlockAndPublish() ;
need_to_send_msg_ = false ;
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp 2009-01-22 01:06:42 UTC (rev 9947)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_odom.cpp 2009-01-22 01:30:15 UTC (rev 9948)
@@ -146,7 +146,7 @@
libTF::Vector ang_rates;
ros::Time start_time = ros::Time::now();
- ros::Duration sleep_time(0.01);
+ ros::Duration sleep_time = ros::Duration().fromSec(0.01);
std::ofstream odom_log_file;
odom_log_file.open("odom_log.txt");
Modified: pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/highlevel_controller.hh
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/highlevel_controller.hh 2009-01-22 01:06:42 UTC (rev 9947)
+++ pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/highlevel_controller.hh 2009-01-22 01:30:15 UTC (rev 9948)
@@ -413,7 +413,7 @@
ros::Duration diff = desiredCycleEnd - curr;
- if(diff <= ros::Duration(0)){
+ if(diff <= ros::Duration()){
ROS_DEBUG("Missed deadline and not sleeping; check machine load. Started %f, ended %f, wanted end %f. Wanted delta %f, got %f. Tryed to correct %f sec.\n",
loopstart.toSec(), curr.toSec(), desiredCycleEnd.toSec(), cycleTime.toSec(), (curr - loopstart).toSec(), diff.toSec());
} else {
Modified: pkg/trunk/mapping/collision_map/src/collision_map.cpp
===================================================================
--- pkg/trunk/mapping/collision_map/src/collision_map.cpp 2009-01-22 01:06:42 UTC (rev 9947)
+++ pkg/trunk/mapping/collision_map/src/collision_map.cpp 2009-01-22 01:30:15 UTC (rev 9948)
@@ -218,7 +218,7 @@
PointStamped base_origin, torso_lift_origin;
base_origin.point.x = base_origin.point.y = base_origin.point.z = 0.0;
base_origin.header.frame_id = "torso_lift_link";
- base_origin.header.stamp = ros::Time(0.0);
+ base_origin.header.stamp = ros::Time();
try
{
Modified: pkg/trunk/nav/trajectory_rollout/src/governor_node.cpp
===================================================================
--- pkg/trunk/nav/trajectory_rollout/src/governor_node.cpp 2009-01-22 01:06:42 UTC (rev 9947)
+++ pkg/trunk/nav/trajectory_rollout/src/governor_node.cpp 2009-01-22 01:30:15 UTC (rev 9948)
@@ -212,7 +212,7 @@
ros::Duration sleepTime = ros::Time::now() - end;
- if (sleepTime <= ros::Duration(0))
+ if (sleepTime <= ros::Duration())
printf("Governor Node missed deadline and is not sleeping\n");
else
sleepTime.sleep();
Modified: pkg/trunk/vision/people/src/face_detection.cpp
===================================================================
--- pkg/trunk/vision/people/src/face_detection.cpp 2009-01-22 01:06:42 UTC (rev 9947)
+++ pkg/trunk/vision/people/src/face_detection.cpp 2009-01-22 01:30:15 UTC (rev 9948)
@@ -206,7 +206,7 @@
if (it == pos_list_.end()) {
pos_list_.insert(pair<string, RestampedPositionMeasurement>(pos_.object_id, rpm));
}
- else if (pos_.header.stamp - (*it).second.pos.header.stamp > ros::Duration(-1.0) ){
+ else if (pos_.header.stamp - (*it).second.pos.header.stamp > ros::Duration().fromSec(-1.0) ){
(*it).second = rpm;
}
lock.unlock();
Modified: pkg/trunk/vision/people/src/train_leg_detector.cpp
===================================================================
--- pkg/trunk/vision/people/src/train_leg_detector.cpp 2009-01-22 01:06:42 UTC (rev 9947)
+++ pkg/trunk/vision/people/src/train_leg_detector.cpp 2009-01-22 01:30:15 UTC (rev 9948)
@@ -86,7 +86,7 @@
}
ros::record::Player p;
- if (p.open(file, ros::Time(0ull)))
+ if (p.open(file, ros::Time()))
{
mask_.clear();
mask_count_ = 0;
Modified: pkg/trunk/world_models/costmap_2d/src/observation_buffer.cc
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/observation_buffer.cc 2009-01-22 01:06:42 UTC (rev 9947)
+++ pkg/trunk/world_models/costmap_2d/src/observation_buffer.cc 2009-01-22 01:30:15 UTC (rev 9948)
@@ -85,7 +85,7 @@
void ObservationBuffer::get_observations(std::vector<Observation>& observations){
// If the duration is 0, then we just keep the latest one, so we must have a limit of one.
// If the duration is non-zero, we want to delete all the observations.
- unsigned int min_observations = (keep_alive_ == ros::Duration(0)) ? 1 : 0;
+ unsigned int min_observations = (keep_alive_ == ros::Duration()) ? 1 : 0;
while(buffer_.size() > min_observations){
std::list<Observation>::iterator it = buffer_.begin();
Modified: pkg/trunk/world_models/map_server/test/rtest.cpp
===================================================================
--- pkg/trunk/world_models/map_server/test/rtest.cpp 2009-01-22 01:06:42 UTC (rev 9947)
+++ pkg/trunk/world_models/map_server/test/rtest.cpp 2009-01-22 01:30:15 UTC (rev 9948)
@@ -77,7 +77,7 @@
break;
else
{
- ros::Duration d(0.25);
+ ros::Duration d = ros::Duration().fromSec(0.25);
d.sleep();
}
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2009-01-22 17:01:23
|
Revision: 9963
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9963&view=rev
Author: gerkey
Date: 2009-01-22 17:01:19 +0000 (Thu, 22 Jan 2009)
Log Message:
-----------
Fixed libTF / tf build on OS X
Modified Paths:
--------------
pkg/trunk/deprecated/libTF/CMakeLists.txt
pkg/trunk/deprecated/libTF/manifest.xml
pkg/trunk/util/tf/test/cache_unittest.cpp
pkg/trunk/util/tf/test/tf_unittest.cpp
Modified: pkg/trunk/deprecated/libTF/CMakeLists.txt
===================================================================
--- pkg/trunk/deprecated/libTF/CMakeLists.txt 2009-01-22 16:08:54 UTC (rev 9962)
+++ pkg/trunk/deprecated/libTF/CMakeLists.txt 2009-01-22 17:01:19 UTC (rev 9963)
@@ -2,18 +2,18 @@
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
rospack(libTF)
-rospack_add_library(TF src/libTF.cpp src/Pose3DCache.cpp src/Pose3D.cpp)
+rospack_add_library(oldTF src/libTF.cpp src/Pose3DCache.cpp src/Pose3D.cpp)
rospack_add_executable(simpletest src/test/main.cpp)
-target_link_libraries(simpletest TF)
+target_link_libraries(simpletest oldTF)
rospack_add_executable(testtf src/test/testtf.cc)
-target_link_libraries(testtf TF)
+target_link_libraries(testtf oldTF)
rospack_add_executable(test_interp src/test/test_interp.cc)
-target_link_libraries(test_interp TF)
+target_link_libraries(test_interp oldTF)
rospack_add_executable(testMatrix src/test/testMatrix.cc)
-target_link_libraries(testMatrix TF)
+target_link_libraries(testMatrix oldTF)
rospack_add_executable(testMatrixQuaternion src/test/testMatrixQuaternion.cc)
-target_link_libraries(testMatrixQuaternion TF)
+target_link_libraries(testMatrixQuaternion oldTF)
rospack_add_gtest(pose3d_unittest test/pose3d_unittest.cpp src/Pose3D.cpp)
-target_link_libraries(pose3d_unittest TF)
+target_link_libraries(pose3d_unittest oldTF)
rospack_add_gtest(libTF_unittest test/libTF_unittest.cpp)
-target_link_libraries(libTF_unittest TF)
+target_link_libraries(libTF_unittest oldTF)
Modified: pkg/trunk/deprecated/libTF/manifest.xml
===================================================================
--- pkg/trunk/deprecated/libTF/manifest.xml 2009-01-22 16:08:54 UTC (rev 9962)
+++ pkg/trunk/deprecated/libTF/manifest.xml 2009-01-22 17:01:19 UTC (rev 9963)
@@ -15,6 +15,6 @@
<depend package="std_msgs"/>
<depend package="roscpp"/>
<export>
-<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lTF"/>
+<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -loldTF"/>
</export>
</package>
Modified: pkg/trunk/util/tf/test/cache_unittest.cpp
===================================================================
--- pkg/trunk/util/tf/test/cache_unittest.cpp 2009-01-22 16:08:54 UTC (rev 9962)
+++ pkg/trunk/util/tf/test/cache_unittest.cpp 2009-01-22 17:01:19 UTC (rev 9963)
@@ -465,13 +465,13 @@
cache.getData(ros::Time().fromNSec(1), stor);
printf(" stor is %f\n", stor.getOrigin().x());
- EXPECT_TRUE(!isnan(stor.getOrigin().x()));
- EXPECT_TRUE(!isnan(stor.getOrigin().y()));
- EXPECT_TRUE(!isnan(stor.getOrigin().z()));
- EXPECT_TRUE(!isnan(stor.getRotation().x()));
- EXPECT_TRUE(!isnan(stor.getRotation().y()));
- EXPECT_TRUE(!isnan(stor.getRotation().z()));
- EXPECT_TRUE(!isnan(stor.getRotation().w()));
+ EXPECT_TRUE(!std::isnan(stor.getOrigin().x()));
+ EXPECT_TRUE(!std::isnan(stor.getOrigin().y()));
+ EXPECT_TRUE(!std::isnan(stor.getOrigin().z()));
+ EXPECT_TRUE(!std::isnan(stor.getRotation().x()));
+ EXPECT_TRUE(!std::isnan(stor.getRotation().y()));
+ EXPECT_TRUE(!std::isnan(stor.getRotation().z()));
+ EXPECT_TRUE(!std::isnan(stor.getRotation().w()));
}
int main(int argc, char **argv){
Modified: pkg/trunk/util/tf/test/tf_unittest.cpp
===================================================================
--- pkg/trunk/util/tf/test/tf_unittest.cpp 2009-01-22 16:08:54 UTC (rev 9962)
+++ pkg/trunk/util/tf/test/tf_unittest.cpp 2009-01-22 17:01:19 UTC (rev 9963)
@@ -1199,13 +1199,13 @@
tf::Stamped<tf::Transform> output;
try{
mTR.lookupTransform("parent", "b" , ros::Time().fromNSec(4000), output);
- EXPECT_TRUE(!isnan(output.getOrigin().x()));
- EXPECT_TRUE(!isnan(output.getOrigin().y()));
- EXPECT_TRUE(!isnan(output.getOrigin().z()));
- EXPECT_TRUE(!isnan(output.getRotation().x()));
- EXPECT_TRUE(!isnan(output.getRotation().y()));
- EXPECT_TRUE(!isnan(output.getRotation().z()));
- EXPECT_TRUE(!isnan(output.getRotation().w()));
+ EXPECT_TRUE(!std::isnan(output.getOrigin().x()));
+ EXPECT_TRUE(!std::isnan(output.getOrigin().y()));
+ EXPECT_TRUE(!std::isnan(output.getOrigin().z()));
+ EXPECT_TRUE(!std::isnan(output.getRotation().x()));
+ EXPECT_TRUE(!std::isnan(output.getRotation().y()));
+ EXPECT_TRUE(!std::isnan(output.getRotation().z()));
+ EXPECT_TRUE(!std::isnan(output.getRotation().w()));
}
catch (...)
{
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rdi...@us...> - 2009-01-22 17:03:36
|
Revision: 9964
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9964&view=rev
Author: rdiankov
Date: 2009-01-22 17:03:29 +0000 (Thu, 22 Jan 2009)
Log Message:
-----------
64bit portability issues with printing
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/calibration/laser_camera_calibration/manifest.xml
pkg/trunk/openrave_planning/orrosplanning/src/plugindefs.h
pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h
pkg/trunk/util/robot_self_filter/src/robotlinks_filter_node.cpp
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2009-01-22 17:01:19 UTC (rev 9963)
+++ pkg/trunk/3rdparty/openrave/Makefile 2009-01-22 17:03:29 UTC (rev 9964)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 610
+SVN_REVISION = -r 618
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/calibration/laser_camera_calibration/manifest.xml
===================================================================
--- pkg/trunk/calibration/laser_camera_calibration/manifest.xml 2009-01-22 17:01:19 UTC (rev 9963)
+++ pkg/trunk/calibration/laser_camera_calibration/manifest.xml 2009-01-22 17:03:29 UTC (rev 9964)
@@ -9,8 +9,9 @@
<url>http://pr.willowgarage.com/wiki/laser_camera_calibration</url>
<license>BSD</license>
<review status="unreviewed" notes=""/>
- <depend package="robot_msgs" />
- <depend package="std_msgs" />
+ <depend package="robot_msgs"/>
+ <depend package="std_msgs"/>
+ <depend package="rosrecord"/>
<depend package="rospy"/>
<depend package="rosoct"/>
<depend package="openraveros"/>
Modified: pkg/trunk/openrave_planning/orrosplanning/src/plugindefs.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/plugindefs.h 2009-01-22 17:01:19 UTC (rev 9963)
+++ pkg/trunk/openrave_planning/orrosplanning/src/plugindefs.h 2009-01-22 17:03:29 UTC (rev 9964)
@@ -52,8 +52,13 @@
#include <iostream>
#include <sstream>
+#ifdef _MSC_VER
+#define PRIdS "Id"
+#else
+#define PRIdS "zd"
+#endif
+
using namespace std;
-typedef unsigned int u32;
#include <sys/timeb.h> // ftime(), struct timeb
Modified: pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h 2009-01-22 17:01:19 UTC (rev 9963)
+++ pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h 2009-01-22 17:03:29 UTC (rev 9964)
@@ -260,7 +260,7 @@
}
}
else
- RAVELOG_WARNA("number of current values (%d) != desird values (%d)\n", _vcurvalues.size(), vnewvalues.size());
+ RAVELOG_WARNA("number of current values (%"PRIdS") != desird values (%"PRIdS")\n", _vcurvalues.size(), vnewvalues.size());
FOREACH(ittrajcontroller, _listControllers) {
if( !(*ittrajcontroller)->_srvTrajectoryStart ) {
@@ -552,7 +552,7 @@
if( bPopTrajectory ) {
FOREACH(ittraj, _listControllers)
(*ittraj)->_listTrajectories.pop_front();
- RAVELOG_DEBUGA("robot trajectory finished, left: %d\n", _listControllers.front()->_listTrajectories.size());
+ RAVELOG_DEBUGA("robot trajectory finished, left: %"PRIdS"\n", _listControllers.front()->_listTrajectories.size());
}
if( _bIsDone != _listControllers.front()->_listTrajectories.size()==0 ) {
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-22 17:01:19 UTC (rev 9963)
+++ pkg/trunk/util/robot_self_filter/src/robotlinks_filter_node.cpp 2009-01-22 17:03:29 UTC (rev 9964)
@@ -54,6 +54,12 @@
#define FOREACH(it, v) for(typeof((v).begin()) it = (v).begin(); it != (v).end(); (it)++)
+#ifdef _MSC_VER
+#define PRIdS "Id"
+#else
+#define PRIdS "zd"
+#endif
+
boost::shared_ptr<ros::Node> s_pmasternode;
inline string _stdwcstombs(const wchar_t* pname)
@@ -151,7 +157,7 @@
}
RobotBase* probot = penv->GetRobots().front();
- ROS_INFO("generating convex hulls for robot %S, num links: %lu", probot->GetName(), probot->GetLinks().size());
+ ROS_INFO("generating convex hulls for robot %S, num links: %"PRIdS, probot->GetName(), probot->GetLinks().size());
ros::Time starthull = ros::Time::now();
_vLinkHulls.resize(probot->GetLinks().size());
@@ -161,7 +167,7 @@
// compute convex hull
if( compute_convex_hull((*itlink)->GetCollisionData().vertices, ithull->vconvexhull) ) {
totalplanes += ithull->vconvexhull.size();
- ROS_DEBUG("link %S convex hull has %lu planes", (*itlink)->GetName(), ithull->vconvexhull.size());
+ ROS_DEBUG("link %S convex hull has %"PRIdS" planes", (*itlink)->GetName(), ithull->vconvexhull.size());
}
else
ROS_ERROR("failed to compute convex hull for link %S", (*itlink)->GetName());
@@ -170,7 +176,7 @@
++ithull;
}
- ROS_INFO("total convex planes: %lu, time: %fs", totalplanes, (ros::Time::now()-starthull).toSec());
+ ROS_INFO("total convex planes: %"PRIdS", time: %fs", totalplanes, (ros::Time::now()-starthull).toSec());
return true;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2009-01-22 19:20:39
|
Revision: 9972
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9972&view=rev
Author: hsujohnhsu
Date: 2009-01-22 19:20:34 +0000 (Thu, 22 Jan 2009)
Log Message:
-----------
skip requirement for safety_length_min and safety_length_max if limits are not specified.
fix ptz defs, so test will pass.
Modified Paths:
--------------
pkg/trunk/deprecated/wg_robot_description_parser/src/URDF.cpp
pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/ptz_defs.xml
Modified: pkg/trunk/deprecated/wg_robot_description_parser/src/URDF.cpp
===================================================================
--- pkg/trunk/deprecated/wg_robot_description_parser/src/URDF.cpp 2009-01-22 19:09:29 UTC (rev 9971)
+++ pkg/trunk/deprecated/wg_robot_description_parser/src/URDF.cpp 2009-01-22 19:20:34 UTC (rev 9972)
@@ -1218,20 +1218,28 @@
if (loadDoubleValues(node, 1, &joint->velocityLimit, "velocity"))
MARK_SET(node, joint, velocityLimit);
- if (loadDoubleValues(node, 1, joint->safetyLength + 0, "safety_length_min", true))
- MARK_SET(node, joint, safetyLengthMin);
- if (loadDoubleValues(node, 1, joint->safetyLength + 1, "safety_length_max", true))
- MARK_SET(node, joint, safetyLengthMax);
+ // skip safety_length_min/max check if no joint limits specified
+ if (joint->isSet["limit"] || joint->type != Link::Joint::REVOLUTE)
+ {
+ if (loadDoubleValues(node, 1, joint->safetyLength + 0, "safety_length_min", true))
+ MARK_SET(node, joint, safetyLengthMin);
+ if (loadDoubleValues(node, 1, joint->safetyLength + 1, "safety_length_max", true))
+ MARK_SET(node, joint, safetyLengthMax);
+ }
}
else if (node->ValueStr() == "safety_length_min" && !free)
{
if (loadDoubleValues(node, 1, joint->safetyLength + 0, "safety_length", true))
MARK_SET(node, joint, safetyLengthMin);
+ if (!joint->isSet["limit"] && joint->type == Link::Joint::REVOLUTE)
+ errorMessage("setting safe_length_min with no joint limits specified (continuous joint?).");
}
else if (node->ValueStr() == "safety_length_max" && !free)
{
if (loadDoubleValues(node, 1, joint->safetyLength + 1, "safety_length", true))
MARK_SET(node, joint, safetyLengthMax);
+ if (!joint->isSet["limit"] && joint->type == Link::Joint::REVOLUTE)
+ errorMessage("setting safe_length_max with no joint limits specified (continuous joint?).");
}
else if (node->ValueStr() == "mimic" && !free)
{
Modified: pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/ptz_defs.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/ptz_defs.xml 2009-01-22 19:09:29 UTC (rev 9971)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/ptz_defs.xml 2009-01-22 19:20:34 UTC (rev 9972)
@@ -9,7 +9,7 @@
<macro name="pr2_ptz" params="side parent reflect *origin">
<joint name="${side}_ptz_pan_joint" type="revolute">
- <limit min="-M_PI/2" max="M_PI/2" effort="10" velocity="5" k_velocity="1.0" k_position="1.0" />
+ <limit min="-M_PI/2" max="M_PI/2" effort="10" velocity="5" k_velocity="1.0" k_position="1.0" safety_length_min="0" safety_length_max="0" />
<axis xyz="0 0 -1" />
<joint_properties damping="0.01" friction="0.0" />
</joint>
@@ -46,7 +46,7 @@
</link>
<joint name="${side}_ptz_tilt_joint" type="revolute">
- <limit min="-M_PI/2" max="M_PI/2" effort="10" velocity="5" k_velocity="1.0" k_position="1.0" />
+ <limit min="-M_PI/2" max="M_PI/2" effort="10" velocity="5" k_velocity="1.0" k_position="1.0" safety_length_min="0" safety_length_max="0" />
<axis xyz="0 -1 0" />
<anchor xyz="0 0 0" />
<joint_properties damping="0.01" friction="0.0" />
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2009-01-22 19:26:56
|
Revision: 9975
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9975&view=rev
Author: stuglaser
Date: 2009-01-22 19:26:50 +0000 (Thu, 22 Jan 2009)
Log Message:
-----------
Added the cylinder constraint to the endeffector constraint controller
Modified Paths:
--------------
pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp
pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/arm_defs.xml
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp 2009-01-22 19:24:44 UTC (rev 9974)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp 2009-01-22 19:26:50 UTC (rev 9975)
@@ -87,12 +87,12 @@
fprintf(stderr, "Got NULL Chain\n") ;
// some parameters
- node->param("constraint/wall_x" , wall_x , 0.6) ; /// location of the wall
- node->param("constraint/threshold_x" , threshold_x , 0.05 ) ; /// distance within the wall to apply constraint force
+ node->param("constraint/wall_x" , wall_x , 0.8) ; /// location of the wall
+ node->param("constraint/threshold_x" , threshold_x , 0.1 ) ; /// distance within the wall to apply constraint force
node->param("constraint/wall_r" , wall_r , 0.2 ) ; /// cylinder radius
- node->param("constraint/threshold_r" , threshold_r , 0.05) ; /// radius over with constraint is applied
- node->param("constraint/f_x_max" , f_x_max , -10000.0) ; /// max x force
- node->param("constraint/f_y_max" , f_r_max , -20.0) ; /// max r force
+ node->param("constraint/threshold_r" , threshold_r , 0.1) ; /// radius over with constraint is applied
+ node->param("constraint/f_x_max" , f_x_max , -1000.0) ; /// max x force
+ node->param("constraint/f_r_max" , f_r_max , -1000.0) ; /// max r force
// convert description to KDL chain
@@ -217,12 +217,12 @@
double f_x = 0;
double f_r = 0;
- double ee_theta = atan2((endeffector_frame_.p(2)-1),endeffector_frame_.p(1) );
+ double ee_theta = atan2(endeffector_frame_.p(2), endeffector_frame_.p(1));
//Constarint for a cylinder centered around the x axis
constraint_jac_(0,0) = 1; // this is the end of the cylinder
constraint_jac_(1,1) = cos(ee_theta);
- constraint_jac_(2,1) = sin(ee_theta)+1;
+ constraint_jac_(2,1) = sin(ee_theta);
//Constraint Force Vector
double x_dist_to_wall = endeffector_frame_.p(0) - wall_x + threshold_x;
@@ -249,7 +249,7 @@
// assign x-direction constraint force f_x if within range of the wall
if (r_dist_to_wall > 0)
{
- f_r =0;// pow(r_dist_to_wall,3) * f_r_max;
+ f_r = r_dist_to_wall * f_r_max; // pow(r_dist_to_wall,3) * f_r_max;
if(r_dist_to_wall > threshold_r)
{
//ROS_ERROR("wall radius breach! by: %f m\n", (r_dist_to_wall-threshold_r));
@@ -318,7 +318,7 @@
std_msgs::VisualizationMarker marker;
marker.header.frame_id = "torso_lift_link";
marker.id = 0;
- marker.type = 1;
+ marker.type = std_msgs::VisualizationMarker::CUBE;
marker.action = 0;
marker.x = 0.6;
marker.y = 0;
@@ -342,7 +342,7 @@
std_msgs::VisualizationMarker marker;
marker.header.frame_id = "torso_lift_link";
marker.id = 1;
- marker.type = 2;
+ marker.type = std_msgs::VisualizationMarker::SPHERE;
marker.action = 0;
marker.x = 0.6;
marker.y = 0;
@@ -351,8 +351,8 @@
marker.pitch = 0;
marker.roll = 0.0;
marker.xScale = 0.01;
- marker.yScale = 0.2;
- marker.zScale = 0.2;
+ marker.yScale = 0.5;
+ marker.zScale = 0.5;
marker.alpha = 200;
marker.r = 255;
marker.g = 0;
Modified: pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/arm_defs.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/arm_defs.xml 2009-01-22 19:24:44 UTC (rev 9974)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/defs/arm_defs.xml 2009-01-22 19:26:50 UTC (rev 9975)
@@ -96,7 +96,7 @@
<limit min="-0.4" max="1.5" effort="30" velocity="${VELOCITY_LIMIT_SCALE*3.47}"
k_position="100" k_velocity="10"
- safety_length_min="0.1" safety_length_max="0.1" />
+ safety_length_min="0.05" safety_length_max="0.05" />
<calibration reference_position="shoulder_lift_cal" values="1.5 -1 " />
<joint_properties damping="100.0" />
@@ -272,7 +272,7 @@
<limit min="-2.3" max="0.1" effort="30" velocity="${VELOCITY_LIMIT_SCALE*5.5}"
k_position="100" k_velocity="3"
- safety_length_min="0.4" safety_length_max="0.1" />
+ safety_length_min="0.25" safety_length_max="0.1" />
<calibration reference_position="${-1.1+elbow_flex_cal}" values="1.5 -1" />
<joint_properties damping="10.0" />
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-01-22 20:31:06
|
Revision: 9983
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9983&view=rev
Author: sfkwc
Date: 2009-01-22 20:30:57 +0000 (Thu, 22 Jan 2009)
Log Message:
-----------
string_utils deprecated, per pr_utils review
Added Paths:
-----------
pkg/trunk/deprecated/string_utils/
Removed Paths:
-------------
pkg/trunk/util/string_utils/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-01-22 20:31:34
|
Revision: 9984
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9984&view=rev
Author: sfkwc
Date: 2009-01-22 20:31:25 +0000 (Thu, 22 Jan 2009)
Log Message:
-----------
misc_utils deprecated, per pr_utils review
Added Paths:
-----------
pkg/trunk/deprecated/misc_utils/
Removed Paths:
-------------
pkg/trunk/util/misc_utils/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-01-22 20:32:34
|
Revision: 9985
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9985&view=rev
Author: sfkwc
Date: 2009-01-22 20:32:29 +0000 (Thu, 22 Jan 2009)
Log Message:
-----------
random_utils deprecated per pr_utils review
Added Paths:
-----------
pkg/trunk/deprecated/random_utils/
Removed Paths:
-------------
pkg/trunk/util/random_utils/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-01-22 20:33:32
|
Revision: 9987
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9987&view=rev
Author: sfkwc
Date: 2009-01-22 20:33:22 +0000 (Thu, 22 Jan 2009)
Log Message:
-----------
deprecating stl_utils per pr_utils review
Added Paths:
-----------
pkg/trunk/deprecated/stl_utils/
Removed Paths:
-------------
pkg/trunk/util/stl_utils/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-01-22 22:28:15
|
Revision: 10002
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10002&view=rev
Author: sfkwc
Date: 2009-01-22 21:47:45 +0000 (Thu, 22 Jan 2009)
Log Message:
-----------
marking cv_wrappers deprecated, per review
Added Paths:
-----------
pkg/trunk/deprecated/cv_wrappers/
Removed Paths:
-------------
pkg/trunk/vision/cv_wrappers/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-01-22 23:30:14
|
Revision: 10010
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10010&view=rev
Author: isucan
Date: 2009-01-22 23:30:12 +0000 (Thu, 22 Jan 2009)
Log Message:
-----------
service to enable/disable collision checking
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h
pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environment.h
pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environmentODE.h
pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environment.cpp
pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environmentODE.cpp
Added Paths:
-----------
pkg/trunk/robot_srvs/srv/CollisionCheckState.srv
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h 2009-01-22 23:11:39 UTC (rev 10009)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h 2009-01-22 23:30:12 UTC (rev 10010)
@@ -36,6 +36,7 @@
#include "kinematic_planning/KinematicStateMonitor.h"
#include <std_msgs/PointCloud.h>
+#include <robot_srvs/CollisionCheckState.h>
#include <collision_space/environmentODE.h>
#include <collision_map/CollisionMap.h>
@@ -103,6 +104,8 @@
m_collisionSpace->addStaticPlane(0.0, 0.0, 1.0, -0.01);
m_node->subscribe("collision_map", m_collisionMap, &CollisionSpaceMonitor::collisionMapCallback, this, 1);
+ m_node->advertiseService("set_collision_state", &CollisionSpaceMonitor::setCollisionState, this);
+
}
virtual ~CollisionSpaceMonitor(void)
@@ -114,6 +117,22 @@
}
}
+ bool setCollisionState(robot_srvs::CollisionCheckState::request &req, robot_srvs::CollisionCheckState::response &res)
+ {
+ m_collisionSpace->lock();
+ int model_id = m_collisionSpace->getModelID(req.robot_name);
+ if (model_id >= 0)
+ res.value = m_collisionSpace->setCollisionCheck(model_id, req.link_name, req.value ? true : false);
+ else
+ res.value = -1;
+ m_collisionSpace->unlock();
+ if (res.value == -1)
+ ROS_WARN("Unable to change collision checking state for link '%s' on '%s'", req.link_name.c_str(), req.robot_name.c_str());
+ else
+ ROS_INFO("Collision checking for link '%s' on '%s' is now %s", req.link_name.c_str(), req.robot_name.c_str(), res.value ? "enabled" : "disabled");
+ return true;
+ }
+
virtual void setRobotDescription(robot_desc::URDF *file)
{
KinematicStateMonitor::setRobotDescription(file);
Added: pkg/trunk/robot_srvs/srv/CollisionCheckState.srv
===================================================================
--- pkg/trunk/robot_srvs/srv/CollisionCheckState.srv (rev 0)
+++ pkg/trunk/robot_srvs/srv/CollisionCheckState.srv 2009-01-22 23:30:12 UTC (rev 10010)
@@ -0,0 +1,18 @@
+# This service is used to enable / disable the collision checking for
+# various links. The returned value is the old state of the collision
+# checking
+
+# The robot which contains the link
+string robot_name
+
+# The link name
+string link_name
+
+# The new state for collision checking on this link
+byte value
+
+---
+
+# If succesful, the old state of collision checking (0 or 1)
+# If not succesful, -1
+byte value
Modified: pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environment.h
===================================================================
--- pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environment.h 2009-01-22 23:11:39 UTC (rev 10009)
+++ pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environment.h 2009-01-22 23:30:12 UTC (rev 10010)
@@ -99,13 +99,19 @@
/** Add a group of links to be checked for self collision */
virtual void addSelfCollisionGroup(unsigned int model_id, std::vector<std::string> &links) = 0;
-
+
+ /** Enable/Disable collision checking for specific links. Return the previous value of the state (1 or 0) if succesful; -1 otherwise */
+ virtual int setCollisionCheck(unsigned int model_id, std::string &link, bool state) = 0;
+
/** Get the number of loaded models */
unsigned int getModelCount(void) const;
/** Get a specific model */
planning_models::KinematicModel* getRobotModel(unsigned int model_id) const;
+ /** Get the model ID based on the model (robot) name; returns -1 if model not found. */
+ int getModelID(const std::string& robot_name) const;
+
/** Provide interface to a lock. Use carefully! */
void lock(void);
Modified: pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environmentODE.h
===================================================================
--- pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environmentODE.h 2009-01-22 23:11:39 UTC (rev 10009)
+++ pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environmentODE.h 2009-01-22 23:30:12 UTC (rev 10010)
@@ -106,8 +106,8 @@
/** Add a group of links to be checked for self collision */
virtual void addSelfCollisionGroup(unsigned int model_id, std::vector<std::string> &links);
- /** Enable/Disable collision checking for specific links */
- virtual void setCollisionCheck(unsigned int model_id, std::string &link, bool state);
+ /** Enable/Disable collision checking for specific links. Return the previous value of the state (1 or 0) if succesful; -1 otherwise */
+ virtual int setCollisionCheck(unsigned int model_id, std::string &link, bool state);
protected:
Modified: pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environment.cpp
===================================================================
--- pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environment.cpp 2009-01-22 23:11:39 UTC (rev 10009)
+++ pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environment.cpp 2009-01-22 23:30:12 UTC (rev 10010)
@@ -69,6 +69,14 @@
return m_models.size();
}
+int collision_space::EnvironmentModel::getModelID(const std::string& robot_name) const
+{
+ for (unsigned int i = 0 ; i < m_models.size() ; ++i)
+ if (m_models[i]->name == robot_name)
+ return i;
+ return -1;
+}
+
planning_models::KinematicModel* collision_space::EnvironmentModel::getRobotModel(unsigned int model_id) const
{
return m_models[model_id];
Modified: pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environmentODE.cpp
===================================================================
--- pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environmentODE.cpp 2009-01-22 23:11:39 UTC (rev 10009)
+++ pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environmentODE.cpp 2009-01-22 23:30:12 UTC (rev 10010)
@@ -465,17 +465,20 @@
}
}
-void collision_space::EnvironmentModelODE::setCollisionCheck(unsigned int model_id, std::string &link, bool state)
+int collision_space::EnvironmentModelODE::setCollisionCheck(unsigned int model_id, std::string &link, bool state)
{
+ int result = -1;
if (model_id < m_modelsGeom.size())
{
for (unsigned int j = 0 ; j < m_modelsGeom[model_id].linkGeom.size() ; ++j)
{
if (m_modelsGeom[model_id].linkGeom[j]->link->name == link)
{
+ result = m_modelsGeom[model_id].linkGeom[j]->enabled ? 1 : 0;
m_modelsGeom[model_id].linkGeom[j]->enabled = state;
break;
}
}
}
+ return result;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2009-01-23 02:30:40
|
Revision: 10036
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10036&view=rev
Author: gerkey
Date: 2009-01-23 02:30:34 +0000 (Fri, 23 Jan 2009)
Log Message:
-----------
Added /global_frame_id parameter, which is retrieved by nav_view and move_base
to set the "global" frame. By default, it's "map," but it can now be
changed. Required changes in observation buffers to pass down the frame.
Modified Paths:
--------------
pkg/trunk/highlevel/executive_python/manifest.xml
pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/move_base.hh
pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp
pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp
pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h
pkg/trunk/nav/nav_view/src/nav_view/tools.cpp
pkg/trunk/world_models/costmap_2d/include/costmap_2d/basic_observation_buffer.h
pkg/trunk/world_models/costmap_2d/include/costmap_2d/observation_buffer.h
pkg/trunk/world_models/costmap_2d/src/basic_observation_buffer.cc
pkg/trunk/world_models/costmap_2d/src/observation_buffer.cc
pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc
Added Paths:
-----------
pkg/trunk/demos/2dnav_stage/2dnav_stage_movebase_empty_room.launch
pkg/trunk/demos/2dnav_stage/maps/willow-empty-0.05.pgm
pkg/trunk/demos/2dnav_stage/worlds/willow-gps-pr2-5cm.world
Added: pkg/trunk/demos/2dnav_stage/2dnav_stage_movebase_empty_room.launch
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_movebase_empty_room.launch (rev 0)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_movebase_empty_room.launch 2009-01-23 02:30:34 UTC (rev 10036)
@@ -0,0 +1,63 @@
+<launch>
+
+ <group name="wg">
+ <node pkg="rosstage" type="rosstage" args="$(find 2dnav_stage)/worlds/willow-gps-pr2-5cm.world" respawn="false" output="screen"/>
+ <node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-empty-0.05.pgm 0.05" respawn="false" output="screen"/>
+ <param name="robot_radius" value="0.325"/>
+ <!-- node pkg="wavefront_player" type="wavefront_player" respawn="false" output="screen">
+ <remap from="scan" to="base_scan" />
+ </node -->
+ <node pkg="nav_view" type="nav_view" respawn="false" output="screen"/>
+ <!--node pkg="nav_view_sdl" type="nav_view" respawn="false" output="screen"/> -->
+
+ <param name="move_base/environmentList" value="2D,3DKIN"/> <!-- future extension -->
+ <param name="move_base/plannerList" value="ADPlanner,ARAPlanner"/> <!-- future extension -->
+ <param name="move_base/plannerType" value="ARAPlanner"/>
+ <param name="move_base/environmentType" value="2D"/>
+ <param name="move_base/controller_frequency" value="30.0"/>
+ <param name="move_base/planner_frequency" value="0.0"/>
+ <param name="move_base/plannerTimeLimit" value="0.5"/>
+ <param name="move_base/map_update_frequency" value="5.0"/>
+ <param name="/costmap_2d/base_laser_max_range" value="20.0"/>
+ <param name="/costmap_2d/tilt_laser_max_range" value="2.0"/>
+ <param name="/costmap_2d/lethal_obstacle_threshold" value="100.0"/>
+ <param name="/costmap_2d/no_information_value" value="255"/>
+ <param name="/costmap_2d/z_threshold_max" value="2.0"/>
+ <param name="/costmap_2d/z_threshold_min" value="0.13"/>
+ <param name="/costmap_2d/freespace_projection_height" value="2.0"/>
+ <param name="/costmap_2d/inflation_radius" value="0.65"/>
+ <param name="/costmap_2d/circumscribed_radius" value="0.46"/>
+ <param name="/costmap_2d/inscribed_radius" value="0.325"/>
+ <param name="/costmap_2d/raytrace_window" value="10.0"/>
+ <param name="/costmap_2d/weight" value="0.1"/>
+
+ <param name="/global_frame_id" value="odom"/>
+
+ <!-- Forces a check that we are receiving base scans that are correctly up to date at a rate of at least 10 Hz -->
+ <param name="/costmap_2d/base_laser_update_rate" value="2.0"/>
+
+ <!-- Don't Forces a check that we are receiving tilt scans that are correctly up to date at a rate of at least 10 Hz -->
+ <param name="/costmap_2d/tilt_laser_update_rate" value="0.0"/>
+
+ <!-- we don't have stereo data yet -->
+ <param name="/costmap_2d/stereo_update_rate" value="0.0"/>
+
+ <!--- Don't check for ground plane obstacles -->
+ <param name="/costmap_2d/low_obstacle_update_rate" value="0.0"/>
+
+ <param name="/trajectory_rollout/map_size" value="4.0" />
+ <param name="/trajectory_rollout/sim_time" value="1.0" />
+ <param name="/trajectory_rollout/sim_steps" value="20" />
+ <param name="/trajectory_rollout/samples_per_dim" value="25" />
+ <param name="/trajectory_rollout/occdist_scale" value="0.2" />
+ <param name="/trajectory_rollout/path_distance_bias" value="0.6" />
+ <param name="/trajectory_rollout/goal_distance_bias" value="0.8" />
+ <param name="/trajectory_rollout/acc_limit_x" value="2.5" />
+ <param name="/trajectory_rollout/acc_limit_y" value="2.5" />
+ <param name="/trajectory_rollout/acc_limit_th" value="3.2" />
+
+ <node pkg="highlevel_controllers" type="move_base_navfn" respawn="true" output="screen"/>
+
+ </group>
+</launch>
+
Added: pkg/trunk/demos/2dnav_stage/maps/willow-empty-0.05.pgm
===================================================================
(Binary files differ)
Property changes on: pkg/trunk/demos/2dnav_stage/maps/willow-empty-0.05.pgm
___________________________________________________________________
Added: svn:mime-type
+ application/octet-stream
Added: pkg/trunk/demos/2dnav_stage/worlds/willow-gps-pr2-5cm.world
===================================================================
--- pkg/trunk/demos/2dnav_stage/worlds/willow-gps-pr2-5cm.world (rev 0)
+++ pkg/trunk/demos/2dnav_stage/worlds/willow-gps-pr2-5cm.world 2009-01-23 02:30:34 UTC (rev 10036)
@@ -0,0 +1,71 @@
+define block model
+(
+ size3 [0.5 0.5 0.75]
+ gui_nose 0
+)
+
+define topurg laser
+(
+ range_min 0.0
+ range_max 30.0
+ fov 270.25
+ samples 1081
+ # generic model properties
+ color "black"
+ size [ 0.05 0.05 0.1 ]
+)
+
+define pr2 position
+(
+ size3 [0.65 0.65 0.25]
+ origin3 [-0.05 0 0 0]
+ gui_nose 1
+ drive "omni"
+ localization "gps"
+ localization_origin [3 3 90]
+ topurg(pose [0.275 0.000 0.000])
+)
+
+define floorplan model
+(
+ # sombre, sensible, artistic
+ color "gray30"
+
+ # most maps will need a bounding box
+ boundary 1
+
+ gui_nose 0
+ gui_grid 0
+ gui_movemask 0
+ gui_outline 0
+ gripper_return 0
+ fiducial_return 0
+ laser_return 1
+)
+
+# set the resolution of the underlying raytrace model in meters
+resolution 0.02
+
+interval_sim 100 # simulation timestep in milliseconds
+interval_real 100 # real-time interval between simulation updates in milliseconds
+
+window
+(
+ size [ 745.000 448.000 ]
+ center [275.990 494.960]
+ rotate [ 0.000 -1.560 ]
+ scale 18.806
+)
+
+# load an environment bitmap
+floorplan
+(
+ name "willow"
+ bitmap "../maps/willow-full-0.05.pgm"
+ size3 [58.25 47.25 1.0]
+ pose [-23.625 29.125 90.000]
+)
+
+# throw in a robot
+pr2( pose [-28.610 13.562 99.786] name "pr2" color "blue")
+block( pose [-25.062 12.909 180.000] color "red")
Modified: pkg/trunk/highlevel/executive_python/manifest.xml
===================================================================
--- pkg/trunk/highlevel/executive_python/manifest.xml 2009-01-23 02:24:22 UTC (rev 10035)
+++ pkg/trunk/highlevel/executive_python/manifest.xml 2009-01-23 02:30:34 UTC (rev 10036)
@@ -5,6 +5,7 @@
<review status="unreviewed" notes=""/>
<depend package="rospy" />
<depend package="highlevel_controllers" />
+ <depend package="executive_trex_pr2" />
<depend package="std_msgs" />
<depend package="robot_msgs" />
</package>
Modified: pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/move_base.hh
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/move_base.hh 2009-01-23 02:24:22 UTC (rev 10035)
+++ pkg/trunk/highlevel/highlevel_controllers/include/highlevel_controllers/move_base.hh 2009-01-23 02:30:34 UTC (rev 10036)
@@ -225,6 +225,8 @@
tf::TransformListener tf_; /**< Used to do transforms */
+ std::string global_frame_; /**< Which is our global frame? Usually "map" */
+
// Observation Buffers are dynamically allocated since their constructors take
// arguments bound by lookup to the param server. This could be chnaged with some reworking of how paramaters
// are looked up. If we wanted to generalize this node further, we could use a factory pattern to dynamically
Modified: pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp 2009-01-23 02:24:22 UTC (rev 10035)
+++ pkg/trunk/highlevel/highlevel_controllers/src/move_base.cpp 2009-01-23 02:30:34 UTC (rev 10036)
@@ -89,6 +89,8 @@
double circumscribedRadius(0.46);
double inscribedRadius(0.325);
double weight(0.1); // Scale costs down by a factor of 10
+ // Which frame is "global"
+ param("/global_frame_id", global_frame_, std::string("map"));
param("/costmap_2d/base_laser_max_range", baseLaserMaxRange_, baseLaserMaxRange_);
param("/costmap_2d/tilt_laser_max_range", tiltLaserMaxRange_, tiltLaserMaxRange_);
@@ -160,19 +162,19 @@
}
// Then allocate observation buffers
- baseScanBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("base_laser"), tf_,
+ baseScanBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("base_laser"), global_frame_, tf_,
ros::Duration().fromSec(base_laser_keepalive),
costmap_2d::BasicObservationBuffer::computeRefreshInterval(base_laser_update_rate),
inscribedRadius, minZ_, maxZ_, filter_);
- tiltScanBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("laser_tilt_link"), tf_,
+ tiltScanBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("laser_tilt_link"), global_frame_, tf_,
ros::Duration().fromSec(tilt_laser_keepalive),
costmap_2d::BasicObservationBuffer::computeRefreshInterval(tilt_laser_update_rate),
inscribedRadius, minZ_, maxZ_, filter_);
- lowObstacleBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("odom_combined"), tf_,
+ lowObstacleBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("odom_combined"), global_frame_, tf_,
ros::Duration().fromSec(low_obstacle_keepalive),
costmap_2d::BasicObservationBuffer::computeRefreshInterval(low_obstacle_update_rate),
inscribedRadius, -10.0, maxZ_, filter_);
- stereoCloudBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("stereo_link"), tf_,
+ stereoCloudBuffer_ = new costmap_2d::BasicObservationBuffer(std::string("stereo_link"), global_frame_, tf_,
ros::Duration().fromSec(stereo_keepalive),
costmap_2d::BasicObservationBuffer::computeRefreshInterval(stereo_update_rate),
inscribedRadius, minZ_, maxZ_, filter_);
@@ -305,7 +307,7 @@
subscribe("base_scan", baseScanMsg_, &MoveBase::baseScanCallback, 1);
tiltLaserNotifier_ = new tf::MessageNotifier<std_msgs::PointCloud>(&tf_, this,
boost::bind(&MoveBase::tiltCloudCallback, this, _1),
- "tilt_laser_cloud_filtered", "map", 50);
+ "tilt_laser_cloud_filtered", global_frame_, 50);
subscribe("dcam/cloud", stereoCloudMsg_, &MoveBase::stereoCloudCallback, 1);
subscribe("ground_plane", groundPlaneMsg_, &MoveBase::groundPlaneCallback, 1);
subscribe("obstacle_cloud", groundPlaneCloudMsg_, &MoveBase::groundPlaneCloudCallback, 1);
@@ -353,7 +355,7 @@
robotPose.stamp_ = ros::Time();
try{
- tf_.transformPose("map", robotPose, global_pose_);
+ tf_.transformPose(global_frame_, robotPose, global_pose_);
}
catch(tf::LookupException& ex) {
ROS_INFO("No Transform available Error\n");
@@ -391,7 +393,7 @@
goalPose.stamp_ = ros::Time();
try{
- tf_.transformPose("map", goalPose, transformedGoalPose);
+ tf_.transformPose(global_frame_, goalPose, transformedGoalPose);
}
catch(tf::LookupException& ex){
ROS_ERROR("No transform available from %s to map. This may be because the frame_id of the goalMsg is wrong.\n", goalMsg.header.frame_id.c_str());
@@ -743,7 +745,7 @@
target_point.point.y = pty;
target_point.point.z = 1;
target_point.header.stamp = ros::Time::now();
- target_point.header.frame_id = "map";
+ target_point.header.frame_id = global_frame_;
publish("head_controller/head_track_point", target_point);
return planOk;
}
Modified: pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp
===================================================================
--- pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp 2009-01-23 02:24:22 UTC (rev 10035)
+++ pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.cpp 2009-01-23 02:30:34 UTC (rev 10036)
@@ -114,6 +114,8 @@
render_panel_->getViewport()->setCamera( camera_ );
+ ros_node_->param("/global_frame_id", global_frame_id_, std::string("map"));
+
ros_node_->advertise<robot_msgs::Planner2DGoal>("goal", 1);
ros_node_->advertise<std_msgs::Pose2DFloat32>("initialpose", 1);
ros_node_->subscribe("particlecloud", cloud_, &NavViewPanel::incomingParticleCloud, this, 1);
@@ -434,7 +436,7 @@
tf::Stamped<tf::Pose> robot_pose(btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), ros::Time(), "base_link");
tf::Stamped<tf::Pose> map_pose;
- tf_client_->transformPose("map", robot_pose, map_pose);
+ tf_client_->transformPose("odom", robot_pose, map_pose);
double yaw, pitch, roll;
map_pose.getBasis().getEulerZYX(yaw, pitch, roll);
Modified: pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h
===================================================================
--- pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h 2009-01-23 02:24:22 UTC (rev 10035)
+++ pkg/trunk/nav/nav_view/src/nav_view/nav_view_panel.h 2009-01-23 02:30:34 UTC (rev 10036)
@@ -156,6 +156,7 @@
float getMapResolution() { return map_resolution_; }
int getMapWidth() { return map_width_; }
int getMapHeight() { return map_height_; }
+ const std::string& getGlobalFrame() { return global_frame_id_; }
protected:
@@ -234,6 +235,9 @@
float scale_;
+ // Global frame (usually "map")
+ std::string global_frame_id_;
+
bool new_cloud_;
bool new_gui_path_;
bool new_local_path_;
Modified: pkg/trunk/nav/nav_view/src/nav_view/tools.cpp
===================================================================
--- pkg/trunk/nav/nav_view/src/nav_view/tools.cpp 2009-01-23 02:24:22 UTC (rev 10035)
+++ pkg/trunk/nav/nav_view/src/nav_view/tools.cpp 2009-01-23 02:30:34 UTC (rev 10036)
@@ -163,7 +163,7 @@
goal.goal.y = pos_.y;
goal.goal.th = angle;
goal.enable = 1;
- goal.header.frame_id = "map";
+ goal.header.frame_id = panel_->getGlobalFrame();
printf("setting goal: %.3f %.3f %.3f\n", goal.goal.x, goal.goal.y, goal.goal.th);
ros_node_->publish( "goal", goal );
}
Modified: pkg/trunk/world_models/costmap_2d/include/costmap_2d/basic_observation_buffer.h
===================================================================
--- pkg/trunk/world_models/costmap_2d/include/costmap_2d/basic_observation_buffer.h 2009-01-23 02:24:22 UTC (rev 10035)
+++ pkg/trunk/world_models/costmap_2d/include/costmap_2d/basic_observation_buffer.h 2009-01-23 02:30:34 UTC (rev 10036)
@@ -47,7 +47,7 @@
*/
class BasicObservationBuffer : public ObservationBuffer {
public:
- BasicObservationBuffer(const std::string& frame_id, tf::TransformListener& tf, ros::Duration keepAlive, ros::Duration refresh_interval,double robotRadius, double minZ, double maxZ, robot_filter::RobotFilter* filter = NULL);
+ BasicObservationBuffer(const std::string& frame_id, const std::string& global_frame_id, tf::TransformListener& tf, ros::Duration keepAlive, ros::Duration refresh_interval,double robotRadius, double minZ, double maxZ, robot_filter::RobotFilter* filter = NULL);
virtual void buffer_cloud(const std_msgs::PointCloud& local_cloud);
Modified: pkg/trunk/world_models/costmap_2d/include/costmap_2d/observation_buffer.h
===================================================================
--- pkg/trunk/world_models/costmap_2d/include/costmap_2d/observation_buffer.h 2009-01-23 02:24:22 UTC (rev 10035)
+++ pkg/trunk/world_models/costmap_2d/include/costmap_2d/observation_buffer.h 2009-01-23 02:30:34 UTC (rev 10036)
@@ -47,7 +47,7 @@
*/
class ObservationBuffer {
public:
- ObservationBuffer(const std::string& frame_id, ros::Duration keep_alive, ros::Duration refresh_interval);
+ ObservationBuffer(const std::string& frame_id, const std::string& global_frame_id, ros::Duration keep_alive, ros::Duration refresh_interval);
virtual ~ObservationBuffer();
@@ -79,6 +79,8 @@
protected:
const std::string frame_id_;
+ /**< Which frame is global; usually "map" */
+ const std::string global_frame_id_;
private:
std::list<Observation> buffer_;
Modified: pkg/trunk/world_models/costmap_2d/src/basic_observation_buffer.cc
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/basic_observation_buffer.cc 2009-01-23 02:24:22 UTC (rev 10035)
+++ pkg/trunk/world_models/costmap_2d/src/basic_observation_buffer.cc 2009-01-23 02:30:34 UTC (rev 10036)
@@ -3,9 +3,9 @@
namespace costmap_2d {
- BasicObservationBuffer::BasicObservationBuffer(const std::string& frame_id, tf::TransformListener& tf, ros::Duration keepAlive, ros::Duration refresh_interval,
+ BasicObservationBuffer::BasicObservationBuffer(const std::string& frame_id, const std::string& global_frame_id, tf::TransformListener& tf, ros::Duration keepAlive, ros::Duration refresh_interval,
double robotRadius, double minZ, double maxZ, robot_filter::RobotFilter* filter)
- : ObservationBuffer(frame_id, keepAlive, refresh_interval), tf_(tf), robotRadius_(robotRadius), minZ_(minZ), maxZ_(maxZ), filter_(filter) {}
+ : ObservationBuffer(frame_id, global_frame_id, keepAlive, refresh_interval), tf_(tf), robotRadius_(robotRadius), minZ_(minZ), maxZ_(maxZ), filter_(filter) {}
void BasicObservationBuffer::buffer_cloud(const std_msgs::PointCloud& local_cloud)
{
@@ -35,12 +35,12 @@
{
// First we want the origin for the sensor
tf::Stamped<btVector3> local_origin(btVector3(0, 0, 0), point_cloud.header.stamp, frame_id_);
- tf_.transformPoint("map", local_origin, map_origin);
+ tf_.transformPoint(global_frame_id_, local_origin, map_origin);
tf_.transformPointCloud("base_footprint", point_cloud, base_cloud);
newData = extractFootprintAndGround(base_cloud);
map_cloud = new std_msgs::PointCloud();
- tf_.transformPointCloud("map", *newData, *map_cloud);
+ tf_.transformPointCloud(global_frame_id_, *newData, *map_cloud);
ROS_DEBUG("Buffering cloud for %s at origin <%f, %f, %f>\n", frame_id_.c_str(), map_origin.getX(), map_origin.getY(), map_origin.getZ());
}
Modified: pkg/trunk/world_models/costmap_2d/src/observation_buffer.cc
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/observation_buffer.cc 2009-01-23 02:24:22 UTC (rev 10035)
+++ pkg/trunk/world_models/costmap_2d/src/observation_buffer.cc 2009-01-23 02:30:34 UTC (rev 10036)
@@ -41,8 +41,8 @@
return ros::Duration((int) rint(period), (int) rint((period - rint(period)) * pow(10.0, 9.0)));
}
- ObservationBuffer::ObservationBuffer(const std::string& frame_id, ros::Duration keep_alive, ros::Duration refresh_interval)
- :frame_id_(frame_id), received_obseration_(false), keep_alive_(keep_alive),
+ ObservationBuffer::ObservationBuffer(const std::string& frame_id, const std::string& global_frame_id, ros::Duration keep_alive, ros::Duration refresh_interval)
+ :frame_id_(frame_id), global_frame_id_(global_frame_id), received_obseration_(false), keep_alive_(keep_alive),
refresh_interval_(refresh_interval), last_updated_(ros::Time::now()) {
ROS_INFO("Initializing observation buffer for %s with keepAlive = %f and refresh_interval = %f\n",
frame_id_.c_str(), keep_alive.toSec(), refresh_interval_.toSec());
@@ -66,7 +66,7 @@
last_updated_ = ros::Time::now();
received_obseration_ = true;
- if(observation.cloud_->header.frame_id != "map") {
+ if(observation.cloud_->header.frame_id != global_frame_id_) {
ROS_ERROR("Observation in frame %s, must be in frame \"map\".",
observation.cloud_->header.frame_id.c_str());
return false;
Modified: pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc 2009-01-23 02:24:22 UTC (rev 10035)
+++ pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc 2009-01-23 02:30:34 UTC (rev 10036)
@@ -187,7 +187,7 @@
ros::Duration keep_alive(10, 0);
ros::Duration refresh_interval(0, 200000000); // 200 ms
- ObservationBuffer buffer("Foo", keep_alive, refresh_interval);
+ ObservationBuffer buffer("Foo", "map", keep_alive, refresh_interval);
// Initially it should be false
ASSERT_EQ(buffer.isCurrent(), false);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-01-23 18:02:55
|
Revision: 10049
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10049&view=rev
Author: sfkwc
Date: 2009-01-23 18:02:47 +0000 (Fri, 23 Jan 2009)
Log Message:
-----------
Moving to prcore stack
Added Paths:
-----------
pkg/trunk/prcore/angles/
Removed Paths:
-------------
pkg/trunk/util/angles/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-01-23 18:03:21
|
Revision: 10050
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10050&view=rev
Author: sfkwc
Date: 2009-01-23 18:03:17 +0000 (Fri, 23 Jan 2009)
Log Message:
-----------
Moving robot_msgs to prcore
Added Paths:
-----------
pkg/trunk/prcore/robot_msgs/
Removed Paths:
-------------
pkg/trunk/robot_msgs/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-01-23 18:04:12
|
Revision: 10051
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10051&view=rev
Author: sfkwc
Date: 2009-01-23 18:04:08 +0000 (Fri, 23 Jan 2009)
Log Message:
-----------
Moving robot_srvs to prcore
Added Paths:
-----------
pkg/trunk/prcore/robot_srvs/
Removed Paths:
-------------
pkg/trunk/robot_srvs/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-01-23 18:04:56
|
Revision: 10052
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10052&view=rev
Author: sfkwc
Date: 2009-01-23 18:04:52 +0000 (Fri, 23 Jan 2009)
Log Message:
-----------
Moving pytf to prcore
Added Paths:
-----------
pkg/trunk/prcore/pytf/
Removed Paths:
-------------
pkg/trunk/util/pytf/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-01-23 18:05:39
|
Revision: 10053
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10053&view=rev
Author: sfkwc
Date: 2009-01-23 18:05:33 +0000 (Fri, 23 Jan 2009)
Log Message:
-----------
Moving to prcore
Added Paths:
-----------
pkg/trunk/prcore/tf/
Removed Paths:
-------------
pkg/trunk/util/tf/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-01-23 18:06:08
|
Revision: 10054
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10054&view=rev
Author: sfkwc
Date: 2009-01-23 18:06:04 +0000 (Fri, 23 Jan 2009)
Log Message:
-----------
Moving xacro to prcore
Added Paths:
-----------
pkg/trunk/prcore/xacro/
Removed Paths:
-------------
pkg/trunk/util/xacro/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-01-23 19:31:29
|
Revision: 10064
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10064&view=rev
Author: tfoote
Date: 2009-01-23 19:31:22 +0000 (Fri, 23 Jan 2009)
Log Message:
-----------
bullet is in the prcore now
Added Paths:
-----------
pkg/trunk/prcore/bullet/
Removed Paths:
-------------
pkg/trunk/3rdparty/bullet/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-01-23 19:57:13
|
Revision: 10072
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10072&view=rev
Author: isucan
Date: 2009-01-23 19:57:07 +0000 (Fri, 23 Jan 2009)
Log Message:
-----------
waiting for state
Modified Paths:
--------------
pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
pkg/trunk/robot_descriptions/pr2/pr2_configurations/arm_cart/run_trajectory_controller.launch
pkg/trunk/robot_descriptions/pr2/pr2_configurations/arm_cart/trajectory_controllers.xml
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf.launch
pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/pr2_groups_collision.xml
Modified: pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2009-01-23 19:46:31 UTC (rev 10071)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2009-01-23 19:57:07 UTC (rev 10072)
@@ -421,9 +421,9 @@
plan->loadRobotDescription();
if (plan->loadedRobot())
{
- // plan->waitForState();
-
- sleep(2);
+ sleep(1);
+ plan->waitForState();
+ ROS_INFO("Received robot state");
char test = (argc < 3) ? ' ' : argv[2][0];
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/arm_cart/run_trajectory_controller.launch
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/arm_cart/run_trajectory_controller.launch 2009-01-23 19:46:31 UTC (rev 10071)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/arm_cart/run_trajectory_controller.launch 2009-01-23 19:57:07 UTC (rev 10072)
@@ -1,19 +1,20 @@
+
<launch>
<!-- Arm trajectory controller -->
- <param name="right_arm_trajectory_controller/velocity_scaling_factor" type="double" value="0.25"/>
- <param name="right_arm_trajectory_controller/trajectory_wait_timeout" type="double" value="2.5"/>
+ <param name="right_arm_trajectory_controller/velocity_scaling_factor" type="double" value="1.0"/>
+ <param name="right_arm_trajectory_controller/trajectory_wait_timeout" type="double" value=".25"/>
- <param name="right_arm_trajectory_controller/r_shoulder_pan_joint/goal_reached_threshold" type="double" value="0.01"/>
- <param name="right_arm_trajectory_controller/r_shoulder_lift_joint/goal_reached_threshold" type="double" value="0.01"/>
- <param name="right_arm_trajectory_controller/r_shoulder_roll_joint/goal_reached_threshold" type="double" value="0.01"/>
- <param name="right_arm_trajectory_controller/r_elbow_flex_joint/goal_reached_threshold" type="double" value="0.01"/>
- <param name="right_arm_trajectory_controller/r_forearm_roll_joint/goal_reached_threshold" type="double" value="0.01"/>
- <param name="right_arm_trajectory_controller/r_wrist_flex_joint/goal_reached_threshold" type="double" value="0.01"/>
- <param name="right_arm_trajectory_controller/r_wrist_roll_joint/goal_reached_threshold" type="double" value="0.01"/>
- <param name="right_arm_trajectory_controller/r_gripper_l_finger_joint/goal_reached_threshold" type="double" value="0.01"/>
+ <param name="right_arm_trajectory_controller/r_shoulder_pan_joint/goal_reached_threshold" type="double" value="0.1"/>
+ <param name="right_arm_trajectory_controller/r_shoulder_lift_joint/goal_reached_threshold" type="double" value="0.1"/>
+ <param name="right_arm_trajectory_controller/r_shoulder_roll_joint/goal_reached_threshold" type="double" value="0.1"/>
+ <param name="right_arm_trajectory_controller/r_elbow_flex_joint/goal_reached_threshold" type="double" value="0.1"/>
+ <param name="right_arm_trajectory_controller/r_forearm_roll_joint/goal_reached_threshold" type="double" value="0.1"/>
+ <param name="right_arm_trajectory_controller/r_wrist_flex_joint/goal_reached_threshold" type="double" value="0.1"/>
+ <param name="right_arm_trajectory_controller/r_wrist_roll_joint/goal_reached_threshold" type="double" value="0.1"/>
+ <param name="right_arm_trajectory_controller/r_gripper_l_finger_joint/goal_reached_threshold" type="double" value="0.1"/>
- <node pkg="mechanism_control" type="spawner.py" args="$(find arm_cart)/trajectory_controllers.xml" />
+ <node pkg="mechanism_control" type="spawner.py" args="$(find arm_cart)/trajectory_controllers.xml" output="screen"/>
</launch>
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/arm_cart/trajectory_controllers.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/arm_cart/trajectory_controllers.xml 2009-01-23 19:46:31 UTC (rev 10071)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/arm_cart/trajectory_controllers.xml 2009-01-23 19:57:07 UTC (rev 10072)
@@ -13,7 +13,7 @@
</map>
<controller name="r_shoulder_pan_controller" topic="r_shoulder_pan_controller" type="JointPDController">
<joint name="r_shoulder_pan_joint" >
- <pid p="140.0" d="15.0" i="0.8" iClamp="100.0" />
+ <pid p="140.0" d="30.0" i="0.8" iClamp="100.0" />
</joint>
</controller>
<controller name="r_shoulder_lift_controller" topic="r_shoulder_pitch_controller" type="JointPDController">
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf.launch
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf.launch 2009-01-23 19:46:31 UTC (rev 10071)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf.launch 2009-01-23 19:57:07 UTC (rev 10072)
@@ -12,8 +12,10 @@
<include file="$(find pr2_full)/calibrate.launch" />
<!-- Joystick -->
+<!--
<param name="joy/deadzone" value="5000"/>
<node machine="four" pkg="joy" type="joy" respawn="true"/>
+-->
<!-- spacenav -->
<node machine="four" pkg="spacenav_node" type="spacenav_node" />
Modified: pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/pr2_groups_collision.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/pr2_groups_collision.xml 2009-01-23 19:46:31 UTC (rev 10071)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/pr2_groups_collision.xml 2009-01-23 19:57:07 UTC (rev 10072)
@@ -36,22 +36,6 @@
torso_lift_link
head_pan_link
head_tilt_link
-
- l_shoulder_pan_link
- l_shoulder_lift_link
- l_upper_arm_roll_link
- l_upper_arm_link
- l_elbow_flex_link
- l_forearm_roll_link
- l_forearm_link
- l_wrist_flex_link
- l_wrist_roll_link
- l_gripper_palm_link
- l_gripper_l_finger_link
- l_gripper_r_finger_link
- l_gripper_l_finger_tip_link
- l_gripper_r_finger_tip_link
-
r_shoulder_pan_link
r_shoulder_lift_link
r_upper_arm_roll_link
@@ -66,7 +50,6 @@
r_gripper_r_finger_link
r_gripper_l_finger_tip_link
r_gripper_r_finger_tip_link
-
</group>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <vij...@us...> - 2009-01-23 22:36:15
|
Revision: 10085
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10085&view=rev
Author: vijaypradeep
Date: 2009-01-23 21:42:00 +0000 (Fri, 23 Jan 2009)
Log Message:
-----------
Point cloud snapshotter now ignores scans while transitioning between profiles
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_snapshotter.cpp
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_laser_tilt/manifest.xml
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_controller.h 2009-01-23 21:41:24 UTC (rev 10084)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_controller.h 2009-01-23 21:42:00 UTC (rev 10085)
@@ -68,7 +68,7 @@
FIRST_HALF = 1, //!< Specifies that we're in the first half of our current profile
SECOND_HALF = 2 //!< Specifies that we're in the second half of our current profile
} ;
-
+
/*!
* \brief Default Constructor of the JointController class.
*
@@ -170,7 +170,7 @@
* Will return NotApplicable if we're not currently following a profile
*/
ProfileExecutionState getProfileExecutionState() ;
-
+
double getTime();
private:
/*!
@@ -221,7 +221,7 @@
*
*/
LaserScannerControllerNode();
-
+
/*!
* \brief Destructor
*/
@@ -253,7 +253,10 @@
* \brief A pointer to ros node
*/
ros::Node *node_;
-
+
+ //! True if a profile was just initiated, and it has not yet hit either end of it's traj
+ bool first_time_ ;
+
LaserScannerController::ProfileExecutionState prev_profile_exec_state_ ; //!< Store the previous profileExecutionState. Need this to compare to the current state to detect transitions
pr2_mechanism_controllers::LaserScannerSignal m_scanner_signal_ ; //!< Stores the message that we want to send at the end of each sweep, and halfway through each sweep
bool need_to_send_msg_ ; //!< Tracks whether we still need to send out the m_scanner_signal_ message.
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp 2009-01-23 21:41:24 UTC (rev 10084)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp 2009-01-23 21:42:00 UTC (rev 10085)
@@ -463,7 +463,13 @@
case LaserScannerController::FIRST_HALF :
if (cur_profile_exec_state == LaserScannerController::SECOND_HALF) // We just transitioned from 1st->2nd, so send msg saying half-scan is done
{
- m_scanner_signal_.signal = 0 ; // 0 -> Half profile complete
+ if (first_time_)
+ {
+ m_scanner_signal_.signal = 128 ;
+ first_time_ = false ;
+ }
+ else
+ m_scanner_signal_.signal = 0 ; // 0 -> Half profile complete
//if (need_to_send_msg_) // Is there a message we were supposed to send, but never got to?
// printf("LaserScannerController:: Missed sending a msg\n") ; // Is there a better way to output this error msg?
@@ -474,7 +480,13 @@
case LaserScannerController::SECOND_HALF : // Send msg saying full scan is done
if (cur_profile_exec_state == LaserScannerController::FIRST_HALF) // We just transitioned from 2nd->1st, so send msg saying full-scan is done
{
- m_scanner_signal_.signal = 1 ; // 1 -> Full profile complete
+ if(first_time_)
+ {
+ m_scanner_signal_.signal = 129 ;
+ first_time_ = false ;
+ }
+ else
+ m_scanner_signal_.signal = 1 ; // 1 -> Full profile complete
//if (need_to_send_msg_) // Is there a message we were supposed to send, but never got to?
// printf("LaserScannerController:: Missed sending a msg\n") ; // Is there a better way to output this error msg?
@@ -541,6 +553,8 @@
{
const double num_elem = -1.0 ; // We should only be using the dynamicProfiles, so we don't need num_elem
+ first_time_ = true ;
+
setProfile(LaserScannerController::LaserControllerMode(req.profile),req.period,req.amplitude,num_elem,req.offset);
resp.time = c_->getTime();
return true;
Modified: pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_snapshotter.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_snapshotter.cpp 2009-01-23 21:41:24 UTC (rev 10084)
+++ pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_snapshotter.cpp 2009-01-23 21:42:00 UTC (rev 10085)
@@ -83,6 +83,10 @@
void scannerSignalCallback()
{
+ if (cur_signal_.signal == 128 || cur_signal_.signal == 129) // These codes imply that this is the first signal during a given profile type
+ first_time_ = true ;
+
+
if (first_time_)
{
prev_signal_ = cur_signal_ ;
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_laser_tilt/manifest.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_laser_tilt/manifest.xml 2009-01-23 21:41:24 UTC (rev 10084)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_laser_tilt/manifest.xml 2009-01-23 21:42:00 UTC (rev 10085)
@@ -15,8 +15,7 @@
<depend package="pr2_etherCAT"/>
<depend package="pr2_power_board"/>
<depend package="ocean_battery_driver"/>
- <depend package="joy"/>
<depend package="rosrecord"/>
<depend package="hokuyo_node"/>
- <depend package="imu_node"/>
+ <depend package="point_cloud_assembler" />
</package>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2009-01-23 22:54:28
|
Revision: 10089
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10089&view=rev
Author: isucan
Date: 2009-01-23 22:54:25 +0000 (Fri, 23 Jan 2009)
Log Message:
-----------
more flexibility in specifying constraints
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPConstraintEvaluator.h
pkg/trunk/prcore/robot_msgs/msg/PoseConstraint.msg
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h 2009-01-23 22:48:37 UTC (rev 10088)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPBasicRequestLinkPosition.h 2009-01-23 22:54:25 UTC (rev 10089)
@@ -104,7 +104,7 @@
m_pce[i]->evaluate(&dPos, &dAng);
if (decision)
(*decision)[i] = m_pce[i]->decide(dPos, dAng);
- distance += dPos + dAng;
+ distance += dPos + m_pce[i]->getConstraintMessage().orientation_importance * dAng;
}
return distance;
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPConstraintEvaluator.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPConstraintEvaluator.h 2009-01-23 22:48:37 UTC (rev 10088)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/RKPConstraintEvaluator.h 2009-01-23 22:54:25 UTC (rev 10089)
@@ -165,6 +165,11 @@
return result;
}
+ const robot_msgs::PoseConstraint& getConstraintMessage(void) const
+ {
+ return m_pc;
+ }
+
virtual void print(std::ostream &out = std::cout) const
{
if (m_link)
Modified: pkg/trunk/prcore/robot_msgs/msg/PoseConstraint.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/PoseConstraint.msg 2009-01-23 22:48:37 UTC (rev 10088)
+++ pkg/trunk/prcore/robot_msgs/msg/PoseConstraint.msg 2009-01-23 22:54:25 UTC (rev 10089)
@@ -12,10 +12,15 @@
string robot_link
# The desired pose of the robot link
-std_msgs/Pose3D pose
+std_msgs/Pose pose
# the allowed difference (square of euclidian distance) for position
float64 position_distance
# the allowed difference (shortest angular distance) for orientation
float64 orientation_distance
+
+# a factor that gets multiplied to the orientation when computing the
+# distance to the goal.
+# Distance_to_goal = position_distance + orientation_importance * orientation_distance
+float64 orientation_importance
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
Thanks for helping keep SourceForge clean.
X