Menu
▾
▴
personalrobots-commit
|
From: <hsu...@us...> - 2008-12-12 18:46:18
|
Revision: 8017
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8017&view=rev
Author: hsujohnhsu
Date: 2008-12-12 18:46:09 +0000 (Fri, 12 Dec 2008)
Log Message:
-----------
moved gazebo examples into demo directory.
Modified Paths:
--------------
pkg/trunk/demos/examples_gazebo/dual_link.xml
pkg/trunk/demos/examples_gazebo/dual_link_no_x.xml
pkg/trunk/demos/examples_gazebo/multi_link.xml
pkg/trunk/demos/examples_gazebo/single_link.xml
Added Paths:
-----------
pkg/trunk/demos/examples_gazebo/dual_link_defs/
pkg/trunk/demos/examples_gazebo/multi_link_defs/
pkg/trunk/demos/examples_gazebo/single_link_defs/
Removed Paths:
-------------
pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/
pkg/trunk/robot_descriptions/wg_robot_description/multi_link_test/
pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/
Modified: pkg/trunk/demos/examples_gazebo/dual_link.xml
===================================================================
--- pkg/trunk/demos/examples_gazebo/dual_link.xml 2008-12-12 18:43:52 UTC (rev 8016)
+++ pkg/trunk/demos/examples_gazebo/dual_link.xml 2008-12-12 18:46:09 UTC (rev 8017)
@@ -1,7 +1,7 @@
<launch>
<group name="wg">
<!-- send dual_link.xml to param server -->
- <include file="$(find wg_robot_description)/dual_link_test/send_description.launch" />
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find examples_gazebo)/dual_link_defs/dual_link.xml'" />
<!-- start gazebo -->
<node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
@@ -14,7 +14,7 @@
<!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
<node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
- <!--node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/dual_link_test/controllers_dual_link.xml" respawn="false" output="screen" /--> <!-- load default arm controller -->
+ <!--node pkg="mechanism_control" type="mech.py" args="sp $(find examples_gazebo)/dual_link_defs/controllers_dual_link.xml" respawn="false" output="screen" /--> <!-- load default arm controller -->
<!--node pkg="robot_mechanism_controllers" type="control.py" args="set test_controller 0.5" respawn="false" output="screen" /--> <!-- open gripper .5 radians -->
</launch>
Modified: pkg/trunk/demos/examples_gazebo/dual_link_no_x.xml
===================================================================
--- pkg/trunk/demos/examples_gazebo/dual_link_no_x.xml 2008-12-12 18:43:52 UTC (rev 8016)
+++ pkg/trunk/demos/examples_gazebo/dual_link_no_x.xml 2008-12-12 18:46:09 UTC (rev 8017)
@@ -1,7 +1,7 @@
<launch>
<group name="wg">
<!-- send dual_link.xml to param server -->
- <include file="$(find wg_robot_description)/dual_link_test/send_description.launch" />
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find examples_gazebo)/dual_link_defs/dual_link.xml'" />
<!-- start gazebo -->
<node pkg="gazebo" type="gazebo" args="-r $(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
@@ -14,7 +14,7 @@
<!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
<node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
- <!--node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/dual_link_test/controllers_dual_link.xml" respawn="false" output="screen" /--> <!-- load default arm controller -->
+ <!--node pkg="mechanism_control" type="mech.py" args="sp $(find examples_gazebo)/dual_link_defs/controllers_dual_link.xml" respawn="false" output="screen" /--> <!-- load default arm controller -->
<!--node pkg="robot_mechanism_controllers" type="control.py" args="set test_controller 0.5" respawn="false" output="screen" /--> <!-- open gripper .5 radians -->
</launch>
Modified: pkg/trunk/demos/examples_gazebo/multi_link.xml
===================================================================
--- pkg/trunk/demos/examples_gazebo/multi_link.xml 2008-12-12 18:43:52 UTC (rev 8016)
+++ pkg/trunk/demos/examples_gazebo/multi_link.xml 2008-12-12 18:46:09 UTC (rev 8017)
@@ -1,7 +1,7 @@
<launch>
<group name="wg">
<!-- send multi_link.xml to param server -->
- <include file="$(find wg_robot_description)/multi_link_test/send_description.launch" />
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find examples_gazebo)/multi_link_defs/multi_link.xml'" />
<!-- start gazebo -->
<node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
@@ -14,7 +14,7 @@
<!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
<node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
- <node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/multi_link_test/controllers_multi_link.xml" respawn="false" output="screen" /> <!-- load default arm controller -->
+ <node pkg="mechanism_control" type="mech.py" args="sp $(find examples_gazebo)/multi_link_defs/controllers_multi_link.xml" respawn="false" output="screen" /> <!-- load default arm controller -->
<!--node pkg="robot_mechanism_controllers" type="control.py" args="set test_controller 0.5" respawn="false" output="screen" /--> <!-- open gripper .5 radians -->
</launch>
Modified: pkg/trunk/demos/examples_gazebo/single_link.xml
===================================================================
--- pkg/trunk/demos/examples_gazebo/single_link.xml 2008-12-12 18:43:52 UTC (rev 8016)
+++ pkg/trunk/demos/examples_gazebo/single_link.xml 2008-12-12 18:46:09 UTC (rev 8017)
@@ -1,7 +1,7 @@
<launch>
<group name="wg">
<!-- send single_link.xml to param server -->
- <include file="$(find wg_robot_description)/single_link_test/send_description.launch" />
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find examples_gazebo)/single_link_defs/single_link.xml'" />
<!-- start gazebo -->
<node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
@@ -14,7 +14,7 @@
<!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
<node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
- <node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/single_link_test/controllers_single_link.xml" respawn="false" output="screen" /> <!-- load default arm controller -->
+ <node pkg="mechanism_control" type="mech.py" args="sp $(find examples_gazebo)/single_link_defs/controllers_single_link.xml" respawn="false" output="screen" /> <!-- load default arm controller -->
<node pkg="robot_mechanism_controllers" type="control.py" args="set test_controller 0.5" respawn="false" output="screen" /> <!-- open gripper .5 radians -->
</launch>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-12-12 19:59:40
|
Revision: 8028
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8028&view=rev
Author: hsujohnhsu
Date: 2008-12-12 19:59:35 +0000 (Fri, 12 Dec 2008)
Log Message:
-----------
* rename send_description.xml to send_description.launch.
* call xacro directly instead of using send_description.launch where fitting.
* update scan for test_scan due to ray number change.
Modified Paths:
--------------
pkg/trunk/demos/arm_gazebo/arm.launch
pkg/trunk/demos/arm_gazebo/arm_no_x.launch
pkg/trunk/demos/pr2_gazebo/pr2_empty.launch
pkg/trunk/demos/pr2_gazebo/pr2_empty_no_x.launch
pkg/trunk/demos/pr2_gazebo/pr2_floorobj.launch
pkg/trunk/demos/pr2_gazebo/pr2_obs.launch
pkg/trunk/demos/pr2_gazebo/pr2_simple.launch
pkg/trunk/demos/pr2_gazebo/pr2_wg.launch
pkg/trunk/demos/pr2_gazebo/pr2_wg_no_x.launch
pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_obs.launch
pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_simple.launch
pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_tables.launch
pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_tables_no_x.launch
pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_wg.launch
pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_wg_no_x.launch
pkg/trunk/drivers/simulator/gazebo_plugin/test/test_camera.xml
pkg/trunk/drivers/simulator/gazebo_plugin/test/test_scan.py
pkg/trunk/drivers/simulator/gazebo_plugin/test/test_scan.xml
pkg/trunk/drivers/simulator/gazebo_plugin/test/test_slide.xml
pkg/trunk/robot_descriptions/head_test_cart/test.launch
pkg/trunk/robot_descriptions/pr2_alpha/pre.launch
pkg/trunk/robot_descriptions/pr2_alpha/prg.launch
pkg/trunk/robot_descriptions/robo_dev_demo/demo.launch
pkg/trunk/robot_descriptions/wg_robot_description/arm/init.launch
pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/init.launch
Added Paths:
-----------
pkg/trunk/robot_descriptions/wg_robot_description/arm/send_description.launch
pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/send_description.launch
Removed Paths:
-------------
pkg/trunk/robot_descriptions/wg_robot_description/arm/send_description.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/send_description.xml
Modified: pkg/trunk/demos/arm_gazebo/arm.launch
===================================================================
--- pkg/trunk/demos/arm_gazebo/arm.launch 2008-12-12 19:31:57 UTC (rev 8027)
+++ pkg/trunk/demos/arm_gazebo/arm.launch 2008-12-12 19:59:35 UTC (rev 8028)
@@ -1,7 +1,7 @@
<launch>
<group name="wg">
<!-- send pr2_arm.xml to param server -->
- <include file="$(find wg_robot_description)/pr2_arm_test/send_description.launch" />
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find wg_robot_description)/pr2_arm_test/pr2_arm.xacro.xml'" />
<!-- -g flag runs gazebo in gui-less mode -->
<node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
Modified: pkg/trunk/demos/arm_gazebo/arm_no_x.launch
===================================================================
--- pkg/trunk/demos/arm_gazebo/arm_no_x.launch 2008-12-12 19:31:57 UTC (rev 8027)
+++ pkg/trunk/demos/arm_gazebo/arm_no_x.launch 2008-12-12 19:59:35 UTC (rev 8028)
@@ -1,7 +1,7 @@
<launch>
<group name="wg">
<!-- send pr2_arm.xml to param server -->
- <include file="$(find wg_robot_description)/pr2_arm_test/send_description.launch" />
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find wg_robot_description)/pr2_arm_test/pr2_arm.xacro.xml'" />
<!-- -g flag runs gazebo in gui-less mode -->
<node pkg="gazebo" type="gazebo" args="-r $(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
Modified: pkg/trunk/demos/pr2_gazebo/pr2_empty.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_empty.launch 2008-12-12 19:31:57 UTC (rev 8027)
+++ pkg/trunk/demos/pr2_gazebo/pr2_empty.launch 2008-12-12 19:59:35 UTC (rev 8028)
@@ -2,7 +2,7 @@
<!-- this launch file corresponds to robot model in ros-pkg/robot_descriptions/wg_robot_description/pr2 -->
<group name="wg">
<!-- send pr2.xml to param server -->
- <include file="$(find wg_robot_description)/pr2/send_description.launch" />
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find wg_robot_description)/pr2/pr2.xacro.xml'" />
<!-- start gazebo -->
<node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
Modified: pkg/trunk/demos/pr2_gazebo/pr2_empty_no_x.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_empty_no_x.launch 2008-12-12 19:31:57 UTC (rev 8027)
+++ pkg/trunk/demos/pr2_gazebo/pr2_empty_no_x.launch 2008-12-12 19:59:35 UTC (rev 8028)
@@ -2,7 +2,7 @@
<!-- this launch file corresponds to robot model in ros-pkg/robot_descriptions/wg_robot_description/pr2 -->
<group name="wg">
<!-- send pr2.xml to param server -->
- <include file="$(find wg_robot_description)/pr2/send_description.launch" />
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find wg_robot_description)/pr2/pr2.xacro.xml'" />
<!-- start gazebo -->
<node pkg="gazebo" type="gazebo" args="-r $(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
Modified: pkg/trunk/demos/pr2_gazebo/pr2_floorobj.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_floorobj.launch 2008-12-12 19:31:57 UTC (rev 8027)
+++ pkg/trunk/demos/pr2_gazebo/pr2_floorobj.launch 2008-12-12 19:59:35 UTC (rev 8028)
@@ -1,7 +1,7 @@
<launch>
<group name="wg">
<!-- create model file for gazebo -->
- <include file="$(find wg_robot_description)/pr2/send_description.launch" />
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find wg_robot_description)/pr2/pr2.xacro.xml'" />
<!-- -g flag runs gazebo in gui-less mode -->
<node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/floorobj.world" respawn="false" output="screen">
Modified: pkg/trunk/demos/pr2_gazebo/pr2_obs.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_obs.launch 2008-12-12 19:31:57 UTC (rev 8027)
+++ pkg/trunk/demos/pr2_gazebo/pr2_obs.launch 2008-12-12 19:59:35 UTC (rev 8028)
@@ -1,7 +1,7 @@
<launch>
<group name="wg">
<!-- send pr2.xml to param server -->
- <include file="$(find wg_robot_description)/pr2/send_description.launch" />
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find wg_robot_description)/pr2/pr2.xacro.xml'" />
<!-- start gazebo -->
<node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/obstacle.world" respawn="false" output="screen">
Modified: pkg/trunk/demos/pr2_gazebo/pr2_simple.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_simple.launch 2008-12-12 19:31:57 UTC (rev 8027)
+++ pkg/trunk/demos/pr2_gazebo/pr2_simple.launch 2008-12-12 19:59:35 UTC (rev 8028)
@@ -2,7 +2,7 @@
<!-- this launch file corresponds to robot model in ros-pkg/robot_descriptions/wg_robot_description/pr2 -->
<group name="wg">
<!-- send pr2.xml to param server -->
- <include file="$(find wg_robot_description)/pr2/send_description.launch" />
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find wg_robot_description)/pr2/pr2.xacro.xml'" />
<!-- start gazebo -->
<node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/simple.world" respawn="false" output="screen">
Modified: pkg/trunk/demos/pr2_gazebo/pr2_wg.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_wg.launch 2008-12-12 19:31:57 UTC (rev 8027)
+++ pkg/trunk/demos/pr2_gazebo/pr2_wg.launch 2008-12-12 19:59:35 UTC (rev 8028)
@@ -1,7 +1,7 @@
<launch>
<group name="wg">
<!-- send pr2.xml to param server -->
- <include file="$(find wg_robot_description)/pr2/send_description.launch" />
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find wg_robot_description)/pr2/pr2.xacro.xml'" />
<!-- start gazebo -->
<node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/wg.world" respawn="false" output="screen">
Modified: pkg/trunk/demos/pr2_gazebo/pr2_wg_no_x.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_wg_no_x.launch 2008-12-12 19:31:57 UTC (rev 8027)
+++ pkg/trunk/demos/pr2_gazebo/pr2_wg_no_x.launch 2008-12-12 19:59:35 UTC (rev 8028)
@@ -1,7 +1,7 @@
<launch>
<group name="wg">
<!-- send pr2.xml to param server -->
- <include file="$(find wg_robot_description)/pr2/send_description.launch" />
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find wg_robot_description)/pr2/pr2.xacro.xml'" />
<!-- start gazebo -->
<node pkg="gazebo" type="gazebo" args="-r -n $(find gazebo_robot_description)/gazebo_worlds/wg.world" respawn="false" output="screen">
Modified: pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_obs.launch
===================================================================
--- pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_obs.launch 2008-12-12 19:31:57 UTC (rev 8027)
+++ pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_obs.launch 2008-12-12 19:59:35 UTC (rev 8028)
@@ -1,7 +1,7 @@
<launch>
<group name="wg">
<!-- send pr2.xml to param server -->
- <include file="$(find wg_robot_description)/pr2_prototype1/send_description.xml" />
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find wg_robot_description)/pr2_prototype1/pr2_prototype1.xacro.xml'" />
<!-- start gazebo -->
<node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/obstacle.world" respawn="false" output="screen">
Modified: pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_simple.launch
===================================================================
--- pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_simple.launch 2008-12-12 19:31:57 UTC (rev 8027)
+++ pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_simple.launch 2008-12-12 19:59:35 UTC (rev 8028)
@@ -5,7 +5,7 @@
<!-- send pr2.xml to parameter server as a string, allow retrieval by various components whe needs it
(Mechanism Control, BaseControllerNode, etc...) -->
- <include file="$(find wg_robot_description)/pr2_prototype1/send_description.xml" />
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find wg_robot_description)/pr2_prototype1/pr2_prototype1.xacro.xml'" />
<!-- assign environment variables for gazebo and startup gazebo with argument containing the world file. -->
<node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/simple.world" respawn="false" output="screen">
Modified: pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_tables.launch
===================================================================
--- pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_tables.launch 2008-12-12 19:31:57 UTC (rev 8027)
+++ pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_tables.launch 2008-12-12 19:59:35 UTC (rev 8028)
@@ -5,7 +5,7 @@
<!-- send pr2.xml to parameter server as a string, allow retrieval by various components whe needs it
(Mechanism Control, BaseControllerNode, etc...) -->
- <include file="$(find wg_robot_description)/pr2_prototype1/send_description.xml" />
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find wg_robot_description)/pr2_prototype1/pr2_prototype1.xacro.xml'" />
<!-- assign environment variables for gazebo and startup gazebo with argument containing the world file. -->
<node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/tables.world" respawn="false" output="screen">
Modified: pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_tables_no_x.launch
===================================================================
--- pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_tables_no_x.launch 2008-12-12 19:31:57 UTC (rev 8027)
+++ pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_tables_no_x.launch 2008-12-12 19:59:35 UTC (rev 8028)
@@ -5,7 +5,7 @@
<!-- send pr2.xml to parameter server as a string, allow retrieval by various components whe needs it
(Mechanism Control, BaseControllerNode, etc...) -->
- <include file="$(find wg_robot_description)/pr2_prototype1/send_description.xml" />
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find wg_robot_description)/pr2_prototype1/pr2_prototype1.xacro.xml'" />
<!-- assign environment variables for gazebo and startup gazebo with argument containing the world file. -->
<node pkg="gazebo" type="gazebo" args="-r $(find gazebo_robot_description)/gazebo_worlds/tables.world" respawn="false" output="screen">
Modified: pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_wg.launch
===================================================================
--- pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_wg.launch 2008-12-12 19:31:57 UTC (rev 8027)
+++ pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_wg.launch 2008-12-12 19:59:35 UTC (rev 8028)
@@ -5,7 +5,7 @@
<!-- send pr2.xml to parameter server as a string, allow retrieval by various components whe needs it
(Mechanism Control, BaseControllerNode, etc...) -->
- <include file="$(find wg_robot_description)/pr2_prototype1/send_description.xml" />
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find wg_robot_description)/pr2_prototype1/pr2_prototype1.xacro.xml'" />
<!-- assign environment variables for gazebo and startup gazebo with argument containing the world file. -->
<node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/wg.world" respawn="false" output="screen">
Modified: pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_wg_no_x.launch
===================================================================
--- pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_wg_no_x.launch 2008-12-12 19:31:57 UTC (rev 8027)
+++ pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_wg_no_x.launch 2008-12-12 19:59:35 UTC (rev 8028)
@@ -5,7 +5,7 @@
<!-- send pr2.xml to parameter server as a string, allow retrieval by various components whe needs it
(Mechanism Control, BaseControllerNode, etc...) -->
- <include file="$(find wg_robot_description)/pr2_prototype1/send_description.xml" />
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find wg_robot_description)/pr2_prototype1/pr2_prototype1.xacro.xml'" />
<!-- assign environment variables for gazebo and startup gazebo with argument containing the world file. -->
<node pkg="gazebo" type="gazebo" args="-r -n $(find gazebo_robot_description)/gazebo_worlds/wg.world" respawn="false" output="screen">
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/test/test_camera.xml
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/test/test_camera.xml 2008-12-12 19:31:57 UTC (rev 8027)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/test/test_camera.xml 2008-12-12 19:59:35 UTC (rev 8028)
@@ -9,7 +9,7 @@
</node>
<!-- send single_link.xml to param server -->
- <include file="$(find wg_robot_description)/pr2/send_description.launch" />
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find wg_robot_description)/pr2/pr2.xacro.xml'" />
<!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
<node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2 -3 0 0" respawn="false" output="screen" /> <!-- load default arm controller -->
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/test/test_scan.py
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/test/test_scan.py 2008-12-12 19:31:57 UTC (rev 8027)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/test/test_scan.py 2008-12-12 19:59:35 UTC (rev 8028)
@@ -187,42 +187,40 @@
10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907,
10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907,
10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907,
+10.0500001907, 2.04787278175, 2.04592037201, 2.03293919563, 2.0309150219, 2.02612328529, 2.02249693871,
+2.01050376892, 1.9971985817, 1.99220633507, 1.99093174934, 1.98233509064, 1.96967220306, 1.96407520771,
+1.95673263073, 1.9632999897, 1.9541169405, 1.94116449356, 1.93745577335, 1.93260037899, 1.91873133183,
+1.92071986198, 1.92447936535, 1.91405880451, 1.90388011932, 1.90442991257, 1.92065799236, 1.94389533997,
+10.0500001907, 10.0500001907, 10.0500001907, 2.20284819603, 2.11506819725, 2.07276725769, 2.05478954315,
+2.03410387039, 2.01044726372, 1.99864196777, 1.97362887859, 1.95649659634, 1.94725096226, 1.93579721451,
+1.92892599106, 1.9206674099, 1.89596438408, 1.89780378342, 1.89246237278, 1.8796428442, 1.87438380718,
+1.85257148743, 1.8596534729, 1.84719610214, 1.83455812931, 1.85068559647, 1.83815693855, 1.83944654465,
+1.83141326904, 1.83223962784, 1.83295285702, 1.81912827492, 1.82103550434, 1.81053507328, 1.8073772192,
+1.81735467911, 1.81044983864, 1.80440604687, 1.80720567703, 1.79462218285, 1.8113617897, 1.80637574196,
+1.81443309784, 1.7996609211, 1.80886435509, 1.80233979225, 1.81417393684, 1.81837320328, 1.81973767281,
+1.81899499893, 1.81897163391, 1.81569230556, 1.81440281868, 1.82588279247, 1.82763755322, 1.8355230093,
+1.82483589649, 1.8410063982, 1.84740209579, 1.85418522358, 1.85382521152, 1.86047744751, 1.87290942669,
+1.87637448311, 1.87997066975, 1.88822221756, 1.90849196911, 1.90068280697, 1.92333364487, 1.92135071754,
+1.9316290617, 1.94617557526, 1.96299791336, 1.98042583466, 1.9886174202, 2.01968574524, 2.04413557053,
+2.06229710579, 2.09267020226, 2.14132356644, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907,
10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907,
10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907,
-10.0500001907, 10.0500001907, 2.04438686371, 2.03526091576, 2.03843474388, 2.02810740471, 2.01536941528,
-2.0106742382, 2.00239062309, 1.99854290485, 2.00354862213, 1.98480772972, 1.98543548584, 1.97376585007,
-1.97784543037, 1.95745873451, 1.95659172535, 1.94629871845, 1.94227647781, 1.94253218174, 1.93757581711,
-1.93427300453, 1.92613852024, 1.92173802853, 1.91625797749, 1.90750837326, 1.91858124733, 1.89570868015,
-1.9184615612, 1.94624936581, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 2.14344358444,
-2.1046128273, 2.06721520424, 2.04437351227, 2.0196518898, 2.00137424469, 1.99092102051, 1.96682059765,
-1.9552834034, 1.95001757145, 1.93734264374, 1.93557405472, 1.91600286961, 1.90478920937, 1.90029597282,
-1.89263594151, 1.87531137466, 1.87568151951, 1.86833143234, 1.8595045805, 1.85612237453, 1.84199500084,
-1.85124623775, 1.8307158947, 1.82791626453, 1.83716058731, 1.83264183998, 1.83296573162, 1.81996226311,
-1.82218003273, 1.82357537746, 1.82011902332, 1.81928098202, 1.80879497528, 1.80926239491, 1.80285894871,
-1.80863332748, 1.80659532547, 1.80666220188, 1.80548453331, 1.79809331894, 1.81591355801, 1.80456137657,
-1.80835199356, 1.81127357483, 1.81024718285, 1.80711269379, 1.80918550491, 1.81158769131, 1.81054377556,
-1.81785166264, 1.80969893932, 1.82439088821, 1.83087062836, 1.82215571404, 1.82838749886, 1.8384206295,
-1.84195029736, 1.83588850498, 1.85495364666, 1.85131633282, 1.86470663548, 1.86034476757, 1.87930703163,
-1.87743628025, 1.88554620743, 1.89345943928, 1.88949298859, 1.90567290783, 1.92090845108, 1.92672729492,
-1.93174529076, 1.94988548756, 1.96379482746, 1.97736215591, 1.9952737093, 2.0085003376, 2.03174781799,
-2.04610800743, 2.08065462112, 2.11122274399, 2.17865300179, 10.0500001907, 10.0500001907, 10.0500001907,
10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907,
10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907,
10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907,
10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907,
10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907,
+1.80345368385, 1.7794598341, 1.7536636591, 1.74016606808, 1.72928035259, 1.71435511112, 1.68924510479,
+1.67889750004, 1.66685688496, 1.66631317139, 1.66503357887, 1.65074276924, 1.64622533321, 1.64321935177,
+1.64316165447, 1.62697017193, 1.62436592579, 1.62134301662, 1.62517416477, 1.61807000637, 1.62088704109,
+1.61336827278, 1.5963088274, 1.6035785675, 1.59667503834, 1.60347390175, 1.59573459625, 1.5959597826,
+1.60779702663, 1.59334671497, 1.60121428967, 1.60183036327, 1.59678864479, 1.60666680336, 1.60501253605,
+1.60348498821, 1.61345434189, 1.61245715618, 1.62518751621, 1.62398457527, 1.62988734245, 1.62476825714,
+1.64009869099, 1.63341450691, 1.64568448067, 1.64412105083, 1.65664947033, 1.66021370888, 1.67184150219,
+1.68355834484, 1.69032812119, 1.70137858391, 1.7188808918, 1.72428512573, 1.74748575687, 1.77116715908,
+1.7868065834, 1.84455692768, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907,
10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907,
10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907,
-10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 1.82291102409, 1.77777791023, 1.75317430496,
-1.7438467741, 1.73084497452, 1.7287248373, 1.70747983456, 1.69615650177, 1.68979692459, 1.67314648628,
-1.66798436642, 1.64787042141, 1.65843641758, 1.64694333076, 1.639498353, 1.63458120823, 1.6370729208,
-1.62373054028, 1.62701678276, 1.62720894814, 1.6159889698, 1.61263751984, 1.60884582996, 1.60460424423,
-1.60788786411, 1.61228084564, 1.60764610767, 1.59833323956, 1.60253632069, 1.60882735252, 1.60960781574,
-1.60044157505, 1.59952819347, 1.59946334362, 1.60755443573, 1.61359798908, 1.6060333252, 1.60476875305,
-1.60351586342, 1.61390519142, 1.61251187325, 1.62213218212, 1.62440621853, 1.62318503857, 1.63000869751,
-1.6230815649, 1.63896048069, 1.6419955492, 1.64225423336, 1.65474712849, 1.65734326839, 1.66962206364,
-1.67535257339, 1.67523550987, 1.70329809189, 1.70518648624, 1.71380269527, 1.73248124123, 1.7559132576,
-1.76032626629, 1.79325187206, 1.83686363697, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907,
10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907,
10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907,
10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907,
@@ -250,15 +248,12 @@
10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907,
10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907,
10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907,
-10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907,
-10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907,
-10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907,
-10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907,
-10.0500001907, 10.0500001907, 10.0500001907, 10.0500001907, ]
+10.0500001907, 10.0500001907, 10.0500001907, ]
+
class PointCloudTest(unittest.TestCase):
def __init__(self, *args):
super(PointCloudTest, self).__init__(*args)
@@ -294,7 +289,7 @@
self.success = True
- def test_pointcloud(self):
+ def test_scan(self):
print "LNK\n"
rospy.TopicSub("base_scan", LaserScan, self.pointInput)
rospy.init_node(NAME, anonymous=True)
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/test/test_scan.xml
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/test/test_scan.xml 2008-12-12 19:31:57 UTC (rev 8027)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/test/test_scan.xml 2008-12-12 19:59:35 UTC (rev 8028)
@@ -9,7 +9,7 @@
</node>
<!-- send single_link.xml to param server -->
- <include file="$(find wg_robot_description)/pr2/send_description.launch" />
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find wg_robot_description)/pr2/pr2.xacro.xml'" />
<!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
<node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
@@ -18,5 +18,5 @@
<include file="$(find pr2_gazebo)/pr2_default_controllers.launch" />
<!--<node pkg="gazebo_plugin" type="groundtruthtransform" args="" respawn="true" />-->
- <test test-name="gazebo_plugin_testpointclouds1" pkg="gazebo_plugin" type="test_scan.py" />
+ <test test-name="gazebo_plugin_test_scan" pkg="gazebo_plugin" type="test_scan.py" />
</launch>
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/test/test_slide.xml
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/test/test_slide.xml 2008-12-12 19:31:57 UTC (rev 8027)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/test/test_slide.xml 2008-12-12 19:59:35 UTC (rev 8028)
@@ -9,7 +9,7 @@
</node>
<!-- send single_link.xml to param server -->
- <include file="$(find wg_robot_description)/pr2/send_description.launch" />
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find wg_robot_description)/pr2/pr2.xacro.xml'" />
<!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
<node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2 0 8 110 0 0 90" respawn="false" output="screen" /> <!-- load default arm controller -->
Modified: pkg/trunk/robot_descriptions/head_test_cart/test.launch
===================================================================
--- pkg/trunk/robot_descriptions/head_test_cart/test.launch 2008-12-12 19:31:57 UTC (rev 8027)
+++ pkg/trunk/robot_descriptions/head_test_cart/test.launch 2008-12-12 19:59:35 UTC (rev 8028)
@@ -1,5 +1,5 @@
<launch>
- <include file="$(find wg_robot_description)/robo_dev_head/send_description.xml" />
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find wg_robot_description)/robo_dev_head/robo_dev_head.xml'" />
<machine name="xenomai_root" user="root" address="localhost" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_REALTIME_PATH)" default="never"/>
Modified: pkg/trunk/robot_descriptions/pr2_alpha/pre.launch
===================================================================
--- pkg/trunk/robot_descriptions/pr2_alpha/pre.launch 2008-12-12 19:31:57 UTC (rev 8027)
+++ pkg/trunk/robot_descriptions/pr2_alpha/pre.launch 2008-12-12 19:59:35 UTC (rev 8028)
@@ -1,5 +1,5 @@
<launch>
- <include file="$(find wg_robot_description)/pr2_prototype1/send_description.xml" />
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find wg_robot_description)/pr2_prototype1/pr2_prototype1.xacro.xml'" />
<machine name="xenomai_root" user="root" address="pre1" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" default="never"/>
Modified: pkg/trunk/robot_descriptions/pr2_alpha/prg.launch
===================================================================
--- pkg/trunk/robot_descriptions/pr2_alpha/prg.launch 2008-12-12 19:31:57 UTC (rev 8027)
+++ pkg/trunk/robot_descriptions/pr2_alpha/prg.launch 2008-12-12 19:59:35 UTC (rev 8028)
@@ -1,5 +1,5 @@
<launch>
- <include file="$(find wg_robot_description)/pr2_prototype1/send_description.xml" />
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find wg_robot_description)/pr2_prototype1/pr2_prototype1.xacro.xml'" />
<machine name="xenomai_root" user="root" address="prg1" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" default="never"/>
Modified: pkg/trunk/robot_descriptions/robo_dev_demo/demo.launch
===================================================================
--- pkg/trunk/robot_descriptions/robo_dev_demo/demo.launch 2008-12-12 19:31:57 UTC (rev 8027)
+++ pkg/trunk/robot_descriptions/robo_dev_demo/demo.launch 2008-12-12 19:59:35 UTC (rev 8028)
@@ -1,5 +1,5 @@
<launch>
- <include file="$(find wg_robot_description)/robo_dev_head/send_description.xml" />
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find wg_robot_description)/robo_dev_head/robo_dev_head.xml'" />
<machine name="xenomai_root" user="root" address="prdemo1" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_REALTIME_PATH)" default="never"/>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/arm/init.launch
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/arm/init.launch 2008-12-12 19:31:57 UTC (rev 8027)
+++ pkg/trunk/robot_descriptions/wg_robot_description/arm/init.launch 2008-12-1...
[truncated message content] |
|
From: <tf...@us...> - 2008-12-13 00:01:06
|
Revision: 8042
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8042&view=rev
Author: tfoote
Date: 2008-12-13 00:01:01 +0000 (Sat, 13 Dec 2008)
Log Message:
-----------
updating review stati
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/IBPSBatteryInterface/manifest.xml
pkg/trunk/drivers/robot/pr2/pr2_power_board/manifest.xml
pkg/trunk/hardware_test/life_test/arm_life_test/manifest.xml
pkg/trunk/hardware_test/life_test/caster_life_test/manifest.xml
pkg/trunk/hardware_test/life_test/elbow_life_test/manifest.xml
pkg/trunk/robot_descriptions/head_test_cart/manifest.xml
Modified: pkg/trunk/drivers/robot/pr2/IBPSBatteryInterface/manifest.xml
===================================================================
--- pkg/trunk/drivers/robot/pr2/IBPSBatteryInterface/manifest.xml 2008-12-12 23:55:51 UTC (rev 8041)
+++ pkg/trunk/drivers/robot/pr2/IBPSBatteryInterface/manifest.xml 2008-12-13 00:01:01 UTC (rev 8042)
@@ -2,7 +2,7 @@
<description>This is an interface to the Ocean Server Technology Intelligent Battery and Power System.</description>
<author>Tully Foote</author>
<license>BSD</license>
- <review status="unreviewed" notes="API review in progress (Tully)"/>
+ <review status="API conditionally cleared" notes="API followup in progress (Tully)"/>
<depend package="rospy" />
<depend package="robot_msgs" />
</package>
Modified: pkg/trunk/drivers/robot/pr2/pr2_power_board/manifest.xml
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2_power_board/manifest.xml 2008-12-12 23:55:51 UTC (rev 8041)
+++ pkg/trunk/drivers/robot/pr2/pr2_power_board/manifest.xml 2008-12-13 00:01:01 UTC (rev 8042)
@@ -2,7 +2,7 @@
<description>This is a driver for the PR2 Power Board</description>
<author>Tully Foote</author>
<license>BSD</license>
- <review status="unreviewed" notes="API review in progress (Tully)"/>
+ <review status="API conditionally cleared" notes="API followup in progress (Tully)"/>
<depend package="roscpp" />
<depend package="rospy" />
<depend package="rosthread" />
Modified: pkg/trunk/hardware_test/life_test/arm_life_test/manifest.xml
===================================================================
--- pkg/trunk/hardware_test/life_test/arm_life_test/manifest.xml 2008-12-12 23:55:51 UTC (rev 8041)
+++ pkg/trunk/hardware_test/life_test/arm_life_test/manifest.xml 2008-12-13 00:01:01 UTC (rev 8042)
@@ -4,7 +4,7 @@
</description>
<author>Stuart Glaser</author>
<license>BSD</license>
- <review status="unreviewed" notes=""/>
+ <review status="na" notes=""/>
<depend package="pr2_etherCAT" />
<depend package="pr2_power_board"/>
Modified: pkg/trunk/hardware_test/life_test/caster_life_test/manifest.xml
===================================================================
--- pkg/trunk/hardware_test/life_test/caster_life_test/manifest.xml 2008-12-12 23:55:51 UTC (rev 8041)
+++ pkg/trunk/hardware_test/life_test/caster_life_test/manifest.xml 2008-12-13 00:01:01 UTC (rev 8042)
@@ -4,7 +4,7 @@
</description>
<author>Stuart Glaser</author>
<license>BSD</license>
- <review status="unreviewed" notes=""/>
+ <review status="na" notes=""/>
<depend package="pr2_etherCAT" />
<depend package="pr2_power_board"/>
Modified: pkg/trunk/hardware_test/life_test/elbow_life_test/manifest.xml
===================================================================
--- pkg/trunk/hardware_test/life_test/elbow_life_test/manifest.xml 2008-12-12 23:55:51 UTC (rev 8041)
+++ pkg/trunk/hardware_test/life_test/elbow_life_test/manifest.xml 2008-12-13 00:01:01 UTC (rev 8042)
@@ -4,7 +4,7 @@
</description>
<author>Stuart Glaser</author>
<license>BSD</license>
- <review status="unreviewed" notes=""/>
+ <review status="na" notes=""/>
<depend package="pr2_etherCAT" />
<depend package="pr2_power_board"/>
Modified: pkg/trunk/robot_descriptions/head_test_cart/manifest.xml
===================================================================
--- pkg/trunk/robot_descriptions/head_test_cart/manifest.xml 2008-12-12 23:55:51 UTC (rev 8041)
+++ pkg/trunk/robot_descriptions/head_test_cart/manifest.xml 2008-12-13 00:01:01 UTC (rev 8042)
@@ -7,6 +7,7 @@
</description>
<author>Melonee Wise</author>
+ <review status="na" notes=""/>
<license>BSD</license>
<depend package="wg_robot_description" />
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-12-13 01:59:01
|
Revision: 8050
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8050&view=rev
Author: hsujohnhsu
Date: 2008-12-13 01:58:55 +0000 (Sat, 13 Dec 2008)
Log Message:
-----------
update battery controller in gazebo with default consumption rate of -10 J/s.
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_battery.h
pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_battery.cpp
pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/gazebo_defs.xml
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_battery.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_battery.h 2008-12-13 01:22:37 UTC (rev 8049)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_battery.h 2008-12-13 01:58:55 UTC (rev 8050)
@@ -151,14 +151,12 @@
/// \brief power drain, if this is negative, we are charging the battery.
private: double consumption_rate_;
+ private: double default_consumption_rate_;
/// \brief listen to ROS to see if we are charging
private: void SetPlug();
private: gazebo_plugin::PlugCommand plug_msg_;
-/// @todo make DISCHAGE_RATE something else
-#define DISCHARGE_RATE 1.0
-
};
/** \} */
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_battery.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_battery.cpp 2008-12-13 01:22:37 UTC (rev 8049)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_battery.cpp 2008-12-13 01:58:55 UTC (rev 8050)
@@ -82,8 +82,9 @@
/// faking the plug and unplug of robot
rosnode_->subscribe("plugged_in",this->plug_msg_,&GazeboBattery::SetPlug,this,10);
+ this->default_consumption_rate_ = node->GetDouble("default_consumption_rate",-10.0,0);
this->full_capacity_ = node->GetDouble("full_charge_energy",0.0,0);
- this->default_charge_rate_ = node->GetDouble("default_charge_rate",-2.0,0);
+ this->default_charge_rate_ = node->GetDouble("default_charge_rate",-10.0,0);
/// @todo make below useful
//this->diagnostic_rate_ = node->GetDouble("diagnostic_rate",1.0,0);
@@ -95,9 +96,9 @@
{
this->lock_.lock();
if (this->plug_msg_.status == "the robot is very much plugged into the wall")
- this->consumption_rate_ = this->default_charge_rate_ + DISCHARGE_RATE;
+ this->consumption_rate_ = this->default_charge_rate_ + this->default_consumption_rate_;
else
- this->consumption_rate_ = DISCHARGE_RATE;
+ this->consumption_rate_ = this->default_consumption_rate_;
this->lock_.unlock();
}
@@ -108,7 +109,7 @@
/// initialize battery
this->charge_ = this->full_capacity_; /// our convention is joules
- this->consumption_rate_ = DISCHARGE_RATE; /// time based decay rate in watts
+ this->consumption_rate_ = this->default_consumption_rate_; /// time based decay rate in watts
}
void GazeboBattery::UpdateChild()
@@ -120,7 +121,7 @@
/* update battery */
/* */
/**********************************************************/
- this->charge_ = this->charge_ - (this->current_time_ - this->last_time_)*this->consumption_rate_;
+ this->charge_ = this->charge_ + (this->current_time_ - this->last_time_)*this->consumption_rate_;
if (this->charge_ < 0) this->charge_ = 0;
if (this->charge_ > this->full_capacity_) this->charge_ = this->full_capacity_;
//std::cout << " battery charge remaining: " << this->charge_ << " Joules " << std::endl;
@@ -134,6 +135,7 @@
this->battery_state_.header.stamp.sec = (unsigned long)floor(this->current_time_);
this->battery_state_.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->current_time_ - this->battery_state_.header.stamp.sec) );
this->battery_state_.energy_remaining = this->charge_;
+ this->battery_state_.energy_capacity = this->full_capacity_;
this->battery_state_.power_consumption = this->consumption_rate_;
this->lock_.lock();
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/gazebo_defs.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/gazebo_defs.xml 2008-12-13 01:22:37 UTC (rev 8049)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/gazebo_defs.xml 2008-12-13 01:58:55 UTC (rev 8050)
@@ -16,9 +16,10 @@
<alwaysOn>true</alwaysOn>
<updateRate>1.0</updateRate>
<timeout>5</timeout>
+ <default_consumption_rate>-10.0</default_consumption_rate> <!-- -5 is the magic number, need to be smaller than that CM -->
<diagnostic_rate>1.0</diagnostic_rate>
- <battery_state_rate>1.0</battery_state_rate>
- <full_charge_energy>120.0</full_charge_energy>
+ <battery_state_rate>10.0</battery_state_rate> <!-- does nothing for now-->
+ <full_charge_energy>1200000.0</full_charge_energy>
<diagnosticTopicName>diagnostic</diagnosticTopicName>
<stateTopicName>battery_state</stateTopicName>
<selfTestServiceName>self_test</selfTestServiceName>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rdi...@us...> - 2008-12-14 00:26:14
|
Revision: 8072
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8072&view=rev
Author: rdiankov
Date: 2008-12-14 00:26:09 +0000 (Sun, 14 Dec 2008)
Log Message:
-----------
openraveros octave client-side added
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/openrave_planning/openraveros/CMakeLists.txt
pkg/trunk/openrave_planning/openraveros/manifest.xml
pkg/trunk/openrave_planning/openraveros/src/openraveros.cpp
pkg/trunk/openrave_planning/openraveros/src/rosserver.h
pkg/trunk/openrave_planning/openraveros/src/session.h
pkg/trunk/openrave_planning/openraveros/srv/body_destroy.srv
pkg/trunk/openrave_planning/openraveros/srv/body_enable.srv
pkg/trunk/openrave_planning/openraveros/srv/body_getaabb.srv
pkg/trunk/openrave_planning/openraveros/srv/body_getaabbs.srv
pkg/trunk/openrave_planning/openraveros/srv/body_getdof.srv
pkg/trunk/openrave_planning/openraveros/srv/body_getjointvalues.srv
pkg/trunk/openrave_planning/openraveros/srv/body_getlinks.srv
pkg/trunk/openrave_planning/openraveros/srv/body_setjointvalues.srv
pkg/trunk/openrave_planning/openraveros/srv/body_settransform.srv
pkg/trunk/openrave_planning/openraveros/srv/env_checkcollision.srv
pkg/trunk/openrave_planning/openraveros/srv/env_closefigures.srv
pkg/trunk/openrave_planning/openraveros/srv/env_createbody.srv
pkg/trunk/openrave_planning/openraveros/srv/env_createplanner.srv
pkg/trunk/openrave_planning/openraveros/srv/env_createproblem.srv
pkg/trunk/openrave_planning/openraveros/srv/env_createrobot.srv
pkg/trunk/openrave_planning/openraveros/srv/env_destroyproblem.srv
pkg/trunk/openrave_planning/openraveros/srv/env_getbodies.srv
pkg/trunk/openrave_planning/openraveros/srv/env_getbody.srv
pkg/trunk/openrave_planning/openraveros/srv/env_getrobots.srv
pkg/trunk/openrave_planning/openraveros/srv/env_loadplugin.srv
pkg/trunk/openrave_planning/openraveros/srv/env_loadscene.srv
pkg/trunk/openrave_planning/openraveros/srv/env_plot.srv
pkg/trunk/openrave_planning/openraveros/srv/env_raycollision.srv
pkg/trunk/openrave_planning/openraveros/srv/env_set.srv
pkg/trunk/openrave_planning/openraveros/srv/env_triangulate.srv
pkg/trunk/openrave_planning/openraveros/srv/env_wait.srv
pkg/trunk/openrave_planning/openraveros/srv/openrave_session.srv
pkg/trunk/openrave_planning/openraveros/srv/planner_init.srv
pkg/trunk/openrave_planning/openraveros/srv/planner_plan.srv
pkg/trunk/openrave_planning/openraveros/srv/problem_sendcommand.srv
pkg/trunk/openrave_planning/openraveros/srv/robot_controllersend.srv
pkg/trunk/openrave_planning/openraveros/srv/robot_controllerset.srv
pkg/trunk/openrave_planning/openraveros/srv/robot_getactivevalues.srv
pkg/trunk/openrave_planning/openraveros/srv/robot_sensorgetdata.srv
pkg/trunk/openrave_planning/openraveros/srv/robot_sensorsend.srv
pkg/trunk/openrave_planning/openraveros/srv/robot_setactivedofs.srv
pkg/trunk/openrave_planning/openraveros/srv/robot_setactivevalues.srv
pkg/trunk/openrave_planning/openraveros/srv/robot_starttrajectory.srv
Added Paths:
-----------
pkg/trunk/openrave_planning/openraveros/octave/
pkg/trunk/openrave_planning/openraveros/octave/openraveros_createsession.m
pkg/trunk/openrave_planning/openraveros/octave/openraveros_destroysession.m
pkg/trunk/openrave_planning/openraveros/octave/openraveros_getbodyinfo.m
pkg/trunk/openrave_planning/openraveros/octave/openraveros_getglobalsession.m
pkg/trunk/openrave_planning/openraveros/octave/openraveros_getrobotinfo.m
pkg/trunk/openrave_planning/openraveros/octave/openraveros_restart.m
pkg/trunk/openrave_planning/openraveros/octave/openraveros_rotfromquat.m
pkg/trunk/openrave_planning/openraveros/octave/openraveros_setglobalsession.m
pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m
pkg/trunk/openrave_planning/openraveros/octave/orBodyDestroy.m
pkg/trunk/openrave_planning/openraveros/octave/orBodyEnable.m
pkg/trunk/openrave_planning/openraveros/octave/orBodyGetAABB.m
pkg/trunk/openrave_planning/openraveros/octave/orBodyGetAABBs.m
pkg/trunk/openrave_planning/openraveros/octave/orBodyGetDOF.m
pkg/trunk/openrave_planning/openraveros/octave/orBodyGetJointValues.m
pkg/trunk/openrave_planning/openraveros/octave/orBodyGetLinks.m
pkg/trunk/openrave_planning/openraveros/octave/orBodyGetTransform.m
pkg/trunk/openrave_planning/openraveros/octave/orBodySetJointValues.m
pkg/trunk/openrave_planning/openraveros/octave/orBodySetTransform.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvCheckCollision.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvClose.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvCreateKinBody.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvCreatePlanner.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvCreateProblem.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvCreateRobot.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvDestroyProblem.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvGetBodies.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvGetBody.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvGetRobots.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvLoadPlugin.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvLoadScene.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvPlot.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvRayCollision.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvSetOptions.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvTriangulate.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvWait.m
pkg/trunk/openrave_planning/openraveros/octave/orPlannerInit.m
pkg/trunk/openrave_planning/openraveros/octave/orPlannerPlan.m
pkg/trunk/openrave_planning/openraveros/octave/orProblemSendCommand.m
pkg/trunk/openrave_planning/openraveros/octave/orRobotControllerSend.m
pkg/trunk/openrave_planning/openraveros/octave/orRobotControllerSet.m
pkg/trunk/openrave_planning/openraveros/octave/orRobotGetActiveDOF.m
pkg/trunk/openrave_planning/openraveros/octave/orRobotGetAttachedSensors.m
pkg/trunk/openrave_planning/openraveros/octave/orRobotGetDOFLimits.m
pkg/trunk/openrave_planning/openraveros/octave/orRobotGetDOFValues.m
pkg/trunk/openrave_planning/openraveros/octave/orRobotGetManipulators.m
pkg/trunk/openrave_planning/openraveros/octave/orRobotSensorGetData.m
pkg/trunk/openrave_planning/openraveros/octave/orRobotSensorSend.m
pkg/trunk/openrave_planning/openraveros/octave/orRobotSetActiveDOFs.m
pkg/trunk/openrave_planning/openraveros/octave/orRobotSetDOFValues.m
pkg/trunk/openrave_planning/openraveros/octave/orRobotStartActiveTrajectory.m
pkg/trunk/openrave_planning/openraveros/test/
pkg/trunk/openrave_planning/openraveros/test/testsessioncreation.m
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2008-12-13 21:49:47 UTC (rev 8071)
+++ pkg/trunk/3rdparty/openrave/Makefile 2008-12-14 00:26:09 UTC (rev 8072)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 543
+SVN_REVISION = -r 549
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
include $(shell rospack find mk)/svn_checkout.mk
@@ -11,7 +11,7 @@
build: SVN_UP openrave
openrave: SVN_UP
- @cd $(SVN_DIR) && export PATH="$(shell rospack find soqt)/bin:$(shell rospack find opende)/opende/bin:$(PATH)" && export CPATH="$(shell rospack find bullet)/include" && export LD_LIBRARY_PATH=$(shell rospack find bullet)/lib:$(LD_LIBRARY_PATH) && export BOOST_ROOT=$(shell rospack find boost)/boost && svn up && make prefix=$(PWD) && make install
+ @cd $(SVN_DIR) && export PATH="$(shell rospack find soqt)/bin:$(shell rospack find opende)/opende/bin:$(PATH)" && export CPATH="$(shell rospack find bullet)/include" && export LD_LIBRARY_PATH=$(shell rospack find bullet)/lib:$(LD_LIBRARY_PATH) && export BOOST_ROOT=$(shell rospack find boost)/boost && svn up && mkdir -p build && cd build && $(shell rospack find cmake)/cmake/bin/cmake -DCMAKE_INSTALL_PREFIX=$(PWD) -DCMAKE_BUILD_TYPE=Release .. && make install
clean:
make -C $(SVN_DIR) clean
Modified: pkg/trunk/openrave_planning/openraveros/CMakeLists.txt
===================================================================
--- pkg/trunk/openrave_planning/openraveros/CMakeLists.txt 2008-12-13 21:49:47 UTC (rev 8071)
+++ pkg/trunk/openrave_planning/openraveros/CMakeLists.txt 2008-12-14 00:26:09 UTC (rev 8072)
@@ -6,3 +6,7 @@
gensrv()
rospack_add_executable(openraveros src/openraveros.cpp)
target_link_libraries (openraveros openrave-core boost_thread)
+
+# add sessions dependency
+find_ros_package(roscpp_sessions)
+set_source_files_properties(src/openraveros.cpp PROPERTIES OBJECT_DEPENDS ${roscpp_sessions_PACKAGE_PATH}/include/ros/session.h)
Modified: pkg/trunk/openrave_planning/openraveros/manifest.xml
===================================================================
--- pkg/trunk/openrave_planning/openraveros/manifest.xml 2008-12-13 21:49:47 UTC (rev 8071)
+++ pkg/trunk/openrave_planning/openraveros/manifest.xml 2008-12-14 00:26:09 UTC (rev 8072)
@@ -4,8 +4,10 @@
</description>
<author>Rosen Diankov (rdi...@cs...)</author>
<license>BSD</license>
+ <depend package="boost"/>
<depend package="roscpp"/>
+ <depend package="roscpp_sessions"/>
<depend package="openrave"/>
<depend package="std_msgs"/>
- <depend package="boost"/>
+ <depend package="rosoct"/>
</package>
Added: pkg/trunk/openrave_planning/openraveros/octave/openraveros_createsession.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_createsession.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_createsession.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,61 @@
+%% session = openraveros_createsession(sessionserver, cloneuuid)
+%
+%% creates a session and returns its id.
+%% clonesession (optional) - if passed in, will clone the new session with this session
+%% the environment id that uniquely determines a session is passed in the uuid
+%% Output:
+%% session.id - local identifier used to make session calls or session terminations
+%% session.uuid - unique identifier for the session server across the entire ROS network
+%% session.server - name of the session server
+
+%% Software License Agreement (BSD License)
+%% Copyright (c) 2008, Willow Garage, Inc.
+%% Redistribution and use in source and binary forms, with or without
+%% modification, are permitted provided that the following conditions are met:
+%% * Redistributions of source code must retain the above copyright notice,
+%% this list of conditions and the following disclaimer.
+%% * Redistributions in binary form must reproduce the above copyright
+%% notice, this list of conditions and the following disclaimer in the
+%% documentation and/or other materials provided with the distribution.
+%% * 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
+function session = openraveros_createsession(sessionserver, cloneuuid, cloneoptions)
+
+if( ~exist('sessionserver','var') )
+ sessionserver = 'openrave_session';
+end
+openraveros_startup(sessionserver);
+
+req = openraveros_openrave_session();
+if( exist('cloneuuid','var') )
+ req.clone_sessionid = cloneuuid;
+ if( exist('cloneoptions','var') )
+ req.clone_options = cloneoptions;
+ end
+end
+
+session = [];
+[localid,res] = rosoct_create_session(sessionserver,req);
+
+if( ~isempty(localid) && ~isempty(res) )
+ if( res.sessionid==0 )
+ error('bad session id');
+ end
+ session.id = localid;
+ session.server = sessionserver;
+ session.uuid = res.sessionid;
+end
Added: pkg/trunk/openrave_planning/openraveros/octave/openraveros_destroysession.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_destroysession.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_destroysession.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,50 @@
+%% openraveros_destroysession(session)
+%%
+%% destroys a session part of a session server
+%% session.id - local session id
+%% if session does not exist will delete all sessions
+
+%% Software License Agreement (BSD License)
+%% Copyright (c) 2008, Willow Garage, Inc.
+%% Redistribution and use in source and binary forms, with or without
+%% modification, are permitted provided that the following conditions are met:
+%% * Redistributions of source code must retain the above copyright notice,
+%% this list of conditions and the following disclaimer.
+%% * Redistributions in binary form must reproduce the above copyright
+%% notice, this list of conditions and the following disclaimer in the
+%% documentation and/or other materials provided with the distribution.
+%% * 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
+function openraveros_destroysession(session)
+
+openraveros_startup([],0);
+
+gsession = openraveros_getglobalsession();
+if( ~exist('session','var') )
+ if( isempty(gsession) )
+ return;
+ end
+ session = gsession;
+end
+
+%% destroys all the sessions of a session server
+__rosoct_terminate_session(session.id);
+
+%% if this is the global session, make sure to propagate the changes
+if( ~isempty(gsession) && session.id == gsession.id )
+ openraveros_setglobalsession([]);
+end
Added: pkg/trunk/openrave_planning/openraveros/octave/openraveros_getbodyinfo.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_getbodyinfo.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_getbodyinfo.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,24 @@
+%% body = openraveros_getbodyinfo(bodyinfo)
+%%
+%% parses the BodyInfo.msg into a simpler form for octave consumption
+function body = openraveros_getbodyinfo(bodyinfo)
+
+body.id = bodyinfo.bodyid;
+body.name = bodyinfo.name;
+body.type = bodyinfo.type;
+body.filename = bodyinfo.filename;
+
+%% extra
+body.enabled = bodyinfo.enabled;
+body.dof = bodyinfo.dof;
+body.T = reshape(cell2mat(bodyinfo.transform.m),[3 4]);
+
+body.jointvalues = cell2mat(bodyinfo.jointvalues);
+body.links = zeros(1,length(bodyinfo.links));
+for i = 1:length(bodyinfo.links)
+ body.links(:,i) = cell2mat(bodyinfo.links{i}.m);
+end
+
+body.linknames = bodyinfo.linknames;
+body.lowerlimit = cell2mat(bodyinfo.lowerlimit);
+body.upperlimit = cell2mat(bodyinfo.upperlimit);
Added: pkg/trunk/openrave_planning/openraveros/octave/openraveros_getglobalsession.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_getglobalsession.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_getglobalsession.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,34 @@
+%% session = openraveros_getglobalsession()
+%%
+%% gets the current session of openrave, if session is not active, tries to create
+%% a new session. If that also fails, returns an error
+
+%% Software License Agreement (BSD License)
+%% Copyright (c) 2008, Willow Garage, Inc.
+%% Redistribution and use in source and binary forms, with or without
+%% modification, are permitted provided that the following conditions are met:
+%% * Redistributions of source code must retain the above copyright notice,
+%% this list of conditions and the following disclaimer.
+%% * Redistributions in binary form must reproduce the above copyright
+%% notice, this list of conditions and the following disclaimer in the
+%% documentation and/or other materials provided with the distribution.
+%% * 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
+function session = openraveros_getglobalsession()
+global openraveros_globalsession
+openraveros_startup();
+session = openraveros_globalsession;
Added: pkg/trunk/openrave_planning/openraveros/octave/openraveros_getrobotinfo.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_getrobotinfo.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_getrobotinfo.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,32 @@
+%% robot = openraveros_getrobotinfo(robotinfo)
+%%
+%% parses the RobotInfo.msg into a simpler form for octave consumption
+function robot = openraveros_getrobotinfo(robotinfo)
+
+robot = openraveros_getbodyinfo(robotinfo.bodyinfo);
+
+robot.manips = cell(length(robotinfo.manips),1);
+for i = 1:length(robot.manips)
+ mi = robotinfo.manips{i};
+ robot.manips{i} = struct('baselink',mi.baselink+1,...
+ 'eelink',eelink+1,...
+ 'Tgrasp',reshape(cell2mat(mi.tgrasp.m),[3 4]),...
+ 'handjoints',cell2mat(mi.handjoints),...
+ 'armjoints',cell2mat(mi.armjoints),...
+ 'iksolvername',mi.iksolvername);
+end
+
+robot.sensors = cell(length(robotinfo.sensors),1);
+for i = 1:length(robot.sensors)
+ si = robotinfo.sensors{i};
+ robot.sensors{i} = struct('name',si.name,...
+ 'attachedlink',si.attachedlink+1,...
+ 'Trelative',reshape(cell2mat(si.trelative.m),[3 4]),...
+ 'Tglobal',reshape(cell2mat(si.tglobal.m),[3 4]),...
+ 'type',si.type);
+end
+
+robot.activedof = robotinfo.activedof;
+robot.affinedof = robotinfo.active.affine;
+robot.activejoints = cell2mat(robotinfo.active.joints);
+robot.rotationaxis = cell2mat(robotinfo.active.rotationaxis);
Added: pkg/trunk/openrave_planning/openraveros/octave/openraveros_restart.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_restart.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_restart.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,47 @@
+%% openraveros_restart(sessionserver)
+%%
+%% restars an openraveros session if the current one is invalid
+
+%% Software License Agreement (BSD License)
+%% Copyright (c) 2008, Willow Garage, Inc.
+%% Redistribution and use in source and binary forms, with or without
+%% modification, are permitted provided that the following conditions are met:
+%% * Redistributions of source code must retain the above copyright notice,
+%% this list of conditions and the following disclaimer.
+%% * Redistributions in binary form must reproduce the above copyright
+%% notice, this list of conditions and the following disclaimer in the
+%% documentation and/or other materials provided with the distribution.
+%% * 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
+function openraveros_restart(sessionserver)
+global openraveros_globalsession
+
+openraveros_startup();
+
+if( ~isempty(openraveros_globalsession) )
+ %% send a dummy env_set command
+ res = rosoct_session_call(openraveros_globalsession.id,'env_set',openraveros_env_set());
+ if( ~isempty(res) )
+ %% success
+ return;
+ end
+end
+
+if( ~exist('sessionserver','var') )
+ sessionserver = 'openrave_session';
+end
+openraveros_globalsession = openraveros_createsession(sessionserver);
Added: pkg/trunk/openrave_planning/openraveros/octave/openraveros_rotfromquat.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_rotfromquat.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_rotfromquat.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,16 @@
+% R = openraveros_rotfromquat(quat)
+function R = openraveros_rotfromquat(quat)
+
+R = zeros(3,3);
+qq1 = 2*quat(2)*quat(2);
+qq2 = 2*quat(3)*quat(3);
+qq3 = 2*quat(4)*quat(4);
+R(1,1) = 1 - qq2 - qq3;
+R(1,2) = 2*(quat(2)*quat(3) - quat(1)*quat(4));
+R(1,3) = 2*(quat(2)*quat(4) + quat(1)*quat(3));
+R(2,1) = 2*(quat(2)*quat(3) + quat(1)*quat(4));
+R(2,2) = 1 - qq1 - qq3;
+R(2,3) = 2*(quat(3)*quat(4) - quat(1)*quat(2));
+R(3,1) = 2*(quat(2)*quat(4) - quat(1)*quat(3));
+R(3,2) = 2*(quat(3)*quat(4) + quat(1)*quat(2));
+R(3,3) = 1 - qq1 - qq2;
Added: pkg/trunk/openrave_planning/openraveros/octave/openraveros_setglobalsession.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_setglobalsession.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_setglobalsession.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,36 @@
+%% openraveros_setglobalsession(session)
+%%
+
+%% Software License Agreement (BSD License)
+%% Copyright (c) 2008, Willow Garage, Inc.
+%% Redistribution and use in source and binary forms, with or without
+%% modification, are permitted provided that the following conditions are met:
+%% * Redistributions of source code must retain the above copyright notice,
+%% this list of conditions and the following disclaimer.
+%% * Redistributions in binary form must reproduce the above copyright
+%% notice, this list of conditions and the following disclaimer in the
+%% documentation and/or other materials provided with the distribution.
+%% * 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
+function openraveros_setglobalsession(session)
+global openraveros_globalsession
+
+if( ~isempty(session) && (~isfield(session,'id') || ~isfield(session,'uuid') || ~isfield(session,'server')) )
+ error('session in wrong format');
+end
+
+openraveros_globalsession = session;
\ No newline at end of file
Added: pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m (rev 0)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m 2008-12-14 00:26:09 UTC (rev 8072)
@@ -0,0 +1,66 @@
+%% openraveros_startup(sessionserver)
+%% adds all the necessary paths for the openraveros octave client
+
+%% Software License Agreement (BSD License)
+%% Copyright (c) 2008, Willow Garage, Inc.
+%% Redistribution and use in source and binary forms, with or without
+%% modification, are permitted provided that the following conditions are met:
+%% * Redistributions of source code must retain the above copyright notice,
+%% this list of conditions and the following disclaimer.
+%% * Redistributions in binary form must reproduce the above copyright
+%% notice, this list of conditions and the following disclaimer in the
+%% documentation and/or other materials provided with the distribution.
+%% * 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
+function openraveros_startup(sessionserver,createsession)
+global openraveros_globalsession
+persistent openraveros_initialized
+
+if( ~exist('createsession','var') )
+ createsession = 1;
+end
+
+if( isempty(openraveros_initialized))
+ openraveros_initialized = 1;
+ [status,rosoctpath] = system('rospack find rosoct');
+ rosoctpath = strtrim(rosoctpath);
+ addpath(fullfile(rosoctpath,'octave'));
+
+ rosoct_add_msgs('openraveros');
+ rosoct_add_srvs('openraveros');
+
+ rosoct('shutdown'); % restart the client
+end
+
+if( ~exist('sessionserver','var') )
+ sessionserver = 'openrave_session';
+end
+
+if( createsession && isempty(openraveros_globalsession) )
+ req = openraveros_openrave_session();
+ [localid,res] = rosoct_create_session(sessionserver,req);
+
+ if( ~isempty(localid) && ~isempty(res) )
+ if( res.sessionid==0 )
+ error('bad session id');
+ end
+ openraveros_globalsession.id = localid;
+ openraveros_globalsession.server = sessionserver;
+ openraveros_globalsession.uuid = res.sessionid;
+ display(sprintf('created openraveros session uuid %d',res.sessionid));
+ end
+end
Added: pkg/trunk/openrave_planning/openraveros/octave/orBodyDestroy.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orBodyDestroy.m ...
[truncated message content] |
|
From: <ve...@us...> - 2008-12-14 22:29:31
|
Revision: 8078
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8078&view=rev
Author: veedee
Date: 2008-12-14 22:04:35 +0000 (Sun, 14 Dec 2008)
Log Message:
-----------
first batch commit with 3 libraries (80%, 70%, 50% done) and one simple node. nothing to get excited over yet :)
Added Paths:
-----------
pkg/trunk/mapping/
pkg/trunk/mapping/bag_pcd/
pkg/trunk/mapping/bag_pcd/CMakeLists.txt
pkg/trunk/mapping/bag_pcd/Makefile
pkg/trunk/mapping/bag_pcd/manifest.xml
pkg/trunk/mapping/bag_pcd/src/
pkg/trunk/mapping/bag_pcd/src/bag_pcd.cpp
pkg/trunk/mapping/cloud_geometry/
pkg/trunk/mapping/cloud_geometry/CMakeLists.txt
pkg/trunk/mapping/cloud_geometry/Makefile
pkg/trunk/mapping/cloud_geometry/include/
pkg/trunk/mapping/cloud_geometry/include/cloud_geometry/
pkg/trunk/mapping/cloud_geometry/include/cloud_geometry/lapack.h
pkg/trunk/mapping/cloud_geometry/include/cloud_geometry/nearest.h
pkg/trunk/mapping/cloud_geometry/include/cloud_geometry/norms.h
pkg/trunk/mapping/cloud_geometry/include/cloud_geometry/point.h
pkg/trunk/mapping/cloud_geometry/mainpage.dox
pkg/trunk/mapping/cloud_geometry/manifest.xml
pkg/trunk/mapping/cloud_geometry/src/
pkg/trunk/mapping/cloud_geometry/src/lapack.cpp
pkg/trunk/mapping/cloud_geometry/src/nearest.cpp
pkg/trunk/mapping/cloud_geometry/src/point.cpp
pkg/trunk/mapping/cloud_io/
pkg/trunk/mapping/cloud_io/CMakeLists.txt
pkg/trunk/mapping/cloud_io/Makefile
pkg/trunk/mapping/cloud_io/include/
pkg/trunk/mapping/cloud_io/include/cloud_io/
pkg/trunk/mapping/cloud_io/include/cloud_io/cloud_io.h
pkg/trunk/mapping/cloud_io/mainpage.dox
pkg/trunk/mapping/cloud_io/manifest.xml
pkg/trunk/mapping/cloud_io/src/
pkg/trunk/mapping/cloud_io/src/misc.cpp
pkg/trunk/mapping/cloud_io/src/read.cpp
pkg/trunk/mapping/cloud_io/src/write.cpp
pkg/trunk/mapping/sample_consensus/
pkg/trunk/mapping/sample_consensus/CMakeLists.txt
pkg/trunk/mapping/sample_consensus/Makefile
pkg/trunk/mapping/sample_consensus/include/
pkg/trunk/mapping/sample_consensus/include/sample_consensus/
pkg/trunk/mapping/sample_consensus/include/sample_consensus/lmeds.h
pkg/trunk/mapping/sample_consensus/include/sample_consensus/mlesac.h
pkg/trunk/mapping/sample_consensus/include/sample_consensus/model_types.h
pkg/trunk/mapping/sample_consensus/include/sample_consensus/msac.h
pkg/trunk/mapping/sample_consensus/include/sample_consensus/ransac.h
pkg/trunk/mapping/sample_consensus/include/sample_consensus/rmsac.h
pkg/trunk/mapping/sample_consensus/include/sample_consensus/rransac.h
pkg/trunk/mapping/sample_consensus/include/sample_consensus/sac.h
pkg/trunk/mapping/sample_consensus/include/sample_consensus/sac_model.h
pkg/trunk/mapping/sample_consensus/include/sample_consensus/sac_model_line.h
pkg/trunk/mapping/sample_consensus/include/sample_consensus/sac_model_plane.h
pkg/trunk/mapping/sample_consensus/mainpage.dox
pkg/trunk/mapping/sample_consensus/manifest.xml
pkg/trunk/mapping/sample_consensus/src/
pkg/trunk/mapping/sample_consensus/src/lmeds.cpp
pkg/trunk/mapping/sample_consensus/src/mlesac.cpp
pkg/trunk/mapping/sample_consensus/src/msac.cpp
pkg/trunk/mapping/sample_consensus/src/ransac.cpp
pkg/trunk/mapping/sample_consensus/src/rmsac.cpp
pkg/trunk/mapping/sample_consensus/src/rransac.cpp
pkg/trunk/mapping/sample_consensus/src/sac.cpp
pkg/trunk/mapping/sample_consensus/src/sac_model.cpp
pkg/trunk/mapping/sample_consensus/src/sac_model_line.cpp
pkg/trunk/mapping/sample_consensus/src/sac_model_plane.cpp
pkg/trunk/mapping/sample_consensus/test/
pkg/trunk/mapping/sample_consensus/test/test_plane_fit.cpp
Added: pkg/trunk/mapping/bag_pcd/CMakeLists.txt
===================================================================
--- pkg/trunk/mapping/bag_pcd/CMakeLists.txt (rev 0)
+++ pkg/trunk/mapping/bag_pcd/CMakeLists.txt 2008-12-14 22:04:35 UTC (rev 8078)
@@ -0,0 +1,10 @@
+cmake_minimum_required(VERSION 2.6)
+
+include(rosbuild)
+set(ROS_BUILD_TYPE Release)
+rospack(bag_pcd)
+add_definitions(-Wall)
+
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+
+rospack_add_executable(bag_pcd src/bag_pcd.cpp)
Added: pkg/trunk/mapping/bag_pcd/Makefile
===================================================================
--- pkg/trunk/mapping/bag_pcd/Makefile (rev 0)
+++ pkg/trunk/mapping/bag_pcd/Makefile 2008-12-14 22:04:35 UTC (rev 8078)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Added: pkg/trunk/mapping/bag_pcd/manifest.xml
===================================================================
--- pkg/trunk/mapping/bag_pcd/manifest.xml (rev 0)
+++ pkg/trunk/mapping/bag_pcd/manifest.xml 2008-12-14 22:04:35 UTC (rev 8078)
@@ -0,0 +1,16 @@
+<package>
+ <description>
+ A node which converts 3D point cloud data from BAG log files into PCD (Point Cloud Data) files.
+ </description>
+
+ <author>Radu Bogdan Rusu (ru...@cs...)</author>
+ <license>BSD</license>
+ <url>http://pr.willowgarage.com</url>
+ <review status="unreviewed" notes="beta"/>
+
+ <depend package="roscpp" />
+ <depend package="std_msgs" />
+ <depend package="tf" />
+ <depend package="cloud_io" />
+
+</package>
Added: pkg/trunk/mapping/bag_pcd/src/bag_pcd.cpp
===================================================================
--- pkg/trunk/mapping/bag_pcd/src/bag_pcd.cpp (rev 0)
+++ pkg/trunk/mapping/bag_pcd/src/bag_pcd.cpp 2008-12-14 22:04:35 UTC (rev 8078)
@@ -0,0 +1,168 @@
+/*
+ * Copyright (c) 2008 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ *
+ * 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.
+ *
+ * $Id: bag_pcd.cpp,v 1.0 2008/12/04 12:00:00 rusu Exp $
+ *
+ */
+
+/**
+@mainpage
+
+@htmlinclude manifest.html
+
+\author Radu Bogdan Rusu
+
+@b bag_pcd is a simple node that gets a complete point cloud and saves it into a PCD (Point Cloud Data) format.
+
+ **/
+
+// ROS core
+#include "ros/node.h"
+
+#include "tf/transform_listener.h"
+
+#include <fstream>
+
+#include "std_msgs/PointStamped.h"
+#include "std_msgs/PointCloud.h"
+
+#include "cloud_io/cloud_io.h"
+
+using namespace std_msgs;
+
+class BagToPcd: public ros::node
+{
+ public:
+
+ // ROS messages
+ PointCloud cloud_;
+
+ // Save data to disk ?
+ char fn_[80];
+ bool dump_to_disk_;
+
+ tf::TransformListener tf_;
+
+
+ ////////////////////////////////////////////////////////////////////////////////
+ BagToPcd () : ros::node ("bag_pcd"), dump_to_disk_(false), tf_(*this)
+ {
+ subscribe ("tilt_laser_cloud", cloud_, &BagToPcd::cloud_cb, 1);
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+ virtual ~BagToPcd () { }
+
+ ////////////////////////////////////////////////////////////////////////////////
+ /** \brief Dump a point cloud to disk
+ * \param fn the name of the output file
+ * \param cloud the point cloud data message
+ * \param vx the X coordinate of the viewpoint
+ * \param vy the Y coordinate of the viewpoint
+ * \param vz the Z coordinate of the viewpoint
+ */
+ void
+ saveCloud (char *fn, PointCloud cloud, double vx, double vy, double vz)
+ {
+ std::ofstream fs;
+ fs.precision (5);
+ fs.open (fn);
+
+ int nr_pts = cloud.get_pts_size ();
+ int dim = cloud.get_chan_size ();
+ fs << "# [MetaInfo] Viewpoint " << vx << "," << vy << "," << vz << std::endl;
+ fs << "COLUMNS x y z";
+ for (int d = 0; d < dim; d++)
+ fs << " " << cloud.chan[d].name;
+ fs << std::endl;
+ fs << "POINTS " << nr_pts << std::endl;
+ fs << "DATA ascii" << std::endl;
+
+ for (int i = 0; i < nr_pts; i++)
+ {
+ fs << cloud.pts[i].x << " " << cloud.pts[i].y << " " << cloud.pts[i].z;
+ for (int d = 0; d < dim; d++)
+ fs << " " << cloud.chan[d].vals[i];
+ fs << std::endl;
+ }
+ fs.close ();
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+ // Callback
+ void cloud_cb ()
+ {
+ PointStamped pin, pout;
+ pin.header.frame_id = "laser_tilt_mount_link";
+ pin.point.x = pin.point.y = pin.point.z = 0.0;
+
+ tf_.transformPoint ("base_link", pin, pout);
+
+ fprintf (stderr, "Received %d data points. Viewpoint is <%.3f, %.3f, %.3f>\n", cloud_.pts.size (), pout.point.x, pout.point.y, pout.point.z);
+
+ double c_time = cloud_.header.stamp.sec * 1e3 + cloud_.header.stamp.nsec;
+ sprintf (fn_, "%.0f.pcd", c_time);
+ if (dump_to_disk_)
+ {
+ {
+ // Add information about the viewpoint - rudimentary stuff
+ cloud_.chan.resize (cloud_.chan.size () + 3);
+ cloud_.chan[cloud_.chan.size () - 3].name = "vx";
+ cloud_.chan[cloud_.chan.size () - 2].name = "vy";
+ cloud_.chan[cloud_.chan.size () - 1].name = "vz";
+ cloud_.chan[cloud_.chan.size () - 3].vals.resize (cloud_.pts.size ());
+ cloud_.chan[cloud_.chan.size () - 2].vals.resize (cloud_.pts.size ());
+ cloud_.chan[cloud_.chan.size () - 1].vals.resize (cloud_.pts.size ());
+ for (unsigned int i = 0; i < cloud_.pts.size (); i++)
+ {
+ cloud_.chan[cloud_.chan.size () - 3].vals[i] = pout.point.x;
+ cloud_.chan[cloud_.chan.size () - 2].vals[i] = pout.point.y;
+ cloud_.chan[cloud_.chan.size () - 1].vals[i] = pout.point.z;
+ }
+ cloud_io::savePCDFile (fn_, cloud_, 5);
+ }
+ //saveCloud (fn_, cloud_, pout.point.x, pout.point.y, pout.point.z);
+ fprintf (stderr, "Data saved to %s.\n", fn_);
+ }
+ }
+};
+
+/* ---[ */
+int
+ main (int argc, char** argv)
+{
+ ros::init (argc, argv);
+
+ BagToPcd b;
+ b.dump_to_disk_ = true;
+ b.spin ();
+
+ ros::fini ();
+
+ return (0);
+}
+/* ]--- */
+
Added: pkg/trunk/mapping/cloud_geometry/CMakeLists.txt
===================================================================
--- pkg/trunk/mapping/cloud_geometry/CMakeLists.txt (rev 0)
+++ pkg/trunk/mapping/cloud_geometry/CMakeLists.txt 2008-12-14 22:04:35 UTC (rev 8078)
@@ -0,0 +1,8 @@
+cmake_minimum_required(VERSION 2.6)
+
+include(rosbuild)
+set(ROS_BUILD_TYPE Release)
+rospack(cloud_geometry)
+add_definitions(-Wall)
+
+rospack_add_library(cloud_geometry src/lapack.cpp src/nearest.cpp src/point.cpp)
Added: pkg/trunk/mapping/cloud_geometry/Makefile
===================================================================
--- pkg/trunk/mapping/cloud_geometry/Makefile (rev 0)
+++ pkg/trunk/mapping/cloud_geometry/Makefile 2008-12-14 22:04:35 UTC (rev 8078)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Added: pkg/trunk/mapping/cloud_geometry/include/cloud_geometry/lapack.h
===================================================================
--- pkg/trunk/mapping/cloud_geometry/include/cloud_geometry/lapack.h (rev 0)
+++ pkg/trunk/mapping/cloud_geometry/include/cloud_geometry/lapack.h 2008-12-14 22:04:35 UTC (rev 8078)
@@ -0,0 +1,52 @@
+/*
+ * Copyright (c) 2008 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ *
+ * 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.
+ *
+ * $Id: lapack.h,v 1.0 2008/12/04 12:00:00 rusu Exp $
+ *
+ */
+
+/** \author Radu Bogdan Rusu */
+
+#ifndef _CLOUD_GEOMETRY_LAPACK_H_
+#define _CLOUD_GEOMETRY_LAPACK_H_
+
+#include "Eigen/Core"
+
+extern "C" void dsyev_ (char *jobz, char *uplo, int *n, double *a, int *lda, double *w, double *work, int *lwork, int *info);
+
+namespace cloud_geometry
+{
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Compute the 3 eigen values and eigenvectors for a 3x3 covariance matrix
+ * \param covariance_matrix a 3x3 covariance matrix in Eigen2::Matrix3d format
+ * \param eigen_values the resulted eigenvalues in Eigen2::Vector3d
+ * \param eigen_vectors a 3x3 matrix in Eigen2::Matrix3d format, containing each eigenvector on a new line */
+ bool eigen_cov (Eigen::Matrix3d covariance_matrix, Eigen::Vector3d &eigen_values, Eigen::Matrix3d &eigen_vectors);
+
+}
+
+#endif
Added: pkg/trunk/mapping/cloud_geometry/include/cloud_geometry/nearest.h
===================================================================
--- pkg/trunk/mapping/cloud_geometry/include/cloud_geometry/nearest.h (rev 0)
+++ pkg/trunk/mapping/cloud_geometry/include/cloud_geometry/nearest.h 2008-12-14 22:04:35 UTC (rev 8078)
@@ -0,0 +1,105 @@
+/*
+ * Copyright (c) 2008 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ *
+ * 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.
+ *
+ * $Id: nearest.h,v 1.0 2008/12/04 12:00:00 rusu Exp $
+ *
+ */
+
+/** \author Radu Bogdan Rusu */
+
+#ifndef _CLOUD_GEOMETRY_NEAREST_H_
+#define _CLOUD_GEOMETRY_NEAREST_H_
+
+#include "std_msgs/PointCloud.h"
+#include "std_msgs/Point32.h"
+
+#include "Eigen/Core"
+#include "cloud_geometry/lapack.h"
+
+namespace cloud_geometry
+{
+
+ namespace nearest
+ {
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Compute the centroid of a set of points and return it as a Point32 message.
+ * \param points the input point cloud
+ * \param centroid the output centroid
+ */
+ inline void
+ computeCentroid (std_msgs::PointCloud points, std_msgs::Point32 ¢roid)
+ {
+ // For each point in the cloud
+ for (unsigned int i = 0; i < points.get_pts_size (); i++)
+ {
+ centroid.x += points.pts.at (i).x;
+ centroid.y += points.pts.at (i).y;
+ centroid.z += points.pts.at (i).z;
+ }
+
+ centroid.x /= points.get_pts_size ();
+ centroid.y /= points.get_pts_size ();
+ centroid.z /= points.get_pts_size ();
+ }
+
+ //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+ /** \brief Compute the centroid of a set of points using their indices and return it as a Point32 message.
+ * \param points the input point cloud
+ * \param indices the point cloud indices that need to be used
+ * \param centroid the output centroid
+ */
+ inline void
+ computeCentroid (std_msgs::PointCloud points, std::vector<int> indices, std_msgs::Point32 ¢roid)
+ {
+ // For each point in the cloud
+ for (unsigned int i = 0; i < indices.size (); i++)
+ {
+ centroid.x += points.pts.at (indices.at (i)).x;
+ centroid.y += points.pts.at (indices.at (i)).y;
+ centroid.z += points.pts.at (indices.at (i)).z;
+ }
+
+ centroid.x /= indices.size ();
+ centroid.y /= indices.size ();
+ centroid.z /= indices.size ();
+ }
+
+ void computeCentroid (std_msgs::PointCloud points, std_msgs::PointCloud ¢roid);
+ void computeCentroid (std_msgs::PointCloud points, std::vector<int> indices, std_msgs::PointCloud ¢roid);
+
+ void computeCovarianceMatrix (std_msgs::PointCloud points, Eigen::Matrix3d &covariance_matrix);
+ void computeCovarianceMatrix (std_msgs::PointCloud points, Eigen::Matrix3d &covariance_matrix, std_msgs::Point32 ¢roid);
+ void computeCovarianceMatrix (std_msgs::PointCloud points, std::vector<int> indices, Eigen::Matrix3d &covariance_matrix);
+ void computeCovarianceMatrix (std_msgs::PointCloud points, std::vector<int> indices, Eigen::Matrix3d &covariance_matrix, std_msgs::Point32 ¢roid);
+
+ void computeSurfaceNormalCurvature (std_msgs::PointCloud points, Eigen::Vector4d &plane_parameters, double &curvature);
+ void computeSurfaceNormalCurvature (std_msgs::PointCloud points, std::vector<int> indices, Eigen::Vector4d &plane_parameters, double &curvature);
+
+ }
+}
+
+#endif
Added: pkg/trunk/mapping/cloud_geometry/include/cloud_geometry/norms.h
===================================================================
--- pkg/trunk/mapping/cloud_geometry/include/cloud_geometry/norms.h (rev 0)
+++ pkg/trunk/mapping/cloud_geometry/include/cloud_geometry/norms.h 2008-12-14 22:04:35 UTC (rev 8078)
@@ -0,0 +1,268 @@
+/*
+ * Copyright (c) 2008 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ *
+ * 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.
+ *
+ * $Id: norms.h,v 1.0 2008/12/04 12:00:00 rusu Exp $
+ *
+ */
+
+/** \author Radu Bogdan Rusu */
+
+#ifndef _CLOUD_GEOMETRY_NORMS_H_
+#define _CLOUD_GEOMETRY_NORMS_H_
+
+namespace cloud_geometry
+{
+
+ namespace norms
+ {
+ ////////////////////////////////////////////////////////////////////////////////
+ /** \brief Compute the L1 norm between two nD points (aka aka Manhattan norm,
+ * rectilinear distance, Minkowski's L1 distance, taxi cab metric, or city
+ * block distance)
+ * L1 = Sum (|x_i|), i=1..n
+ * \param A first point
+ * \param B second point
+ * \param dim number of dimensions
+ */
+ inline double
+ L1_Norm (float *A, float *B, int dim)
+ {
+ double norm = 0.0;
+
+ for (int i = 0; i < dim; i++)
+ norm += fabs (A[i] - B[i]);
+
+ return norm;
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+ /** \brief Compute the squared L2 norm between two nD points (aka Euclidean metric)
+ * L2_SQR = Sum (|x_i|^2), i=1..n
+ * \param A first point
+ * \param B second point
+ * \param dim number of dimensions
+ */
+ inline double
+ L2_Norm_SQR (float *A, float *B, int dim)
+ {
+ double norm = 0.0;
+
+ for (int i = 0; i < dim; i++)
+ norm += (A[i] - B[i]) * (A[i] - B[i]);
+
+ return norm;
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+ /** \brief Compute the L2 norm between two nD points (aka Euclidean metric)
+ * L2 = SQRT (Sum (|x_i|^2)), i=1..n
+ * \param A first point
+ * \param B second point
+ * \param dim number of dimensions
+ */
+ inline double
+ L2_Norm (float *A, float *B, int dim)
+ {
+ double norm = 0.0;
+
+ for (int i = 0; i < dim; i++)
+ norm += (A[i] - B[i]) * (A[i] - B[i]);
+
+ return sqrt (norm);
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+ /** \brief Computes the Linf norm between two nD points (aka Minkowski distance,
+ * Chebyshev norm, or supremum norm)
+ * Linf = max(|xi|), i=1..n
+ * \param A first point
+ * \param B second point
+ * \param dim number of dimensions
+ */
+ inline double
+ Linf_Norm (float *A, float *B, int dim)
+ {
+ double norm = 0.0;
+
+ for (int i = 0; i < dim; i++)
+ norm = (fabs (A[i] - B[i]) > norm) ? fabs (A[i] - B[i]) : norm;
+
+ return norm;
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+ /** \brief Computes the Jeffries-Matusita (JM) distance between two nD points (aka
+ * Hellinger distance)
+ * \param A first point
+ * \param B second point
+ * \param dim number of dimensions
+ */
+ inline double
+ JM_Norm (float *A, float *B, int dim)
+ {
+ double norm = 0.0;
+
+ for (int i = 0; i < dim; i++)
+ norm += (sqrt (A[i]) - sqrt (B[i])) * (sqrt (A[i]) - sqrt (B[i]));
+
+ return sqrt (norm);
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+ /** \brief Computes the Bhattacharyya (B) distance between two nD points
+ * \param A first point
+ * \param B second point
+ * \param dim number of dimensions
+ */
+ inline double
+ B_Norm (float *A, float *B, int dim)
+ {
+ double norm = 0.0, result;
+
+ for (int i = 0; i < dim; i++)
+ norm += sqrt (A[i] * B[i]);
+
+ if (norm > 0)
+ result = -log (norm);
+ else
+ result = 0;
+
+ return result;
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+ /** \brief Computes the Sublinear kernel distance between two nD points
+ * \param A first point
+ * \param B second point
+ * \param dim number of dimensions
+ */
+ inline double
+ Sublinear_Norm (float *A, float *B, int dim)
+ {
+ double norm = 0.0;
+
+ for (int i = 0; i < dim; i++)
+ norm += sqrt (fabs (A[i] - B[i]));
+
+ return norm;
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+ /** \brief Computes the Chi-Square (CS) distance between two nD points
+ * \param A first point
+ * \param B second point
+ * \param dim number of dimensions
+ */
+ inline double
+ CS_Norm (float *A, float *B, int dim)
+ {
+ double norm = 0.0;
+
+ for (int i = 0; i < dim; i++)
+ if ((A[i] + B[i]) != 0)
+ norm += (A[i] - B[i]) * (A[i] - B[i]) / (A[i] + B[i]);
+ else
+ norm += 0;
+ return norm;
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+ /** \brief Computes the Divergence distance between two nD points
+ * \param A first point
+ * \param B second point
+ * \param dim number of dimensions
+ */
+ inline double
+ Div_Norm (float *A, float *B, int dim)
+ {
+ double norm = 0.0;
+
+ for (int i = 0; i < dim; i++)
+ if ((A[i] / B[i]) > 0)
+ norm += (A[i] - B[i]) * log (A[i] / B[i]);
+ else
+ norm += 0;
+ return norm;
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+ /** \brief Computes the Patrick-Fisher distance between two nD points (Same as L2 ! - when P1 = P2)
+ * \param A first point
+ * \param B second point
+ * \param dim number of dimensions
+ * \param P1 P1
+ * \param P2 P2
+ */
+ inline double
+ PF_Norm (float *A, float *B, int dim, double P1, double P2)
+ {
+ double norm = 0.0;
+
+ for (int i = 0; i < dim; i++)
+ norm += (P1 * A[i] - P2 * B[i]) * (P1 * A[i] - P2 * B[i]);
+ return sqrt (norm);
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+ /** \brief Computes the Kolmogorov distance between two nD points (Same as L1 ! - when P1 = P2)
+ * \param A first point
+ * \param B second point
+ * \param dim number of dimensions
+ * \param P1 P1
+ * \param P2 P2
+ */
+ inline double
+ K_Norm (float *A, float *B, int dim, double P1, double P2)
+ {
+ double norm = 0.0;
+
+ for (int i = 0; i < dim; i++)
+ norm += fabs (P1 * A[i] - P2 * B[i]);
+ return norm;
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+ /** \brief Computes the Kullback-Leibler distance between two nD points
+ * \param A first point
+ * \param B second point
+ * \param dim number of dimensions
+ */
+ inline double
+ KL_Norm (float *A, float *B, int dim)
+ {
+ double norm = 0.0;
+
+ for (int i = 0; i < dim; i++)
+ if ( (B[i] != 0) && ((A[i] / B[i]) > 0) )
+ norm += A[i] * log (A[i] / B[i]);
+ else
+ norm += 0;
+ return norm;
+ }
+ }
+}
+
+#endif
Added: pkg/trunk/mapping/cloud_geometry/include/cloud_geometry/point.h
===================================================================
--- pkg/trunk/mapping/cloud_geometry/include/cloud_geometry/point.h (rev 0)
+++ pkg/trunk/mapping/cloud_geometry/include/cloud_geometry/point.h 2008-12-14 22:04:35 UTC (rev 8078)
@@ -0,0 +1,154 @@
+/*
+ * Copyright (c) 2008 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ *
+ * 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...
[truncated message content] |
|
From: <is...@us...> - 2008-12-15 02:19:34
|
Revision: 8083
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8083&view=rev
Author: isucan
Date: 2008-12-15 02:19:31 +0000 (Mon, 15 Dec 2008)
Log Message:
-----------
ported motion planning code to using bullet instead of libTF
Modified Paths:
--------------
pkg/trunk/motion_planning/collision_space/include/collision_space/environment.h
pkg/trunk/motion_planning/collision_space/include/collision_space/environmentODE.h
pkg/trunk/motion_planning/collision_space/include/collision_space/util.h
pkg/trunk/motion_planning/collision_space/manifest.xml
pkg/trunk/motion_planning/collision_space/src/collision_space/environmentODE.cpp
pkg/trunk/motion_planning/kinematic_planning/include/RKPBasicRequest.h
pkg/trunk/motion_planning/kinematic_planning/include/RKPConstraintEvaluator.h
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/plan_kinematic_path/manifest.xml
pkg/trunk/motion_planning/planning_node_util/manifest.xml
pkg/trunk/visualization/ogre_visualizer/manifest.xml
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/helpers/robot.cpp
pkg/trunk/world_models/world_3d_map/manifest.xml
Removed Paths:
-------------
pkg/trunk/deprecated/planning_models_with_libTF/
pkg/trunk/motion_planning/collision_space/include/collision_space/environmentOctree.h
pkg/trunk/motion_planning/collision_space/src/collision_space/environmentOctree.cpp
Modified: pkg/trunk/motion_planning/collision_space/include/collision_space/environment.h
===================================================================
--- pkg/trunk/motion_planning/collision_space/include/collision_space/environment.h 2008-12-15 00:40:22 UTC (rev 8082)
+++ pkg/trunk/motion_planning/collision_space/include/collision_space/environment.h 2008-12-15 02:19:31 UTC (rev 8083)
@@ -38,7 +38,6 @@
#define COLLISION_SPACE_ENVIRONMENT_MODEL_
#include <planning_models/kinematic.h>
-#include <profiling_utils/profiler.h>
#include <rosthread/mutex.h>
#include <vector>
#include <string>
Modified: pkg/trunk/motion_planning/collision_space/include/collision_space/environmentODE.h
===================================================================
--- pkg/trunk/motion_planning/collision_space/include/collision_space/environmentODE.h 2008-12-15 00:40:22 UTC (rev 8082)
+++ pkg/trunk/motion_planning/collision_space/include/collision_space/environmentODE.h 2008-12-15 02:19:31 UTC (rev 8083)
@@ -40,9 +40,6 @@
#include <collision_space/environment.h>
#include <ode/ode.h>
-// check() is apparently defined as a macro on OS X (?)
-#undef check
-
/** @htmlinclude ../../manifest.html
A class describing an environment for a kinematic robot using ODE */
@@ -195,8 +192,8 @@
std::vector<Geom*> m_geomsY;
std::vector<Geom*> m_geomsZ;
- void check(std::vector<Geom*>::iterator posStart, std::vector<Geom*>::iterator posEnd,
- Geom *g, void *data, dNearCallback *nearCallback);
+ void checkColl(std::vector<Geom*>::iterator posStart, std::vector<Geom*>::iterator posEnd,
+ Geom *g, void *data, dNearCallback *nearCallback);
};
struct kGeom
Deleted: pkg/trunk/motion_planning/collision_space/include/collision_space/environmentOctree.h
===================================================================
--- pkg/trunk/motion_planning/collision_space/include/collision_space/environmentOctree.h 2008-12-15 00:40:22 UTC (rev 8082)
+++ pkg/trunk/motion_planning/collision_space/include/collision_space/environmentOctree.h 2008-12-15 02:19:31 UTC (rev 8083)
@@ -1,100 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*********************************************************************/
-
-/** \author Ioan Sucan, Matei Ciocarlie */
-
-#ifndef COLLISION_SPACE_ENVIRONMENT_MODEL_OCTREE_
-#define COLLISION_SPACE_ENVIRONMENT_MODEL_OCTREE_
-
-#include <collision_space/environment.h>
-#include <octree.h>
-#include <vector>
-
-/** @htmlinclude ../../manifest.html
-
- A class describing an environment for a kinematic robot using ODE */
-
-namespace collision_space
-{
-
- static const char OCTREE_CELL_EMPTY = 0;
- static const char OCTREE_CELL_OCCUPIED = 1;
-
- class EnvironmentModelOctree : public EnvironmentModel
- {
- public:
-
- EnvironmentModelOctree(void) : EnvironmentModel(),
- m_octree(0.0f, 0.0f, 0.0f, 0.02f, 0.02f, 0.02f, 1, OCTREE_CELL_EMPTY)
- {
- m_octree.setAutoExpand(true);
- m_selfCollision = false;
- }
-
- virtual ~EnvironmentModelOctree(void)
- {
- }
-
- /** Check if a model is in collision */
- virtual bool isCollision(unsigned int model_id);
-
- /** Remove all obstacles from collision model */
- virtual void clearObstacles(void);
-
- /** Add a point cloud to the collision space */
- virtual void addPointCloud(unsigned int n, const double *points, double radius = 0.01);
-
- /** Add a plane to the collision space. Equation it satisfies is a*x+b*y+c*z = d*/
- virtual void addStaticPlane(double a, double b, double c, double d);
-
- /** Add a robot model. Ignore robot links if their name is not specified in the string vector */
- virtual unsigned int addRobotModel(planning_models::KinematicModel *model, const std::vector<std::string> &links);
-
- /** Update the positions of the geometry used in collision detection */
- virtual void updateRobotModel(unsigned int model_id);
-
- /** Add a group of links to be checked for self collision */
- virtual void addSelfCollisionGroup(unsigned int model_id, std::vector<std::string> &links);
-
- const scan_utils::Octree<char>* getOctree(void) const;
-
- protected:
-
- scan_utils::Octree<char> m_octree;
- std::vector<std::vector< planning_models::KinematicModel::Link*> > m_modelLinks;
-
- };
-}
-
-#endif
Modified: pkg/trunk/motion_planning/collision_space/include/collision_space/util.h
===================================================================
--- pkg/trunk/motion_planning/collision_space/include/collision_space/util.h 2008-12-15 00:40:22 UTC (rev 8082)
+++ pkg/trunk/motion_planning/collision_space/include/collision_space/util.h 2008-12-15 02:19:31 UTC (rev 8083)
@@ -37,7 +37,7 @@
#ifndef COLLISION_SPACE_UTIL_
#define COLLISION_SPACE_UTIL_
-#include <libTF/Pose3D.h>
+#include <LinearMath/btTransform.h>
#include <cmath>
/**
@@ -72,7 +72,7 @@
updateInternalData();
}
- void setPose(const libTF::Pose3D &pose)
+ void setPose(const btTransform &pose)
{
m_pose = pose;
updateInternalData();
@@ -86,20 +86,18 @@
bool containsPoint(double x, double y, double z) const
{
- libTF::Position pt(x, y, z);
- return containsPoint(pt);
+ return containsPoint(btVector3(btScalar(x), btScalar(y), btScalar(z)));
}
- virtual bool containsPoint(const libTF::Position &p) const = 0;
+ virtual bool containsPoint(const btVector3 &p) const = 0;
protected:
virtual void updateInternalData(void) = 0;
virtual void useDimensions(const double *dims) = 0;
- libTF::Pose3D m_pose;
- double m_scale;
-
+ btTransform m_pose;
+ double m_scale;
};
class Sphere : public Shape
@@ -114,12 +112,9 @@
{
}
- virtual bool containsPoint(const libTF::Position &p) const
+ virtual bool containsPoint(const btVector3 &p) const
{
- double dx = m_center.x - p.x;
- double dy = m_center.y - p.y;
- double dz = m_center.z - p.z;
- return dx * dx + dy * dy + dz * dz < m_radius2;
+ return (m_center - p).length2() < m_radius2;
}
protected:
@@ -132,20 +127,18 @@
virtual void updateInternalData(void)
{
m_radius2 = m_radius * m_radius * m_scale * m_scale;
-
- m_pose.getPosition(m_center);
+ m_center = m_pose.getOrigin();
}
- libTF::Position m_center;
- double m_radius;
- double m_radius2;
-
+ btVector3 m_center;
+ double m_radius;
+ double m_radius2;
};
class Cylinder : public Shape
{
public:
- Cylinder(void) : Shape()
+ Cylinder(void) : Shape(), m_normalH(btScalar(0.0), btScalar(0.0), btScalar(1.0))
{
m_length = m_radius = 0.0;
}
@@ -154,19 +147,16 @@
{
}
- virtual bool containsPoint(const libTF::Position &p) const
+ virtual bool containsPoint(const btVector3 &p) const
{
- double vx = p.x - m_center.x;
- double vy = p.y - m_center.y;
- double vz = p.z - m_center.z;
+ btVector3 v = p - m_center;
+ double pH = v.dot(m_normalH);
- double pH = vx * m_normalH.x + vy * m_normalH.y + vz * m_normalH.z;
-
if (fabs(pH) > m_length2)
return false;
- double pB1 = vx * m_normalB1.x + vy * m_normalB1.y + vz * m_normalB1.z;
- double pB2 = vx * m_normalB2.x + vy * m_normalB2.y + vz * m_normalB2.z;
+ double pB1 = v.dot(m_normalB1);
+ double pB2 = v.dot(m_normalB2);
return pB1 * pB2 < m_radius2;
}
@@ -182,31 +172,28 @@
virtual void updateInternalData(void)
{
m_radius2 = m_radius * m_radius * m_scale * m_scale;
- m_length2 = m_scale * m_length / 2.0;
+ m_length2 = m_scale * m_length / 2.0;
+ m_center = m_pose.getOrigin();
- m_pose.getPosition(m_center);
+ m_normalH.setValue(btScalar(0.0), btScalar(0.0), btScalar(1.0));
+ m_normalH = m_pose * m_normalH;
- // this can probably be optimized by simply taking
- // the columns of the transform matrix
- m_normalH.x = m_normalH.y = 0.0; m_normalH.z = 1.0;
- m_pose.applyToVector(m_normalH);
-
- m_normalB1.y = m_normalB1.z = 0.0; m_normalB1.x = 1.0;
- m_pose.applyToVector(m_normalB1);
-
- m_normalB2.x = m_normalB2.z = 0.0; m_normalB2.y = 1.0;
- m_pose.applyToVector(m_normalB2);
+ m_normalB1.setValue(btScalar(1.0), btScalar(0.0), btScalar(0.0));
+ m_normalB1 = m_pose * m_normalB1;
+
+ m_normalB2.setValue(btScalar(0.0), btScalar(1.0), btScalar(0.0));
+ m_normalB2 = m_pose * m_normalB2;
}
- libTF::Position m_center;
- libTF::Vector m_normalH;
- libTF::Vector m_normalB1;
- libTF::Vector m_normalB2;
+ btVector3 m_center;
+ btVector3 m_normalH;
+ btVector3 m_normalB1;
+ btVector3 m_normalB2;
- double m_length;
- double m_length2;
- double m_radius;
- double m_radius2;
+ double m_length;
+ double m_length2;
+ double m_radius;
+ double m_radius2;
};
@@ -222,23 +209,20 @@
{
}
- virtual bool containsPoint(const libTF::Position &p) const
+ virtual bool containsPoint(const btVector3 &p) const
{
- double vx = p.x - m_center.x;
- double vy = p.y - m_center.y;
- double vz = p.z - m_center.z;
+ btVector3 v = p - m_center;
+ double pL = v.dot(m_normalL);
- double pL = vx * m_normalL.x + vy * m_normalL.y + vz * m_normalL.z;
-
if (fabs(pL) > m_length2)
return false;
- double pW = vx * m_normalW.x + vy * m_normalW.y + vz * m_normalW.z;
+ double pW = v.dot(m_normalW);
if (fabs(pW) > m_width2)
return false;
- double pH = vx * m_normalH.x + vy * m_normalH.y + vz * m_normalH.z;
+ double pH = v.dot(m_normalH);
if (fabs(pH) > m_height2)
return false;
@@ -261,33 +245,32 @@
m_width2 = m_scale * m_width / 2.0;
m_height2 = m_scale * m_height / 2.0;
- m_pose.getPosition(m_center);
+ m_center = m_pose.getOrigin();
- m_normalH.x = m_normalH.y = 0.0; m_normalH.z = 1.0;
- m_pose.applyToVector(m_normalH);
-
- m_normalL.y = m_normalL.z = 0.0; m_normalL.x = 1.0;
- m_pose.applyToVector(m_normalL);
-
- m_normalW.x = m_normalW.z = 0.0; m_normalW.y = 1.0;
- m_pose.applyToVector(m_normalW);
+ m_normalH.setValue(btScalar(0.0), btScalar(0.0), btScalar(1.0));
+ m_normalH = m_pose * m_normalH;
+
+ m_normalL.setValue(btScalar(1.0), btScalar(0.0), btScalar(0.0));
+ m_normalL = m_pose * m_normalL;
+
+ m_normalW.setValue(btScalar(0.0), btScalar(1.0), btScalar(0.0));
+ m_normalW = m_pose * m_normalW;
}
- libTF::Position m_center;
- libTF::Vector m_normalL;
- libTF::Vector m_normalW;
- libTF::Vector m_normalH;
+ btVector3 m_center;
+ btVector3 m_normalL;
+ btVector3 m_normalW;
+ btVector3 m_normalH;
- double m_length;
- double m_width;
- double m_height;
- double m_length2;
- double m_width2;
- double m_height2;
+ double m_length;
+ double m_width;
+ double m_height;
+ double m_length2;
+ double m_width2;
+ double m_height2;
};
}
}
#endif
-
Modified: pkg/trunk/motion_planning/collision_space/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/collision_space/manifest.xml 2008-12-15 00:40:22 UTC (rev 8082)
+++ pkg/trunk/motion_planning/collision_space/manifest.xml 2008-12-15 02:19:31 UTC (rev 8083)
@@ -8,19 +8,9 @@
<review status="unreviewed" notes=""/>
<depend package="rosthread"/>
- <depend package="planning_models_with_libTF"/>
-
-<!--
- We currently disable Octree collision checking since dependency on
- scan_utils means a lot of compile time and collision checking with
- octrees is not capable of certain operations. Octrees could be used
- in a combined collision checker (combined with ODE, for example).
-
- <depend package="scan_utils"/>
--->
-
+ <depend package="planning_models"/>
<depend package="opende"/>
- <depend package="profiling_utils"/>
+ <depend package="bullet"/>
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lcollision_space"/>
Modified: pkg/trunk/motion_planning/collision_space/src/collision_space/environmentODE.cpp
===================================================================
--- pkg/trunk/motion_planning/collision_space/src/collision_space/environmentODE.cpp 2008-12-15 00:40:22 UTC (rev 8082)
+++ pkg/trunk/motion_planning/collision_space/src/collision_space/environmentODE.cpp 2008-12-15 02:19:31 UTC (rev 8083)
@@ -128,13 +128,14 @@
for (unsigned int i = 0 ; i < n ; ++i)
{
- libTF::Pose3D &pose = m_kgeoms[model_id].geom[i]->link->globalTrans;
- dGeomID geom = m_kgeoms[model_id].geom[i]->geom;
+ btTransform &pose = m_kgeoms[model_id].geom[i]->link->globalTrans;
+ dGeomID geom = m_kgeoms[model_id].geom[i]->geom;
- libTF::Position pos = pose.getPosition();
- dGeomSetPosition(geom, pos.x, pos.y, pos.z);
- libTF::Quaternion quat = pose.getQuaternion();
- dQuaternion q; q[0] = quat.w; q[1] = quat.x; q[2] = quat.y; q[3] = quat.z;
+ btVector3 pos = pose.getOrigin();
+ dGeomSetPosition(geom, pos.getX(), pos.getY(), pos.getZ());
+ btQuaternion quat = pose.getRotation();
+ dQuaternion q;
+ q[0] = quat.getW(); q[1] = quat.getX(); q[2] = quat.getY(); q[3] = quat.getZ();
dGeomSetQuaternion(geom, q);
}
}
@@ -178,8 +179,8 @@
}
}
-void collision_space::EnvironmentModelODE::ODECollide2::check(std::vector<Geom*>::iterator posStart, std::vector<Geom*>::iterator posEnd,
- Geom *g, void *data, dNearCallback *nearCallback)
+void collision_space::EnvironmentModelODE::ODECollide2::checkColl(std::vector<Geom*>::iterator posStart, std::vector<Geom*>::iterator posEnd,
+ Geom *g, void *data, dNearCallback *nearCallback)
{
/* posStart now identifies the first geom which has an AABB
that could overlap the AABB of geom on the X axis. posEnd
@@ -235,23 +236,23 @@
if (d3 > CUTOFF)
{
if (d3 <= d2 && d3 <= d1)
- check(posStart3, posEnd3, &g, data, nearCallback);
+ checkColl(posStart3, posEnd3, &g, data, nearCallback);
else
if (d2 <= d3 && d2 <= d1)
- check(posStart2, posEnd2, &g, data, nearCallback);
+ checkColl(posStart2, posEnd2, &g, data, nearCallback);
else
- check(posStart1, posEnd1, &g, data, nearCallback);
+ checkColl(posStart1, posEnd1, &g, data, nearCallback);
}
else
- check(posStart3, posEnd3, &g, data, nearCallback);
+ checkColl(posStart3, posEnd3, &g, data, nearCallback);
}
}
else
- check(posStart2, posEnd2, &g, data, nearCallback);
+ checkColl(posStart2, posEnd2, &g, data, nearCallback);
}
}
else
- check(posStart1, posEnd1, &g, data, nearCallback);
+ checkColl(posStart1, posEnd1, &g, data, nearCallback);
}
}
@@ -294,8 +295,6 @@
CollisionData cdata;
cdata.collides = false;
- profiling_utils::Profiler::Begin("Check self collision");
-
/* check self collision */
if (m_selfCollision)
{
@@ -332,10 +331,6 @@
/* check collision with standalone ode bodies */
OUT1:
- profiling_utils::Profiler::End("Check self collision");
-
- profiling_utils::Profiler::Begin("Check basic objects collision");
-
if (!cdata.collides)
{
for (int i = m_kgeoms[model_id].geom.size() - 1 ; i >= 0 ; --i)
@@ -364,9 +359,6 @@
/* check collision with pointclouds */
OUT2:
- profiling_utils::Profiler::End("Check basic objects collision");
-
- profiling_utils::Profiler::Begin("Check pointcloud collision");
if (!cdata.collides)
{
m_collide2.setup();
@@ -374,8 +366,6 @@
m_collide2.collide(m_kgeoms[model_id].geom[i]->geom, reinterpret_cast<void*>(&cdata), nearCallbackFn);
}
- profiling_utils::Profiler::End("Check pointcloud collision");
-
return cdata.collides;
}
Deleted: pkg/trunk/motion_planning/collision_space/src/collision_space/environmentOctree.cpp
===================================================================
--- pkg/trunk/motion_planning/collision_space/src/collision_space/environmentOctree.cpp 2008-12-15 00:40:22 UTC (rev 8082)
+++ pkg/trunk/motion_planning/collision_space/src/collision_space/environmentOctree.cpp 2008-12-15 02:19:31 UTC (rev 8083)
@@ -1,159 +0,0 @@
-/*********************************************************************
-* Software License Agreement (BSD License)
-*
-* Copyright (c) 2008, Willow Garage, Inc.
-* All rights reserved.
-*
-* Redistribution and use in source and binary forms, with or without
-* modification, are permitted provided that the following conditions
-* are met:
-*
-* * Redistributions of source code must retain the above copyright
-* notice, this list of conditions and the following disclaimer.
-* * Redistributions in binary form must reproduce the above
-* copyright notice, this list of conditions and the following
-* disclaimer in the documentation and/or other materials provided
-* with the distribution.
-* * Neither the name of the Willow Garage nor the names of its
-* contributors may be used to endorse or promote products derived
-* from this software without specific prior written permission.
-*
-* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-* POSSIBILITY OF SUCH DAMAGE.
-*********************************************************************/
-
-/** \author Ioan Sucan, Matei Ciocarlie */
-
-#include <collision_space/environmentOctree.h>
-
-unsigned int collision_space::EnvironmentModelOctree::addRobotModel(planning_models::KinematicModel *model, const std::vector<std::string> &links)
-{
- unsigned int id = collision_space::EnvironmentModel::addRobotModel(model, links);
- if (id <= m_modelLinks.size())
- m_modelLinks.resize(id + 1);
-
- std::vector< planning_models::KinematicModel::Link*> allLinks;
-
- std::map<std::string, bool> exists;
- for (unsigned int i = 0 ; i < links.size() ; ++i)
- exists[links[i]] = true;
-
- for (unsigned int i = 0 ; i < allLinks.size() ; ++i)
- if (exists.find(allLinks[i]->name) != exists.end())
- m_modelLinks[id].push_back(allLinks[i]);
-
- return id;
-}
-
-void collision_space::EnvironmentModelOctree::updateRobotModel(unsigned int model_id)
-{
-}
-
-bool collision_space::EnvironmentModelOctree::isCollision(unsigned int model_id)
-{
- bool result = false;
-
- const std::vector<planning_models::KinematicModel::Link*> &links = m_modelLinks[model_id];
-
- for (unsigned int i = 0 ; !result && i < links.size() ; ++i)
- {
- switch (links[i]->shape->type)
- {
- case planning_models::KinematicModel::Shape::BOX:
- {
- /** \todo math here is a bit clumsy.... should be fixed when bette libTF is available */
- NEWMAT::Matrix mat = links[i]->globalTrans.asMatrix();
- float axes[3][3];
-
- for (unsigned int j=0; j<3; j++)
- for (unsigned int k=0; k<3; k++)
- axes[j][k] = mat.element(j,k);
-
- libTF::Position libTFpos;
- links[i]->globalTrans.getPosition(libTFpos);
- float pos[3] = {libTFpos.x, libTFpos.y, libTFpos.z};
- const double *size = static_cast<planning_models::KinematicModel::Box*>(links[i]->shape)->size;
- const float sizef[3] = {size[0], size[1], size[2]};
-
- result = m_octree.intersectsBox(pos, sizef, axes);
- }
-
- break;
-
- case planning_models::KinematicModel::Shape::CYLINDER:
- {
- NEWMAT::Matrix mat = links[i]->globalTrans.asMatrix();
- float axes[3][3];
-
- for (unsigned int j=0; j<3; j++)
- for (unsigned int k=0; k<3; k++)
- axes[j][k] = mat.element(j,k);
-
- libTF::Position libTFpos;
- links[i]->globalTrans.getPosition(libTFpos);
- float pos[3] = {libTFpos.x, libTFpos.y, libTFpos.z };
- float radius = static_cast<planning_models::KinematicModel::Cylinder*>(links[i]->shape)->radius;
- float length = static_cast<planning_models::KinematicModel::Cylinder*>(links[i]->shape)->length;
- float L = radius * 2.0;
- const float sizef[3] = {length, L, L};
-
- result = m_octree.intersectsBox(pos, sizef, axes);
- }
- break;
-
- case planning_models::KinematicModel::Shape::SPHERE:
- {
- libTF::Position libTFpos;
- links[i]->globalTrans.getPosition(libTFpos);
- float pos[3] = {libTFpos.x, libTFpos.y, libTFpos.z};
- float radius = static_cast<planning_models::KinematicModel::Sphere*>(links[i]->shape)->radius;
- result = m_octree.intersectsSphere(pos, radius);
- }
- break;
- default:
- fprintf(stderr, "Geometry type not implemented: %d\n", links[i]->shape->type);
- break;
- }
- }
-
- return result;
-}
-
-void collision_space::EnvironmentModelOctree::addPointCloud(unsigned int n, const double *points, double radius)
-{
- for (unsigned int i = 0 ; i < n ; ++i)
- {
- unsigned int i3 = i * 3;
- m_octree.insert(points[i3], points[i3 + 1], points[i3 + 2], OCTREE_CELL_OCCUPIED);
- }
-}
-
-void collision_space::EnvironmentModelOctree::clearObstacles(void)
-{
- m_octree.clear();
-}
-
-const scan_utils::Octree<char>* collision_space::EnvironmentModelOctree::getOctree(void) const
-{
- return &m_octree;
-}
-
-void collision_space::EnvironmentModelOctree::addStaticPlane(double a, double b, double c, double d)
-{
- fprintf(stderr, "Octree collision checking does not support planes\n");
-}
-
-void collision_space::EnvironmentModelOctree::addSelfCollisionGroup(unsigned int model_id, std::vector<std::string> &links)
-{
- fprintf(stderr, "Octree collision checking does not support self collision\n");
-}
Modified: pkg/trunk/motion_planning/kinematic_planning/include/RKPBasicRequest.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/RKPBasicRequest.h 2008-12-15 00:40:22 UTC (rev 8082)
+++ pkg/trunk/motion_planning/kinematic_planning/include/RKPBasicRequest.h 2008-12-15 02:19:31 UTC (rev 8083)
@@ -269,11 +269,7 @@
double bestDifference = 0.0;
m->collisionSpace->lock();
- profiling_utils::Profiler::Start();
-
computePlan(psetup, req.times, req.allowed_time, req.interpolate, bestPath, bestDifference);
-
- profiling_utils::Profiler::Stop();
m->collisionSpace->unlock();
/* fill in the results */
Modified: pkg/trunk/motion_planning/kinematic_planning/include/RKPConstraintEvaluator.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/RKPConstraintEvaluator.h 2008-12-15 00:40:22 UTC (rev 8082)
+++ pkg/trunk/motion_planning/kinematic_planning/include/RKPConstraintEvaluator.h 2008-12-15 02:19:31 UTC (rev 8083)
@@ -105,33 +105,30 @@
void evaluate(double *distPos, double *distAng)
{
if (m_link)
- {
- switch (m_pc.type)
+ {
+ if (distPos)
{
- case robot_msgs::PoseConstraint::ONLY_POSITION:
- if (distPos)
+ if ((m_pc.type == robot_msgs::PoseConstraint::ONLY_POSITION || m_pc.type == robot_msgs::PoseConstraint::COMPLETE_POSE))
+ {
+ btVector3 bodyPos = m_link->globalTrans.getOrigin();
+ double dx = bodyPos.getX() - m_pc.pose.position.x;
+ double dy = bodyPos.getY() - m_pc.pose.position.y;
+ double dz = bodyPos.getZ() - m_pc.pose.position.z;
+ *distPos = dx * dx + dy * dy + dz * dz;
+ }
+ else
+ *distPos = 0.0;
+ }
+
+ if (distAng)
+ {
+ if ((m_pc.type == robot_msgs::PoseConstraint::ONLY_ORIENTATION || m_pc.type == robot_msgs::PoseConstraint::COMPLETE_POSE))
{
- libTF::Position bodyPos;
- m_link->globalTrans.getPosition(bodyPos);
-
- double dx = bodyPos.x - m_pc.pose.position.x;
- double dy = bodyPos.y - m_pc.pose.position.y;
- double dz = bodyPos.z - m_pc.pose.position.z;
-
- *distPos = dx * dx + dy * dy + dz * dz;
+ btQuaternion quat(m_pc.pose.orientation.x, m_pc.pose.orientation.y, m_pc.pose.orientation.z, m_pc.pose.orientation.w);
+ *distAng = quat.angle(m_link->globalTrans.getRotation());
}
- if (distAng)
+ else
*distAng = 0.0;
- break;
-
- case robot_msgs::PoseConstraint::ONLY_ORIENTATION:
- case robot_msgs::PoseConstraint::COMPLETE_POSE:
- default:
- if (distPos)
- *distPos = 0.0;
- if (distAng)
- *distAng = 0.0;
- break;
}
}
else
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-12-15 00:40:22 UTC (rev 8082)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-12-15 02:19:31 UTC (rev 8083)
@@ -466,7 +466,5 @@
else
usage(argv[0]);
- profiling_utils::Profiler::Status();
-
return 0;
}
Modified: pkg/trunk/motion_planning/plan_kinematic_path/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/manifest.xml 2008-12-15 00:40:22 UTC (rev 8082)
+++ pkg/trunk/motion_planning/plan_kinematic_path/manifest.xml 2008-12-15 02:19:31 UTC (rev 8083)
@@ -7,6 +7,6 @@
<depend package="robot_msgs" />
<depend package="robot_srvs" />
<depend package="planning_node_util" />
- <depend package="planning_models_with_libTF" />
+ <depend package="planning_models" />
<depend package="pr2_mechanism_controllers" />
</package>
Modified: pkg/trunk/motion_planning/planning_node_util/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/planning_node_util/manifest.xml 2008-12-15 00:40:22 UTC (rev 8082)
+++ pkg/trunk/motion_planning/planning_node_util/manifest.xml 2008-12-15 02:19:31 UTC (rev 8083)
@@ -13,7 +13,7 @@
<depend package="robot_msgs"/>
<depend package="tf"/>
<depend package="wg_robot_description_parser"/>
- <depend package="planning_models_with_libTF"/>
+ <depend package="planning_models"/>
<depend package="collision_space"/>
<export>
Modified: pkg/trunk/visualization/ogre_visualizer/m...
[truncated message content] |
|
From: <is...@us...> - 2008-12-15 02:20:54
|
Revision: 8084
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8084&view=rev
Author: isucan
Date: 2008-12-15 02:20:48 +0000 (Mon, 15 Dec 2008)
Log Message:
-----------
deprecating plan_kinematic_path
Added Paths:
-----------
pkg/trunk/deprecated/plan_kinematic_path/
pkg/trunk/deprecated/plan_kinematic_path/manifest.xml
Removed Paths:
-------------
pkg/trunk/deprecated/plan_kinematic_path/manifest.xml
pkg/trunk/motion_planning/plan_kinematic_path/
Property changes on: pkg/trunk/deprecated/plan_kinematic_path
___________________________________________________________________
Added: svn:mergeinfo
+
Deleted: pkg/trunk/deprecated/plan_kinematic_path/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/manifest.xml 2008-12-15 00:40:22 UTC (rev 8082)
+++ pkg/trunk/deprecated/plan_kinematic_path/manifest.xml 2008-12-15 02:20:48 UTC (rev 8084)
@@ -1,12 +0,0 @@
-<package>
- <description>Test package that can plan a kinematic path</description>
- <author>Ioan A. Sucan</author>
- <license>BSD</license>
- <review status="unreviewed" notes=""/>
- <depend package="roscpp" />
- <depend package="robot_msgs" />
- <depend package="robot_srvs" />
- <depend package="planning_node_util" />
- <depend package="planning_models_with_libTF" />
- <depend package="pr2_mechanism_controllers" />
-</package>
Copied: pkg/trunk/deprecated/plan_kinematic_path/manifest.xml (from rev 8083, pkg/trunk/motion_planning/plan_kinematic_path/manifest.xml)
===================================================================
--- pkg/trunk/deprecated/plan_kinematic_path/manifest.xml (rev 0)
+++ pkg/trunk/deprecated/plan_kinematic_path/manifest.xml 2008-12-15 02:20:48 UTC (rev 8084)
@@ -0,0 +1,12 @@
+<package>
+ <description>Test package that can plan a kinematic path</description>
+ <author>Ioan A. Sucan</author>
+ <license>BSD</license>
+ <review status="unreviewed" notes=""/>
+ <depend package="roscpp" />
+ <depend package="robot_msgs" />
+ <depend package="robot_srvs" />
+ <depend package="planning_node_util" />
+ <depend package="planning_models" />
+ <depend package="pr2_mechanism_controllers" />
+</package>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rdi...@us...> - 2008-12-15 18:02:45
|
Revision: 8092
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8092&view=rev
Author: rdiankov
Date: 2008-12-15 18:02:38 +0000 (Mon, 15 Dec 2008)
Log Message:
-----------
fixed many bugs in openraveros server, viewers can be freely attached, environments can be cloned
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/openrave_planning/openraveros/ROS_BUILD_BLACKLIST
pkg/trunk/openrave_planning/openraveros/msg/BodyInfo.msg
pkg/trunk/openrave_planning/openraveros/msg/RobotInfo.msg
pkg/trunk/openrave_planning/openraveros/octave/openraveros_getbodyinfo.m
pkg/trunk/openrave_planning/openraveros/octave/openraveros_getrobotinfo.m
pkg/trunk/openrave_planning/openraveros/octave/openraveros_restart.m
pkg/trunk/openrave_planning/openraveros/octave/openraveros_rotfromquat.m
pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m
pkg/trunk/openrave_planning/openraveros/octave/orBodyGetAABB.m
pkg/trunk/openrave_planning/openraveros/octave/orBodyGetLinks.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvSetOptions.m
pkg/trunk/openrave_planning/openraveros/octave/orRobotGetDOFLimits.m
pkg/trunk/openrave_planning/openraveros/src/openraveros.cpp
pkg/trunk/openrave_planning/openraveros/src/openraveros.h
pkg/trunk/openrave_planning/openraveros/src/rosserver.h
pkg/trunk/openrave_planning/openraveros/src/session.h
pkg/trunk/openrave_planning/openraveros/srv/env_set.srv
pkg/trunk/openrave_planning/openraveros/srv/openrave_session.srv
pkg/trunk/openrave_planning/openraveros/test/testsessioncreation.m
Added Paths:
-----------
pkg/trunk/openrave_planning/openraveros/test/addexamplesdir.m
pkg/trunk/openrave_planning/openraveros/test/testhanoi.m
Removed Paths:
-------------
pkg/trunk/openrave_planning/openraveros/srv/body_getlinks.srv
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2008-12-15 17:39:49 UTC (rev 8091)
+++ pkg/trunk/3rdparty/openrave/Makefile 2008-12-15 18:02:38 UTC (rev 8092)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 549
+SVN_REVISION = -r 554
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/openrave_planning/openraveros/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/openrave_planning/openraveros/ROS_BUILD_BLACKLIST 2008-12-15 17:39:49 UTC (rev 8091)
+++ pkg/trunk/openrave_planning/openraveros/ROS_BUILD_BLACKLIST 2008-12-15 18:02:38 UTC (rev 8092)
@@ -1,2 +1,2 @@
-depends on roscpp sessions, which can be found in
-https://ros.svn.sourceforge.net/svnroot/ros/branches/sessions-rosen/core/roscpp
+depends on new roscpp protocol, which can be found in this branch:
+https://ros.svn.sourceforge.net/svnroot/ros/branches/sessions-dec2008/
Modified: pkg/trunk/openrave_planning/openraveros/msg/BodyInfo.msg
===================================================================
--- pkg/trunk/openrave_planning/openraveros/msg/BodyInfo.msg 2008-12-15 17:39:49 UTC (rev 8091)
+++ pkg/trunk/openrave_planning/openraveros/msg/BodyInfo.msg 2008-12-15 18:02:38 UTC (rev 8092)
@@ -33,4 +33,4 @@
uint16 Req_Links=2
uint16 Req_LinkNames=4
uint16 Req_JointLimits=8
-uint16 Req_Names=16 # if set, fills filename, name, and type
\ No newline at end of file
+uint16 Req_Names=16 # if set, fills filename, name, and type
Modified: pkg/trunk/openrave_planning/openraveros/msg/RobotInfo.msg
===================================================================
--- pkg/trunk/openrave_planning/openraveros/msg/RobotInfo.msg 2008-12-15 17:39:49 UTC (rev 8091)
+++ pkg/trunk/openrave_planning/openraveros/msg/RobotInfo.msg 2008-12-15 18:02:38 UTC (rev 8092)
@@ -10,7 +10,12 @@
# information about the active dofs
ActiveDOFs active
+# joint limits
+float32[] activelowerlimit
+float32[] activeupperlimit
+
# upper 8 bits
uint16 Req_Manipulators=256
uint16 Req_Sensors=512
uint16 Req_ActiveDOFs=1024
+uint16 Req_ActiveLimits=2048
Modified: pkg/trunk/openrave_planning/openraveros/octave/openraveros_getbodyinfo.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_getbodyinfo.m 2008-12-15 17:39:49 UTC (rev 8091)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_getbodyinfo.m 2008-12-15 18:02:38 UTC (rev 8092)
@@ -14,7 +14,7 @@
body.T = reshape(cell2mat(bodyinfo.transform.m),[3 4]);
body.jointvalues = cell2mat(bodyinfo.jointvalues);
-body.links = zeros(1,length(bodyinfo.links));
+body.links = zeros(12,length(bodyinfo.links));
for i = 1:length(bodyinfo.links)
body.links(:,i) = cell2mat(bodyinfo.links{i}.m);
end
Modified: pkg/trunk/openrave_planning/openraveros/octave/openraveros_getrobotinfo.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_getrobotinfo.m 2008-12-15 17:39:49 UTC (rev 8091)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_getrobotinfo.m 2008-12-15 18:02:38 UTC (rev 8092)
@@ -30,3 +30,7 @@
robot.affinedof = robotinfo.active.affine;
robot.activejoints = cell2mat(robotinfo.active.joints);
robot.rotationaxis = cell2mat(robotinfo.active.rotationaxis);
+
+robot.activelowerlimit = cell2mat(bodyinfo.activelowerlimit);
+robot.activeupperlimit = cell2mat(bodyinfo.activeupperlimit);
+
Modified: pkg/trunk/openrave_planning/openraveros/octave/openraveros_restart.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_restart.m 2008-12-15 17:39:49 UTC (rev 8091)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_restart.m 2008-12-15 18:02:38 UTC (rev 8092)
@@ -1,4 +1,4 @@
-%% openraveros_restart(sessionserver)
+%% openraveros_restart(sessionserver, viewer)
%%
%% restars an openraveros session if the current one is invalid
@@ -27,7 +27,7 @@
%% POSSIBILITY OF SUCH DAMAGE.
%%
%% author: Rosen Diankov
-function openraveros_restart(sessionserver)
+function openraveros_restart(sessionserver,viewer)
global openraveros_globalsession
openraveros_startup();
@@ -45,3 +45,15 @@
sessionserver = 'openrave_session';
end
openraveros_globalsession = openraveros_createsession(sessionserver);
+
+if( ~exist('viewer','var') )
+ viewer = 'qtcoin';
+end
+
+if( ~isempty(viewer) && ~isempty(openraveros_globalsession) )
+ %% set the viewer
+ reqset = openraveros_env_set();
+ reqset.setmask = reqset.Set_Viewer();
+ reqset.viewer = viewer;
+ resset = rosoct_session_call(openraveros_globalsession.id,'env_set',reqset);
+end
Modified: pkg/trunk/openrave_planning/openraveros/octave/openraveros_rotfromquat.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_rotfromquat.m 2008-12-15 17:39:49 UTC (rev 8091)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_rotfromquat.m 2008-12-15 18:02:38 UTC (rev 8092)
@@ -1,4 +1,6 @@
-% R = openraveros_rotfromquat(quat)
+%% R = openraveros_rotfromquat(quat)
+%%
+%% quat - the format is [cos(angle/2) axis*sin(angle/2)]
function R = openraveros_rotfromquat(quat)
R = zeros(3,3);
Modified: pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m 2008-12-15 17:39:49 UTC (rev 8091)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m 2008-12-15 18:02:38 UTC (rev 8092)
@@ -1,4 +1,4 @@
-%% openraveros_startup(sessionserver)
+%% openraveros_startup(sessionserver, createsession, viewer)
%% adds all the necessary paths for the openraveros octave client
%% Software License Agreement (BSD License)
@@ -26,7 +26,7 @@
%% POSSIBILITY OF SUCH DAMAGE.
%%
%% author: Rosen Diankov
-function openraveros_startup(sessionserver,createsession)
+function openraveros_startup(sessionserver,createsession, veiwer)
global openraveros_globalsession
persistent openraveros_initialized
@@ -50,8 +50,13 @@
sessionserver = 'openrave_session';
end
+if( ~exist('viewer','var') )
+ viewer = 'qtcoin';
+end
+
if( createsession && isempty(openraveros_globalsession) )
req = openraveros_openrave_session();
+ req.viewer = viewer; % default viewer
[localid,res] = rosoct_create_session(sessionserver,req);
if( ~isempty(localid) && ~isempty(res) )
@@ -61,6 +66,6 @@
openraveros_globalsession.id = localid;
openraveros_globalsession.server = sessionserver;
openraveros_globalsession.uuid = res.sessionid;
- display(sprintf('created openraveros session uuid %d',res.sessionid));
+ %display(sprintf('created openraveros session uuid %d',res.sessionid));
end
end
Modified: pkg/trunk/openrave_planning/openraveros/octave/orBodyGetAABB.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orBodyGetAABB.m 2008-12-15 17:39:49 UTC (rev 8091)
+++ pkg/trunk/openrave_planning/openraveros/octave/orBodyGetAABB.m 2008-12-15 18:02:38 UTC (rev 8092)
@@ -11,7 +11,7 @@
res = rosoct_session_call(session.id,'body_getaabb',req);
if(~isempty(res))
- aabb = [res.center res.extents];
+ aabb = [cell2mat(res.box.center) cell2mat(res.box.extents)];
else
aabb = [];
end
Modified: pkg/trunk/openrave_planning/openraveros/octave/orBodyGetLinks.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orBodyGetLinks.m 2008-12-15 17:39:49 UTC (rev 8091)
+++ pkg/trunk/openrave_planning/openraveros/octave/orBodyGetLinks.m 2008-12-15 18:02:38 UTC (rev 8092)
@@ -1,22 +1,24 @@
-% values = orBodyGetLinks(bodyid)
+% links = orBodyGetLinks(bodyid)
%
% Returns the transformations of all the body's links in a 12 x L matrix. Where L
% is the number of links and each column is a 3x4 transformation
% (use T=reshape(., [3 4]) to recover).
% T * [X;1] = Xnew
-function values = orBodyGetLinks(bodyid)
+function links = orBodyGetLinks(bodyid)
+
+%% get some body info
session = openraveros_getglobalsession();
-req = openraveros_body_getlinks();
+req = openraveros_env_getbodies();
req.bodyid = bodyid;
-res = rosoct_session_call(session.id,'body_getlinks',req);
+req.options = openraveros_BodyInfo().Req_Links();
+res = rosoct_session_call(session.id,'env_getbodies',req);
+if( ~isempty(res) )
-if(~isempty(res))
- values = zeros(1,length(res.links));
- for i = 1:length(res.links)
- values(:,i) = cell2mat(res.links{i}.m);
+ links = zeros(12,length(res.bodies{1}.links));
+ for i = 1:size(links,2)
+ links(:,i) = cell2mat(res.bodies{1}.links{i}.m);
end
else
- values = [];
+ links = [];
end
-
Modified: pkg/trunk/openrave_planning/openraveros/octave/orEnvSetOptions.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orEnvSetOptions.m 2008-12-15 17:39:49 UTC (rev 8091)
+++ pkg/trunk/openrave_planning/openraveros/octave/orEnvSetOptions.m 2008-12-15 18:02:38 UTC (rev 8092)
@@ -61,7 +61,7 @@
req.publishanytime = str2num(rem);
case 'debug'
req.setmask = req.Set_DebugLevel();
- req.debuglevel = str2num(rem);
+ req.debuglevel = strtrim(rem);
otherwise
display('unknown command');
end
Modified: pkg/trunk/openrave_planning/openraveros/octave/orRobotGetDOFLimits.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orRobotGetDOFLimits.m 2008-12-15 17:39:49 UTC (rev 8091)
+++ pkg/trunk/openrave_planning/openraveros/octave/orRobotGetDOFLimits.m 2008-12-15 18:02:38 UTC (rev 8092)
@@ -7,17 +7,24 @@
%% get some robot info
session = openraveros_getglobalsession();
-req = openraveros_env_getbodies();
+req = openraveros_env_getrobots();
req.bodyid = robotid;
-res = rosoct_session_call(session.id,'env_getbodies',req);
-if( ~isempty(resinfo) )
- if( length(res.lowerlimit) ~= length(res.upperlimit) )
+req.options = openraveros_RobotInfo().Req_ActiveLimits();
+res = rosoct_session_call(session.id,'env_getrobots',req);
+if( ~isempty(res) )
+ if( res.robots{1}.id ~= robotid )
+ error('wrong robot id');
+ end
+
+ lowerlimit = res.robots{1}.activelowerlimit;
+ upperlimit = res.robots{1}.activeupperlimit;
+ if( length(lowerlimit) ~= length(upperlimit) )
error('limits not same size');
end
- values = zeros(length(res.lowerlimit),2);
- values(:,1) = cell2mat(res.lowerlimit);
- values(:,2) = cell2mat(res.upperlimit);
+ values = zeros(length(lowerlimit),2);
+ values(:,1) = cell2mat(lowerlimit);
+ values(:,2) = cell2mat(upperlimit);
else
values = [];
end
Modified: pkg/trunk/openrave_planning/openraveros/src/openraveros.cpp
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/openraveros.cpp 2008-12-15 17:39:49 UTC (rev 8091)
+++ pkg/trunk/openrave_planning/openraveros/src/openraveros.cpp 2008-12-15 18:02:38 UTC (rev 8092)
@@ -25,22 +25,36 @@
// author: Rosen Diankov
#include "openraveros.h"
#include "session.h"
+#include <signal.h>
+void sigint_handler(int);
+
+boost::shared_ptr<ros::node> s_pmasternode;
+boost::shared_ptr<SessionServer> s_sessionserver;
+
int main(int argc, char ** argv)
{
+ signal(SIGINT,sigint_handler); // control C
+
ros::init(argc,argv);
- ros::node masternode("openraveserver");
+ s_pmasternode.reset(new ros::node("openraveserver", ros::node::DONT_HANDLE_SIGINT));
- if( !masternode.checkMaster() )
+ if( !s_pmasternode->checkMaster() )
return -1;
- boost::shared_ptr<SessionServer> sessionserver(new SessionServer());
- if( !sessionserver->Init() )
+ s_sessionserver.reset(new SessionServer());
+ if( !s_sessionserver->Init() )
return -1;
- masternode.spin();
-
- sessionserver.reset();
+ s_sessionserver->spin();
+ s_sessionserver.reset();
ros::fini();
+ s_pmasternode.reset();
return 0;
}
+
+void sigint_handler(int)
+{
+ s_sessionserver->selfDestruct();
+ s_pmasternode->selfDestruct();
+}
Modified: pkg/trunk/openrave_planning/openraveros/src/openraveros.h
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/openraveros.h 2008-12-15 17:39:49 UTC (rev 8091)
+++ pkg/trunk/openrave_planning/openraveros/src/openraveros.h 2008-12-15 18:02:38 UTC (rev 8092)
@@ -99,6 +99,7 @@
#include <boost/thread/thread.hpp>
#include <boost/thread/mutex.hpp>
+#include <boost/thread/condition.hpp>
#include <boost/static_assert.hpp>
#include <boost/bind.hpp>
@@ -109,7 +110,6 @@
#include <openraveros/body_getaabbs.h>
#include <openraveros/body_getdof.h>
#include <openraveros/body_getjointvalues.h>
-#include <openraveros/body_getlinks.h>
#include <openraveros/body_setjointvalues.h>
#include <openraveros/body_settransform.h>
#include <openraveros/env_checkcollision.h>
@@ -145,4 +145,10 @@
using namespace std;
using namespace openraveros;
+class SetViewerFunc
+{
+public:
+ virtual bool SetViewer(EnvironmentBase* penv, const string& viewername) = 0;
+};
+
#endif
Modified: pkg/trunk/openrave_planning/openraveros/src/rosserver.h
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/rosserver.h 2008-12-15 17:39:49 UTC (rev 8091)
+++ pkg/trunk/openrave_planning/openraveros/src/rosserver.h 2008-12-15 18:02:38 UTC (rev 8092)
@@ -49,12 +49,17 @@
};
public:
- ROSServer(EnvironmentBase* penv) : RaveServerBase(penv), _nNextFigureId(1), _nNextPlannerId(1), _nNextProblemId(1) {
+ ROSServer(SetViewerFunc* psetviewer, EnvironmentBase* penv, const string& physicsengine, const string& collisionchecker, const string& viewer)
+ : RaveServerBase(penv), _nNextFigureId(1), _nNextPlannerId(1), _nNextProblemId(1) {
+ _psetviewer.reset(psetviewer);
_bThreadDestroyed = false;
_bCloseClient = false;
_fSimulationTimestep = 0.01;
_vgravity = Vector(0,0,-9.8f);
- GetEnv()->SetDebugLevel(1);
+
+ SetPhysicsEngine(physicsengine);
+ SetCollisionChecker(collisionchecker);
+ SetViewer(viewer);
}
virtual ~ROSServer() {
Destroy();
@@ -72,13 +77,8 @@
// have to maintain a certain destruction order
_pphysics.reset();
_pcolchecker.reset();
+ _threadviewer.join();
- if( !!_pviewer ) {
- _pviewer->quitmainloop();
- _threadviewer.join();
- _pviewer.reset();
- }
-
_bCloseClient = false;
}
@@ -111,6 +111,80 @@
return _bCloseClient;
}
+ /// viewer thread assuming you can create different viewers in their own therads
+ virtual void ViewerThread(const string& strviewer)
+ {
+ {
+ boost::mutex::scoped_lock lock(_mutexViewer);
+ _pviewer.reset(GetEnv()->CreateViewer(strviewer.c_str()));
+ if( !!_pviewer ) {
+ GetEnv()->AttachViewer(_pviewer.get());
+ _pviewer->ViewerSetSize(1024,768);
+ }
+ _conditionViewer.notify_all();
+ }
+
+ if( !_pviewer )
+ return;
+
+ _pviewer->main(); // spin until quitfrommainloop is called
+ RAVELOG_DEBUGA("destroying viewer\n");
+ _pviewer.reset();
+ }
+
+ bool SetPhysicsEngine(const string& physicsengine)
+ {
+ if( physicsengine.size() > 0 ) {
+ _pphysics.reset(GetEnv()->CreatePhysicsEngine(physicsengine.c_str()));
+ GetEnv()->SetPhysicsEngine(_pphysics.get());
+ if( !_pphysics )
+ return false;
+ _pphysics->SetGravity(_vgravity);
+ }
+
+ return true;
+ }
+
+ bool SetCollisionChecker(const string& collisionchecker)
+ {
+ if( collisionchecker.size() > 0 ) {
+ _pcolchecker.reset(GetEnv()->CreateCollisionChecker(collisionchecker.c_str()));
+ if( !_pcolchecker )
+ return false;
+ GetEnv()->SetCollisionChecker(_pcolchecker.get());
+ }
+
+ return true;
+ }
+
+ bool SetViewer(const string& viewer)
+ {
+ if( !!_psetviewer )
+ return _psetviewer->SetViewer(GetEnv(),viewer);
+
+ _threadviewer.join(); // wait for the viewer
+
+ if( viewer.size() > 0 ) {
+ boost::mutex::scoped_lock lock(_mutexViewer);
+ _threadviewer = boost::thread(boost::bind(&ROSServer::ViewerThread, this, viewer));
+ _conditionViewer.wait(lock);
+
+ if( !_pviewer ) {
+ RAVELOG_WARNA("failed to create viewer %s\n", viewer.c_str());
+ _threadviewer.join();
+ return false;
+ }
+ else
+ RAVELOG_INFOA("viewer %s successfully attached\n", viewer.c_str());
+ }
+
+ return true;
+ }
+
+ //////////////
+ // services //
+ //////////////
+
bool body_destroy_srv(body_destroy::request& req, body_destroy::response& res)
{
KinBody* pbody = GetEnv()->GetBodyFromNetworkId(req.bodyid);
@@ -200,22 +274,6 @@
return true;
}
- bool body_getlinks_srv(body_getlinks::request& req, body_getlinks::response& res)
- {
- KinBody* pbody = GetEnv()->GetBodyFromNetworkId(req.bodyid);
- if( pbody == NULL )
- return false;
-
- LockEnvironment envlock(this);
- vector<Transform> vtrans; pbody->GetBodyTransformations(vtrans);
-
- res.links.resize(vtrans.size()); int index = 0;
- FOREACH(ittrans, vtrans)
- res.links[index++] = GetAffineTransform(*ittrans);
-
- return true;
- }
-
bool body_setjointvalues_srv(body_setjointvalues::request& req, body_setjointvalues::response& res)
{
KinBody* pbody = GetEnv()->GetBodyFromNetworkId(req.bodyid);
@@ -423,8 +481,10 @@
}
}
- if( !GetEnv()->LoadProblem(pproblem.get(), req.args.c_str()) )
+ if( GetEnv()->LoadProblem(pproblem.get(), req.args.c_str()) != 0 ) {
+ RAVELOG_WARNA("failed to load problem %s with args %s\n", req.problemtype.c_str(), req.args.c_str());
return false;
+ }
_mapproblems[++_nNextProblemId] = pproblem;
res.problemid = _nNextProblemId;
@@ -600,6 +660,17 @@
FillActiveDOFs(probot, info.active);
info.activedof = probot->GetActiveDOF();
}
+ if( options & BodyInfo::Req_JointLimits ) {
+ vector<dReal> vlower, vupper;
+ probot->GetActiveDOFLimits(vlower,vupper);
+ ROS_ASSERT( vlower.size() == vupper.size() );
+ info.activelowerlimit.resize(vlower.size());
+ info.activeupperlimit.resize(vupper.size());
+ for(size_t i = 0; i < vlower.size(); ++i) {
+ info.activelowerlimit[i] = vlower[i];
+ info.activeupperlimit[i] = vupper[i];
+ }
+ }
}
bool env_getrobots_srv(env_getrobots::request& req, env_getrobots::response& res)
@@ -781,16 +852,12 @@
}
}
if( req.setmask & env_set::request::Set_PhysicsEngine ) {
- _pphysics.reset(GetEnv()->CreatePhysicsEngine(req.physicsengine.c_str()));
- GetEnv()->SetPhysicsEngine(_pphysics.get());
- if( !!_pphysics )
- _pphysics->SetGravity(_vgravity);
+ SetPhysicsEngine(req.physicsengine);
}
if( req.setmask & env_set::request::Set_CollisionChecker ) {
int options = GetEnv()->GetCollisionOptions();
- _pcolchecker.reset(GetEnv()->CreateCollisionChecker(req.collisionchecker.c_str()));
- GetEnv()->SetCollisionChecker(_pcolchecker.get());
- GetEnv()->SetCollisionOptions(options);
+ if( SetCollisionChecker(req.collisionchecker) )
+ GetEnv()->SetCollisionOptions(options);
}
if( req.setmask & env_set::request::Set_Gravity ) {
_vgravity = Vector(req.gravity[0],req.gravity[1],req.gravity[2]);
@@ -801,19 +868,27 @@
GetEnv()->SetPublishBodiesAnytime(req.publishanytime>0);
}
if( req.setmask & env_set::request::Set_DebugLevel ) {
- GetEnv()->SetDebugLevel(req.debuglevel);
+ map<string,DebugLevel> mlevels;
+ mlevels["fatal"] = Level_Fatal;
+ mlevels["error"] = Level_Error;
+ mlevels["info"] = Level_Info;
+ mlevels["warn"] = Level_Warn;
+ mlevels["debug"] = Level_Debug;
+ DebugLevel level = GetEnv()->GetDebugLevel();
+ if( mlevels.find(req.debuglevel) != mlevels.end() )
+ level = mlevels[req.debuglevel];
+ else {
+ stringstream ss(req.debuglevel);
+ int nlevel;
+ ss >> nlevel;
+ if( !!ss )
+ level = (DebugLevel)nlevel;
+ }
+ GetEnv()->SetDebugLevel(level);
}
if( req.setmask & env_set::request::Set_Viewer ) {
- if( !!_pviewer ) {
- _pviewer->quitmainloop();
- // no need to wait for joins
- //_threadviewer.join();
- }
-
- _pviewer.reset(GetEnv()->CreateViewer(req.viewer.c_str()));
- GetEnv()->AttachViewer(_pviewer.get());
- if( !!_pviewer )
- _threadviewer = boost::thread(boost::bind(&RaveViewerBase::main, _pviewer.get()));
+ GetEnv()->AttachViewer(NULL);
+ SetViewer(req.viewer);
}
return true;
@@ -1287,15 +1362,20 @@
return am;
}
+ boost::shared_ptr<SetViewerFunc> _psetviewer;
boost::shared_ptr<PhysicsEngineBase> _pphysics;
boost::shared_ptr<CollisionCheckerBase> _pcolchecker;
- boost::shared_ptr<RaveViewerBase> _pviewer;
map<int, boost::shared_ptr<PlannerBase> > _mapplanners;
map<int, boost::shared_ptr<ProblemInstance> > _mapproblems;
map<int, boost::shared_ptr<FIGURE> > _mapFigureIds;
int _nNextFigureId, _nNextPlannerId, _nNextProblemId;
- boost::thread _threadviewer;
float _fSimulationTimestep;
Vector _vgravity;
bool _bThreadDestroyed, _bCloseClient;
+
+ /// viewer control variables
+ boost::shared_ptr<RaveViewerBase> _pviewer;
+ boost::thread _threadviewer;
+ boost::mutex _mutexViewer;
+ boost::condition _conditionViewer;
};
Modified: pkg/trunk/openrave_planning/openraveros/src/session.h
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/session.h 2008-12-15 17:39:49 UTC (rev 8091)
+++ pkg/trunk/openrave_planning/openraveros/src/session.h 2008-12-15 18:02:38 UTC (rev 8092)
@@ -34,10 +34,11 @@
#define REFLECT_SERVICE(srvname) \
bool srvname##_srv(srvname::request& req, srvname::response& res) \
{ \
- /* need separate copy in order to guarantee thread safety */ \
- SessionState state = getstate(req); \
- if( !state._pserver ) \
+ SessionState state = getstate(req); /* need separate copy in order to guarantee thread safety */ \
+ if( !state._pserver ) { \
+ ROS_INFO("failed to find session for service %s\n", #srvname); \
return false; \
+ } \
return state._pserver->srvname##_srv(req,res); \
}
@@ -57,20 +58,33 @@
boost::shared_ptr<EnvironmentBase> _penv;
};
+ class SessionSetViewerFunc : public SetViewerFunc
+ {
+ public:
+ SessionSetViewerFunc(SessionServer* pserv) : _pserv(pserv) {}
+ virtual bool SetViewer(EnvironmentBase* penv, const string& viewername) {
+ return _pserv->SetViewer(penv,viewername);
+ }
+
+ private:
+ SessionServer* _pserv;
+ };
+
string _sessionname;
public:
- SessionServer() {
+ SessionServer() : _penvViewer(NULL), _ok(true) {
_pParentEnvironment.reset(CreateEnvironment());
}
virtual ~SessionServer() {
Destroy();
}
- bool Init() {
+ bool Init()
+ {
node* pnode = node::instance();
if( pnode == NULL )
return false;
-
+
if( !pnode->advertiseService("openrave_session",&SessionServer::session_callback,this, 1) )
return false;
_sessionname = pnode->mapName("openrave_session");
@@ -88,8 +102,6 @@
return false;
if( !pnode->advertiseService("body_getjointvalues",&SessionServer::body_getjointvalues_srv,this,-1) )
return false;
- if( !pnode->advertiseService("body_getlinks",&SessionServer::body_getlinks_srv,this,-1) )
- return false;
if( !pnode->advertiseService("body_setjointvalues",&SessionServer::body_setjointvalues_srv,this,-1) )
return false;
if( !pnode->advertiseService("body_settransform",&SessionServer::body_settransform_srv,this,-1) )
@@ -149,11 +161,16 @@
if( !pnode->advertiseService("robot_setactivevalues",&SessionServer::robot_setactivevalues_srv,this,-1) )
return false;
+ _ok = true;
+ _threadviewer = boost::thread(boost::bind(&SessionServer::ViewerThread, this));
+
return true;
}
void Destroy()
{
+ selfDestruct();
+
node* pnode = node::instance();
if( pnode == NULL )
return;
@@ -165,7 +182,6 @@
pnode->unadvertiseService("body_getaabbs");
pnode->unadvertiseService("body_getdof");
pnode->unadvertiseService("body_getjointvalues");
- pnode->unadvertiseService("body_getlinks");
pnode->unadvertiseService("body_setjointvalues");
pnode->unadvertiseService("body_settransform");
pnode->unadvertiseService("env_checkcollision");
@@ -195,13 +211,97 @@
pnode->unadvertiseService("robot_sensorsend");
pnode->unadvertiseService("robot_setactivedofs");
pnode->unadvertiseService("robot_setactivevalues");
+
+ _threadviewer.join();
}
+
+ virtual void spin()
+ {
+ while (_ok) {
+ usleep(1000);
+ };
+ }
+ virtual void selfDestruct()
+ {
+ _ok = false;
+ boost::mutex::scoped_lock lockcreate(_mutexViewer);
+ if( !!_penvViewer )
+ _penvViewer->AttachViewer(NULL);
+ }
+
+ bool SetViewer(EnvironmentBase* penv, const string& viewer)
+ {
+ boost::mutex::scoped_lock lock(_mutexViewer);
+
+ // destroy the old viewer
+ if( !!_penvViewer ) {
+ _penvViewer->AttachViewer(NULL);
+
+ _conditionViewer.wait(lock);
+ }
+
+ ROS_ASSERT(!_penvViewer);
+
+ _strviewer = viewer;
+ _penvViewer = penv;
+
+ if( viewer.size() == 0 || !_penvViewer )
+ return true;
+
+ _conditionViewer.wait(lock);
+ return !!_pviewer;
+ }
+
private:
map<int,SessionState> _mapsessions;
boost::mutex _mutexsession;
boost::shared_ptr<EnvironmentBase> _pParentEnvironment;
+ // persistent viewer
+ boost::shared_ptr<RaveViewerBase> _pviewer;
+ boost::thread _threadviewer; ///< persistent thread (qtcoin openrave plugin needs this)
+ boost::mutex _mutexViewer;
+ boost::condition _conditionViewer;
+ EnvironmentBase* _penvViewer;
+ string _strviewer;
+
+ bool _ok;
+
+ virtual void ViewerThread()
+ {
+ while(_ok) {
+
+ {
+ boost::mutex::scoped_lock lockcreate(_mutexViewer);
+ if( _strviewer.size() == 0 || !_penvViewer ) {
+ usleep(1000);
+ continue;
+ }
+
+ _pviewer.reset(_penvViewer->CreateViewer(_strviewer.c_str()...
[truncated message content] |
|
From: <is...@us...> - 2008-12-15 22:30:45
|
Revision: 8118
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8118&view=rev
Author: isucan
Date: 2008-12-15 22:30:41 +0000 (Mon, 15 Dec 2008)
Log Message:
-----------
add a simple test of visual markers
Added Paths:
-----------
pkg/trunk/test_cs/
pkg/trunk/test_cs/CMakeLists.txt
pkg/trunk/test_cs/Makefile
pkg/trunk/test_cs/manifest.xml
pkg/trunk/test_cs/src/
pkg/trunk/test_cs/src/test_cs.cpp
Added: pkg/trunk/test_cs/CMakeLists.txt
===================================================================
--- pkg/trunk/test_cs/CMakeLists.txt (rev 0)
+++ pkg/trunk/test_cs/CMakeLists.txt 2008-12-15 22:30:41 UTC (rev 8118)
@@ -0,0 +1,5 @@
+cmake_minimum_required(VERSION 2.6)
+include(rosbuild)
+rospack(test_cs)
+
+rospack_add_executable(test_cs src/test_cs.cpp)
Added: pkg/trunk/test_cs/Makefile
===================================================================
--- pkg/trunk/test_cs/Makefile (rev 0)
+++ pkg/trunk/test_cs/Makefile 2008-12-15 22:30:41 UTC (rev 8118)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Added: pkg/trunk/test_cs/manifest.xml
===================================================================
--- pkg/trunk/test_cs/manifest.xml (rev 0)
+++ pkg/trunk/test_cs/manifest.xml 2008-12-15 22:30:41 UTC (rev 8118)
@@ -0,0 +1,16 @@
+<package>
+ <description brief="Test collision spaces">
+ A set of robot models that can do collision checking
+ </description>
+
+ <author>Ioan Sucan/is...@wi...</author>
+ <license>BSD</license>
+ <review status="deprecated" notes=""/>
+
+ <depend package="roscpp"/>
+ <depend package="tf"/>
+ <depend package="std_msgs"/>
+ <depend package="collision_space"/>
+
+
+</package>
Added: pkg/trunk/test_cs/src/test_cs.cpp
===================================================================
--- pkg/trunk/test_cs/src/test_cs.cpp (rev 0)
+++ pkg/trunk/test_cs/src/test_cs.cpp 2008-12-15 22:30:41 UTC (rev 8118)
@@ -0,0 +1,135 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include <ros/node.h>
+#include <collision_space/environmentODE.h>
+#include <algorithm>
+#include <std_msgs/VisualizationMarker.h>
+#include <rostools/Time.h>
+#include "tf/transform_broadcaster.h"
+
+using namespace collision_space;
+
+class TestVM : public ros::node
+{
+public:
+
+ TestVM(void) : ros::node("TVM")
+ {
+ advertise<std_msgs::VisualizationMarker>("visualizationMarker", 1);
+ advertise<rostools::Time>("time", 1);
+ m_tfServer = new tf::TransformBroadcaster(*this);
+ }
+
+ virtual ~TestVM(void)
+ {
+ if (m_tfServer)
+ delete m_tfServer;
+ }
+
+ void test1(void)
+ {
+
+ // Set ROS time:
+ rostools::Time tm;
+ tm.rostime.sec = 1;
+ tm.rostime.nsec = 0;
+ // tm.header.frame_id = "base";
+ // Do I need to set this?
+ publish("time", tm);
+
+
+ // Send a transform
+ // 'map' is assumed to be the fixed frame in ROS, right?
+ tf::Transform t;
+ t.setIdentity();
+
+ // m_tfServer->sendTransform(tf::Stamped<tf::Transform>(t, tm.rostime, "base", "map"));
+ m_tfServer->sendTransform(tf::Stamped<tf::Transform>(t, tm.rostime, "base", "map"));
+ sleep(1);
+
+
+ // send my marker:
+ std_msgs::VisualizationMarker mk;
+
+ mk.header.stamp = tm.rostime;
+
+ mk.header.frame_id = "map";
+
+ mk.id = 52;
+ mk.type = std_msgs::VisualizationMarker::SPHERE;
+ mk.action = std_msgs::VisualizationMarker::ADD;
+ mk.x = 0;
+ mk.y = 1;
+ mk.z = 0;
+
+ mk.roll = 0;
+ mk.pitch = 0;
+ mk.yaw = 0;
+
+ mk.xScale = 10;
+ mk.yScale = 10;
+ mk.zScale = 10;
+
+ mk.alpha = 0.1;
+ mk.r = 100;
+ mk.g = 100;
+ mk.b = 250;
+
+ publish("visualizationMarker", mk);
+
+ }
+
+protected:
+
+ tf::TransformBroadcaster *m_tfServer;
+
+};
+
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv);
+
+ TestVM tvm;
+ sleep(1);
+
+ tvm.test1();
+
+ sleep(3);
+ tvm.shutdown();
+
+ return 0;
+}
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rdi...@us...> - 2008-12-16 02:22:40
|
Revision: 8142
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8142&view=rev
Author: rdiankov
Date: 2008-12-16 02:00:14 +0000 (Tue, 16 Dec 2008)
Log Message:
-----------
fixed many problems with the openrave scripts, all example scripts from default openrave distro run with the ros backend now
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/openrave_planning/openraveros/msg/RobotInfo.msg
pkg/trunk/openrave_planning/openraveros/octave/openraveros_getrobotinfo.m
pkg/trunk/openrave_planning/openraveros/octave/orBodyGetAABBs.m
pkg/trunk/openrave_planning/openraveros/octave/orBodyGetJointValues.m
pkg/trunk/openrave_planning/openraveros/octave/orBodySetJointValues.m
pkg/trunk/openrave_planning/openraveros/octave/orBodySetTransform.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvCheckCollision.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvClose.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvGetBody.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvPlot.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvRayCollision.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvSetOptions.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvTriangulate.m
pkg/trunk/openrave_planning/openraveros/octave/orRobotGetActiveDOF.m
pkg/trunk/openrave_planning/openraveros/octave/orRobotGetAttachedSensors.m
pkg/trunk/openrave_planning/openraveros/octave/orRobotGetDOFLimits.m
pkg/trunk/openrave_planning/openraveros/octave/orRobotGetDOFValues.m
pkg/trunk/openrave_planning/openraveros/octave/orRobotGetManipulators.m
pkg/trunk/openrave_planning/openraveros/octave/orRobotSetActiveDOFs.m
pkg/trunk/openrave_planning/openraveros/octave/orRobotSetDOFValues.m
pkg/trunk/openrave_planning/openraveros/octave/orRobotStartActiveTrajectory.m
pkg/trunk/openrave_planning/openraveros/src/rosserver.h
pkg/trunk/openrave_planning/openraveros/src/session.h
pkg/trunk/openrave_planning/openraveros/test/testhanoi.m
Added Paths:
-----------
pkg/trunk/openrave_planning/openraveros/test/getexamplesdir.m
pkg/trunk/openrave_planning/openraveros/test/testgrasping.m
pkg/trunk/openrave_planning/openraveros/test/testrosplotting.m
Removed Paths:
-------------
pkg/trunk/openrave_planning/openraveros/test/addexamplesdir.m
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/3rdparty/openrave/Makefile 2008-12-16 02:00:14 UTC (rev 8142)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 554
+SVN_REVISION = -r 557
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/openrave_planning/openraveros/msg/RobotInfo.msg
===================================================================
--- pkg/trunk/openrave_planning/openraveros/msg/RobotInfo.msg 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/msg/RobotInfo.msg 2008-12-16 02:00:14 UTC (rev 8142)
@@ -1,4 +1,4 @@
-# robot information
+ # robot information
BodyInfo bodyinfo
Manipulator[] manips
Modified: pkg/trunk/openrave_planning/openraveros/octave/openraveros_getrobotinfo.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_getrobotinfo.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_getrobotinfo.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -9,7 +9,7 @@
for i = 1:length(robot.manips)
mi = robotinfo.manips{i};
robot.manips{i} = struct('baselink',mi.baselink+1,...
- 'eelink',eelink+1,...
+ 'eelink',mi.eelink+1,...
'Tgrasp',reshape(cell2mat(mi.tgrasp.m),[3 4]),...
'handjoints',cell2mat(mi.handjoints),...
'armjoints',cell2mat(mi.armjoints),...
@@ -31,6 +31,6 @@
robot.activejoints = cell2mat(robotinfo.active.joints);
robot.rotationaxis = cell2mat(robotinfo.active.rotationaxis);
-robot.activelowerlimit = cell2mat(bodyinfo.activelowerlimit);
-robot.activeupperlimit = cell2mat(bodyinfo.activeupperlimit);
+robot.activelowerlimit = cell2mat(robotinfo.activelowerlimit);
+robot.activeupperlimit = cell2mat(robotinfo.activeupperlimit);
Modified: pkg/trunk/openrave_planning/openraveros/octave/orBodyGetAABBs.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orBodyGetAABBs.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/octave/orBodyGetAABBs.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -7,14 +7,14 @@
function aabbs = orBodyGetAABBs(bodyid)
session = openraveros_getglobalsession();
-req = openraveros_body_aabbs();
+req = openraveros_body_getaabbs();
req.bodyid = bodyid;
res = rosoct_session_call(session.id,'body_getaabbs',req);
if(~isempty(res))
aabbs = zeros(6,length(res.boxes));
for i = 1:length(res.boxes)
- aabbs(:,i) = [res.boxes{i}.center;res.boxes{i}.extents];
+ aabbs(:,i) = cell2mat([res.boxes{i}.center; res.boxes{i}.extents]);
end
else
aabbs = [];
Modified: pkg/trunk/openrave_planning/openraveros/octave/orBodyGetJointValues.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orBodyGetJointValues.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/octave/orBodyGetJointValues.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -9,7 +9,7 @@
req = openraveros_body_getjointvalues();
req.bodyid = bodyid;
if( exist('indices','var') && length(indices)>0 )
- req.indices = mat2cell(indices,1,ones(1,length(indices)));
+ req.indices = mat2cell(indices(:)',1,ones(1,length(indices)));
end
res = rosoct_session_call(session.id,'body_getjointvalues',req);
Modified: pkg/trunk/openrave_planning/openraveros/octave/orBodySetJointValues.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orBodySetJointValues.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/octave/orBodySetJointValues.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -10,9 +10,9 @@
session = openraveros_getglobalsession();
req = openraveros_body_setjointvalues();
req.bodyid = bodyid;
-req.jointvalues = mat2cell(values,1,ones(1,length(values)));
+req.jointvalues = mat2cell(values(:)',1,ones(1,length(values)));
if( exist('indices','var') )
- req.indices = mat2cell(indices,1,ones(1,length(indices)));
+ req.indices = mat2cell(indices(:)',1,ones(1,length(indices)));
end
res = rosoct_session_call(session.id,'body_setjointvalues',req);
success = ~isempty(res);
Modified: pkg/trunk/openrave_planning/openraveros/octave/orBodySetTransform.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orBodySetTransform.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/octave/orBodySetTransform.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -12,11 +12,12 @@
if(nargin >= 3)
R = openraveros_rotfromquat(varargin{3});
- req.transform.m(1:9) = mat2cell(R(:));
- req.transform.m(10:12) = mat2cell(varargin{2});
+ req.transform.m(1:9) = mat2cell(R(:)',1,ones(9,1));
+ trans = varargin{2};
+ req.transform.m(10:12) = mat2cell(trans(:)',1,ones(3,1));
elseif(nargin == 2)
t = varargin{2};
- req.transform.m(1:12) = mat2cell(t(:));
+ req.transform.m(1:12) = mat2cell(t(:)',1,ones(12,1));
else
error('orBodySetTransform not enough arguments');
end
Modified: pkg/trunk/openrave_planning/openraveros/octave/orEnvCheckCollision.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orEnvCheckCollision.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/octave/orEnvCheckCollision.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -1,10 +1,10 @@
-% [collision, colbodyid,contacts, mindist] = orEnvCheckCollision(bodyid,excludeid,req_contacts)
+% [collision, colbodyid,contacts,hitbodies,mindist] = orEnvCheckCollision(bodyid,excludeid,req_contacts,req_distance)
%
% Check collision of the robot with the environment. collision is 1 if the robot
% is colliding, colbodyid is the id of the object that body collided with
%% bodyid - the uid of the body, if size > 1, bodyidD(2) narrows collision down to specific body link (one-indexed)
-function [collision, colbodyid, contacts, mindist] = orEnvCheckCollision(bodyid,excludeids,req_contacts)
+function [collision, colbodyid, contacts, hitbodies, mindist] = orEnvCheckCollision(bodyid,excludeids,req_contacts,req_distance)
session = openraveros_getglobalsession();
req = openraveros_env_checkcollision();
@@ -16,12 +16,15 @@
req.linkid = -1; % all links of the body
end
-if( ~exist('excludeid', 'var') )
- req.excludeids = mat2cell(excludeids,1,ones(length(excludeids),1));
+if( exist('excludeid', 'var') )
+ req.excludeids = mat2cell(excludeids(:)',1,ones(length(excludeids),1));
end
if( exist('req_contacts','var') && req_contacts )
req.options = req.options + req.CO_Contacts();
end
+if( exist('req_distance','var') && req_distance )
+ req.options = req.options + req.CO_Distance();
+end
res = rosoct_session_call(session.id,'env_checkcollision',req);
@@ -30,19 +33,21 @@
colbodyid = res.collidingbodyid;
if( ~isempty(res.contacts) )
- contacts = zeros(6,numrays);
+ contacts = zeros(6,length(res.contacts));
for i = 1:length(res.contacts)
- colinfo(1:3,i) = cell2mat(res.contacts{i}.position);
- colinfo(4:6,i) = cell2mat(res.contacts{i}.normal);
+ contacts(1:3,i) = cell2mat(res.contacts{i}.position);
+ contacts(4:6,i) = cell2mat(res.contacts{i}.normal);
end
else
contacts = [];
end
- hitbodies = cell2mat(res.hitbodies);
+ hitbodies = [];%cell2mat(res.hitbodies);
+ mindist = res.mindist;
else
collision = [];
colbodyid = [];
contacts = [];
+ hitbodies = [];
mindist = [];
end
Modified: pkg/trunk/openrave_planning/openraveros/octave/orEnvClose.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orEnvClose.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/octave/orEnvClose.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -6,7 +6,7 @@
session = openraveros_getglobalsession();
req = openraveros_env_closefigures();
if( exist('figureids','var') && ~isempty(figureids) )
- req.figureids = mat2cell(figureids,1,ones(1,length(figureids)));
+ req.figureids = mat2cell(figureids(:)',1,ones(1,length(figureids)));
end
res = rosoct_session_call(session.id,'env_closefigures',req);
Modified: pkg/trunk/openrave_planning/openraveros/octave/orEnvGetBody.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orEnvGetBody.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/octave/orEnvGetBody.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -11,5 +11,5 @@
if(~isempty(res))
id = res.bodyid;
else
- id = [];
+ id = 0;
end
Modified: pkg/trunk/openrave_planning/openraveros/octave/orEnvPlot.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orEnvPlot.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/octave/orEnvPlot.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -18,7 +18,7 @@
points = varargin{1}';
numpoints = size(points,2);
-req.points = mat2cell(points(:),1,ones(numel(points),1));
+req.points = mat2cell(points(:)',1,ones(numel(points),1));
req.size = 0.5;
req.drawtype = req.Draw_Point();
@@ -26,13 +26,13 @@
i = 2;
while(i <= nargin)
- if( strcmp(varargin{i},'size') )
+ if( strcmp(varargin{i},'color') )
i = i + 1;
- colors = varargin{i};
- req.size = mat2cell(colors(:),1,ones(numel(colors;),1));
- elseif( strcmp(varargin{i},'color') )
+ colors = varargin{i}';
+ req.colors = mat2cell(colors(:)',1,ones(numel(colors'),1));
+ elseif( strcmp(varargin{i},'size') )
i = i + 1;
- req.colors = varargin{i};
+ req.size = varargin{i};
elseif( strcmp(varargin{i},'line') | strcmp(varargin{i},'linestrip') )
req.drawtype = req.Draw_LineStrip();
elseif( strcmp(varargin{i},'linelist') )
@@ -52,7 +52,7 @@
res = rosoct_session_call(session.id,'env_plot',req);
if(~isempty(res))
- figureid = req.figureid;
+ figureid = res.figureid;
else
figureid = [];
end
Modified: pkg/trunk/openrave_planning/openraveros/octave/orEnvRayCollision.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orEnvRayCollision.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/octave/orEnvRayCollision.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -19,16 +19,18 @@
numrays = size(rays,2);
req.rays = cell(numrays,1);
for i = 1:numrays
- req.rays{i}.position = {rays(1,i),rays(2,i),rays(3,i)};
- req.rays{i}.direction = {rays(4,i),rays(5,i),rays(6,i)};
+ req.rays{i} = openraveros_Ray();
+ req.rays{i}.position(1:3) = {rays(1,i),rays(2,i),rays(3,i)};
+ req.rays{i}.direction(1:3) = {rays(4,i),rays(5,i),rays(6,i)};
end
if( exist('bodyid','var') )
req.bodyid = bodyid;
end
-if( exist('req_contacts','var') && req_contacts )
- req.request_contacts = 1;
+if( ~exist('req_contacts','var') )
+ req_contacts = 1;
end
+req.request_contacts = req_contacts>0;
if( exist('req_bodyinfo','var') && req_bodyinfo )
req.request_bodies = req_bodyinfo;
end
Modified: pkg/trunk/openrave_planning/openraveros/octave/orEnvSetOptions.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orEnvSetOptions.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/octave/orEnvSetOptions.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -55,7 +55,7 @@
req.viewer = strtrim(rem);
case 'gravity'
req.setmask = req.Set_Gravity();
- req.gravity(1:3) = mat2cell(str2num(rem),1,[1 1 1]);
+ req.gravity(1:3) = mat2cell(str2num(rem)',1,[1 1 1]);
case 'publishanytime'
req.setmask = req.Set_PublishAnytime();
req.publishanytime = str2num(rem);
Modified: pkg/trunk/openrave_planning/openraveros/octave/orEnvTriangulate.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orEnvTriangulate.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/octave/orEnvTriangulate.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -15,9 +15,12 @@
session = openraveros_getglobalsession();
req = openraveros_env_triangulate();
-req.inclusive = inclusive;
+req.inclusive = 0;
+if( exist('inclusive','var') )
+ req.inclusive = inclusive;
+end
if( exist('ids','var') )
- req.bodyids = mat2cell(ids,1,ones(length(ids),1));
+ req.bodyids = mat2cell(ids(:)',1,ones(length(ids),1));
end
res = rosoct_session_call(session.id,'env_triangulate',req);
Modified: pkg/trunk/openrave_planning/openraveros/octave/orRobotGetActiveDOF.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orRobotGetActiveDOF.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/octave/orRobotGetActiveDOF.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -8,8 +8,11 @@
req = openraveros_env_getrobots();
req.bodyid = robotid;
res = rosoct_session_call(session.id,'env_getrobots',req);
-if( ~isempty(resinfo) )
- dof = res.activedof;
+if( ~isempty(res) )
+ if( res.robots{1}.bodyinfo.bodyid ~= robotid )
+ error('wrong robot id');
+ end
+ dof = res.robots{1}.activedof;
else
dof = [];
end
Modified: pkg/trunk/openrave_planning/openraveros/octave/orRobotGetAttachedSensors.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orRobotGetAttachedSensors.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/octave/orRobotGetAttachedSensors.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -9,5 +9,9 @@
% Tglobal = Tlink * Trelative
% type - the xml id of the sensor that is attached
function sensors = orRobotGetAttachedSensors(robotid)
-robotinfo = orEnvGetRobots(robotid);
-sensors = robotinfo.sensors;
+robots = orEnvGetRobots(robotid);
+if( isempty(robots) )
+ sensors = [];
+else
+ sensors = robots{1}.sensors;
+end
Modified: pkg/trunk/openrave_planning/openraveros/octave/orRobotGetDOFLimits.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orRobotGetDOFLimits.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/octave/orRobotGetDOFLimits.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -12,7 +12,7 @@
req.options = openraveros_RobotInfo().Req_ActiveLimits();
res = rosoct_session_call(session.id,'env_getrobots',req);
if( ~isempty(res) )
- if( res.robots{1}.id ~= robotid )
+ if( res.robots{1}.bodyinfo.bodyid ~= robotid )
error('wrong robot id');
end
Modified: pkg/trunk/openrave_planning/openraveros/octave/orRobotGetDOFValues.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orRobotGetDOFValues.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/octave/orRobotGetDOFValues.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -12,7 +12,7 @@
req = openraveros_robot_getactivevalues();
req.bodyid = robotid;
if( exist('indices','var') )
- req.indices = mat2cell(indices,1,ones(length(indices),1));
+ req.indices = mat2cell(indices(:)',1,ones(length(indices),1));
end
res = rosoct_session_call(session.id,'robot_getactivevalues',req);
Modified: pkg/trunk/openrave_planning/openraveros/octave/orRobotGetManipulators.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orRobotGetManipulators.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/octave/orRobotGetManipulators.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -11,5 +11,9 @@
% effect on the end effector
% iksolvername - name of ik solver to use
function manipulators = orRobotGetManipulators(robotid)
-robotinfo = orEnvGetRobots(robotid);
-sensors = robotinfo.manips;
+robots = orEnvGetRobots(robotid);
+if( isempty(robots) )
+ manipulators = [];
+else
+ manipulators = robots{1}.manips;
+end
Modified: pkg/trunk/openrave_planning/openraveros/octave/orRobotSetActiveDOFs.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orRobotSetActiveDOFs.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/octave/orRobotSetActiveDOFs.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -17,12 +17,14 @@
session = openraveros_getglobalsession();
req = openraveros_robot_setactivedofs();
req.bodyid = robotid;
-req.active.affine = affinedofs;
+if( exist('affinedofs','var') )
+ req.active.affine = affinedofs;
+end
if( exist('indices','var') )
- req.active.joints = mat2cell(indices,1,ones(length(indices),1));
+ req.active.joints = mat2cell(indices(:)',1,ones(length(indices),1));
end
if( exist('rotationaxis','var') )
- req.active.rotationaxis(1:3) = mat2cell(rotationaxis,1,[1 1 1]);
+ req.active.rotationaxis(1:3) = mat2cell(rotationaxis(:)',1,[1 1 1]);
end
res = rosoct_session_call(session.id,'robot_setactivedofs',req);
Modified: pkg/trunk/openrave_planning/openraveros/octave/orRobotSetDOFValues.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orRobotSetDOFValues.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/octave/orRobotSetDOFValues.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -13,9 +13,9 @@
session = openraveros_getglobalsession();
req = openraveros_robot_setactivevalues();
req.bodyid = robotid;
-req.values = mat2cell(values,1,ones(1,length(values)));
+req.values = mat2cell(values(:)',1,ones(1,length(values)));
if( exist('indices','var') )
- req.indices = mat2cell(indices,1,ones(1,length(indices)));
+ req.indices = mat2cell(indices(:)',1,ones(1,length(indices)));
end
res = rosoct_session_call(session.id,'robot_setactivevalues',req);
success = ~isempty(res);
Modified: pkg/trunk/openrave_planning/openraveros/octave/orRobotStartActiveTrajectory.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orRobotStartActiveTrajectory.m 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/octave/orRobotStartActiveTrajectory.m 2008-12-16 02:00:14 UTC (rev 8142)
@@ -37,7 +37,7 @@
req.trajectory.points = cell(numpts,1);
for i = 1:numpts
pt = openraveros_TrajectoryPoint();
- pt.position = mat2cell(jointvalues(:,i),1,arr);
+ pt.position = mat2cell(jointvalues(:,i)',1,arr);
req.trajectory.points{i} = pt;
end
@@ -54,11 +54,11 @@
%% convert from quaternions
for i = 1:numpts
R = openraveros_rotfromquat(transformations(1:4,i));
- req.trajectory.points{i}.transform.m(1:12) = mat2cell([R(:);transformations(5:7,i)],1,ones(9,1));
+ req.trajectory.points{i}.transform.m(1:12) = mat2cell([R(:);transformations(5:7,i)]',1,ones(12,1));
end
elseif( size(transformations,1) == 12 )
for i = 1:numpts
- req.trajectory.points{i}.transform.m(1:12) = mat2cell(transformations(:,i),1,ones(12,1));
+ req.trajectory.points{i}.transform.m(1:12) = mat2cell(transformations(:,i)',1,ones(12,1));
end
else
error('transformations wrong size');
Modified: pkg/trunk/openrave_planning/openraveros/src/rosserver.h
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/rosserver.h 2008-12-16 01:45:37 UTC (rev 8141)
+++ pkg/trunk/openrave_planning/openraveros/src/rosserver.h 2008-12-16 02:00:14 UTC (rev 8142)
@@ -41,13 +41,47 @@
class FIGURE
{
public:
- FIGURE(EnvironmentBase* penv, void* handle) : _handle(handle) {}
+ FIGURE(EnvironmentBase* penv, void* handle) : _penv(penv), _handle(handle) {}
~FIGURE() { _penv->closegraph(_handle); }
private:
+ FIGURE(const FIGURE& r ) { ROS_ASSERT(0); }
EnvironmentBase* _penv;
void* _handle;
};
+ class ServerWorker
+ {
+ public:
+ virtual void work() = 0;
+// ServerWorker(boost::condition& cond) : _cond(cond) {}
+// ~ServerWorker() { _cond.notify_all(); }
+// private:
+// boost::condition& _cond;
+ };
+
+ class WorkExecutor
+ {
+ public:
+ WorkExecutor(ServerWorker* pworker) : _worker(pworker) {}
+ ~WorkExecutor() { _worker->work(); }
+ private:
+ boost::shared_ptr<ServerWorker> _worker;
+ };
+
+ class SendProblemWorker : public ServerWorker
+ {
+ public:
+ SendProblemWorker(boost::shared_ptr<ProblemInstance> prob, const string& cmd, string& out) : _prob(prob), _cmd(cmd), _out(out) {}
+ virtual void work() {
+ _prob->SendCommand(_cmd.c_str(),_out);
+ }
+
+ private:
+ boost::shared_ptr<ProblemInstance> _prob;
+ const string& _cmd;
+ string& _out;
+ };
+
public:
ROSServer(SetViewerFunc* psetviewer, EnvironmentBase* penv, const string& physicsengine, const string& collisionchecker, const string& viewer)
: RaveServerBase(penv), _nNextFigureId(1), _nNextPlannerId(1), _nNextProblemId(1) {
@@ -84,6 +118,8 @@
virtual void Reset()
{
+ Worker();
+
_mapFigureIds.clear();
// destroy environment specific state: problems, planners, figures
@@ -99,8 +135,24 @@
/// worker thread called from the main environment thread
virtual void Worker()
{
+ list<boost::shared_ptr<WorkExecutor> > listlocalworkers;
+ {
+ boost::mutex::scoped_lock lock(_mutexWorker);
+ listlocalworkers.swap(_listWorkers);
+ }
+
+ listlocalworkers.clear();
+ _conditionWorkers.notify_all();
}
+ virtual void AddWorker(ServerWorker* pworker, bool bWait)
+ {
+ boost::mutex::scoped_lock lock(_mutexWorker);
+ _listWorkers.push_back(boost::shared_ptr<WorkExecutor>(new WorkExecutor(pworker)));
+ if( bWait )
+ _conditionWorkers.wait(lock);
+ }
+
virtual bool IsInit()
{
return true;
@@ -267,7 +319,7 @@
pbody->GetJointValues(vtemp);
res.values.resize(vtemp.size());
- for(size_t i = 0; i < req.indices.size(); ++i)
+ for(size_t i = 0; i < res.values.size(); ++i)
res.values[i] = vtemp[i];
}
@@ -526,7 +578,7 @@
void FillKinBodyInfo(KinBody* pbody, BodyInfo& info, uint32_t options)
{
- info.bodyid = pbody->GetDOF();
+ info.bodyid = pbody->GetNetworkId();
info.transform = GetAffineTransform(pbody->GetTransform());
info.enabled = pbody->IsEnabled();
@@ -619,6 +671,8 @@
{
FillKinBodyInfo(probot,info.bodyinfo,options);
+ info.activedof = probot->GetActiveDOF();
+
if( options & RobotInfo::Req_Manipulators ) {
info.manips.resize(probot->GetManipulators().size()); int index = 0;
FOREACHC(itmanip, probot->GetManipulators()) {
@@ -658,9 +712,8 @@
}
if( options & RobotInfo::Req_ActiveDOFs ) {
FillActiveDOFs(probot, info.active);
- info.activedof = probot->GetActiveDOF();
}
- if( options & BodyInfo::Req_JointLimits ) {
+ if( options & RobotInfo::Req_ActiveLimits ) {
vector<dReal> vlower, vupper;
probot->GetActiveDOFLimits(vlower,vupper);
ROS_ASSERT( vlower.size() == vupper.size() );
@@ -731,6 +784,8 @@
float falpha = max(0.0f, 1.0f-req.transparency);
falpha = min(1.0f,falpha);
RaveVector<float> vOneColor(1.0f,0.5f,0.5f,falpha);
+ if( req.colors.size() >= 3 )
+ vOneColor = RaveVector<float>(req.colors[0], req.colors[1], req.colors[2],falpha);
void* figure = NULL;
switch(req.drawtype) {
@@ -754,7 +809,7 @@
break;
case env_plot::request::Draw_TriList:
//if( bOneColor )
- figure = GetEnv()->drawtrimesh(&req.points[0],3*sizeof(req.points[0]), NULL, req.points.size()/3, vOneColor);
+ figure = GetEnv()->drawtrimesh(&req.points[0],3*sizeof(req.points[0]), NULL, req.points.size()/9, vOneColor);
//else
//figure = GetEnv()->plot3(&req.points[0],req.points.size()/3,3*sizeof(req.points[0]),req.size,&req.colors[0], 0);
break;
@@ -799,19 +854,29 @@
if( !GetEnv()->SetCollisionOptions(req.request_contacts ? CO_Contacts : 0) )
RAVELOG_WARNA("failed to set collision options\n");
+ int index = 0;
FOREACHC(itray, req.rays) {
RAY r;
r.pos = Vector(itray->position[0], itray->position[1], itray->position[2]);
r.dir = Vector(itray->direction[0], itray->direction[1], itray->direction[2]);
+
+ RAVELOG_WARN("%d: %f %f %f, %f %f %f\n", index++,r.pos.x,r.pos.y,r.pos.z,r.dir.x,r.dir.y,r.dir.z);
- uint8_t bCollision;
- if( pbody != NULL )
- bCollision = GetEnv()->CheckCollision(r,pbody,&report);
+ uint8_t bCollision = 0;
+
+ if( r.dir.lengthsqr3() > 1e-7 ) {
+ r.dir.normalize3();
+
+ if( pbody != NULL )
+ bCollision = GetEnv()->CheckCollision(r,pbody,&report);
+ else
+ bCollision = GetEnv()->CheckCollision(r,&report);
+ }
else
- bCollision = GetEnv()->CheckCollision(r,&report);
+ RAVELOG_WARN("ray has zero direction\n");
res.collision.push_back(bCollision);
- if( req.request_contacts ) {
+ if( bCollision && req.request_contacts ) {
openraveros::Contact rosc;
if( report.contacts.size() > 0 ) {
COLLISIONREPORT::CONTACT& c = report.contacts.front();
@@ -820,11 +885,16 @@
}
res.contacts.push_back(rosc);
}
+ else
+ res.contacts.push_back(openraveros::Contact());
+
- if( req.request_bodies ) {
+ if( bCollision && req.request_bodies ) {
KinBody::Link* plink = report.plink1 != NULL ? report.plink1 : report.plink2;
res.hitbodies.push_back(plink != NULL ? plink->GetParent()->GetNetworkId() : 0);
}
+ else
+ res.hitbodies.push_back(0);
}
GetEnv()->SetCollisionOptions(oldopts);
@@ -1016,8 +1086,8 @@
map<int, boost::shared_ptr<ProblemInstance> >::iterator itprob = _mapproblems.find(req.problemid);
if( itprob == _mapproblems.end() )
return false;
-
- itprob->second->SendCommand(req.cmd.c_str(),res.output);
+
+ AddWorker(new S...
[truncated message content] |
|
From: <hsu...@us...> - 2008-12-16 09:40:58
|
Revision: 8164
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8164&view=rev
Author: hsujohnhsu
Date: 2008-12-16 09:40:54 +0000 (Tue, 16 Dec 2008)
Log Message:
-----------
bug fix, maps in joint are not passed to gazebo (e.g. mimicKp).
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cc
pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cc 2008-12-16 09:08:23 UTC (rev 8163)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/urdf2factory.cc 2008-12-16 09:40:54 UTC (rev 8164)
@@ -351,6 +351,9 @@
}
+ /* copy gazebo data */
+ copyGazeboMap(link->joint->data, joint);
+
/* add joint to document */
root->LinkEndChild(joint);
}
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp 2008-12-16 09:08:23 UTC (rev 8163)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp 2008-12-16 09:40:54 UTC (rev 8164)
@@ -344,6 +344,9 @@
addKeyValue(joint,"mimicOffset", values2str(1, &(link->joint->fMimicOffset) ));
}
+
+ /* copy gazebo data */
+ copyGazeboMap(link->joint->data, joint);
/* add joint to document */
root->LinkEndChild(joint);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <vij...@us...> - 2008-12-16 18:44:38
|
Revision: 8180
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8180&view=rev
Author: vijaypradeep
Date: 2008-12-16 18:44:27 +0000 (Tue, 16 Dec 2008)
Log Message:
-----------
LaserScannerTrajController works in gazebo. Added PeriodicCmd message type and script to interface with controller.
Still need to test controller on hardware. Also moved xml for this controller to pr2_experimental_controllers.
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp
Added Paths:
-----------
pkg/trunk/controllers/pr2_mechanism_controllers/msg/PeriodicCmd.msg
pkg/trunk/controllers/pr2_mechanism_controllers/scripts/send_periodic_cmd.py
pkg/trunk/robot_descriptions/pr2/pr2_experimental_controllers/laser_tilt/
pkg/trunk/robot_descriptions/pr2/pr2_experimental_controllers/laser_tilt/laser_tilt_controller.xml
Removed Paths:
-------------
pkg/trunk/robot_descriptions/wg_robot_description/tilt_laser_deprecated/tilt_laser_traj_controller.xml
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h 2008-12-16 18:36:11 UTC (rev 8179)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h 2008-12-16 18:44:27 UTC (rev 8180)
@@ -43,6 +43,7 @@
// Messages
#include <pr2_mechanism_controllers/LaserScannerSignal.h>
+#include <pr2_mechanism_controllers/PeriodicCmd.h>
// Services
#include <robot_mechanism_controllers/SetCommand.h>
@@ -60,31 +61,35 @@
public:
LaserScannerTrajController() ;
~LaserScannerTrajController() ;
-
-
+
bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
- /*!
- * \brief Issues commands to the joint. Should be called at regular intervals
- */
- virtual void update();
+ virtual void update() ;
- void setTrajectory(const std::vector<trajectory::Trajectory::TPoint>& traj_points, double max_rate, double max_acc, std::string interp ) ;
- void sampleTrajectory(double time_from_start) ;
+ void setPeriodicCmd(const pr2_mechanism_controllers::PeriodicCmd& cmd) ;
+ void setTrajectory(const std::vector<trajectory::Trajectory::TPoint>& traj_points,
+ double max_rate, double max_acc, std::string interp ) ;
+
+ //! \brief Returns what time we're currently at in the profile being executed
+ inline double getCurProfileTime() ;
+ //! \brief Returns the length (in seconds) of our current profile
+ inline double getProfileDuration() ;
+
private:
mechanism::RobotState *robot_ ;
mechanism::JointState *joint_state_ ; // Need this to check the calibrated flag on the joint
-
+
ros::thread::mutex traj_lock_ ; // Mutex for traj_
trajectory::Trajectory traj_ ; // Stores the current trajectory being executed
JointPositionSmoothController joint_position_controller_ ; // The PID position controller that is doing all the under-the-hood controls stuff
-
- double traj_start_time_ ;
-
- unsigned int update_count_ ;
-
+
+ double traj_start_time_ ; // The time that the trajectory was started (in seconds)
+ double traj_duration_ ; // The length of the current profile (in seconds)
+
+ double max_rate_ ; // Max allowable rate/velocity
+ double max_acc_ ; // Max allowable acceleration
};
class LaserScannerTrajControllerNode : public Controller
@@ -98,15 +103,20 @@
bool initXml(mechanism::RobotState *robot, TiXmlElement *config) ;
// Message Callbacks
- void setCommand() ;
+ void setPeriodicCmd() ;
private:
- LaserScannerTrajController *c_ ;
+ ros::node *node_ ;
+ LaserScannerTrajController c_ ;
std::string service_prefix_ ;
- ros::node *node_ ;
+ double prev_profile_time_ ; //!< The time in the current profile when update() was last called
- std_msgs::Float64 cmd_; //! \todo Change interface to accept arbitrary trajectories
+ pr2_mechanism_controllers::PeriodicCmd cmd_ ;
+
+ 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.
+ misc_utils::RealtimePublisher <pr2_mechanism_controllers::LaserScannerSignal>* publisher_ ; //!< Publishes the m_scanner_signal msg from the update() realtime loop
};
}
Added: pkg/trunk/controllers/pr2_mechanism_controllers/msg/PeriodicCmd.msg
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/msg/PeriodicCmd.msg (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/msg/PeriodicCmd.msg 2008-12-16 18:44:27 UTC (rev 8180)
@@ -0,0 +1,5 @@
+Header header
+string profile
+float64 period
+float64 amplitude
+float64 offset
Added: pkg/trunk/controllers/pr2_mechanism_controllers/scripts/send_periodic_cmd.py
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/scripts/send_periodic_cmd.py (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/scripts/send_periodic_cmd.py 2008-12-16 18:44:27 UTC (rev 8180)
@@ -0,0 +1,52 @@
+#!/usr/bin/env python
+
+PKG = "pr2_mechanism_controllers"
+
+import rostools; rostools.update_path(PKG)
+
+import sys
+import os
+import string
+
+import rospy
+from std_msgs import *
+
+from pr2_mechanism_controllers.msg import PeriodicCmd
+from time import sleep
+
+def print_usage(exit_code = 0):
+ print '''Usage:
+ send_periodic_cmd.py [controller] [profile] [period] [amplitude] [offset]
+ - [profile] - Possible options are linear, linear_blended, sine
+ - [period] - Time for one entire cycle to execute (in seconds)
+ - [amplitude] - Distance max value to min value of profile (In radians for laser_tilt controller)
+ - [offset] - Constant cmd to add to profile (offset=0 results in profile centered around 0)
+'''
+ sys.exit(exit_code)
+
+if __name__ == '__main__':
+ if len(sys.argv) != 6:
+ print_usage()
+
+ cmd = PeriodicCmd()
+ controller = sys.argv[1]
+ cmd.header = rostools.msg.Header(None, None, None)
+ cmd.profile = sys.argv[2]
+ cmd.period = float (sys.argv[3])
+ cmd.amplitude = float (sys.argv[4])
+ cmd.offset = float (sys.argv[5])
+
+ print 'Sending Command to %s: ' % controller
+ print ' Profile Type: %s' % cmd.profile
+ print ' Period: %f Seconds' % cmd.period
+ print ' Amplitude: %f Radians' % cmd.amplitude
+ print ' Offset: %f Radians' % cmd.offset
+
+ command_publisher = rospy.Publisher(controller + '/set_periodic_cmd', PeriodicCmd)
+ rospy.init_node('periodic_cmd_commander', anonymous=True)
+ sleep(1)
+ command_publisher.publish( cmd )
+
+ sleep(1)
+
+ print 'Command sent!'
Property changes on: pkg/trunk/controllers/pr2_mechanism_controllers/scripts/send_periodic_cmd.py
___________________________________________________________________
Added: svn:executable
+ *
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp 2008-12-16 18:36:11 UTC (rev 8179)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp 2008-12-16 18:44:27 UTC (rev 8180)
@@ -66,12 +66,25 @@
const char *jn = j->Attribute("name") ;
if (!jn)
- return false ;
+ return false ;
std::string joint_name = jn ;
joint_state_ = robot_->getJointState(joint_name) ; // Need joint state to check calibrated flag
-
+
joint_position_controller_.initXml(robot, config) ; //Pass down XML snippet to encapsulated joint_position_controller_
+
+ TiXmlElement *max_rate_elem = config->FirstChildElement("max_rate") ;
+ if (!max_rate_elem)
+ return false ;
+ if(max_rate_elem->QueryDoubleAttribute("value", &max_rate_) != TIXML_SUCCESS )
+ return false ;
+
+ TiXmlElement *max_acc_elem = config->FirstChildElement("max_acc") ;
+ if (!max_acc_elem)
+ return false ;
+ if(max_acc_elem->QueryDoubleAttribute("value", &max_acc_) != TIXML_SUCCESS )
+ return false ;
+
return true ;
}
@@ -79,35 +92,42 @@
{
//if (!joint_state_->calibrated_)
// return;
-
- double time = robot_->hw_->current_time_ ;
-
if (traj_lock_.trylock())
{
- const double traj_duration = traj_.getTotalTime() ;
- if (traj_duration > 1e-6) // Short trajectories make the mod_time calculation unstable
+ if (traj_duration_ > 1e-6) // Short trajectories could make the mod_time calculation unstable
{
- double time_from_start = time - traj_start_time_ ;
- double mod_time = time_from_start - floor(time_from_start/traj_.getTotalTime())*traj_.getTotalTime() ;
-
+ double profile_time = getCurProfileTime() ;
+
trajectory::Trajectory::TPoint sampled_point ;
sampled_point.dimension_ = 1 ;
sampled_point.q_.resize(1) ;
sampled_point.qdot_.resize(1) ;
int result ;
-
- result = traj_.sample(sampled_point, mod_time) ;
+
+ result = traj_.sample(sampled_point, profile_time) ;
if (result > 0)
{
joint_position_controller_.setCommand(sampled_point.q_[0]) ;
joint_position_controller_.update() ;
}
-
}
traj_lock_.unlock() ;
}
}
+double LaserScannerTrajController::getCurProfileTime()
+{
+ double time = robot_->hw_->current_time_ ;
+ double time_from_start = time - traj_start_time_ ;
+ double mod_time = time_from_start - floor(time_from_start/traj_.getTotalTime())*traj_.getTotalTime() ;
+ return mod_time ;
+}
+
+double LaserScannerTrajController::getProfileDuration()
+{
+ return traj_duration_ ;
+}
+
void LaserScannerTrajController::setTrajectory(const std::vector<trajectory::Trajectory::TPoint>& traj_points, double max_rate, double max_acc, std::string interp)
{
while (!traj_lock_.trylock())
@@ -121,122 +141,126 @@
traj_.setMaxRates(max_rates) ;
traj_.setMaxAcc(max_accs) ;
traj_.setInterpolationMethod(interp) ;
-
- //for (unsigned int i=0; i<traj_points.
-
+
traj_.setTrajectory(traj_points) ;
traj_start_time_ = robot_->hw_->current_time_ ;
-
+
+ traj_duration_ = traj_.getTotalTime() ;
+
traj_lock_.unlock() ;
}
+void LaserScannerTrajController::setPeriodicCmd(const pr2_mechanism_controllers::PeriodicCmd& cmd_)
+{
+ if (cmd_.profile == "linear" ||
+ cmd_.profile == "blended_linear")
+ {
+ double high_pt = cmd_.amplitude + cmd_.offset ;
+ double low_pt = -cmd_.amplitude + cmd_.offset ;
+
+ std::vector<trajectory::Trajectory::TPoint> tpoints ;
+
+ trajectory::Trajectory::TPoint cur_point(1) ;
+
+ cur_point.dimension_ = 1 ;
+
+ cur_point.q_[0] = low_pt ;
+ cur_point.time_ = 0.0 ;
+ tpoints.push_back(cur_point) ;
+
+ cur_point.q_[0] = high_pt ;
+ cur_point.time_ = cmd_.period/2.0 ;
+ tpoints.push_back(cur_point) ;
+
+ cur_point.q_[0] = low_pt ;
+ cur_point.time_ = cmd_.period ;
+ tpoints.push_back(cur_point) ;
+
+ setTrajectory(tpoints, max_rate_, max_acc_, cmd_.profile) ;
+ ROS_INFO("LaserScannerTrajController: Periodic Command set") ;
+ }
+ else
+ {
+ ROS_WARN("Unknown Periodic Trajectory Type. Not setting command.") ;
+ }
+}
+
ROS_REGISTER_CONTROLLER(LaserScannerTrajControllerNode)
-LaserScannerTrajControllerNode::LaserScannerTrajControllerNode(): node_(ros::node::instance())
+LaserScannerTrajControllerNode::LaserScannerTrajControllerNode(): node_(ros::node::instance()), c_()
{
- c_ = new LaserScannerTrajController();
+ need_to_send_msg_ = false ; // Haven't completed a sweep yet, so don't need to send a msg
+ publisher_ = NULL ; // We don't know our topic yet, so we can't build it
}
LaserScannerTrajControllerNode::~LaserScannerTrajControllerNode()
{
- node_->unsubscribe(service_prefix_ + "/set_command");
- delete c_;
+ node_->unsubscribe(service_prefix_ + "/set_periodic_cmd") ;
+
+ publisher_->stop() ;
+ delete publisher_ ; // Probably should wait on publish_->is_running() before exiting. Need to
+ // look into shutdown semantics for realtime_publisher
}
void LaserScannerTrajControllerNode::update()
{
- c_->update();
-}
+ c_.update() ;
+ double cur_profile_time = c_.getCurProfileTime() ;
-void LaserScannerTrajControllerNode::setCommand()
-{
- if (cmd_.data >= -.5 && cmd_.data <= .5)
+ // Check if we crossed the middle point of our profile
+ if (cur_profile_time >= c_.getProfileDuration()/2.0 &&
+ prev_profile_time_ < c_.getProfileDuration()/2.0)
{
- std::vector<trajectory::Trajectory::TPoint> tpoints ;
-
- trajectory::Trajectory::TPoint cur_point(1) ;
-
- cur_point.dimension_ = 1 ;
-
- cur_point.q_[0] = 0 ;
- cur_point.time_ = 0.0 ;
- tpoints.push_back(cur_point) ;
-
- cur_point.q_[0] = 0 ;
- cur_point.time_ = 50.0 ;
- tpoints.push_back(cur_point) ;
-
- cur_point.q_[0] = 0 ;
- cur_point.time_ = 100.0 ;
- tpoints.push_back(cur_point) ;
-
- double max_rate = 1 ;
- double max_acc = 1 ;
-
- c_->setTrajectory(tpoints, max_rate, max_acc, "linear") ;
+ // Should we be populating header.stamp here? Or, we can simply let ros take care of the timestamp
+ m_scanner_signal_.signal = 0 ;
+ need_to_send_msg_ = true ;
}
- else if (cmd_.data >= -52.1 && cmd_.data <= -51.9)
+ else if (cur_profile_time < prev_profile_time_) // Check if we wrapped around
{
- std::vector<trajectory::Trajectory::TPoint> tpoints ;
-
- trajectory::Trajectory::TPoint cur_point(1) ;
-
- cur_point.dimension_ = 1 ;
-
- cur_point.q_[0] = .5 ;
- cur_point.time_ = 0.0 ;
- tpoints.push_back(cur_point) ;
-
- cur_point.q_[0] = -.5 ;
- cur_point.time_ = 5.0 ;
- tpoints.push_back(cur_point) ;
-
- cur_point.q_[0] = .5 ;
- cur_point.time_ = 10.0 ;
- tpoints.push_back(cur_point) ;
-
- double max_rate = 1 ;
- double max_acc = 1 ;
-
- c_->setTrajectory(tpoints, max_rate, max_acc, "linear") ;
+ m_scanner_signal_.signal = 1 ;
+ need_to_send_msg_ = true ;
}
- else if (cmd_.data >= -53.1 && cmd_.data <= -52.9)
+ prev_profile_time_ = cur_profile_time ;
+
+ // Use the realtime_publisher to try to send the message.
+ // If it fails sending, it's not a big deal, since we can just try again 1 ms later. No one will notice.
+ if (need_to_send_msg_)
{
- std::vector<trajectory::Trajectory::TPoint> tpoints ;
-
- trajectory::Trajectory::TPoint cur_point(1) ;
-
- cur_point.dimension_ = 1 ;
-
- cur_point.q_[0] = .5 ;
- cur_point.time_ = 0.0 ;
- tpoints.push_back(cur_point) ;
-
- cur_point.q_[0] = -.5 ;
- cur_point.time_ = 5.0 ;
- tpoints.push_back(cur_point) ;
-
- cur_point.q_[0] = .5 ;
- cur_point.time_ = 10.0 ;
- tpoints.push_back(cur_point) ;
-
- double max_rate = 1 ;
- double max_acc = 1 ;
-
- c_->setTrajectory(tpoints, max_rate, max_acc, "blended_linear") ;
+ if (publisher_->trylock())
+ {
+ publisher_->msg_.header = m_scanner_signal_.header ;
+ publisher_->msg_.signal = m_scanner_signal_.signal ;
+ publisher_->unlockAndPublish() ;
+ need_to_send_msg_ = false ;
+ }
+ //printf("tilt_laser: Signal trigger (%u)\n", m_scanner_signal_.signal) ;
+ //std::cout << std::flush ;
}
}
bool LaserScannerTrajControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
{
- service_prefix_ = config->Attribute("name");
+ service_prefix_ = config->Attribute("name") ;
- if (!c_->initXml(robot, config))
+ if (!c_.initXml(robot, config))
{
- return false;
+ ROS_ERROR("Error Loading LaserScannerTrajControllerNode XML") ;
+ return false ;
}
- node_->subscribe(service_prefix_ + "/set_command", cmd_, &LaserScannerTrajControllerNode::setCommand, this, 1);
- return true;
+ node_->subscribe(service_prefix_ + "/set_periodic_cmd", cmd_, &LaserScannerTrajControllerNode::setPeriodicCmd, this, 1) ;
+
+ if (publisher_ != NULL) // Make sure that we don't memory leak if initXml gets called twice
+ delete publisher_ ;
+ publisher_ = new misc_utils::RealtimePublisher <pr2_mechanism_controllers::LaserScannerSignal> (service_prefix_ + "/laser_scanner_signal", 1) ;
+
+ prev_profile_time_ = 0.0 ;
+
+ return true ;
}
+
+void LaserScannerTrajControllerNode::setPeriodicCmd()
+{
+ c_.setPeriodicCmd(cmd_) ;
+}
Copied: pkg/trunk/robot_descriptions/pr2/pr2_experimental_controllers/laser_tilt/laser_tilt_controller.xml (from rev 8179, pkg/trunk/robot_descriptions/wg_robot_description/tilt_laser_deprecated/tilt_laser_traj_controller.xml)
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_experimental_controllers/laser_tilt/laser_tilt_controller.xml (rev 0)
+++ pkg/trunk/robot_descriptions/pr2/pr2_experimental_controllers/laser_tilt/laser_tilt_controller.xml 2008-12-16 18:44:27 UTC (rev 8180)
@@ -0,0 +1,10 @@
+<controllers>
+ <controller name="laser_controller" topic="laser_controller" type="LaserScannerTrajControllerNode">
+ <max_acc value="1.0" />
+ <max_rate value="100.0" />
+ <filter smoothing_factor="0.1" />
+ <joint name="laser_tilt_mount_joint">
+ <pid p="12" i=".1" d="1" iClamp="0.5" />
+ </joint>
+ </controller>
+</controllers>
Deleted: pkg/trunk/robot_descriptions/wg_robot_description/tilt_laser_deprecated/tilt_laser_traj_controller.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/tilt_laser_deprecated/tilt_laser_traj_controller.xml 2008-12-16 18:36:11 UTC (rev 8179)
+++ pkg/trunk/robot_descriptions/wg_robot_description/tilt_laser_deprecated/tilt_laser_traj_controller.xml 2008-12-16 18:44:27 UTC (rev 8180)
@@ -1,8 +0,0 @@
-<controllers>
- <controller name="laser_controller" topic="laser_controller" type="LaserScannerTrajControllerNode">
- <filter smoothing_factor="0.1" />
- <joint name="laser_tilt_mount_joint">
- <pid p="12" i=".1" d="1" iClamp="0.5" />
- </joint>
- </controller>
-</controllers>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-12-16 20:07:09
|
Revision: 8192
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8192&view=rev
Author: hsujohnhsu
Date: 2008-12-16 20:07:04 +0000 (Tue, 16 Dec 2008)
Log Message:
-----------
more updates to make gazebo stable due to mass changes.
Modified Paths:
--------------
pkg/trunk/demos/arm_gazebo/l_arm_default_controller.xml
pkg/trunk/demos/arm_gazebo/r_arm_default_controller.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/arm_defs.xml
Modified: pkg/trunk/demos/arm_gazebo/l_arm_default_controller.xml
===================================================================
--- pkg/trunk/demos/arm_gazebo/l_arm_default_controller.xml 2008-12-16 19:37:02 UTC (rev 8191)
+++ pkg/trunk/demos/arm_gazebo/l_arm_default_controller.xml 2008-12-16 20:07:04 UTC (rev 8192)
@@ -64,7 +64,7 @@
</controller>
<controller name="l_gripper_controller" topic="l_gripper_controller" type="JointPositionControllerNode">
<joint name="l_gripper_l_finger_joint">
- <pid p="1.0" d="0.0001" i="0.01" iClamp="0.1" />
+ <pid p="0.5" d="0.000001" i="0.001" iClamp="0.01" />
</joint>
</controller>
</controllers>
Modified: pkg/trunk/demos/arm_gazebo/r_arm_default_controller.xml
===================================================================
--- pkg/trunk/demos/arm_gazebo/r_arm_default_controller.xml 2008-12-16 19:37:02 UTC (rev 8191)
+++ pkg/trunk/demos/arm_gazebo/r_arm_default_controller.xml 2008-12-16 20:07:04 UTC (rev 8192)
@@ -64,7 +64,7 @@
</controller>
<controller name="r_gripper_controller" topic="r_gripper_controller" type="JointPositionControllerNode">
<joint name="r_gripper_l_finger_joint">
- <pid p="1.0" d="0.0001" i="0.01" iClamp="0.1" />
+ <pid p="0.5" d="0.000001" i="0.001" iClamp="0.01" />
</joint>
</controller>
</controllers>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/arm_defs.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/arm_defs.xml 2008-12-16 19:37:02 UTC (rev 8191)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/arm_defs.xml 2008-12-16 20:07:04 UTC (rev 8192)
@@ -22,7 +22,7 @@
<limit effort="10" min="0" max="${gripper_max_angle}"
k_position="0" k_velocity="0"
safety_length_min="0.0" safety_length_max="0.0" />
- <joint_properties damping="0.5" />
+ <joint_properties damping="0.2" />
</joint>
<link name="${prefix}_l_finger_link">
@@ -69,6 +69,8 @@
</map>
<map name="${prefix}_l_finger_gravity" flag="gazebo">
<elem key="turnGravityOff">true</elem>
+ <elem key="kp">1000000.0</elem>
+ <elem key="kd">1.0</elem>
</map>
</link>
@@ -81,12 +83,12 @@
<limit effort="10" min="${-gripper_max_angle}" max="0"
k_position="0" k_velocity="0"
safety_length_min="0.0" safety_length_max="0.0" />
- <joint_properties damping="0.5" />
+ <joint_properties damping="0.2" />
<mimic name="${prefix}_l_finger_joint" mult="${-1}" offset="0"/>
<map name="${prefix}_r_finger_mimic" flag="gazebo">
- <elem key="mimicKp">12.0</elem>
+ <elem key="mimicKp">10.0</elem>
<elem key="mimicKd">0.0</elem>
- <elem key="mimicFMax">1000.0</elem>
+ <elem key="mimicFMax">100.0</elem> <!-- ode bug: this force is set for _l_finger_joint -->
</map>
</joint>
@@ -134,6 +136,8 @@
</map>
<map name="${prefix}_r_finger_gravity" flag="gazebo">
<elem key="turnGravityOff">true</elem>
+ <elem key="kp">1000000.0</elem>
+ <elem key="kd">1.0</elem>
</map>
</link>
@@ -146,10 +150,10 @@
<limit effort="10" min="${-gripper_max_angle}" max="0"
k_position="0" k_velocity="0"
safety_length_min="0.0" safety_length_max="0.0" />
- <joint_properties damping="0.02" />
+ <joint_properties damping="0.05" />
<mimic name="${prefix}_l_finger_joint" mult="-1" offset="0"/>
<map name="${prefix}_l_finger_tip_mimic" flag="gazebo">
- <elem key="mimicKp">8.0</elem>
+ <elem key="mimicKp">1.0</elem>
<elem key="mimicKd">0.0</elem>
<elem key="mimicFMax">100.0</elem>
</map>
@@ -199,6 +203,8 @@
</map>
<map name="${prefix}_l_finger_tip_gravity" flag="gazebo">
<elem key="turnGravityOff">true</elem>
+ <elem key="kp">1000000.0</elem>
+ <elem key="kd">1.0</elem>
</map>
</link>
@@ -211,10 +217,10 @@
<limit effort="10" min="0" max="${gripper_max_angle}"
k_position="0" k_velocity="0"
safety_length_min="0.0" safety_length_max="0.0" />
- <joint_properties damping="0.02" />
+ <joint_properties damping="0.05" />
<mimic name="${prefix}_l_finger_joint" mult="1" offset="0"/>
<map name="${prefix}_r_finger_tip_mimic" flag="gazebo">
- <elem key="mimicKp">8.0</elem>
+ <elem key="mimicKp">1.0</elem>
<elem key="mimicKd">0.0</elem>
<elem key="mimicFMax">100.0</elem>
</map>
@@ -264,6 +270,8 @@
</map>
<map name="${prefix}_r_finger_tip_gravity" flag="gazebo">
<elem key="turnGravityOff">true</elem>
+ <elem key="kp">1000000.0</elem>
+ <elem key="kd">1.0</elem>
</map>
</link>
@@ -890,11 +898,11 @@
<origin xyz="0 0 0" rpy="0 0 0" />
<joint name="${side}_wrist_roll_joint" />
<inertial>
- <mass value="0.001" />
+ <mass value="0.1" />
<com xyz="0 0 0" />
- <inertia ixx="0.0001" ixy="0" ixz="0"
- iyy="0.0001" iyz="0"
- izz="0.0001" />
+ <inertia ixx="0.01" ixy="0" ixz="0"
+ iyy="0.01" iyz="0"
+ izz="0.01" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rdi...@us...> - 2008-12-16 22:49:31
|
Revision: 8206
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8206&view=rev
Author: rdiankov
Date: 2008-12-16 22:41:28 +0000 (Tue, 16 Dec 2008)
Log Message:
-----------
several openraveros server bug fixes
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/openrave_planning/openraveros/octave/openraveros_createsession.m
pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m
pkg/trunk/openrave_planning/openraveros/octave/orBodySetTransform.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvLoadScene.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvRayCollision.m
pkg/trunk/openrave_planning/openraveros/src/session.h
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2008-12-16 22:26:28 UTC (rev 8205)
+++ pkg/trunk/3rdparty/openrave/Makefile 2008-12-16 22:41:28 UTC (rev 8206)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 557
+SVN_REVISION = -r 559
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/openrave_planning/openraveros/octave/openraveros_createsession.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_createsession.m 2008-12-16 22:26:28 UTC (rev 8205)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_createsession.m 2008-12-16 22:41:28 UTC (rev 8206)
@@ -49,13 +49,18 @@
end
session = [];
-[localid,res] = rosoct_create_session(sessionserver,req);
+while(1)
+ [localid,res] = rosoct_create_session(sessionserver,req);
+
+ if( ~isempty(localid) && ~isempty(res) )
+ if( res.sessionid==0 )
+ error('bad session id');
+ end
+ session.id = localid;
+ session.server = sessionserver;
+ session.uuid = res.sessionid;
+ return;
+ end
-if( ~isempty(localid) && ~isempty(res) )
- if( res.sessionid==0 )
- error('bad session id');
- end
- session.id = localid;
- session.server = sessionserver;
- session.uuid = res.sessionid;
+ sleep(0.2); % give some time
end
Modified: pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m 2008-12-16 22:26:28 UTC (rev 8205)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m 2008-12-16 22:41:28 UTC (rev 8206)
@@ -57,15 +57,20 @@
if( createsession && isempty(openraveros_globalsession) )
req = openraveros_openrave_session();
req.viewer = viewer; % default viewer
- [localid,res] = rosoct_create_session(sessionserver,req);
-
- if( ~isempty(localid) && ~isempty(res) )
- if( res.sessionid==0 )
- error('bad session id');
+ while(1)
+ [localid,res] = rosoct_create_session(sessionserver,req);
+
+ if( ~isempty(localid) && ~isempty(res) )
+ if( res.sessionid==0 )
+ error('bad session id');
+ end
+ openraveros_globalsession.id = localid;
+ openraveros_globalsession.server = sessionserver;
+ openraveros_globalsession.uuid = res.sessionid;
+ %%display(sprintf('created openraveros session uuid %d',res.sessionid));
+ return;
end
- openraveros_globalsession.id = localid;
- openraveros_globalsession.server = sessionserver;
- openraveros_globalsession.uuid = res.sessionid;
- %display(sprintf('created openraveros session uuid %d',res.sessionid));
+
+ sleep(0.2); % give some time
end
end
Modified: pkg/trunk/openrave_planning/openraveros/octave/orBodySetTransform.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orBodySetTransform.m 2008-12-16 22:26:28 UTC (rev 8205)
+++ pkg/trunk/openrave_planning/openraveros/octave/orBodySetTransform.m 2008-12-16 22:41:28 UTC (rev 8206)
@@ -13,7 +13,7 @@
if(nargin >= 3)
R = openraveros_rotfromquat(varargin{3});
req.transform.m(1:9) = R(:);
- req.transform.m(10:12) = trans(:);
+ req.transform.m(10:12) = varargin{2};
elseif(nargin == 2)
t = varargin{2};
req.transform.m(1:12) = t(:);
Modified: pkg/trunk/openrave_planning/openraveros/octave/orEnvLoadScene.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orEnvLoadScene.m 2008-12-16 22:26:28 UTC (rev 8205)
+++ pkg/trunk/openrave_planning/openraveros/octave/orEnvLoadScene.m 2008-12-16 22:41:28 UTC (rev 8206)
@@ -8,7 +8,7 @@
% scene alone and loads in addition to it.
function success = orEnvLoadScene(filename, ClearScene)
-session = openraveros_getglobalsession();
+session = openraveros_getglobalsession()
req = openraveros_env_loadscene();
req.filename = filename;
if( exist('ClearScene','var') )
Modified: pkg/trunk/openrave_planning/openraveros/octave/orEnvRayCollision.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orEnvRayCollision.m 2008-12-16 22:26:28 UTC (rev 8205)
+++ pkg/trunk/openrave_planning/openraveros/octave/orEnvRayCollision.m 2008-12-16 22:41:28 UTC (rev 8206)
@@ -20,8 +20,8 @@
req.rays = cell(numrays,1);
for i = 1:numrays
req.rays{i} = openraveros_Ray();
- req.rays{i}.position(1:3) = {rays(1,i),rays(2,i),rays(3,i)};
- req.rays{i}.direction(1:3) = {rays(4,i),rays(5,i),rays(6,i)};
+ req.rays{i}.position(1:3) = rays(1:3,i);
+ req.rays{i}.direction(1:3) = rays(4:6,i);
end
if( exist('bodyid','var') )
Modified: pkg/trunk/openrave_planning/openraveros/src/session.h
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/session.h 2008-12-16 22:26:28 UTC (rev 8205)
+++ pkg/trunk/openrave_planning/openraveros/src/session.h 2008-12-16 22:41:28 UTC (rev 8206)
@@ -36,7 +36,7 @@
{ \
SessionState state = getstate(req); /* need separate copy in order to guarantee thread safety */ \
if( !state._pserver ) { \
- ROS_INFO("failed to find session for service %s\n", #srvname); \
+ ROS_INFO("failed to find session for service %s", #srvname); \
return false; \
} \
return state._pserver->srvname##_srv(req,res); \
@@ -280,7 +280,6 @@
if( !!_pviewer ) {
_penvViewer->AttachViewer(_pviewer.get());
_pviewer->ViewerSetSize(1024,768);
- usleep(100000); // give it some time to initialize
}
if( !_pviewer )
@@ -300,7 +299,6 @@
_penvViewer->AttachViewer(NULL);
_pviewer.reset();
_penvViewer = NULL;
- usleep(200000); // give some time for destruction
_conditionViewer.notify_all();
}
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-12-17 00:27:05
|
Revision: 8217
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8217&view=rev
Author: isucan
Date: 2008-12-17 00:27:01 +0000 (Wed, 17 Dec 2008)
Log Message:
-----------
moved test_cs to test_collision_space
Modified Paths:
--------------
pkg/trunk/motion_planning/test_collision_space/CMakeLists.txt
Added Paths:
-----------
pkg/trunk/motion_planning/test_collision_space/
Removed Paths:
-------------
pkg/trunk/test_cs/
Property changes on: pkg/trunk/motion_planning/test_collision_space
___________________________________________________________________
Added: svn:mergeinfo
+
Modified: pkg/trunk/motion_planning/test_collision_space/CMakeLists.txt
===================================================================
--- pkg/trunk/test_cs/CMakeLists.txt 2008-12-17 00:24:05 UTC (rev 8216)
+++ pkg/trunk/motion_planning/test_collision_space/CMakeLists.txt 2008-12-17 00:27:01 UTC (rev 8217)
@@ -1,5 +1,5 @@
cmake_minimum_required(VERSION 2.6)
include(rosbuild)
-rospack(test_cs)
+rospack(test_collision_space)
rospack_add_executable(test_cs src/test_cs.cpp)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-12-17 00:28:19
|
Revision: 8219
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8219&view=rev
Author: hsujohnhsu
Date: 2008-12-17 00:28:15 +0000 (Wed, 17 Dec 2008)
Log Message:
-----------
* added patch to use Kd for mimic joints.
* obtain stable mimic finger tips with light mass, recover time steps of 1ms for tests in gazebo_plugin.
Modified Paths:
--------------
pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
pkg/trunk/drivers/simulator/gazebo_plugin/test/test_slide.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/test/test_camera.world
pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/test/test_scan.world
pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/test/test_slide.world
Modified: pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
===================================================================
--- pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2008-12-17 00:28:07 UTC (rev 8218)
+++ pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2008-12-17 00:28:15 UTC (rev 8219)
@@ -899,6 +899,34 @@
return NULL;
}
+Index: server/physics/Joint.hh
+===================================================================
+--- server/physics/Joint.hh (revision 7168)
++++ server/physics/Joint.hh (working copy)
+@@ -163,6 +163,23 @@
+ private: ParamT<bool> *provideFeedbackP;
+ private: ParamT<double> *fudgeFactorP;
+
++ /// Added for mimicing other joints
++ private: ParamT<std::string> *mimicJointP;
++ private: ParamT<double> *mimicMultP;
++ private: ParamT<double> *mimicOffsetP;
++ private: ParamT<double> *mimicKpP;
++ private: ParamT<double> *mimicKdP;
++ private: ParamT<double> *mimicFMaxP;
++ private: Joint *mimicJoint;
++ private: bool enableMimic;
++ private: double mimicMult;
++ private: double mimicOffset;
++ private: double mimicKp;
++ private: double mimicKd;
++ private: double mimicFMax;
++ private: double current_time, last_time;
++ private: double current_error, last_error;
++
+ /// Feedback data for this joint
+ private: dJointFeedback *feedback;
+
Index: server/physics/Body.cc
===================================================================
--- server/physics/Body.cc (revision 7168)
@@ -1159,32 +1187,6 @@
// Settle on the new CoM pose
-Index: server/physics/Joint.hh
-===================================================================
---- server/physics/Joint.hh (revision 7168)
-+++ server/physics/Joint.hh (working copy)
-@@ -163,6 +163,21 @@
- private: ParamT<bool> *provideFeedbackP;
- private: ParamT<double> *fudgeFactorP;
-
-+ /// Added for mimicing other joints
-+ private: ParamT<std::string> *mimicJointP;
-+ private: ParamT<double> *mimicMultP;
-+ private: ParamT<double> *mimicOffsetP;
-+ private: ParamT<double> *mimicKpP;
-+ private: ParamT<double> *mimicKdP;
-+ private: ParamT<double> *mimicFMaxP;
-+ private: Joint *mimicJoint;
-+ private: bool enableMimic;
-+ private: double mimicMult;
-+ private: double mimicOffset;
-+ private: double mimicKp;
-+ private: double mimicKd;
-+ private: double mimicFMax;
-+
- /// Feedback data for this joint
- private: dJointFeedback *feedback;
-
Index: server/physics/HingeJoint.cc
===================================================================
--- server/physics/HingeJoint.cc (revision 7168)
@@ -1510,7 +1512,7 @@
if (**this->provideFeedbackP)
{
-@@ -147,6 +174,25 @@
+@@ -147,6 +174,29 @@
{
this->SetAnchor(anchorVec);
}
@@ -1529,14 +1531,18 @@
+ std::cout << this->nameP->GetValue() << " this joint will mimic joint " << this->mimicJointP->GetValue() << std::endl;
+ if (this->type == Joint::HINGE)
+ {
-+ ((HingeJoint*)this)->SetParam(dParamFMax, this->mimicFMax);
-+ ((HingeJoint*)this)->SetTorque(0);
++ dynamic_cast<HingeJoint*>(this)->SetParam(dParamFMax, this->mimicFMax);
++ dynamic_cast<HingeJoint*>(this)->SetTorque(0);
+ }
++ current_time = Simulator::Instance()->GetSimTime();
++ last_time = current_time;
++ current_error = (this->mimicMult*((dynamic_cast<HingeJoint*>(this->mimicJoint))->GetAngle()) - this->mimicOffset - dynamic_cast<HingeJoint*>(this)->GetAngle());
++ last_error = current_error;
+ }
}
////////////////////////////////////////////////////////////////////////////////
-@@ -184,13 +230,39 @@
+@@ -184,13 +234,45 @@
/// Update the joint
void Joint::Update()
{
@@ -1549,10 +1555,16 @@
+ this->mimicJoint->GetType() == Joint::HINGE &&
+ this->type == Joint::HINGE )
+ {
-+ cmd = this->mimicKp * (this->mimicMult*(((HingeJoint*)(this->mimicJoint))->GetAngle()) - this->mimicOffset - ((HingeJoint*)this)->GetAngle());
-+ ((HingeJoint*)this)->SetParam(dParamVel, cmd);
++ current_time = Simulator::Instance()->GetSimTime();
++ current_error = (this->mimicMult*((dynamic_cast<HingeJoint*>(this->mimicJoint))->GetAngle()) - this->mimicOffset - dynamic_cast<HingeJoint*>(this)->GetAngle());
+
++ cmd = this->mimicKp * current_error
++ + this->mimicKd * (current_error - last_error) * (current_time - last_time);
++ dynamic_cast<HingeJoint*>(this)->SetParam(dParamVel, cmd);
++ last_error = current_error;
++ last_time = current_time;
+ }
-
++
+ // if (this->type == Joint::HINGE )
+ // {
+ // std::cout << " joint name " << this->nameP->GetValue()
@@ -1560,9 +1572,9 @@
+ // << " mimic offset " << this->mimicOffset
+ // << " mimic multip " << this->mimicMult
+ // << " mimic kp " << this->mimicKp
-+ // << " fmax " << ((HingeJoint*)this)->GetParam(dParamFMax)
-+ // << " vel " << ((HingeJoint*)this)->GetParam(dParamVel)
-+ // << " ang " << ((HingeJoint*)this)->GetAngle()
++ // << " fmax " << (dynamic_cast<HingeJoint*>this)->GetParam(dParamFMax)
++ // << " vel " << (dynamic_cast<HingeJoint*>this)->GetParam(dParamVel)
++ // << " ang " << (dynamic_cast<HingeJoint*>this)->GetAngle()
+ // << std::endl;
+ // }
+
@@ -1579,7 +1591,7 @@
Vector3 start;
if (this->body1)
-@@ -206,6 +278,7 @@
+@@ -206,6 +288,7 @@
this->line2->SetPoint(0, start);
this->line2->Update();
}
@@ -2861,123 +2873,6 @@
this->RTTMode="PBuffer";
}
}
-Index: server/Model.cc
-===================================================================
---- server/Model.cc (revision 7168)
-+++ server/Model.cc (working copy)
-@@ -46,7 +46,12 @@
- #include "ControllerFactory.hh"
- #include "IfaceFactory.hh"
- #include "Model.hh"
-+#include "Simulator.hh"
-
-+#ifdef TIMING
-+#include "Simulator.hh"// for timing
-+#endif
-+
- using namespace gazebo;
-
- uint Model::lightNumber = 0;
-@@ -127,7 +132,10 @@
- if (this->type == "physical")
- this->LoadPhysical(node);
- else if (this->type == "renderable")
-- this->LoadRenderable(node);
-+ {
-+ if (Simulator::Instance()->GetRenderEngineEnabled())
-+ this->LoadRenderable(node);
-+ }
- else if (this->type != "empty")
- {
- gzthrow("Invalid model type[" + this->type + "]\n");
-@@ -255,10 +263,11 @@
- }
- else
- {
-- if (!this->lightName.empty())
-- {
-- OgreCreator::SaveLight(p, this->lightName, stream);
-- }
-+ if (Simulator::Instance()->GetRenderEngineEnabled())
-+ if (!this->lightName.empty())
-+ {
-+ OgreCreator::SaveLight(p, this->lightName, stream);
-+ }
- }
-
- if (this->parentBodyNameP && this->myBodyNameP)
-@@ -305,7 +314,7 @@
-
- return this->InitChild();
- }
--
-+
- ////////////////////////////////////////////////////////////////////////////////
- // Update the model
- int Model::Update()
-@@ -316,6 +325,10 @@
-
- Pose3d bodyPose, newPose, oldPose;
-
-+#ifdef TIMING
-+ double tmpT1 = Simulator::Instance()->GetWallTime();
-+#endif
-+
- for (bodyIter=this->bodies.begin(); bodyIter!=this->bodies.end(); bodyIter++)
- {
- if (bodyIter->second)
-@@ -324,6 +337,11 @@
- }
- }
-
-+#ifdef TIMING
-+ double tmpT2 = Simulator::Instance()->GetWallTime();
-+ std::cout << " bodies dt (" << tmpT2-tmpT1 << ")";
-+#endif
-+
- for (contIter=this->controllers.begin();
- contIter!=this->controllers.end(); contIter++)
- {
-@@ -332,6 +350,11 @@
- contIter->second->Update();
- }
-
-+#ifdef TIMING
-+ double tmpT3 = Simulator::Instance()->GetWallTime();
-+ std::cout << " controllers dt (" << tmpT3-tmpT2 << ")";
-+#endif
-+
- for (jointIter = this->joints.begin(); jointIter != this->joints.end(); jointIter++)
- {
- jointIter->second->Update();
-@@ -350,6 +373,11 @@
- this->rpyP->SetValue(this->pose.rot);
- }
-
-+#ifdef TIMING
-+ double tmpT4 = Simulator::Instance()->GetWallTime();
-+ std::cout << " joints/canonical body dt (" << tmpT4-tmpT3 << ")" << std::endl;
-+#endif
-+
- return this->UpdateChild();
- }
-
-@@ -730,10 +758,11 @@
- body->SetPose(Pose3d());
- this->bodies[body->GetName()] = body;
-
-- if ((childNode = node->GetChild("light")))
-- {
-- this->lightName = OgreCreator::CreateLight(childNode, body->GetVisualNode());
-- }
-+ if (Simulator::Instance()->GetRenderEngineEnabled())
-+ if ((childNode = node->GetChild("light")))
-+ {
-+ this->lightName = OgreCreator::CreateLight(childNode, body->GetVisualNode());
-+ }
-
- }
-
Index: server/gui/GLWindow.hh
===================================================================
--- server/gui/GLWindow.hh (revision 7168)
@@ -3155,6 +3050,123 @@
case XK_Up:
case XK_w:
this->directionVec.x += this->moveAmount;
+Index: server/Model.cc
+===================================================================
+--- server/Model.cc (revision 7168)
++++ server/Model.cc (working copy)
+@@ -46,7 +46,12 @@
+ #include "ControllerFactory.hh"
+ #include "IfaceFactory.hh"
+ #include "Model.hh"
++#include "Simulator.hh"
+
++#ifdef TIMING
++#include "Simulator.hh"// for timing
++#endif
++
+ using namespace gazebo;
+
+ uint Model::lightNumber = 0;
+@@ -127,7 +132,10 @@
+ if (this->type == "physical")
+ this->LoadPhysical(node);
+ else if (this->type == "renderable")
+- this->LoadRenderable(node);
++ {
++ if (Simulator::Instance()->GetRenderEngineEnabled())
++ this->LoadRenderable(node);
++ }
+ else if (this->type != "empty")
+ {
+ gzthrow("Invalid model type[" + this->type + "]\n");
+@@ -255,10 +263,11 @@
+ }
+ else
+ {
+- if (!this->lightName.empty())
+- {
+- OgreCreator::SaveLight(p, this->lightName, stream);
+- }
++ if (Simulator::Instance()->GetRenderEngineEnabled())
++ if (!this->lightName.empty())
++ {
++ OgreCreator::SaveLight(p, this->lightName, stream);
++ }
+ }
+
+ if (this->parentBodyNameP && this->myBodyNameP)
+@@ -305,7 +314,7 @@
+
+ return this->InitChild();
+ }
+-
++
+ ////////////////////////////////////////////////////////////////////////////////
+ // Update the model
+ int Model::Update()
+@@ -316,6 +325,10 @@
+
+ Pose3d bodyPose, newPose, oldPose;
+
++#ifdef TIMING
++ double tmpT1 = Simulator::Instance()->GetWallTime();
++#endif
++
+ for (bodyIter=this->bodies.begin(); bodyIter!=this->bodies.end(); bodyIter++)
+ {
+ if (bodyIter->second)
+@@ -324,6 +337,11 @@
+ }
+ }
+
++#ifdef TIMING
++ double tmpT2 = Simulator::Instance()->GetWallTime();
++ std::cout << " bodies dt (" << tmpT2-tmpT1 << ")";
++#endif
++
+ for (contIter=this->controllers.begin();
+ contIter!=this->controllers.end(); contIter++)
+ {
+@@ -332,6 +350,11 @@
+ contIter->second->Update();
+ }
+
++#ifdef TIMING
++ double tmpT3 = Simulator::Instance()->GetWallTime();
++ std::cout << " controllers dt (" << tmpT3-tmpT2 << ")";
++#endif
++
+ for (jointIter = this->joints.begin(); jointIter != this->joints.end(); jointIter++)
+ {
+ jointIter->second->Update();
+@@ -350,6 +373,11 @@
+ this->rpyP->SetValue(this->pose.rot);
+ }
+
++#ifdef TIMING
++ double tmpT4 = Simulator::Instance()->GetWallTime();
++ std::cout << " joints/canonical body dt (" << tmpT4-tmpT3 << ")" << std::endl;
++#endif
++
+ return this->UpdateChild();
+ }
+
+@@ -730,10 +758,11 @@
+ body->SetPose(Pose3d());
+ this->bodies[body->GetName()] = body;
+
+- if ((childNode = node->GetChild("light")))
+- {
+- this->lightName = OgreCreator::CreateLight(childNode, body->GetVisualNode());
+- }
++ if (Simulator::Instance()->GetRenderEngineEnabled())
++ if ((childNode = node->GetChild("light")))
++ {
++ this->lightName = OgreCreator::CreateLight(childNode, body->GetVisualNode());
++ }
+
+ }
+
Index: server/World.hh
===================================================================
--- server/World.hh (revision 7168)
@@ -3240,6 +3252,38 @@
}
////////////////////////////////////////////////////////////////////////////////
+Index: server/controllers/factory/Factory.cc
+===================================================================
+--- server/controllers/factory/Factory.cc (revision 7168)
++++ server/controllers/factory/Factory.cc (working copy)
+@@ -76,6 +76,8 @@
+ // Initialize the controller
+ void Factory::InitChild()
+ {
++ // initialize newModel to blank
++ strcpy((char*)this->factoryIface->data->newModel,"");
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+@@ -83,8 +85,10 @@
+ void Factory::UpdateChild()
+ {
+ // If there is a string, then add the contents to the world
++ this->factoryIface->Lock(1);
+ if (strcmp((const char*)this->factoryIface->data->newModel,"")!=0)
+ {
++ //std::cout << " factory update: " << this->factoryIface->data->newModel << std::endl;
+ std::string xmlString;
+ std::string xmlMiddle = (const char*)(this->factoryIface->data->newModel);
+
+@@ -121,6 +125,7 @@
+
+ strcpy((char*)this->factoryIface->data->deleteModel,"");
+ }
++ this->factoryIface->Unlock();
+
+ }
+
Index: server/controllers/Controller.cc
===================================================================
--- server/controllers/Controller.cc (revision 7168)
@@ -3300,38 +3344,6 @@
for (iter=this->ifaces.begin(); iter!=this->ifaces.end(); iter++)
{
if ((*iter)->GetOpenCount() > 0)
-Index: server/controllers/factory/Factory.cc
-===================================================================
---- server/controllers/factory/Factory.cc (revision 7168)
-+++ server/controllers/factory/Factory.cc (working copy)
-@@ -76,6 +76,8 @@
- // Initialize the controller
- void Factory::InitChild()
- {
-+ // initialize newModel to blank
-+ strcpy((char*)this->factoryIface->data->newModel,"");
- }
-
- ////////////////////////////////////////////////////////////////////////////////
-@@ -83,8 +85,10 @@
- void Factory::UpdateChild()
- {
- // If there is a string, then add the contents to the world
-+ this->factoryIface->Lock(1);
- if (strcmp((const char*)this->factoryIface->data->newModel,"")!=0)
- {
-+ //std::cout << " factory update: " << this->factoryIface->data->newModel << std::endl;
- std::string xmlString;
- std::string xmlMiddle = (const char*)(this->factoryIface->data->newModel);
-
-@@ -121,6 +125,7 @@
-
- strcpy((char*)this->factoryIface->data->deleteModel,"");
- }
-+ this->factoryIface->Unlock();
-
- }
-
Index: server/controllers/ptz/generic/Generic_PTZ.cc
===================================================================
--- server/controllers/ptz/generic/Generic_PTZ.cc (revision 7168)
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/test/test_slide.xml
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/test/test_slide.xml 2008-12-17 00:28:07 UTC (rev 8218)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/test/test_slide.xml 2008-12-17 00:28:15 UTC (rev 8219)
@@ -1,7 +1,7 @@
<launch>
<master auto="start" />
- <node pkg="gazebo" type="gazebo" args="-r $(find gazebo_robot_description)/gazebo_worlds/test/test_slide.world" respawn="false" output="screen">
+ <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/test/test_slide.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(find boost)/boost/lib" />
<env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
<env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
@@ -12,7 +12,7 @@
<param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find wg_robot_description)/pr2/pr2.xacro.xml'" />
<!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
- <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2 0 8 110 0 0 90" respawn="false" output="screen" /> <!-- load default arm controller -->
+ <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2 0 8 90 0 0 90" respawn="false" output="screen" /> <!-- load default arm controller -->
<!-- load controllers -->
<include file="$(find pr2_gazebo)/pr2_default_controllers.launch" />
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/test/test_camera.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/test/test_camera.world 2008-12-17 00:28:07 UTC (rev 8218)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/test/test_camera.world 2008-12-17 00:28:15 UTC (rev 8219)
@@ -25,7 +25,7 @@
<!-- erp is typically .1-.8 -->
<!-- here's the global contact cfm/erp -->
<physics:ode>
- <stepTime>0.0005</stepTime>
+ <stepTime>0.001</stepTime>
<gravity>0 0 -9.8</gravity>
<cfm>0.0000000001</cfm>
<erp>0.2</erp>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/test/test_scan.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/test/test_scan.world 2008-12-17 00:28:07 UTC (rev 8218)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/test/test_scan.world 2008-12-17 00:28:15 UTC (rev 8219)
@@ -24,7 +24,7 @@
<!-- erp is typically .1-.8 -->
<!-- here's the global contact cfm/erp -->
<physics:ode>
- <stepTime>0.0005</stepTime>
+ <stepTime>0.001</stepTime>
<gravity>0 0 -9.8</gravity>
<cfm>0.0000000001</cfm>
<erp>0.2</erp>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/test/test_slide.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/test/test_slide.world 2008-12-17 00:28:07 UTC (rev 8218)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/test/test_slide.world 2008-12-17 00:28:15 UTC (rev 8219)
@@ -24,7 +24,7 @@
<!-- erp is typically .1-.8 -->
<!-- here's the global contact cfm/erp -->
<physics:ode>
- <stepTime>0.0005</stepTime>
+ <stepTime>0.001</stepTime>
<gravity>0 0 -9.8</gravity>
<cfm>0.0000000001</cfm>
<erp>0.2</erp>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-12-17 01:26:55
|
Revision: 8237
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8237&view=rev
Author: gerkey
Date: 2008-12-17 01:26:52 +0000 (Wed, 17 Dec 2008)
Log Message:
-----------
Removed dependencies on roslaunch, now that it's part of the bootstrap build
Modified Paths:
--------------
pkg/trunk/demos/2dnav_erratic/manifest.xml
pkg/trunk/demos/2dnav_gazebo/manifest.xml
pkg/trunk/demos/2dnav_pr2/manifest.xml
pkg/trunk/demos/2dnav_stage/manifest.xml
pkg/trunk/demos/arm_gazebo/manifest.xml
pkg/trunk/demos/examples_gazebo/manifest.xml
pkg/trunk/demos/pr2_gazebo/manifest.xml
pkg/trunk/demos/pr2_prototype1_gazebo/manifest.xml
pkg/trunk/demos/stair1-demos/manifest.xml
pkg/trunk/hardware_test/qualification/manifest.xml
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/manifest.xml
Modified: pkg/trunk/demos/2dnav_erratic/manifest.xml
===================================================================
--- pkg/trunk/demos/2dnav_erratic/manifest.xml 2008-12-17 01:21:26 UTC (rev 8236)
+++ pkg/trunk/demos/2dnav_erratic/manifest.xml 2008-12-17 01:26:52 UTC (rev 8237)
@@ -5,7 +5,6 @@
<review status="na" notes=""/>
<url>http://pr.willowgarage.com/wiki/FIXME</url>
<depend package="nav_view"/>
- <depend package="roslaunch"/>
<depend package="amcl_player"/>
<depend package="erratic_player"/>
<depend package="hokuyo_node"/>
Modified: pkg/trunk/demos/2dnav_gazebo/manifest.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/manifest.xml 2008-12-17 01:21:26 UTC (rev 8236)
+++ pkg/trunk/demos/2dnav_gazebo/manifest.xml 2008-12-17 01:26:52 UTC (rev 8237)
@@ -5,7 +5,6 @@
<review status="NA" notes=""/>
<url>http://pr.willowgarage.com/wiki/Simulator</url>
<depend package="nav_view"/>
- <depend package="roslaunch"/>
<depend package="gazebo_plugin"/>
<depend package="map_server"/>
<depend package="amcl_player"/>
Modified: pkg/trunk/demos/2dnav_pr2/manifest.xml
===================================================================
--- pkg/trunk/demos/2dnav_pr2/manifest.xml 2008-12-17 01:21:26 UTC (rev 8236)
+++ pkg/trunk/demos/2dnav_pr2/manifest.xml 2008-12-17 01:26:52 UTC (rev 8237)
@@ -4,7 +4,6 @@
<license>BSD</license>
<review status="na" notes=""/>
<url>http://pr.willowgarage.com/wiki/FIXME</url>
- <depend package="roslaunch"/>
<depend package="amcl_player"/>
<depend package="hokuyo_node"/>
<depend package="imu_node"/>
Modified: pkg/trunk/demos/2dnav_stage/manifest.xml
===================================================================
--- pkg/trunk/demos/2dnav_stage/manifest.xml 2008-12-17 01:21:26 UTC (rev 8236)
+++ pkg/trunk/demos/2dnav_stage/manifest.xml 2008-12-17 01:26:52 UTC (rev 8237)
@@ -6,7 +6,6 @@
<url>http://pr.willowgarage.com/wiki/FIXME</url>
<depend package="nav_view"/>
<depend package="nav_view_sdl"/>
- <depend package="roslaunch"/>
<depend package="amcl_player"/>
<depend package="fake_localization"/>
<depend package="rosstage"/>
Modified: pkg/trunk/demos/arm_gazebo/manifest.xml
===================================================================
--- pkg/trunk/demos/arm_gazebo/manifest.xml 2008-12-17 01:21:26 UTC (rev 8236)
+++ pkg/trunk/demos/arm_gazebo/manifest.xml 2008-12-17 01:26:52 UTC (rev 8237)
@@ -4,7 +4,6 @@
<license>BSD</license>
<review status="na" notes=""/>
<url>http://pr.willowgarage.com/wiki/Simulator</url>
- <depend package="roslaunch"/>
<depend package="gazebo_plugin"/>
<depend package="teleop_arm_keyboard"/>
<depend package="pr2_gui"/>
Modified: pkg/trunk/demos/examples_gazebo/manifest.xml
===================================================================
--- pkg/trunk/demos/examples_gazebo/manifest.xml 2008-12-17 01:21:26 UTC (rev 8236)
+++ pkg/trunk/demos/examples_gazebo/manifest.xml 2008-12-17 01:26:52 UTC (rev 8237)
@@ -4,7 +4,6 @@
<license>BSD</license>
<review status="na" notes=""/>
<url>http://pr.willowgarage.com/wiki/Simulator</url>
- <depend package="roslaunch"/>
<depend package="gazebo_plugin"/>
<depend package="teleop_base_keyboard"/>
<depend package="teleop_arm_keyboard"/>
Modified: pkg/trunk/demos/pr2_gazebo/manifest.xml
===================================================================
--- pkg/trunk/demos/pr2_gazebo/manifest.xml 2008-12-17 01:21:26 UTC (rev 8236)
+++ pkg/trunk/demos/pr2_gazebo/manifest.xml 2008-12-17 01:26:52 UTC (rev 8237)
@@ -4,7 +4,6 @@
<license>BSD</license>
<review status="na" notes=""/>
<url>http://pr.willowgarage.com/wiki/Simulator</url>
- <depend package="roslaunch"/>
<depend package="gazebo_plugin"/>
<depend package="teleop_arm_keyboard"/>
<depend package="gazebo_robot_description"/>
Modified: pkg/trunk/demos/pr2_prototype1_gazebo/manifest.xml
===================================================================
--- pkg/trunk/demos/pr2_prototype1_gazebo/manifest.xml 2008-12-17 01:21:26 UTC (rev 8236)
+++ pkg/trunk/demos/pr2_prototype1_gazebo/manifest.xml 2008-12-17 01:26:52 UTC (rev 8237)
@@ -4,7 +4,6 @@
<license>BSD</license>
<review status="na" notes="demo scripts"/>
<url>http://pr.willowgarage.com/wiki/Simulator</url>
- <depend package="roslaunch"/>
<depend package="gazebo_plugin"/>
<depend package="teleop_base_keyboard"/>
<depend package="teleop_arm_keyboard"/>
Modified: pkg/trunk/demos/stair1-demos/manifest.xml
===================================================================
--- pkg/trunk/demos/stair1-demos/manifest.xml 2008-12-17 01:21:26 UTC (rev 8236)
+++ pkg/trunk/demos/stair1-demos/manifest.xml 2008-12-17 01:26:52 UTC (rev 8237)
@@ -4,7 +4,6 @@
<license>BSD</license>
<review status="na" notes=""/>
<depend package="nav_view"/>
- <depend package="roslaunch"/>
<depend package="amcl_player"/>
<depend package="segway_apox"/>
<depend package="sicktoolbox_wrapper"/>
Modified: pkg/trunk/hardware_test/qualification/manifest.xml
===================================================================
--- pkg/trunk/hardware_test/qualification/manifest.xml 2008-12-17 01:21:26 UTC (rev 8236)
+++ pkg/trunk/hardware_test/qualification/manifest.xml 2008-12-17 01:26:52 UTC (rev 8237)
@@ -8,7 +8,6 @@
<depend package="std_msgs" />
<depend package="robot_msgs" />
<depend package="robot_srvs" />
- <depend package="roslaunch" />
<depend package="hokuyo_node" />
<depend package="imu_node" />
<depend package="matplotlib" />
Modified: pkg/trunk/highlevel/executive_trex/executive_trex_pr2/manifest.xml
===================================================================
--- pkg/trunk/highlevel/executive_trex/executive_trex_pr2/manifest.xml 2008-12-17 01:21:26 UTC (rev 8236)
+++ pkg/trunk/highlevel/executive_trex/executive_trex_pr2/manifest.xml 2008-12-17 01:26:52 UTC (rev 8237)
@@ -5,7 +5,6 @@
<review status="unreviewed" notes=""/>
<depend package="trex" />
<depend package="rospy" />
- <depend package="rostest" />
<depend package="highlevel_controllers" />
<sysdepend os="ubuntu" version="8.04-hardy" package="jam"/>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <vij...@us...> - 2008-12-17 02:22:25
|
Revision: 8243
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8243&view=rev
Author: vijaypradeep
Date: 2008-12-17 02:02:47 +0000 (Wed, 17 Dec 2008)
Log Message:
-----------
TrackLinkCmd works in gazebo. Need to test it on the robot. Need to also add a filter to this
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp
pkg/trunk/robot_descriptions/pr2/pr2_experimental_controllers/laser_tilt/laser_tilt_controller.xml
Added Paths:
-----------
pkg/trunk/controllers/pr2_mechanism_controllers/msg/TrackLinkCmd.msg
pkg/trunk/controllers/pr2_mechanism_controllers/scripts/send_track_link_cmd.py
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h 2008-12-17 01:52:03 UTC (rev 8242)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/laser_scanner_traj_controller.h 2008-12-17 02:02:47 UTC (rev 8243)
@@ -44,6 +44,7 @@
// Messages
#include <pr2_mechanism_controllers/LaserScannerSignal.h>
#include <pr2_mechanism_controllers/PeriodicCmd.h>
+#include <pr2_mechanism_controllers/TrackLinkCmd.h>
// Services
#include <robot_mechanism_controllers/SetCommand.h>
@@ -68,6 +69,8 @@
void setPeriodicCmd(const pr2_mechanism_controllers::PeriodicCmd& cmd) ;
+ void setTrackLinkCmd(const pr2_mechanism_controllers::TrackLinkCmd& track_link_cmd) ;
+
void setTrajectory(const std::vector<trajectory::Trajectory::TPoint>& traj_points,
double max_rate, double max_acc, std::string interp ) ;
@@ -83,11 +86,19 @@
ros::thread::mutex traj_lock_ ; // Mutex for traj_
trajectory::Trajectory traj_ ; // Stores the current trajectory being executed
+ ros::thread::mutex track_link_lock_ ;
+ bool track_link_enabled_ ;
+ mechanism::LinkState* target_link_ ;
+ mechanism::LinkState* mount_link_ ;
+ tf::Vector3 track_point_ ;
+
JointPositionSmoothController joint_position_controller_ ; // The PID position controller that is doing all the under-the-hood controls stuff
double traj_start_time_ ; // The time that the trajectory was started (in seconds)
double traj_duration_ ; // The length of the current profile (in seconds)
+ double tracking_offset_ ; // Offset generated by the track_link code
+
double max_rate_ ; // Max allowable rate/velocity
double max_acc_ ; // Max allowable acceleration
};
@@ -104,6 +115,7 @@
// Message Callbacks
void setPeriodicCmd() ;
+ void setTrackLinkCmd() ;
private:
ros::node *node_ ;
@@ -113,6 +125,7 @@
double prev_profile_time_ ; //!< The time in the current profile when update() was last called
pr2_mechanism_controllers::PeriodicCmd cmd_ ;
+ pr2_mechanism_controllers::TrackLinkCmd track_link_cmd_ ;
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.
Added: pkg/trunk/controllers/pr2_mechanism_controllers/msg/TrackLinkCmd.msg
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/msg/TrackLinkCmd.msg (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/msg/TrackLinkCmd.msg 2008-12-17 02:02:47 UTC (rev 8243)
@@ -0,0 +1,3 @@
+int8 enable
+string link_name
+std_msgs/Point point
Added: pkg/trunk/controllers/pr2_mechanism_controllers/scripts/send_track_link_cmd.py
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/scripts/send_track_link_cmd.py (rev 0)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/scripts/send_track_link_cmd.py 2008-12-17 02:02:47 UTC (rev 8243)
@@ -0,0 +1,57 @@
+#!/usr/bin/env python
+
+PKG = "pr2_mechanism_controllers"
+
+import rostools; rostools.update_path(PKG)
+
+import sys
+import os
+import string
+
+import rospy
+from std_msgs import *
+
+from std_msgs.msg import Point
+from pr2_mechanism_controllers.msg import TrackLinkCmd
+from time import sleep
+
+def print_usage(exit_code = 0):
+ print '''Usage:
+ send_track_link_cmd.py [controller] [link_name] [x] [y] [z]
+ - [controller] - The controller's name
+ - [link name] - Name of the link that we want to track
+ - [x] [y] [z] - The location on the link (in the link's frame) that we want to track
+ send_track_link_cmd.py [controller]
+ - Sends a 'disable' command to the controller
+'''
+ sys.exit(exit_code)
+
+if __name__ == '__main__':
+ if len(sys.argv) == 6 :
+ cmd = TrackLinkCmd()
+ controller = sys.argv[1]
+ cmd.link_name = sys.argv[2]
+ cmd.point = Point(float(sys.argv[3]), float(sys.argv[4]), float(sys.argv[5]))
+ cmd.enable = 1
+ elif len(sys.argv) == 2 :
+ cmd = TrackLinkCmd()
+ controller = sys.argv[1]
+ cmd.link_name = 'none'
+ cmd.point = Point(0.0, 0.0, 0.0)
+ cmd.enable = 0
+ else :
+ print_usage()
+
+ print 'Sending TrackLinkCmd Command to %s: ' % controller
+ print ' Enable: %u' % cmd.enable
+ print ' Link Name: %s' % cmd.link_name
+ print ' Point: (%f, %f, %f)' % (cmd.point.x, cmd.point.y, cmd.point.z)
+
+ command_publisher = rospy.Publisher(controller + '/set_track_link_cmd', TrackLinkCmd)
+ rospy.init_node('track_link_cmd_commander', anonymous=True)
+ sleep(1)
+ command_publisher.publish( cmd )
+
+ sleep(1)
+ print 'Command sent!'
+
Property changes on: pkg/trunk/controllers/pr2_mechanism_controllers/scripts/send_track_link_cmd.py
___________________________________________________________________
Added: svn:executable
+ *
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp 2008-12-17 01:52:03 UTC (rev 8242)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_traj_controller.cpp 2008-12-17 02:02:47 UTC (rev 8243)
@@ -44,7 +44,8 @@
LaserScannerTrajController::LaserScannerTrajController() : traj_(1)
{
-
+ tracking_offset_ = 0 ;
+ track_link_enabled_ = false ;
}
LaserScannerTrajController::~LaserScannerTrajController()
@@ -92,6 +93,32 @@
{
//if (!joint_state_->calibrated_)
// return;
+
+
+ // ***** Compute the offset from tracking a link *****
+ //! \todo replace this link tracker with a KDL inverse kinematics solver
+ if(track_link_lock_.trylock())
+ {
+ if (track_link_enabled_ && target_link_ && mount_link_)
+ {
+ // Compute the position of track_point_ in the world frame
+ tf::Pose link_pose(target_link_->abs_orientation_, target_link_->abs_position_) ;
+ tf::Point link_point_world ;
+ link_point_world = link_pose*track_point_ ;
+
+ // We're hugely approximating our inverse kinematics. This is probably good enough for now...
+ double dx = link_point_world.x() - mount_link_->abs_position_.x() ;
+ double dz = link_point_world.z() - mount_link_->abs_position_.z() ;
+ tracking_offset_ = atan2(-dz,dx) ;
+ }
+ else
+ {
+ tracking_offset_ = 0.0 ;
+ }
+ track_link_lock_.unlock() ;
+ }
+
+ // ***** Compute the current command from the trajectory profile *****
if (traj_lock_.trylock())
{
if (traj_duration_ > 1e-6) // Short trajectories could make the mod_time calculation unstable
@@ -107,7 +134,7 @@
result = traj_.sample(sampled_point, profile_time) ;
if (result > 0)
{
- joint_position_controller_.setCommand(sampled_point.q_[0]) ;
+ joint_position_controller_.setCommand(sampled_point.q_[0] + tracking_offset_) ;
joint_position_controller_.update() ;
}
}
@@ -151,13 +178,13 @@
traj_lock_.unlock() ;
}
-void LaserScannerTrajController::setPeriodicCmd(const pr2_mechanism_controllers::PeriodicCmd& cmd_)
+void LaserScannerTrajController::setPeriodicCmd(const pr2_mechanism_controllers::PeriodicCmd& cmd)
{
- if (cmd_.profile == "linear" ||
- cmd_.profile == "blended_linear")
+ if (cmd.profile == "linear" ||
+ cmd.profile == "blended_linear")
{
- double high_pt = cmd_.amplitude + cmd_.offset ;
- double low_pt = -cmd_.amplitude + cmd_.offset ;
+ double high_pt = cmd.amplitude + cmd.offset ;
+ double low_pt = -cmd.amplitude + cmd.offset ;
std::vector<trajectory::Trajectory::TPoint> tpoints ;
@@ -170,14 +197,14 @@
tpoints.push_back(cur_point) ;
cur_point.q_[0] = high_pt ;
- cur_point.time_ = cmd_.period/2.0 ;
+ cur_point.time_ = cmd.period/2.0 ;
tpoints.push_back(cur_point) ;
cur_point.q_[0] = low_pt ;
- cur_point.time_ = cmd_.period ;
+ cur_point.time_ = cmd.period ;
tpoints.push_back(cur_point) ;
- setTrajectory(tpoints, max_rate_, max_acc_, cmd_.profile) ;
+ setTrajectory(tpoints, max_rate_, max_acc_, cmd.profile) ;
ROS_INFO("LaserScannerTrajController: Periodic Command set") ;
}
else
@@ -186,6 +213,41 @@
}
}
+void LaserScannerTrajController::setTrackLinkCmd(const pr2_mechanism_controllers::TrackLinkCmd& track_link_cmd)
+{
+ while (!track_link_lock_.trylock())
+ usleep(100) ;
+
+ if (track_link_cmd.enable)
+ {
+ ROS_INFO("LaserScannerTrajController:: Tracking link %s", track_link_cmd.link_name.c_str()) ;
+ track_link_enabled_ = true ;
+ string mount_link_name = "laser_tilt_mount_link" ;
+ target_link_ = robot_->getLinkState(track_link_cmd.link_name) ;
+ mount_link_ = robot_->getLinkState(mount_link_name) ;
+ tf::PointMsgToTF(track_link_cmd.point, track_point_) ;
+
+ if (target_link_ == NULL)
+ {
+ ROS_ERROR("LaserScannerTrajController:: Could not find target link:%s", track_link_cmd.link_name.c_str()) ;
+ track_link_enabled_ = false ;
+ }
+ if (mount_link_ == NULL)
+ {
+ ROS_ERROR("LaserScannerTrajController:: Could not find mount link:%s", mount_link_name.c_str()) ;
+ track_link_enabled_ = false ;
+ }
+ }
+ else
+ {
+ track_link_enabled_ = false ;
+ ROS_INFO("LaserScannerTrajController:: No longer tracking link") ;
+ }
+
+ track_link_lock_.unlock() ;
+}
+
+
ROS_REGISTER_CONTROLLER(LaserScannerTrajControllerNode)
LaserScannerTrajControllerNode::LaserScannerTrajControllerNode(): node_(ros::node::instance()), c_()
{
@@ -196,8 +258,10 @@
LaserScannerTrajControllerNode::~LaserScannerTrajControllerNode()
{
node_->unsubscribe(service_prefix_ + "/set_periodic_cmd") ;
+ node_->unsubscribe(service_prefix_ + "/set_track_link_cmd") ;
publisher_->stop() ;
+
delete publisher_ ; // Probably should wait on publish_->is_running() before exiting. Need to
// look into shutdown semantics for realtime_publisher
}
@@ -250,6 +314,7 @@
}
node_->subscribe(service_prefix_ + "/set_periodic_cmd", cmd_, &LaserScannerTrajControllerNode::setPeriodicCmd, this, 1) ;
+ node_->subscribe(service_prefix_ + "/set_track_link_cmd", track_link_cmd_, &LaserScannerTrajControllerNode::setTrackLinkCmd, this, 1) ;
if (publisher_ != NULL) // Make sure that we don't memory leak if initXml gets called twice
delete publisher_ ;
@@ -264,3 +329,8 @@
{
c_.setPeriodicCmd(cmd_) ;
}
+
+void LaserScannerTrajControllerNode::setTrackLinkCmd()
+{
+ c_.setTrackLinkCmd(track_link_cmd_) ;
+}
Modified: pkg/trunk/robot_descriptions/pr2/pr2_experimental_controllers/laser_tilt/laser_tilt_controller.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_experimental_controllers/laser_tilt/laser_tilt_controller.xml 2008-12-17 01:52:03 UTC (rev 8242)
+++ pkg/trunk/robot_descriptions/pr2/pr2_experimental_controllers/laser_tilt/laser_tilt_controller.xml 2008-12-17 02:02:47 UTC (rev 8243)
@@ -2,9 +2,9 @@
<controller name="laser_controller" topic="laser_controller" type="LaserScannerTrajControllerNode">
<max_acc value="1.0" />
<max_rate value="100.0" />
- <filter smoothing_factor="0.1" />
+ <filter smoothing_factor=".1" />
<joint name="laser_tilt_mount_joint">
- <pid p="12" i=".1" d="1" iClamp="0.5" />
+ <pid p="2" i=".1" d="0.25" iClamp="0.5" />
</joint>
</controller>
</controllers>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-12-17 03:21:16
|
Revision: 8245
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8245&view=rev
Author: hsujohnhsu
Date: 2008-12-17 03:21:11 +0000 (Wed, 17 Dec 2008)
Log Message:
-----------
added gazebo dynamics controller for arm.
Added Paths:
-----------
pkg/trunk/demos/arm_gazebo/arm_grav.launch
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/pr2_grav_arm.xacro.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/arm_grav_defs.xml
Added: pkg/trunk/demos/arm_gazebo/arm_grav.launch
===================================================================
--- pkg/trunk/demos/arm_gazebo/arm_grav.launch (rev 0)
+++ pkg/trunk/demos/arm_gazebo/arm_grav.launch 2008-12-17 03:21:11 UTC (rev 8245)
@@ -0,0 +1,28 @@
+<launch>
+ <group name="wg">
+ <!-- send pr2_arm.xml to param server -->
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find wg_robot_description)/pr2_arm_test/pr2_grav_arm.xacro.xml'" />
+
+ <!-- -g flag runs gazebo in gui-less mode -->
+ <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/empty.world" respawn="false" output="screen">
+ <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(find boost)/boost/lib" />
+ <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
+ <env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ </node>
+
+ <!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
+ <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)/l_arm_default_controller.xml" respawn="false" /--> <!-- load default arm controller -->
+ <node pkg="mechanism_control" type="mech.py" args="sp $(find arm_gazebo)/l_arm_dynamic_controller.xml" respawn="false" /> <!-- load default arm controller -->
+
+ <!-- send arm a command -->
+ <node pkg="robot_mechanism_controllers" type="control.py" args="set l_gripper_controller 0.5" respawn="false" output="screen" /> <!-- open gripper .5 radians -->
+
+ <!-- for visualization -->
+ <!-- node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" / -->
+ </group>
+</launch>
+
Added: pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/pr2_grav_arm.xacro.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/pr2_grav_arm.xacro.xml (rev 0)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/pr2_grav_arm.xacro.xml 2008-12-17 03:21:11 UTC (rev 8245)
@@ -0,0 +1,77 @@
+<?xml version="1.0"?>
+<robot name="pr2"
+ xmlns:xi="http://www.w3.org/2001/XInclude"
+ xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
+ xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
+ xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+ xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
+ xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
+ xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
+ xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
+ xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
+ xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
+ xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+ xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics">
+
+
+ <!-- declare the path where all models/textures/materials are stored -->
+ <resource location="ros-pkg://wg_robot_description/models/pr2"/>
+
+ <include filename="../pr2_robot_defs/arm_grav_defs.xml" />
+ <include filename="../pr2_robot_defs/gripper_defs.xml" />
+ <include filename="../pr2_robot_defs/gazebo_defs.xml" />
+ <include filename="./groups_arm.xml" />
+
+ <pr2_arm side="l" reflect="1" parent="base">
+ <origin xyz="0.0 0.0 1.0" rpy="0 0 0" />
+ </pr2_arm>
+ <pr2_gripper side="l" parent="l_wrist_roll" />
+
+ <!-- Solid Base -->
+ <joint name="base_joint" type="planar">
+ </joint>
+ <link name="base_link"><!-- link specifying the base of the robot -->
+ <parent name=" world" />
+ <!-- rotation of a local coordinate frame attached to the link with respect to a global coordinate frame -->
+ <origin xyz=" 0 0 0.002 " rpy=" 0 0 0" /> <!-- position of a local coordinate frame attached to the link with respect to the parent link's coordinate frame -->
+ <joint name="base_joint" />
+ <inertial>
+ <mass value="1000" />
+ <com xyz=" 0 0 0 " /> <!-- position of the center of mass with respect to the link's own anchor in a local coordinate frame -->
+ <inertia ixx="1000" ixy="0" ixz="0" iyy="1000" iyz="0" izz="1000" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0 " /> <!-- location defined with respect to the link origin in a local coordinate frame -->
+ <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
+ <map name="gazebo_material" flag="gazebo">
+ <elem key="material">Gazebo/White</elem>
+ </map>
+ <geometry name="pr2_base_mesh_file">
+ <mesh scale="20 20 0.01" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0.0 0.0 0.0 " /> <!-- default box is centered at the origin -->
+ <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
+ <geometry name="base_collision_geom"> <!-- think about putting mesh here as well -->
+ <box size="20 20 0.01" />
+ </geometry>
+ </collision>
+ </link>
+
+ <map name="gazebo_material" flag="gazebo">
+ <verbatim>
+ <!-- P3D for position groundtruth -->
+ <controller:P3D name="p3d_l_wrist_controller" plugin="libP3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>l_gripper_palm_link</bodyName>
+ <topicName>l_gripper_palm_pose_ground_truth</topicName>
+ <frameName>map</frameName>
+ <interface:position name="p3d_l_wrist_position"/>
+ </controller:P3D>
+ </verbatim>
+ </map>
+
+
+</robot>
Added: pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/arm_grav_defs.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/arm_grav_defs.xml (rev 0)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/arm_grav_defs.xml 2008-12-17 03:21:11 UTC (rev 8245)
@@ -0,0 +1,630 @@
+<?xml version="1.0"?>
+<robot xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
+ xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
+ xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface">
+
+ <property name="M_PI" value="3.1415926535897931" />
+ <property name="VELOCITY_LIMIT_SCALE" value="0.6" />
+
+ <property name="shoulder_lift_length" value="0.10" />
+ <property name="shoulder_lift_radius" value="0.12" />
+
+
+
+
+
+
+
+
+ <macro name="pr2_upper_arm" params="side parent reflect *origin">
+
+ <!-- Shoulder pan -->
+
+ <joint name="${side}_shoulder_pan_joint" type="revolute">
+ <axis xyz="0 0 1" />
+
+ <limit min="${reflect*M_PI/4-1.5}" max="${reflect*M_PI/4+1.5}"
+ effort="30" velocity="${VELOCITY_LIMIT_SCALE*3.48}"
+ k_position="100" k_velocity="10"
+ safety_length_min="0.15" safety_length_max="0.15" />
+ <calibration reference_position="${reflect*M_PI/4}" values="1.5 -1" />
+
+ <joint_properties damping="10.0" />
+ </joint>
+
+ <link name="${side}_shoulder_pan_link">
+ <parent name="${parent}_link" />
+ <insert_block name="origin" />
+ <joint name="${side}_shoulder_pan_joint" />
+ <inertial>
+ <mass value="16.29553" />
+ <com xyz="-0.005215 -0.030552 -0.175743" />
+ <inertia ixx="0.793291393007" ixy="0.003412032973" ixz="0.0096614481"
+ iyy="0.818419457224" iyz="-0.033999499551"
+ izz="0.13915007406" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <map name="gazebo_material" flag="gazebo">
+ <elem key="material">Gazebo/Blue</elem>
+ </map>
+ <geometry name="${side}_shoulder_pan_visual">
+ <mesh filename="shoulder_yaw" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0.05 0 -0.2" rpy="0 0 0 " />
+ <geometry name="${side}_shoulder_pan_collision">
+ <box size="0.347 0.254 0.646" />
+ </geometry>
+ </collision>
+ <map name="${side}_shoulder_pan_sensor" flag="gazebo">
+ <verbatim key="${side}_shoulder_pan_bumper_sensor">
+ <sensor:contact name="${side}_shoulder_pan_contact_sensor">
+ <geom>${side}_shoulder_pan_collision</geom>
+ <updateRate>1000.0</updateRate>
+ <controller:ros_bumper name="${side}_shoulder_pan_ros_bumper_controller" plugin="libRos_Bumper.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bumperTopicName>${side}_shoulder_pan_bumper</bumperTopicName>
+ <interface:bumper name="${side}_shoulder_pan_ros_bumper_iface" />
+ </controller:ros_bumper>
+ </sensor:contact>
+ </verbatim>
+ </map>
+ <map name="${side}_shoulder_pan_gravity" flag="gazebo">
+ <elem key="turnGravityOff">false</elem>
+ </map>
+ </link>
+
+ <transmission type="SimpleTransmission" name="${side}_shoulder_pan_trans">
+ <actuator name="${side}_shoulder_pan_motor" />
+ <joint name="${side}_shoulder_pan_joint" />
+ <mechanicalReduction>63.16</mechanicalReduction>
+ </transmission>
+
+ <!-- Shoulder lift -->
+
+ <joint name="${side}_shoulder_lift_joint" type="revolute">
+ <axis xyz="0 1 0" />
+ <anchor xyz="0 0 0" />
+
+ <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" />
+ <calibration reference_position="0" values="1.5 -1 " />
+
+ <joint_properties damping="100.0" />
+ </joint>
+
+ <link name="${side}_shoulder_lift_link">
+ <parent name="${side}_shoulder_pan_link" />
+ <origin xyz="0.1 0 0" rpy="0 0 0" />
+ <joint name="${side}_shoulder_lift_joint" />
+ <inertial>
+ <mass value="2.41223" />
+ <com xyz="-0.035895 0.014422 -0.028263" />
+ <inertia ixx="0.016640333585" ixy="0.002696462583" ixz="0.001337742275"
+ iyy="0.017232603914" iyz="-0.003605106514" izz="0.018723553425" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <map name="gazebo_material" flag="gazebo">
+ <elem key="material">Gazebo/Grey</elem>
+ </map>
+ <geometry name="${side}_shoulder_lift_visual">
+ <mesh filename="shoulder_lift" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0 " rpy="${M_PI/2} 0 0" />
+ <geometry name="${side}_shoulder_lift_collision">
+ <cylinder radius="${shoulder_lift_radius}" length="${shoulder_lift_length}" />
+ </geometry>
+ </collision>
+ <map name="${side}_shoulder_lift_sensor" flag="gazebo">
+ <verbatim key="${side}_shoulder_lift_bumper_sensor">
+ <sensor:contact name="${side}_shoulder_lift_contact_sensor">
+ <geom>${side}_shoulder_lift_collision</geom>
+ <updateRate>1000.0</updateRate>
+ <controller:ros_bumper name="${side}_shoulder_lift_ros_bumper_controller" plugin="libRos_Bumper.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bumperTopicName>${side}_shoulder_lift_bumper</bumperTopicName>
+ <interface:bumper name="${side}_shoulder_lift_ros_bumper_iface" />
+ </controller:ros_bumper>
+ </sensor:contact>
+ </verbatim>
+ </map>
+ <map name="${side}_shoulder_lift_gravity" flag="gazebo">
+ <elem key="turnGravityOff">false</elem>
+ </map>
+ </link>
+
+ <transmission type="SimpleTransmission" name="${side}_shoulder_lift_trans">
+ <actuator name="${side}_shoulder_lift_motor" />
+ <joint name="${side}_shoulder_lift_joint" />
+ <mechanicalReduction>57.36</mechanicalReduction>
+ </transmission>
+
+ <!-- Upperarm roll -->
+
+ <joint name="${side}_upper_arm_roll_joint" type="revolute">
+ <axis xyz="1 0 0" />
+ <anchor xyz="0 0 0" />
+
+ <limit min="-3.9" max="0.8" effort="30" velocity="${VELOCITY_LIMIT_SCALE*5.45}"
+ k_position="100" k_velocity="2"
+ safety_length_min="0.15" safety_length_max="0.15" />
+ <calibration reference_position="${-M_PI/2}" values="1.5 -1 " />
+
+ <joint_properties damping="1.0" />
+ </joint>
+
+ <link name="${side}_upper_arm_roll_link">
+ <parent name="${side}_shoulder_lift_link" />
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <joint name="${side}_upper_arm_roll_joint" />
+
+ <inertial>
+ <mass value="4.9481" />
+ <com xyz=" 0.21227 0.001205 -0.016293 " /> <!-- x becomes y, y becomes z, z becomes x, not sure about the signs -->
+ <inertia ixx="0.01338328491" ixy="0.00011947689" ixz="0.00154493231"
+ iyy="0.07306071531" iyz="0.00054774592"
+ izz="0.07212451075" /> <!-- x becomes y, y becomes z, z becomes x, not sure about the signs -->
+ </inertial>
+
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <map name="gazebo_material" flag="gazebo">
+ <elem key="material">Gazebo/Green</elem>
+ </map>
+ <geometry name="${side}_upper_arm_roll_visual">
+ <mesh filename="upperarm_roll" />
+ </geometry>
+ </visual>
+
+ <collision>
+ <origin xyz="0.3 0 0" rpy="0 0 0" />
+ <geometry name="${side}_upper_arm_roll_collision">
+ <box size="0.362 0.144 0.157" />
+ </geometry>
+ </collision>
+ <map name="${side}_upper_arm_roll_sensor" flag="gazebo">
+ <verbatim key="${side}_upper_arm_roll_bumper_sensor">
+ <sensor:contact name="${side}_upper_arm_roll_contact_sensor">
+ <geom>${side}_upper_arm_roll_collision</geom>
+ <updateRate>1000.0</updateRate>
+ <controller:ros_bumper name="${side}_upper_arm_roll_ros_bumper_controller" plugin="libRos_Bumper.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bumperTopicName>${side}_upper_arm_roll_bumper</bumperTopicName>
+ <interface:bumper name="${side}_upper_arm_roll_ros_bumper_iface" />
+ </controller:ros_bumper>
+ </sensor:contact>
+ </verbatim>
+ </map>
+
+ <map name="${side}_upper_arm_roll_gravity" flag="gazebo">
+ <elem key="turnGravityOff">false</elem>
+ </map>
+ </link>
+
+ <transmission type="SimpleTransmission" name="${side}_upper_arm_roll_trans">
+ <actuator name="${side}_upper_arm_roll_motor" />
+ <joint name="${side}_upper_arm_roll_joint" />
+ <mechanicalReduction>32.65</mechanicalReduction>
+ </transmission>
+
+ <!-- Elbow flex -->
+
+ <joint name="${side}_elbow_flex_joint" type="revolute">
+ <axis xyz="0 -1 0" />
+ <anchor xyz="0 0 0" />
+
+ <limit min="-0.1" max="2.3" effort="30" velocity="${VELOCITY_LIMIT_SCALE*5.5}"
+ k_position="100" k_velocity="3"
+ safety_length_min="0.1" safety_length_max="0.4" />
+ <calibration reference_position="1.1" values="1.5 -1" />
+
+ <joint_properties damping="10.0" />
+ </joint>
+
+ <link name="${side}_elbow_flex_link">
+ <parent name="${side}_upper_arm_roll_link" />
+ <origin xyz="0.4 0 0" rpy="0 0 0" />
+ <joint name="${side}_elbow_flex_joint" />
+
+ <inertial>
+ <mass value="1.689246" />
+ <com xyz="0.013173 0.001228 -0.011638" /> <!-- switching y to x, z to y, x to z -->
+ <inertia ixx="0.00323681521" ixy="-0.00000116216" ixz="0.00029204667"
+ iyy="0.00410053774" iyz="-0.00007735928"
+ izz="0.00327529855" /> <!-- switching y to x, z to y, x to z -->
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <map name="gazebo_material" flag="gazebo">
+ <elem key="material">Gazebo/Grey</elem>
+ </map>
+ <geometry name="${side}_elbow_flex_visual">
+ <mesh filename="elbow_flex" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
+ <geometry name="${side}_elbow_flex_collision">
+ <cylinder radius="0.1" length="0.08" />
+ </geometry>
+ </collision>
+ <map name="${side}_elbow_flex_sensor" flag="gazebo">
+ <verbatim key="${side}_elbow_flex_bumper_sensor">
+ <sensor:contact name="${side}_elbow_flex_contact_sensor">
+ <geom>${side}_elbow_flex_collision</geom>
+ <updateRate>1000.0</updateRate>
+ <controller:ros_bumper name="${side}_elbow_flex_ros_bumper_controller" plugin="libRos_Bumper.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bumperTopicName>${side}_elbow_flex_bumper</bumperTopicName>
+ <interface:bumper name="${side}_elbow_flex_ros_bumper_iface" />
+ </controller:ros_bumper>
+ </sensor:contact>
+ </verbatim>
+ </map>
+ <map name="${side}_elbow_flex_gravity" flag="gazebo">
+ <elem key="turnGravityOff">false</elem>
+ </map>
+ </link>
+
+ <transmission type="SimpleTransmission" name="${side}_elbow_flex_trans">
+ <actuator name="${side}_elbow_flex_motor" />
+ <joint name="${side}_elbow_flex_joint" />
+ <mechanicalReduction>36.17</mechanicalReduction>
+ </transmission>
+
+ <map name="sensor" flag="gazebo">
+ <verbatim key="p3d_${side}_upper_arm">
+ <!--
+ <controller:P3D name="p3d_${side}_shoulder_pan_controller" plugin="libP3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>${side}_shoulder_pan_link</bodyName>
+ <topicName>${side}_shoulder_pan_pose_ground_truth</topicName>
+ <gaussianNoise>0.0</gaussianNoise>
+ <frameName>map</frameName>
+ <interface:position name="p3d_${side}_shoulder_pan_position_iface" />
+ </controller:P3D>
+ <controller:P3D name="p3d_${side}_shoulder_lift_controller" plugin="libP3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>${side}_shoulder_lift_link</bodyName>
+ <topicName>${side}_shoulder_lift_pose_ground_truth</topicName>
+ <gaussianNoise>0.0</gaussianNoise>
+ <frameName>map</frameName>
+ <interface:position name="p3d_${side}_shoulder_lift_position_iface" />
+ </controller:P3D>
+ <controller:P3D name="p3d_${side}_upper_arm_roll_controller" plugin="libP3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>${side}_upper_arm_roll_link</bodyName>
+ <topicName>${side}_upper_arm_roll_pose_ground_truth</topicName>
+ <gaussianNoise>0.0</gaussianNoise>
+ <frameName>map</frameName>
+ <interface:position name="p3d_${side}_upper_arm_roll_position_iface" />
+ </controller:P3D>
+ <controller:P3D name="p3d_${side}_elbow_flex_controller" plugin="libP3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>${side}_elbow_flex_link</bodyName>
+ <topicName>${side}_elbow_flex_pose_ground_truth</topicName>
+ <gaussianNoise>0.0</gaussianNoise>
+ <frameName>map</frameName>
+ <interface:position name="p3d_${side}_elbow_flex_position_iface" />
+ </controller:P3D>
+ -->
+ </verbatim>
+ </map>
+
+
+ </macro>
+
+
+
+ <macro name="pr2_forearm" params="side parent reflect">
+
+ <!-- Forearm roll -->
+
+ <joint name="${side}_forearm_roll_joint" type="revolute">
+ <axis xyz="-1 0 0" />
+ <anchor xyz="0 0 0" />
+
+ <limit effort="30" velocity="${VELOCITY_LIMIT_SCALE*6}" k_velocity="1" />
+ <calibration reference_position="0" values="1.5 -1" />
+
+ <joint_properties damping="1.0" />
+ </joint>
+
+ <link name="${side}_forearm_roll_link">
+ <parent name="${parent}_link" />
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <joint name="${side}_forearm_roll_joint" />
+ <inertial>
+ <mass value="1.804155" />
+ <com xyz="0.179474 -0.000058 0.013779" /> <!-- z to x, x to y, y to z -->
+ <inertia ixx="0.00175508987" ixy="0.00002937939" ixz="-0.00042767904"
+ iyy="0.01243055254" iyz="-0.00000367110"
+ izz="0.01356754885" /> <!-- z to x, x to y, y to z -->
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <map name="gazebo_material" flag="gazebo">
+ <elem key="material">Gazebo/Blue</elem>
+ </map>
+ <geometry name="${side}_forearm_roll_visual">
+ <mesh filename="forearm_roll" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0.22 0 0" rpy="0 0 0" />
+ <geometry name="${side}_forearm_roll_collision">
+ <box size="0.27 0.12 0.08" />
+ </geometry>
+ </collision>
+ <map name="${side}_forearm_roll_sensor" flag="gazebo">
+ <verbatim key="${side}_forearm_roll_bumper_sensor">
+ <sensor:contact name="${side}_forearm_roll_contact_sensor">
+ <geom>${side}_forearm_roll_collision</geom>
+ <updateRate>1000.0</updateRate>
+ <controller:ros_bumper name="${side}_forearm_roll_ros_bumper_controller" plugin="libRos_Bumper.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bumperTopicName>${side}_forearm_roll_bumper</bumperTopicName>
+ <interface:bumper name="${side}_forearm_roll_ros_bumper_iface" />
+ </controller:ros_bumper>
+ </sensor:contact>
+ </verbatim>
+ </map>
+ <map name="${side}_forearm_roll_gravity" flag="gazebo">
+ <elem key="turnGravityOff">false</elem>
+ </map>
+ </link>
+
+ <transmission type="SimpleTransmission" name="${side}_forearm_roll_trans">
+ <actuator name="${side}_forearm_roll_motor" />
+ <joint name="${side}_forearm_roll_joint" />
+ <mechanicalReduction>${576/25*55/14}</mechanicalReduction>
+ </transmission>
+
+ <!-- Wrist flex -->
+
+ <joint name="${side}_wrist_flex_joint" type="revolute">
+ <axis xyz="0 -1 0" />
+ <anchor xyz="0 0 0" />
+
+ <limit min="-0.1" max="2.2" effort="200" velocity="${VELOCITY_LIMIT_SCALE*5.13}"
+ k_position="20" k_velocity="4"
+ safety_length_min="0.2" safety_length_max="0.2" />
+ <calibration reference_position="0.4" values="1.5 -1" />
+
+ <joint_properties damping="1.0" />
+ </joint>
+
+ <link name="${side}_wrist_flex_link">
+ <parent name="${side}_forearm_roll_link" />
+ <origin xyz="0.32025 0 0" rpy="0 0 0" />
+ <joint name="${side}_wrist_flex_joint" />
+ <inertial>
+ <mass value="0.80305" />
+ <com xyz="0.012193 0.000017 -0.004929" /> <!-- my best guess z to z, y to x, x to y -->
+ <inertia ixx="0.00077829551" ixy="0.00018491414" ixz="-0.00000036897"
+ iyy="0.00108384500" iyz="0.00000017027"
+ izz="0.00071460255" /> <!-- my best guess z to z, y to x, x to y -->
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <map name="gazebo_material" flag="gazebo">
+ <elem key="material">Gazebo/Grey</elem>
+ </map>
+ <geometry name="${side}_wrist_flex_visual">
+ <mesh filename="wrist_flex" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="${M_PI/2} 0 0" />
+ <geometry name="${side}_wrist_flex_collision">
+ <cylinder radius="0.033" length="0.103" />
+ </geometry>
+ </collision>
+ <map name="${side}_wrist_flex_sensor" flag="gazebo">
+ <verbatim key="${side}_wrist_flex_bumper_sensor">
+ <sensor:contact name="${side}_wrist_flex_contact_sensor">
+ <geom>${side}_wrist_flex_collision</geom>
+ <updateRate>1000.0</updateRate>
+ <controller:ros_bumper name="${side}_wrist_flex_ros_bumper_controller" plugin="libRos_Bumper.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bumperTopicName>${side}_wrist_flex_bumper</bumperTopicName>
+ <interface:bumper name="${side}_wrist_flex_ros_bumper_iface" />
+ </controller:ros_bumper>
+ </sensor:contact>
+ </verbatim>
+ </map>
+ <map name="${side}_wrist_flex_gravity" flag="gazebo">
+ <elem key="turnGravityOff">false</elem>
+ </map>
+ </link>
+
+ <!-- Wrist roll -->
+
+ <joint name="${side}_wrist_roll_joint" type="revolute">
+ <axis xyz="1 0 0" />
+ <anchor xyz="0 0 0" />
+ <limit effort="10" velocity="${VELOCITY_LIMIT_SCALE*6}" k_velocity="2" />
+ <calibration reference_position="1.27" values="1.5 -1" />
+ <joint_properties damping="0.1" />
+ </joint>
+
+
+ <link name="${side}_wrist_roll_link">
+ <parent name="${side}_wrist_flex_link" />
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <joint name="${side}_wrist_roll_joint" />
+ <inertial>
+ <mass value="0.1" />
+ <com xyz="0 0 0" />
+ <inertia ixx="0.01" ixy="0" ixz="0"
+ iyy="0.01" iyz="0"
+ izz="0.01" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <map name="gazebo_material" flag="gazebo">
+ <elem key="material">Gazebo/Red</elem>
+ </map>
+ <geometry name="${side}_wrist_roll_visual">
+ <mesh filename="wr_roll" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0.0 0 0" rpy="0 0 0" />
+ <geometry name="${side}_wrist_roll_collision">
+ <box size="0.01 0.01 0.01" />
+ </geometry>
+ <verbose value="Yes" />
+ </collision>
+ <map name="${side}_wrist_roll_sensor" flag="gazebo">
+ <verbatim key="${side}_wrist_roll_bumper_sensor">
+ <sensor:contact name="${side}_wrist_roll_contact_sensor">
+ <geom>${side}_wrist_roll_collision</geom>
+ <updateRate>1000.0</updateRate>
+ <controller:ros_bumper name="${side}_wrist_roll_ros_bumper_controller" plugin="libRos_Bumper.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bumperTopicName>${side}_wrist_roll_bumper</bumperTopicName>
+ <interface:bumper name="${side}_wrist_roll_ros_bumper_iface" />
+ </controller:ros_bumper>
+ </sensor:contact>
+ </verbatim>
+ </map>
+ <map name="${side}_wrist_roll_gravity" flag="gazebo">
+ <elem key="turnGravityOff">false</elem>
+ </map>
+ </link>
+
+ <transmission type="WristTransmission" name="${side}_wrist_trans">
+ <rightActuator name="${side}_wrist_r_motor" />
+ <leftActuator name="${side}_wrist_l_motor" />
+ <flexJoint name="${side}_wrist_flex_joint" mechanicalReduction="${624/35*54/16}" />
+ <rollJoint name="${side}_wrist_roll_joint" mechanicalReduction="${624/35*54/16}" />
+ </transmission>
+
+ <map name="sensor" flag="gazebo">
+ <verbatim key="p3d_${side}_forearm">
+ <!--
+ <controller:P3D name="p3d_${side}_forearm_roll_controller" plugin="libP3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>${side}_forearm_roll_link</bodyName>
+ <topicName>${side}_forearm_roll_pose_ground_truth</topicName>
+ <gaussianNoise>0.0</gaussianNoise>
+ <frameName>map</frameName>
+ <interface:position name="p3d_${side}_forearm_roll_position_iface" />
+ </controller:P3D>
+ <controller:P3D name="p3d_${side}_wrist_flex_controller" plugin="libP3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>${side}_wrist_flex_link</bodyName>
+ <topicName>${side}_wrist_flex_pose_ground_truth</topicName>
+ <gaussianNoise>0.0</gaussianNoise>
+ <frameName>map</frameName>
+ <interface:position name="p3d_${side}_wrist_flex_position_iface" />
+ </controller:P3D>
+ <controller:P3D name="p3d_${side}_wrist_roll_controller" plugin="libP3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>${side}_wrist_roll_link</bodyName>
+ <topicName>${side}_wrist_roll_pose_ground_truth</topicName>
+ <gaussianNoise>0.0</gaussianNoise>
+ <frameName>map</frameName>
+ <interface:position name="p3d_${side}_wrist_roll_position_iface" />
+ </controller:P3D>
+ -->
+ </verbatim>
+ </map>
+
+ </macro>
+
+
+
+ <macro name="pr2_arm" params="side parent reflect *origin">
+ <pr2_upper_arm side="${side}" reflect="${reflect}" parent="${parent}">
+ <insert_block name="origin" />
+ </pr2_upper_arm>
+ <pr2_forearm side="${side}" reflect="${reflect}" parent="${side}_elb...
[truncated message content] |
|
From: <rdi...@us...> - 2008-12-17 08:32:35
|
Revision: 8251
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8251&view=rev
Author: rdiankov
Date: 2008-12-17 08:32:24 +0000 (Wed, 17 Dec 2008)
Log Message:
-----------
several bug fixes/feature additions for openraveros server, added pr2_gripper robot-model generation, beginning of manipulation scripts in ormanipulation package
Modified Paths:
--------------
pkg/trunk/3rdparty/openrave/Makefile
pkg/trunk/openrave_planning/openraveros/msg/RobotInfo.msg
pkg/trunk/openrave_planning/openraveros/octave/openraveros_getrobotinfo.m
pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m
pkg/trunk/openrave_planning/openraveros/octave/orEnvLoadScene.m
pkg/trunk/openrave_planning/openraveros/src/openraveros.cpp
pkg/trunk/openrave_planning/openraveros/src/rosserver.h
pkg/trunk/openrave_planning/openraveros/src/session.h
pkg/trunk/openrave_planning/orcollision/orcollision.cpp
pkg/trunk/robot_descriptions/openrave_robot_description/CMakeLists.txt
pkg/trunk/robot_descriptions/wg_robot_description/CMakeLists.txt
Added Paths:
-----------
pkg/trunk/openrave_planning/ormanipulation/
pkg/trunk/openrave_planning/ormanipulation/data/
pkg/trunk/openrave_planning/ormanipulation/data/pr2table.env.xml
pkg/trunk/openrave_planning/ormanipulation/data/pr2table_real.env.xml
pkg/trunk/openrave_planning/ormanipulation/data/ricebox.kinbody.xml
pkg/trunk/openrave_planning/ormanipulation/data/riceboxf.kinbody.xml
pkg/trunk/openrave_planning/ormanipulation/data/willow_table.kinbody.xml
pkg/trunk/openrave_planning/ormanipulation/manifest.xml
pkg/trunk/openrave_planning/ormanipulation/octave/
pkg/trunk/openrave_planning/ormanipulation/startopenrave.ros.xml
Modified: pkg/trunk/3rdparty/openrave/Makefile
===================================================================
--- pkg/trunk/3rdparty/openrave/Makefile 2008-12-17 08:26:44 UTC (rev 8250)
+++ pkg/trunk/3rdparty/openrave/Makefile 2008-12-17 08:32:24 UTC (rev 8251)
@@ -2,7 +2,7 @@
SVN_DIR = openrave_svn
# Should really specify a revision
-SVN_REVISION = -r 560
+SVN_REVISION = -r 562
SVN_URL = https://openrave.svn.sourceforge.net/svnroot/openrave
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/openrave_planning/openraveros/msg/RobotInfo.msg
===================================================================
--- pkg/trunk/openrave_planning/openraveros/msg/RobotInfo.msg 2008-12-17 08:26:44 UTC (rev 8250)
+++ pkg/trunk/openrave_planning/openraveros/msg/RobotInfo.msg 2008-12-17 08:32:24 UTC (rev 8251)
@@ -7,6 +7,9 @@
# total active dof
uint8 activedof
+# current active manipulator
+uint8 activemanip
+
# information about the active dofs
ActiveDOFs active
Modified: pkg/trunk/openrave_planning/openraveros/octave/openraveros_getrobotinfo.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_getrobotinfo.m 2008-12-17 08:26:44 UTC (rev 8250)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_getrobotinfo.m 2008-12-17 08:32:24 UTC (rev 8251)
@@ -26,6 +26,7 @@
'type',si.type);
end
+robot.activemanip = robotinfo.activemanip+1;
robot.activedof = robotinfo.activedof;
robot.affinedof = robotinfo.active.affine;
robot.activejoints = robotinfo.active.joints;
Modified: pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m 2008-12-17 08:26:44 UTC (rev 8250)
+++ pkg/trunk/openrave_planning/openraveros/octave/openraveros_startup.m 2008-12-17 08:32:24 UTC (rev 8251)
@@ -35,7 +35,6 @@
end
if( isempty(openraveros_initialized))
- openraveros_initialized = 1;
[status,rosoctpath] = system('rospack find rosoct');
rosoctpath = strtrim(rosoctpath);
addpath(fullfile(rosoctpath,'octave'));
@@ -44,6 +43,7 @@
rosoct_add_srvs('openraveros');
rosoct('shutdown'); % restart the client
+ openraveros_initialized = 1;
end
if( ~exist('sessionserver','var') )
Modified: pkg/trunk/openrave_planning/openraveros/octave/orEnvLoadScene.m
===================================================================
--- pkg/trunk/openrave_planning/openraveros/octave/orEnvLoadScene.m 2008-12-17 08:26:44 UTC (rev 8250)
+++ pkg/trunk/openrave_planning/openraveros/octave/orEnvLoadScene.m 2008-12-17 08:32:24 UTC (rev 8251)
@@ -8,7 +8,7 @@
% scene alone and loads in addition to it.
function success = orEnvLoadScene(filename, ClearScene)
-session = openraveros_getglobalsession()
+session = openraveros_getglobalsession();
req = openraveros_env_loadscene();
req.filename = filename;
if( exist('ClearScene','var') )
Modified: pkg/trunk/openrave_planning/openraveros/src/openraveros.cpp
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/openraveros.cpp 2008-12-17 08:26:44 UTC (rev 8250)
+++ pkg/trunk/openrave_planning/openraveros/src/openraveros.cpp 2008-12-17 08:32:24 UTC (rev 8251)
@@ -32,8 +32,76 @@
boost::shared_ptr<ros::node> s_pmasternode;
boost::shared_ptr<SessionServer> s_sessionserver;
+void printhelp()
+{
+ wprintf(L"openraveros [--list] [--debuglevel [level]]\n"
+ " Starts the OpenRAVE ROS server\n"
+ "--list List all the loadable interfaces (ie, collision checkers).\n"
+ "-d, --debuglevel [level] Set a debug level, higher numbers are more verbose (default is 3)\n");
+}
+
+void printinterfaces(EnvironmentBase* penv)
+{
+ PLUGININFO info;
+ penv->GetLoadedInterfaces(info);
+
+ vector<wstring>::const_iterator itnames;
+ vector<string> names;
+ vector<string>::iterator itname;
+ wstringstream ss;
+
+ ss << endl << L"Loadable interfaces: " << endl;
+
+ ss << L"Collision Checkers (" << info.collisioncheckers.size() << "):" << endl;
+ for(itnames = info.collisioncheckers.begin(); itnames != info.collisioncheckers.end(); ++itnames)
+ ss << " " << itnames->c_str() << endl;
+
+ ss << L"Controllers (" << info.controllers.size() << "):" << endl;
+ for(itnames = info.controllers.begin(); itnames != info.controllers.end(); ++itnames)
+ ss << " " << itnames->c_str() << endl;
+
+ ss << L"Inverse Kinematics Solvers (" << info.iksolvers.size() << "):" << endl;
+ for(itnames = info.iksolvers.begin(); itnames != info.iksolvers.end(); ++itnames)
+ ss << " " << itnames->c_str() << endl;
+
+ ss << L"Physics Engines (" << info.physicsengines.size() << "):" << endl;
+ for(itnames = info.physicsengines.begin(); itnames != info.physicsengines.end(); ++itnames)
+ ss << " " << itnames->c_str() << endl;
+
+ ss << L"Planners (" << info.planners.size() << "):" << endl;
+ for(itnames = info.planners.begin(); itnames != info.planners.end(); ++itnames)
+ ss << " " << itnames->c_str() << endl;
+
+ ss << L"Problems (" << info.problems.size() << "):" << endl;
+ for(itnames = info.problems.begin(); itnames != info.problems.end(); ++itnames)
+ ss << " " << itnames->c_str() << endl;
+
+ ss << L"Robots (" << info.robots.size() << "):" << endl;
+ for(itnames = info.robots.begin(); itnames != info.robots.end(); ++itnames)
+ ss << " " << itnames->c_str() << endl;
+
+ ss << L"Sensors (" << info.sensors.size() << "):" << endl;
+ for(itnames = info.sensors.begin(); itnames != info.sensors.end(); ++itnames)
+ ss << " " << itnames->c_str() << endl;
+
+ ss << L"Sensor Systems (" << info.sensorsystems.size() << "):" << endl;
+ for(itnames = info.sensorsystems.begin(); itnames != info.sensorsystems.end(); ++itnames)
+ ss << " " << itnames->c_str() << endl;
+
+ ss << L"Trajectories (" << info.trajectories.size() << "):" << endl;
+ for(itnames = info.trajectories.begin(); itnames != info.trajectories.end(); ++itnames)
+ ss << " " << itnames->c_str() << endl;
+
+ wprintf(ss.str().c_str());
+}
+
int main(int argc, char ** argv)
{
+ // Set up the output streams to support wide characters
+ if (fwide(stdout,1) < 0) {
+ printf("Unable to set stdout to wide characters\n");
+ }
+
signal(SIGINT,sigint_handler); // control C
ros::init(argc,argv);
@@ -46,7 +114,30 @@
if( !s_sessionserver->Init() )
return -1;
- s_sessionserver->spin();
+ // parse the command line options
+ bool bExit = false;
+ int i = 1;
+ while(i < argc) {
+ if( strcmp(argv[i], "-h") == 0 || strcmp(argv[i], "-?") == 0 || strcmp(argv[i], "/?") == 0 || strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "-help") == 0 ) {
+ printhelp();
+ bExit = true;
+ break;
+ }
+ else if( strcmp(argv[i], "--debuglevel") == 0 || strcmp(argv[i], "-d") == 0 ) {
+ RaveSetDebugLevel((DebugLevel)atoi(argv[i+1]));
+ i += 2;
+ }
+ else if( strcmp(argv[i], "--list") == 0 ) {
+ printinterfaces(s_sessionserver->GetParentEnvironment().get());
+ bExit = true;
+ break;
+ }
+ else
+ break;
+ }
+
+ if( !bExit )
+ s_sessionserver->spin();
s_sessionserver.reset();
ros::fini();
s_pmasternode.reset();
Modified: pkg/trunk/openrave_planning/openraveros/src/rosserver.h
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/rosserver.h 2008-12-17 08:26:44 UTC (rev 8250)
+++ pkg/trunk/openrave_planning/openraveros/src/rosserver.h 2008-12-17 08:32:24 UTC (rev 8251)
@@ -158,6 +158,7 @@
}
virtual ~ROSServer() {
Destroy();
+ GetEnv()->AttachServer(NULL);
}
virtual void Destroy()
@@ -733,6 +734,7 @@
FillKinBodyInfo(probot,info.bodyinfo,options);
info.activedof = probot->GetActiveDOF();
+ info.activemanip = probot->GetActiveManipulatorIndex();
if( options & RobotInfo::Req_Manipulators ) {
info.manips.resize(probot->GetManipulators().size()); int index = 0;
Modified: pkg/trunk/openrave_planning/openraveros/src/session.h
===================================================================
--- pkg/trunk/openrave_planning/openraveros/src/session.h 2008-12-17 08:26:44 UTC (rev 8250)
+++ pkg/trunk/openrave_planning/openraveros/src/session.h 2008-12-17 08:32:24 UTC (rev 8251)
@@ -36,7 +36,7 @@
{ \
SessionState state = getstate(req); /* need separate copy in order to guarantee thread safety */ \
if( !state._pserver ) { \
- ROS_INFO("failed to find session for service %s", #srvname); \
+ RAVELOG_INFOA("failed to find session for service %s", #srvname); \
return false; \
} \
return state._pserver->srvname##_srv(req,res); \
@@ -250,6 +250,8 @@
return !!_pviewer;
}
+ boost::shared_ptr<EnvironmentBase> GetParentEnvironment() const { return _pParentEnvironment; }
+
private:
map<int,SessionState> _mapsessions;
boost::mutex _mutexsession;
@@ -328,7 +330,7 @@
boost::mutex::scoped_lock lock(_mutexsession);
if( _mapsessions.find(req.sessionid) != _mapsessions.end() ) {
_mapsessions.erase(req.sessionid);
- ROS_INFO("destroyed openrave session: %d", req.sessionid);
+ RAVELOG_INFOA("destroyed openrave session: %d", req.sessionid);
return true;
}
@@ -350,14 +352,14 @@
}
if( !clonestate._penv )
- ROS_INFO("failed to find session %d", req.clone_sessionid);
+ RAVELOG_INFOA("failed to find session %d", req.clone_sessionid);
else
state._penv.reset(clonestate._penv->CloneSelf(req.clone_options));
}
if( !state._penv ) {
// cloning from parent
- ROS_DEBUG("cloning from parent");
+ RAVELOG_DEBUGA("cloning from parent");
state._penv.reset(_pParentEnvironment->CloneSelf(0));
}
@@ -366,7 +368,7 @@
_mapsessions[id] = state;
res.sessionid = id;
- ROS_INFO("started openrave session: %d, total: %d", id, _mapsessions.size());
+ RAVELOG_INFOA("started openrave session: %d, total: %d", id, _mapsessions.size());
return true;
}
Modified: pkg/trunk/openrave_planning/orcollision/orcollision.cpp
===================================================================
--- pkg/trunk/openrave_planning/orcollision/orcollision.cpp 2008-12-17 08:26:44 UTC (rev 8250)
+++ pkg/trunk/openrave_planning/orcollision/orcollision.cpp 2008-12-17 08:32:24 UTC (rev 8251)
@@ -137,7 +137,7 @@
printf("or_collision [--list] [--checker checker_name] [--joints #values [values]] robot_model\n"
" Load a robot into the openrave environment, set it at [joint values] and\n"
" check for self collisions. Returns number of contact points.\n"
- "--list Will list all the loadable interfaces (ie, collision checkers).\n"
+ "--list List all the loadable interfaces (ie, collision checkers).\n"
"--checker name Load a different collision checker instead of the default one\n"
"--joints #values [values] Set the robot to specific joint values\n");
}
Added: pkg/trunk/openrave_planning/ormanipulation/data/pr2table.env.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/data/pr2table.env.xml (rev 0)
+++ pkg/trunk/openrave_planning/ormanipulation/data/pr2table.env.xml 2008-12-17 08:32:24 UTC (rev 8251)
@@ -0,0 +1,44 @@
+<Environment>
+ <bkgndcol>0.3 0.7 0.8</bkgndcol>
+ <camtrans>0.792115 1.544696 2.088982</camtrans>
+ <camrotaxis>-0.092050 -0.490650 -0.866481 211</camrotaxis>
+
+ <Robot file="robots/pr2full.robot.xml">
+ <translation>-0.708 0.005 0.012</translation>
+ <jointvalues> 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1.582704 1.081165 0.000000 2.299995 -0.000000 0.000000 0.000000 -0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 -0.979114 -0.400000 0.000000 1.535151 0.000000 0.000000 0.000000 0.000000 0.000000</jointvalues>
+ </Robot>
+
+ <Kinbody name="floor">
+ <Body>
+ <Geom type="box">
+ <extents>4 4 0.01</extents>
+ <translation>0 0 -0.01</translation>
+ <diffusecolor>0.8 0.6 0.1</diffusecolor>
+ </Geom>
+ </Body>
+ </Kinbody>
+
+ <Kinbody file="willow_table.kinbody.xml"/>
+
+ <Kinbody name="floor">
+ <Body>
+ <Geom type="box">
+ <extents>2 2 0.01</extents>
+ <translation>0 0 -0.01</translation>
+ <diffusecolor>0.8 0.6 0.1</diffusecolor>
+ </Geom>
+ </Body>
+ </Kinbody>
+
+ <Kinbody name="ricebox0" file="ricebox.kinbody.xml">
+ <translation>-0.1728 -0.41104 0.747</translation>
+ </Kinbody>
+ <Kinbody name="ricebox1" file="ricebox.kinbody.xml">
+ <rotationmat>0.7917 -0.61092 0 0.61092 0.7917 0 0 0 1</rotationmat>
+ <translation>-0.04727 -0.36746 0.747</translation>
+ </Kinbody>
+ <Kinbody name="ricebox2" file="ricebox.kinbody.xml">
+ <rotationmat>1 0 0 0 0 -1 0 1 0</rotationmat>
+ <translation>-0.177 -0.10672 0.807</translation>
+ </Kinbody>
+</Environment>
Added: pkg/trunk/openrave_planning/ormanipulation/data/pr2table_real.env.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/data/pr2table_real.env.xml (rev 0)
+++ pkg/trunk/openrave_planning/ormanipulation/data/pr2table_real.env.xml 2008-12-17 08:32:24 UTC (rev 8251)
@@ -0,0 +1,19 @@
+<Environment>
+ <bkgndcol>0.3 0.7 0.8</bkgndcol>
+ <camtrans>0.792115 1.544696 2.088982</camtrans>
+ <camrotaxis>-0.092050 -0.490650 -0.866481 211</camrotaxis>
+
+ <Robot file="robots/pr2full.robot.xml">
+ <translation>-0.708 0.005 0.0</translation>
+ </Robot>
+
+ <Kinbody name="floor">
+ <Body>
+ <Geom type="box">
+ <extents>4 4 0.01</extents>
+ <translation>0 0 -0.01</translation>
+ <diffusecolor>0.8 0.6 0.1</diffusecolor>
+ </Geom>
+ </Body>
+ </Kinbody>
+</Environment>
Added: pkg/trunk/openrave_planning/ormanipulation/data/ricebox.kinbody.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/data/ricebox.kinbody.xml (rev 0)
+++ pkg/trunk/openrave_planning/ormanipulation/data/ricebox.kinbody.xml 2008-12-17 08:32:24 UTC (rev 8251)
@@ -0,0 +1,9 @@
+<Kinbody name="ricebox">
+ <Body>
+ <Geom type="box">
+ <extents>0.03 0.0525 0.1125</extents>
+ <translation>0 0 0.1125</translation>
+ <diffusecolor>0.1 0.6 1</diffusecolor>
+ </Geom>
+ </Body>
+</Kinbody>
Added: pkg/trunk/openrave_planning/ormanipulation/data/riceboxf.kinbody.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/data/riceboxf.kinbody.xml (rev 0)
+++ pkg/trunk/openrave_planning/ormanipulation/data/riceboxf.kinbody.xml 2008-12-17 08:32:24 UTC (rev 8251)
@@ -0,0 +1,9 @@
+<Kinbody name="ricebox">
+ <Body>
+ <Geom type="box">
+ <extents>0.04 0.06 0.12</extents>
+ <translation>0 0 0.1125</translation>
+ <diffusecolor>0.1 0.6 1</diffusecolor>
+ </Geom>
+ </Body>
+</Kinbody>
Added: pkg/trunk/openrave_planning/ormanipulation/data/willow_table.kinbody.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/data/willow_table.kinbody.xml (rev 0)
+++ pkg/trunk/openrave_planning/ormanipulation/data/willow_table.kinbody.xml 2008-12-17 08:32:24 UTC (rev 8251)
@@ -0,0 +1,16 @@
+<KinBody name="willow_table">
+ <Body>
+ <Geom type="box">
+ <extents>0.38 0.62 0.035</extents>
+ <translation>0 0 0.711</translation>
+ </Geom>
+ <Geom type="box">
+ <extents>0.14 0.015 0.35</extents>
+ <translation>0 -0.555 0.35</translation>
+ </Geom>
+ <Geom type="box">
+ <extents>0.14 0.015 0.35</extents>
+ <translation>0 0.555 0.35</translation>
+ </Geom>
+ </Body>
+</KinBody>
Added: pkg/trunk/openrave_planning/ormanipulation/manifest.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/manifest.xml (rev 0)
+++ pkg/trunk/openrave_planning/ormanipulation/manifest.xml 2008-12-17 08:32:24 UTC (rev 8251)
@@ -0,0 +1,10 @@
+<package>
+ <description brief="OpenRAVE Manipulation">
+ Scripts to perform manipulation tasks and other contains data resources.
+ </description>
+ <author>Rosen Diankov (rdi...@cs...)</author>
+ <license>BSD</license>
+ <depend package="boost"/>
+ <depend package="openraveros"/>
+ <depend package="orplugins"/>
+</package>
Added: pkg/trunk/openrave_planning/ormanipulation/startopenrave.ros.xml
===================================================================
--- pkg/trunk/openrave_planning/ormanipulation/startopenrave.ros.xml (rev 0)
+++ pkg/trunk/openrave_planning/ormanipulation/startopenrave.ros.xml 2008-12-17 08:32:24 UTC (rev 8251)
@@ -0,0 +1,7 @@
+<launch>
+ <!-- start openraveros server and set all the correct paths -->
+ <node pkg="openraveros" type="openraveros">
+ <env name="OPENRAVE_DATA" value="$(env OPENRAVE_DATA):$(find openrave)/share/openrave:$(find openrave_robot_description):$(find ormanipulation)"/>
+ <env name="OPENRAVE_PLUGINS" value="$(env OPENRAVE_PLUGINS):$(find openrave)/share/openrave/plugins:$(find orplugins)"/>
+ </node>
+</launch>
Modified: pkg/trunk/robot_descriptions/openrave_robot_description/CMakeLists.txt
===================================================================
--- pkg/trunk/robot_descriptions/openrave_robot_description/CMakeLists.txt 2008-12-17 08:26:44 UTC (rev 8250)
+++ pkg/trunk/robot_descriptions/openrave_robot_description/CMakeLists.txt 2008-12-17 08:32:24 UTC (rev 8251)
@@ -14,6 +14,8 @@
# process all urdf files
set(PR2_OUTPUT ${openrave_robot_description_PACKAGE_PATH}/robots/pr2.robot.xml)
set(PR2_INPUT ${wg_robot_description_PACKAGE_PATH}/pr2/pr2.xml)
+set(PR2_GRIPPER_OUTPUT ${openrave_robot_description_PACKAGE_PATH}/robots/pr2_gripper.robot.xml)
+set(PR2_GRIPPER_INPUT ${wg_robot_description_PACKAGE_PATH}/pr2_gripper/pr2_gripper.xml)
set(MODELS ${openrave_robot_description_PACKAGE_PATH}/models)
## create the models directory
@@ -27,7 +29,12 @@
COMMAND ${urdf2openrave_exe}
ARGS ${PR2_INPUT} ${PR2_OUTPUT} ../models
DEPENDS ${PR2_INPUT} ${MODELS} urdf2openrave)
-set(robot_files ${robot_files} ${PR2_OUTPUT})
+add_custom_command(
+ OUTPUT ${PR2_GRIPPER_OUTPUT}
+ COMMAND ${urdf2openrave_exe}
+ ARGS ${PR2_GRIPPER_INPUT} ${PR2_GRIPPER_OUTPUT} ../models
+ DEPENDS ${PR2_GRIPPER_INPUT} ${MODELS} urdf2openrave)
+set(robot_files ${robot_files} ${PR2_OUTPUT} ${PR2_GRIPPER_OUTPUT})
add_custom_target(urdf_robots ALL DEPENDS ${robot_files})
add_dependencies(urdf_robots urdf2openrave)
Modified: pkg/trunk/robot_descriptions/wg_robot_description/CMakeLists.txt
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/CMakeLists.txt 2008-12-17 08:26:44 UTC (rev 8250)
+++ pkg/trunk/robot_descriptions/wg_robot_description/CMakeLists.txt 2008-12-17 08:32:24 UTC (rev 8251)
@@ -38,6 +38,13 @@
ARGS ${pr2arm}.xacro.xml > ${pr2arm}.xml
DEPENDS ${pr2arm}.xacro.xml)
+set(pr2gripper "${CMAKE_CURRENT_SOURCE_DIR}/pr2_gripper/pr2_gripper")
+add_custom_command(
+ OUTPUT ${pr2gripper}.xml
+ COMMAND ${xacro_PACKAGE_PATH}/xacro.py
+ ARGS ${pr2gripper}.xacro.xml > ${pr2gripper}.xml
+ DEPENDS ${pr2gripper}.xacro.xml)
+
#iv files
# iterate through all the stl files and generate *.mesh and *.iv files
file(GLOB pr2_stl_files ${CMAKE_CURRENT_SOURCE_DIR}/models/pr2/*.stl)
@@ -58,4 +65,4 @@
endforeach(it)
-add_custom_target(media_files ALL DEPENDS ${pr2robot}.xml ${pr2prototype1}.xml ${pr2arm}.xml ${pr2_gen_files})
+add_custom_target(media_files ALL DEPENDS ${pr2robot}.xml ${pr2prototype1}.xml ${pr2arm}.xml ${pr2gripper}.xml ${pr2_gen_files})
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-12-17 16:14:01
|
Revision: 8252
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8252&view=rev
Author: gerkey
Date: 2008-12-17 16:13:57 +0000 (Wed, 17 Dec 2008)
Log Message:
-----------
Changed sysdeps from lapack3-dev to liblapack-dev for Hardy
Modified Paths:
--------------
pkg/trunk/3rdparty/scipy/manifest.xml
pkg/trunk/grasp_module/grasp_learner/manifest.xml
pkg/trunk/mapping/cloud_geometry/manifest.xml
pkg/trunk/mapping/sample_consensus/manifest.xml
pkg/trunk/vision/calonder_descriptor/manifest.xml
pkg/trunk/visualization/wxpy_ros/manifest.xml
Modified: pkg/trunk/3rdparty/scipy/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/scipy/manifest.xml 2008-12-17 08:32:24 UTC (rev 8251)
+++ pkg/trunk/3rdparty/scipy/manifest.xml 2008-12-17 16:13:57 UTC (rev 8252)
@@ -21,7 +21,7 @@
<sysdepend os="ubuntu" version="8.04-hardy" package="wget"/>
<sysdepend os="ubuntu" version="8.10-intrepid" package="wget"/>
<sysdepend os="ubuntu" version="7.04-feisty" package="atlas3-sse2-dev"/>
-<sysdepend os="ubuntu" version="8.04-hardy" package="atlas3-sse2-dev"/>
+<sysdepend os="ubuntu" version="8.04-hardy" package="libatlas-sse2-dev"/>
<sysdepend os="ubuntu" version="8.10-intrepid" package="libatlas-sse2-dev"/>
<sysdepend os="ubuntu" version="8.10-intrepid" package="gfortran"/>
<sysdepend os="ubuntu" version="8.10-intrepid" package="fftw3-dev"/>
Modified: pkg/trunk/grasp_module/grasp_learner/manifest.xml
===================================================================
--- pkg/trunk/grasp_module/grasp_learner/manifest.xml 2008-12-17 08:32:24 UTC (rev 8251)
+++ pkg/trunk/grasp_module/grasp_learner/manifest.xml 2008-12-17 16:13:57 UTC (rev 8252)
@@ -13,8 +13,8 @@
<export>
<cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp" lflags=""/>
</export>
-<sysdepend os="ubuntu" version="8.04-hardy" package="refblas3-dev"/>
-<sysdepend os="ubuntu" version="8.04-hardy" package="lapack3-dev"/>
+<sysdepend os="ubuntu" version="8.04-hardy" package="libblas-dev"/>
+<sysdepend os="ubuntu" version="8.04-hardy" package="liblapack-dev"/>
<sysdepend os="ubuntu" version="8.10-intrepid" package="liblapack-dev"/>
<sysdepend os="ubuntu" version="8.10-intrepid" package="libblas-dev"/>
</package>
Modified: pkg/trunk/mapping/cloud_geometry/manifest.xml
===================================================================
--- pkg/trunk/mapping/cloud_geometry/manifest.xml 2008-12-17 08:32:24 UTC (rev 8251)
+++ pkg/trunk/mapping/cloud_geometry/manifest.xml 2008-12-17 16:13:57 UTC (rev 8252)
@@ -12,7 +12,7 @@
<depend package="std_msgs" />
<depend package="eigen" />
- <sysdepend os="ubuntu" version="8.04-hardy" package="lapack3-dev"/>
+ <sysdepend os="ubuntu" version="8.04-hardy" package="liblapack-dev"/>
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lcloud_geometry"/>
Modified: pkg/trunk/mapping/sample_consensus/manifest.xml
===================================================================
--- pkg/trunk/mapping/sample_consensus/manifest.xml 2008-12-17 08:32:24 UTC (rev 8251)
+++ pkg/trunk/mapping/sample_consensus/manifest.xml 2008-12-17 16:13:57 UTC (rev 8252)
@@ -12,7 +12,7 @@
<depend package="std_msgs" />
<depend package="cloud_geometry" />
- <sysdepend os="ubuntu" version="8.04-hardy" package="lapack3-dev"/>
+ <sysdepend os="ubuntu" version="8.04-hardy" package="liblapack-dev"/>
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lsample_consensus"/>
Modified: pkg/trunk/vision/calonder_descriptor/manifest.xml
===================================================================
--- pkg/trunk/vision/calonder_descriptor/manifest.xml 2008-12-17 08:32:24 UTC (rev 8251)
+++ pkg/trunk/vision/calonder_descriptor/manifest.xml 2008-12-17 16:13:57 UTC (rev 8252)
@@ -18,6 +18,6 @@
<sysdepend os="ubuntu" version="8.04-hardy" package="python-imaging"/>
<sysdepend os="ubuntu" version="8.10-intrepid" package="python-imaging"/>
<sysdepend os="ubuntu" version="7.04-feisty" package="atlas3-base-dev"/>
- <sysdepend os="ubuntu" version="8.04-hardy" package="atlas3-base-dev"/>
+ <sysdepend os="ubuntu" version="8.04-hardy" package="libatlas-base-dev"/>
<sysdepend os="ubuntu" version="8.10-intrepid" package="libatlas-base-dev"/>
</package>
Modified: pkg/trunk/visualization/wxpy_ros/manifest.xml
===================================================================
--- pkg/trunk/visualization/wxpy_ros/manifest.xml 2008-12-17 08:32:24 UTC (rev 8251)
+++ pkg/trunk/visualization/wxpy_ros/manifest.xml 2008-12-17 16:13:57 UTC (rev 8252)
@@ -15,7 +15,7 @@
<depend package="scipy"/>
<depend package="joy" />
- <sysdepend os="ubuntu" version="8.04-hardy" package="lapack3-dev" />
+ <sysdepend os="ubuntu" version="8.04-hardy" package="liblapack-dev" />
<sysdepend os="ubuntu" version="8.10-intrepid" package="liblapack-dev" />
</package>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-12-17 21:08:49
|
Revision: 8277
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8277&view=rev
Author: gerkey
Date: 2008-12-17 21:08:45 +0000 (Wed, 17 Dec 2008)
Log Message:
-----------
Reverted to older non-standard atlas and lapack packages on Hardy
Modified Paths:
--------------
pkg/trunk/3rdparty/scipy/manifest.xml
pkg/trunk/grasp_module/grasp_learner/manifest.xml
pkg/trunk/mapping/cloud_geometry/manifest.xml
pkg/trunk/mapping/sample_consensus/manifest.xml
pkg/trunk/vision/calonder_descriptor/manifest.xml
pkg/trunk/visualization/wxpy_ros/manifest.xml
Modified: pkg/trunk/3rdparty/scipy/manifest.xml
===================================================================
--- pkg/trunk/3rdparty/scipy/manifest.xml 2008-12-17 20:36:23 UTC (rev 8276)
+++ pkg/trunk/3rdparty/scipy/manifest.xml 2008-12-17 21:08:45 UTC (rev 8277)
@@ -21,7 +21,7 @@
<sysdepend os="ubuntu" version="8.04-hardy" package="wget"/>
<sysdepend os="ubuntu" version="8.10-intrepid" package="wget"/>
<sysdepend os="ubuntu" version="7.04-feisty" package="atlas3-sse2-dev"/>
-<sysdepend os="ubuntu" version="8.04-hardy" package="libatlas-sse2-dev"/>
+<sysdepend os="ubuntu" version="8.04-hardy" package="atlas3-sse2-dev"/>
<sysdepend os="ubuntu" version="8.10-intrepid" package="libatlas-sse2-dev"/>
<sysdepend os="ubuntu" version="8.10-intrepid" package="gfortran"/>
<sysdepend os="ubuntu" version="8.10-intrepid" package="fftw3-dev"/>
Modified: pkg/trunk/grasp_module/grasp_learner/manifest.xml
===================================================================
--- pkg/trunk/grasp_module/grasp_learner/manifest.xml 2008-12-17 20:36:23 UTC (rev 8276)
+++ pkg/trunk/grasp_module/grasp_learner/manifest.xml 2008-12-17 21:08:45 UTC (rev 8277)
@@ -14,7 +14,7 @@
<cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp" lflags=""/>
</export>
<sysdepend os="ubuntu" version="8.04-hardy" package="libblas-dev"/>
-<sysdepend os="ubuntu" version="8.04-hardy" package="liblapack-dev"/>
+<sysdepend os="ubuntu" version="8.04-hardy" package="lapack3-dev"/>
<sysdepend os="ubuntu" version="8.10-intrepid" package="liblapack-dev"/>
<sysdepend os="ubuntu" version="8.10-intrepid" package="libblas-dev"/>
</package>
Modified: pkg/trunk/mapping/cloud_geometry/manifest.xml
===================================================================
--- pkg/trunk/mapping/cloud_geometry/manifest.xml 2008-12-17 20:36:23 UTC (rev 8276)
+++ pkg/trunk/mapping/cloud_geometry/manifest.xml 2008-12-17 21:08:45 UTC (rev 8277)
@@ -12,7 +12,7 @@
<depend package="std_msgs" />
<depend package="eigen" />
- <sysdepend os="ubuntu" version="8.04-hardy" package="liblapack-dev"/>
+ <sysdepend os="ubuntu" version="8.04-hardy" package="lapack3-dev"/>
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lcloud_geometry"/>
Modified: pkg/trunk/mapping/sample_consensus/manifest.xml
===================================================================
--- pkg/trunk/mapping/sample_consensus/manifest.xml 2008-12-17 20:36:23 UTC (rev 8276)
+++ pkg/trunk/mapping/sample_consensus/manifest.xml 2008-12-17 21:08:45 UTC (rev 8277)
@@ -12,7 +12,7 @@
<depend package="std_msgs" />
<depend package="cloud_geometry" />
- <sysdepend os="ubuntu" version="8.04-hardy" package="liblapack-dev"/>
+ <sysdepend os="ubuntu" version="8.04-hardy" package="lapack3-dev"/>
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lsample_consensus"/>
Modified: pkg/trunk/vision/calonder_descriptor/manifest.xml
===================================================================
--- pkg/trunk/vision/calonder_descriptor/manifest.xml 2008-12-17 20:36:23 UTC (rev 8276)
+++ pkg/trunk/vision/calonder_descriptor/manifest.xml 2008-12-17 21:08:45 UTC (rev 8277)
@@ -17,7 +17,7 @@
<sysdepend os="ubuntu" version="7.04-feisty" package="python-imaging"/>
<sysdepend os="ubuntu" version="8.04-hardy" package="python-imaging"/>
<sysdepend os="ubuntu" version="8.10-intrepid" package="python-imaging"/>
- <sysdepend os="ubuntu" version="7.04-feisty" package="atlas3-base-dev"/>
- <sysdepend os="ubuntu" version="8.04-hardy" package="libatlas-base-dev"/>
+ <sysdepend os="ubuntu" version="7.04-feisty" package="atlas3-sse2-dev"/>
+ <sysdepend os="ubuntu" version="8.04-hardy" package="atlas3-sse2-dev"/>
<sysdepend os="ubuntu" version="8.10-intrepid" package="libatlas-base-dev"/>
</package>
Modified: pkg/trunk/visualization/wxpy_ros/manifest.xml
===================================================================
--- pkg/trunk/visualization/wxpy_ros/manifest.xml 2008-12-17 20:36:23 UTC (rev 8276)
+++ pkg/trunk/visualization/wxpy_ros/manifest.xml 2008-12-17 21:08:45 UTC (rev 8277)
@@ -15,7 +15,7 @@
<depend package="scipy"/>
<depend package="joy" />
- <sysdepend os="ubuntu" version="8.04-hardy" package="liblapack-dev" />
+ <sysdepend os="ubuntu" version="8.04-hardy" package="lapack3-dev" />
<sysdepend os="ubuntu" version="8.10-intrepid" package="liblapack-dev" />
</package>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-12-17 21:59:04
|
Revision: 8284
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8284&view=rev
Author: tfoote
Date: 2008-12-17 21:59:03 +0000 (Wed, 17 Dec 2008)
Log Message:
-----------
fixing ransac linking
Modified Paths:
--------------
pkg/trunk/highlevel/highlevel_controllers/CMakeLists.txt
pkg/trunk/world_models/ransac_ground_plane_extraction/manifest.xml
Modified: pkg/trunk/highlevel/highlevel_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/CMakeLists.txt 2008-12-17 21:55:24 UTC (rev 8283)
+++ pkg/trunk/highlevel/highlevel_controllers/CMakeLists.txt 2008-12-17 21:59:03 UTC (rev 8284)
@@ -12,7 +12,7 @@
# Library
rospack_add_library(highlevel_controllers src/MoveBase.cpp src/VelocityControllers.cpp)
-target_link_libraries(highlevel_controllers trajectory_rollout costmap_2d sbpl_util ransac_ground_plane_extraction)
+target_link_libraries(highlevel_controllers trajectory_rollout costmap_2d sbpl_util)
# MoveArm
rospack_add_executable(move_arm src/MoveArm.cpp)
Modified: pkg/trunk/world_models/ransac_ground_plane_extraction/manifest.xml
===================================================================
--- pkg/trunk/world_models/ransac_ground_plane_extraction/manifest.xml 2008-12-17 21:55:24 UTC (rev 8283)
+++ pkg/trunk/world_models/ransac_ground_plane_extraction/manifest.xml 2008-12-17 21:59:03 UTC (rev 8284)
@@ -12,6 +12,6 @@
<depend package="scan_utils" />
<depend package="pr2_msgs" />
<export>
- <cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib"/>
+ <cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lransac_ground_plane_extraction"/>
</export>
</package>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-12-18 03:20:43
|
Revision: 8317
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8317&view=rev
Author: hsujohnhsu
Date: 2008-12-18 03:20:40 +0000 (Thu, 18 Dec 2008)
Log Message:
-----------
* pr2_prototype1 now using laser_tilt controller xml for laser_tilt controller
* renamed laser_controller to laser_tilt_controller (update corresponding launch scripts)
* added ransac stuff in 2dnav_gazebo
* using hw time in laser scanner controller (thanks Vijay)
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp
pkg/trunk/demos/2dnav_gazebo/2dnav-stack-amcl.xml
pkg/trunk/demos/2dnav_gazebo/2dnav-stack-fake_localization.xml
pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_controllers.launch
pkg/trunk/robot_descriptions/wg_robot_description/laser_tilt/laser_tilt_controller.xml
pkg/trunk/world_models/ransac_ground_plane_extraction/test_ground_plane.launch
Added Paths:
-----------
pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/controllers_head_torso_gazebo.xml
Removed Paths:
-------------
pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/controllers_head_laser_tilt_torso_gazebo.xml
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp 2008-12-18 03:18:26 UTC (rev 8316)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/laser_scanner_controller.cpp 2008-12-18 03:20:40 UTC (rev 8317)
@@ -495,6 +495,7 @@
{
if (publisher_->trylock())
{
+ publisher_->msg_.header.stamp = ros::Time((uint64_t) (c_->getTime()*1000000000)) ;
publisher_->msg_.signal = m_scanner_signal_.signal ;
publisher_->unlockAndPublish() ;
need_to_send_msg_ = false ;
@@ -547,6 +548,7 @@
bool LaserScannerControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
{
+
service_prefix_ = config->Attribute("name");
if (!c_->initXml(robot, config))
Modified: pkg/trunk/demos/2dnav_gazebo/2dnav-stack-amcl.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/2dnav-stack-amcl.xml 2008-12-18 03:18:26 UTC (rev 8316)
+++ pkg/trunk/demos/2dnav_gazebo/2dnav-stack-amcl.xml 2008-12-18 03:20:40 UTC (rev 8317)
@@ -37,7 +37,27 @@
-->
<!-- starting ransac node -->
- <node pkg="ransac_ground_plane_extraction" type="ransacnode" output="screen" />
+ <node pkg="point_cloud_assembler" type="point_cloud_assembler" output="screen" name="assembler">
+ <remap from="scan" to="tilt_scan"/>
+ <param name="ignore_laser_skew" type="bool" value="true" />
+ </node>
+ <node pkg="point_cloud_assembler" type="point_cloud_snapshotter" output="screen" name="snapshotter">
+ <remap from="laser_scanner_signal" to="laser_tilt_controller/laser_scanner_signal"/>
+ <remap from="full_cloud" to="tilt_scan_cloud" />
+ <param name="fixed_frame" type="string" value="base_link" />
+ </node>
+ <param name="ransac_ground_plane_extraction/listen_topic" value="tilt_scan_cloud"/>
+ <param name="ransac_ground_plane_extraction/publish_ground_plane_topic" value="ground_plane"/>
+ <param name="ransac_ground_plane_extraction/publish_obstacle_topic" value="obstacle_cloud"/>
+ <param name="ransac_ground_plane_extraction/min_ignore_distance" value="-0.13"/>
+ <param name="ransac_ground_plane_extraction/max_ignore_distance" value="0.07"/>
+ <param name="ransac_ground_plane_extraction/distance_threshold" value="0.03"/>
+ <param name="ransac_ground_plane_extraction/far_remove_distance_threshold" value="0.06"/>
+ <param name="ransac_ground_plane_extraction/far_remove_distance" value="6.0"/>
+ <param name="ransac_ground_plane_extraction/filter_delta" value="1.0"/>
+ <param name="ransac_ground_plane_extraction/max_ransac_iterations" value="500"/>
+ <param name="ransac_ground_plane_extraction/publish_obstacle_cloud" type="string" value="yes"/>
+ <node pkg="ransac_ground_plane_extraction" type="ransacnode" respawn="false" output="screen" />
<!-- filter out veil effect from range scans -->
<param name="/scan_shadows_filter/filter_min_angle" value="10"/>
@@ -83,7 +103,7 @@
<node pkg="highlevel_controllers" type="recharge_controller" respawn="false" />
<!-- For logging distance traveled -->
- <node pkg="rosrecord" type="rosrecord" args="-f /bags/robot_runs/milestone_trials/attempt /base_scan /tilt_scan /TransformArray /localizedpose /odom /base_controller/odometer /odom_estimation" />
+ <node pkg="rosrecord" type="rosrecord" args="-f /tmp/robot_runs/milestone_trials/attempt /base_scan /tilt_scan /TransformArray /localizedpose /odom /base_controller/odometer /odom_estimation" />
<!-- For telling the robot to go charge -->
<node pkg="highlevel_controllers" type="joy_batt_sender" respawn="true" />
Modified: pkg/trunk/demos/2dnav_gazebo/2dnav-stack-fake_localization.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/2dnav-stack-fake_localization.xml 2008-12-18 03:18:26 UTC (rev 8316)
+++ pkg/trunk/demos/2dnav_gazebo/2dnav-stack-fake_localization.xml 2008-12-18 03:20:40 UTC (rev 8317)
@@ -32,7 +32,27 @@
-->
<!-- starting ransac node -->
- <node pkg="ransac_ground_plane_extraction" type="ransacnode" output="screen" />
+ <node pkg="point_cloud_assembler" type="point_cloud_assembler" output="screen" name="assembler">
+ <remap from="scan" to="tilt_scan"/>
+ <param name="ignore_laser_skew" type="bool" value="true" />
+ </node>
+ <node pkg="point_cloud_assembler" type="point_cloud_snapshotter" output="screen" name="snapshotter">
+ <remap from="laser_scanner_signal" to="laser_tilt_controller/laser_scanner_signal"/>
+ <remap from="full_cloud" to="tilt_scan_cloud" />
+ <param name="fixed_frame" type="string" value="base_link" />
+ </node>
+ <param name="ransac_ground_plane_extraction/listen_topic" value="tilt_scan_cloud"/>
+ <param name="ransac_ground_plane_extraction/publish_ground_plane_topic" value="ground_plane"/>
+ <param name="ransac_ground_plane_extraction/publish_obstacle_topic" value="obstacle_cloud"/>
+ <param name="ransac_ground_plane_extraction/min_ignore_distance" value="-0.13"/>
+ <param name="ransac_ground_plane_extraction/max_ignore_distance" value="0.07"/>
+ <param name="ransac_ground_plane_extraction/distance_threshold" value="0.03"/>
+ <param name="ransac_ground_plane_extraction/far_remove_distance_threshold" value="0.06"/>
+ <param name="ransac_ground_plane_extraction/far_remove_distance" value="6.0"/>
+ <param name="ransac_ground_plane_extraction/filter_delta" value="1.0"/>
+ <param name="ransac_ground_plane_extraction/max_ransac_iterations" value="500"/>
+ <param name="ransac_ground_plane_extraction/publish_obstacle_cloud" type="string" value="yes"/>
+ <node pkg="ransac_ground_plane_extraction" type="ransacnode" respawn="false" output="screen" />
<!-- move_base_sbpl settings -->
<!-- for the moment use e.g.: ./shparam set move_base/plannerType string ADPlanner -->
@@ -72,7 +92,7 @@
<node pkg="highlevel_controllers" type="recharge_controller" respawn="false" />
<!-- For logging distance traveled -->
- <node pkg="rosrecord" type="rosrecord" args="-f /bags/robot_runs/milestone_trials/attempt /base_scan /tilt_scan /TransformArray /localizedpose /odom /base_controller/odometer /odom_estimation" />
+ <node pkg="rosrecord" type="rosrecord" args="-f /tmp/robot_runs/milestone_trials/attempt /base_scan /tilt_scan /TransformArray /localizedpose /odom /base_controller/odometer /odom_estimation" />
<!-- For telling the robot to go charge -->
<node pkg="highlevel_controllers" type="joy_batt_sender" respawn="true" />
Modified: pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_controllers.launch
===================================================================
--- pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_controllers.launch 2008-12-18 03:18:26 UTC (rev 8316)
+++ pkg/trunk/demos/pr2_prototype1_gazebo/pr2_prototype1_controllers.launch 2008-12-18 03:20:40 UTC (rev 8317)
@@ -2,10 +2,12 @@
<!-- use mech.py to spawn all controllers listed in controllers.xml -->
<param name="base_controller/odom_publish_rate" value="10" />
<node pkg="mechanism_control" type="spawner.py" args="$(find pr2_alpha)/controllers_base_lab.xml" output="screen"/>
- <node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/pr2_prototype1/controllers_head_laser_tilt_torso_gazebo.xml" output="screen"/>
+ <node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/pr2_prototype1/controllers_head_torso_gazebo.xml" output="screen"/>
+
<!-- start tilting Hokuyo laser by sending it a preset code of 46, this means sawtooth profile sweep.
for details of the profile, rates, see controller::LaserScannerControllerNode. -->
<!--node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_tilt_controller sine 20 0.872 0.3475" respawn="false" output="screen" /-->
- <node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_tilt_controller sine 1 .45 .40" />
+ <node pkg="mechanism_control" type="spawner.py" args="$(find wg_robot_description)/laser_tilt/laser_tilt_controller.xml" />
+ <node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_tilt_controller sine 2 .45 .40" />
</launch>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/laser_tilt/laser_tilt_controller.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/laser_tilt/laser_tilt_controller.xml 2008-12-18 03:18:26 UTC (rev 8316)
+++ pkg/trunk/robot_descriptions/wg_robot_description/laser_tilt/laser_tilt_controller.xml 2008-12-18 03:20:40 UTC (rev 8317)
@@ -1,5 +1,5 @@
<controllers>
- <controller name="laser_controller" topic="laser_controller" type="LaserScannerControllerNode">
+ <controller name="laser_tilt_controller" topic="laser_tilt_controller" type="LaserScannerControllerNode">
<filter smoothing_factor="0.1" />
<joint name="laser_tilt_mount_joint">
<pid p="12" i=".1" d="1" iClamp="0.5" />
Deleted: pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/controllers_head_laser_tilt_torso_gazebo.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/controllers_head_laser_tilt_torso_gazebo.xml 2008-12-18 03:18:26 UTC (rev 8316)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/controllers_head_laser_tilt_torso_gazebo.xml 2008-12-18 03:20:40 UTC (rev 8317)
@@ -1,34 +0,0 @@
-<?xml version="1.0"?>
-
-<controllers>
-
-
- <!-- ========================================= -->
- <!-- torso_lift array -->
- <controller name="torso_lift_controller" topic="torso_lift_controller" type="JointPositionControllerNode">
- <joint name="torso_lift_joint">
- <pid p="1000" d="0" i="1" iClamp="100" />
- </joint>
- </controller>
- <!-- ========================================= -->
- <!-- head and above array -->
- <controller name="head_pan_controller" topic="head_pan_controller" type="JointPositionControllerNode">
- <listen_topic name="head_pan_commands" />
- <joint name="head_pan_joint" >
- <pid p="100" d="0" i="0" iClamp="0" />
- </joint>
- </controller>
- <controller name="head_tilt_controller" topic="head_tilt_controller" type="JointPositionControllerNode">
- <listen_topic name="head_tilt_commands" />
- <joint name="head_tilt_joint" >
- <pid p="100" d="0" i="0" iClamp="0" />
- </joint>
- </controller>
-
- <controller name="laser_tilt_controller" topic="laser_tilt_controller" type="LaserScannerControllerNode">
- <joint name="laser_tilt_mount_joint" >
- <pid p="12" i=".1" d="1" iClamp="0.5" />
- </joint>
- </controller>
-
-</controllers>
Copied: pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/controllers_head_torso_gazebo.xml (from rev 8315, pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/controllers_head_laser_tilt_torso_gazebo.xml)
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/controllers_head_torso_gazebo.xml (rev 0)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/controllers_head_torso_gazebo.xml 2008-12-18 03:20:40 UTC (rev 8317)
@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+
+<controllers>
+
+
+ <!-- ========================================= -->
+ <!-- torso_lift array -->
+ <controller name="torso_lift_controller" topic="torso_lift_controller" type="JointPositionControllerNode">
+ <joint name="torso_lift_joint">
+ <pid p="1000" d="0" i="1" iClamp="100" />
+ </joint>
+ </controller>
+ <!-- ========================================= -->
+ <!-- head and above array -->
+ <controller name="head_pan_controller" topic="head_pan_controller" type="JointPositionControllerNode">
+ <listen_topic name="head_pan_commands" />
+ <joint name="head_pan_joint" >
+ <pid p="100" d="0" i="0" iClamp="0" />
+ </joint>
+ </controller>
+ <controller name="head_tilt_controller" topic="head_tilt_controller" type="JointPositionControllerNode">
+ <listen_topic name="head_tilt_commands" />
+ <joint name="head_tilt_joint" >
+ <pid p="100" d="0" i="0" iClamp="0" />
+ </joint>
+ </controller>
+
+</controllers>
Modified: pkg/trunk/world_models/ransac_ground_plane_extraction/test_ground_plane.launch
===================================================================
--- pkg/trunk/world_models/ransac_ground_plane_extraction/test_ground_plane.launch 2008-12-18 03:18:26 UTC (rev 8316)
+++ pkg/trunk/world_models/ransac_ground_plane_extraction/test_ground_plane.launch 2008-12-18 03:20:40 UTC (rev 8317)
@@ -2,7 +2,7 @@
<launch>
<group name="wg">
<node pkg="mechanism_control" type="spawner.py" args="$(find wg_robot_description)/laser_tilt/laser_tilt_controller.xml" />
- <node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_controller sine 2 .45 .40" />
+ <node pkg="pr2_mechanism_controllers" type="control_laser.py" args="laser_tilt_controller sine 2 .45 .40" />
<node pkg="point_cloud_assembler" type="point_cloud_assembler" output="screen" name="assembler">
<remap from="scan" to="tilt_scan"/>
@@ -10,11 +10,11 @@
</node>
<node pkg="point_cloud_assembler" type="point_cloud_snapshotter" output="screen" name="snapshotter">
- <remap from="laser_scanner_signal" to="laser_controller/laser_scanner_signal"/>
+ <remap from="laser_scanner_signal" to="laser_tilt_controller/laser_scanner_signal"/>
<param name="fixed_frame" type="string" value="base_link" />
</node>
- <param name="ransac_ground_plane_extraction/listen_topic" value="full_cloud"/>
+ <param name="ransac_ground_plane_extraction/listen_topic" value="tilt_scan_cloud"/>
<param name="ransac_ground_plane_extraction/publish_ground_plane_topic" value="ground_plane"/>
<param name="ransac_ground_plane_extraction/publish_obstacle_topic" value="obstacle_cloud"/>
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