|
From: <wa...@us...> - 2009-07-09 05:33:25
|
Revision: 18548
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18548&view=rev
Author: wattsk
Date: 2009-07-09 05:33:24 +0000 (Thu, 09 Jul 2009)
Log Message:
-----------
Defs, calibration, machine and launch files for head cart HCC
Added Paths:
-----------
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_head_cart/hcc.launch
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_head_cart/hcc.machine
pkg/trunk/stacks/pr2/pr2_default_controllers/calibration_controllers/hcc_calibration_controller.xml
pkg/trunk/stacks/pr2/pr2_defs/robots/hcc.xacro.xml
Added: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_head_cart/hcc.launch
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_head_cart/hcc.launch (rev 0)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_head_cart/hcc.launch 2009-07-09 05:33:24 UTC (rev 18548)
@@ -0,0 +1,30 @@
+<launch>
+ <!-- Machines -->
+ <include file="$(find pr2_head_cart)/hcc.machine" />
+
+ <!-- Description and etherCAT -->
+ <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/hcc.xacro.xml'" />
+
+ <node machine="realtime_root" pkg="pr2_etherCAT" type="pr2_etherCAT" args="-i eth0 -x robotdesc/pr2" />
+
+ <!-- Calibration -->
+ <node pkg="mechanism_bringup" type="calibrate.py"
+ args="$(find pr2_default_controllers)/calibration_controllers/hcc_calibration_controller.xml" output="screen" />
+
+ <!-- Robot state publisher -->
+ <node machine="two" pkg="robot_state_publisher" type="state_publisher" name="robot_state_publisher">
+ <param name="publish_frequency" type="double" value="50.0" />
+ <param name="tf_prefix" type="string" value="" />
+ </node>
+
+ <include file="$(find pr2_head_cart)/cart_sensors.launch" />
+ <include file="$(find pr2_head_cart)/cart_monitors.launch" />
+
+ <!-- NTP monitoring script Warns to console if sync error -->
+ <node pkg="runtime_monitor" type="ntp_monitor.py" args="hcc2" machine="realtime" />
+ <node pkg="runtime_monitor" type="ntp_monitor.py" args="fw1 7000" machine="two" />
+
+ <!-- Runtime Diagnostics Logging -->
+ <node pkg="rosrecord" type="rosrecord" args="-f /hwlog/hcc_runtime_automatic /diagnostics" />
+
+</launch>
\ No newline at end of file
Added: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_head_cart/hcc.machine
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_head_cart/hcc.machine (rev 0)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_head_cart/hcc.machine 2009-07-09 05:33:24 UTC (rev 18548)
@@ -0,0 +1,15 @@
+<launch>
+
+ <machine name="realtime" address="hcc1" ros-root="$(env ROS_ROOT)"
+ ros-package-path="$(env ROS_PACKAGE_PATH)" />
+
+ <machine name="realtime_root" user="root" address="hcc1"
+ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" default="never"/>
+
+ <machine name="two" address="hcc2" ros-root="$(env ROS_ROOT)"
+ros-package-path="$(env ROS_PACKAGE_PATH)" default="true" />
+
+ <machine name="two_root" address="hcc2" ros-root="$(env ROS_ROOT)"
+ros-package-path="$(env ROS_PACKAGE_PATH)" user="root" default="never" />
+
+</launch>
\ No newline at end of file
Added: pkg/trunk/stacks/pr2/pr2_default_controllers/calibration_controllers/hcc_calibration_controller.xml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_default_controllers/calibration_controllers/hcc_calibration_controller.xml (rev 0)
+++ pkg/trunk/stacks/pr2/pr2_default_controllers/calibration_controllers/hcc_calibration_controller.xml 2009-07-09 05:33:24 UTC (rev 18548)
@@ -0,0 +1,5 @@
+<controllers>
+ <include filename="$(find pr2_defs)/defs/head_defs.xml" />
+ <head_calibrator />
+ <tilting_laser_calibrator name="laser_tilt" />
+</controllers>
Added: pkg/trunk/stacks/pr2/pr2_defs/robots/hcc.xacro.xml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_defs/robots/hcc.xacro.xml (rev 0)
+++ pkg/trunk/stacks/pr2/pr2_defs/robots/hcc.xacro.xml 2009-07-09 05:33:24 UTC (rev 18548)
@@ -0,0 +1,71 @@
+<?xml version="1.0"?>
+<robot name="hcb">
+
+ <!-- Include file with calibration parameters -->
+ <include filename="$(find pr2_defs)/calibration/default_cal.xml" />
+
+ <!-- declare the path where all models/textures/materials are stored -->
+ <resource location="$(find pr2_defs)/meshes"/>
+
+ <include filename="$(find pr2_defs)/defs/head_defs.xml" />
+ <include filename="$(find pr2_defs)/defs/prosilica_defs.xml" />
+ <include filename="$(find pr2_defs)/defs/gazebo_defs.xml" />
+
+ <!-- Make base joint for visualization -->
+ <joint name="base_joint" type="fixed" />
+ <link name="base_link">
+ <parent name="world" />
+ <origin xyz=" 0 0 0.53 " rpy=" 0 0 0" />
+ <joint name="base_joint" />
+ <inertial>
+ <mass value="100" />
+ <com xyz=" 0 0 0 " />
+ <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" />
+ </inertial>
+ <visual>
+ <!-- 1.06m above ground -->
+ <origin xyz="0 0 0" rpy="0 0 0 " />
+ <map name="gazebo_material" flag="gazebo">
+ <elem key="material">Gazebo/White</elem>
+ </map>
+ <geometry name="unit_box">
+ <mesh scale=".6 0.45 1.06" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0.0 0.0 0.0 " />
+ <geometry name="box_link_collision_geom">
+ <box size="0.6 0.45 1.06" />
+ </geometry>
+ </collision>
+ </link>
+
+ <!-- These dimensions need to be checked -->
+ <pr2_head name="head" parent="base_link">
+ <!-- 213mm above tilt plate. Pan/tilt intersect is origin. -->
+ <origin xyz="${cal_head_x + 0.13} ${cal_head_y} ${.53 + 0.213 + cal_head_z}"
+ rpy="${cal_head_roll} ${cal_head_pitch} ${cal_head_yaw}" />
+ </pr2_head>
+
+ <pr2_tilting_laser_alpha2 name="laser_tilt" parent="base_link">
+ <!-- Was 0.11m forward of head pan, now 115 mm on HCC -->
+ <!-- 66.5mm above head tilt plate -->
+ <origin xyz="${0.115 + 0.13} 0 ${0.0665 + .53}" rpy="0 0 0" />
+ </pr2_tilting_laser_alpha2>
+
+ <!-- Prosilica Camera -->
+ <prosilica_cam name="high_def" parent="head_plate_frame">
+ <!-- Center of camera is 109mm right of center screw, 35mm up -->
+ <!-- Camera is slightly recessed from front, where is camera origin? Lens? -->
+ <origin xyz="0 -0.109 0.035" rpy="0 0 0" />
+ </prosilica_cam>
+
+ <!-- Stereocam -->
+ <double_stereo_camera name="double_stereo" parent="head_plate_frame">
+ <origin xyz="0.0 0.0 0.003" rpy="0 0 0" />
+ </double_stereo_camera>
+
+ <!-- Define groups of links; a link may be part of multiple groups -->
+ <include filename="$(find pr2_defs)/robots/pr2_groups.xml" />
+
+</robot>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|