Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[17257] pkg/trunk
|
From: <stu...@us...> - 2009-06-17 23:25:45
|
Revision: 17257
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17257&view=rev
Author: stuglaser
Date: 2009-06-17 23:24:43 +0000 (Wed, 17 Jun 2009)
Log Message:
-----------
Migrated code to use the mechanism messages in mechanism_msgs instead of robot_msgs
Modified Paths:
--------------
pkg/trunk/calibration/auto_arm_commander/src/auto_arm_commander/settler.py
pkg/trunk/calibration/kinematic_calibration/manifest.xml
pkg/trunk/calibration/kinematic_calibration/msg/CalSnapshot.msg
pkg/trunk/calibration/kinematic_calibration/msg/CalibrationData.msg
pkg/trunk/calibration/kinematic_calibration/msg/CalibrationData2.msg
pkg/trunk/calibration/kinematic_calibration/msg/SensorKinematics.msg
pkg/trunk/calibration/kinematic_calibration/scripts/auto_arm_commander.py
pkg/trunk/calibration/kinematic_calibration/scripts/auto_arm_commander_original.py
pkg/trunk/calibration/kinematic_calibration/scripts/new_cmd_node.py
pkg/trunk/calibration/kinematic_calibration/src/arm_phasespace_grabber.cpp
pkg/trunk/calibration/kinematic_calibration/src/kinematic_calibration/settler.py
pkg/trunk/calibration/kinematic_calibration/src/laser_head_grabber.cpp
pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp
pkg/trunk/calibration/laser_camera_calibration/gatherdata.py
pkg/trunk/calibration/sandbox/laser_head_updater/manifest.xml
pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf.cpp
pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf_pre.cpp
pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf_prg.cpp
pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h
pkg/trunk/controllers/robot_mechanism_controllers/scripts/wxeffect.py
pkg/trunk/demos/plug_in/fake_camera_frame.py
pkg/trunk/demos/plug_in/fake_vision.py
pkg/trunk/demos/plug_in/plug_in.py
pkg/trunk/demos/tabletop_manipulation/scripts/movearm.py
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/manifest.xml
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/scripts/assembler_node.py
pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/dense_laser_assembler/dense_laser_cache.py
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/manifest.xml
pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/arm_state_adapter.cpp
pkg/trunk/mechanism/mechanism_msgs/msg/JointStates.msg
pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h
pkg/trunk/motion_planning/planning_environment/manifest.xml
pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp
pkg/trunk/openrave_planning/orrosplanning/manifest.xml
pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h
pkg/trunk/pr2/cb_characterization/scripts/cmd_node.py
pkg/trunk/pr2/life_test/caster_life_test/periodic_drive.py
pkg/trunk/pr2/life_test/scripts/periodic_drive.py
pkg/trunk/pr2/motorconf_gui/src/motorconf_gui/ui.py
pkg/trunk/pr2/pr2_msgs/manifest.xml
pkg/trunk/pr2/pr2_msgs/msg/MoveEndEffectorGoal.msg
pkg/trunk/pr2/pr2_msgs/msg/MoveEndEffectorState.msg
pkg/trunk/pr2/qualification/scripts/wait_for_stationary.py
pkg/trunk/sandbox/robot_state_publisher/manifest.xml
pkg/trunk/sandbox/robot_state_publisher/src/main.cpp
pkg/trunk/util/self_watch/self_watch.cpp
pkg/trunk/vision/outlet_detection/scripts/fake_gripper_frame.py
pkg/trunk/visualization/rviz/manifest.xml
pkg/trunk/visualization/rviz/src/rviz/displays/robot_model_display.cpp
pkg/trunk/visualization/rviz/src/rviz/displays/robot_model_display.h
pkg/trunk/visualization/rviz/src/rviz/robot/robot.cpp
pkg/trunk/visualization/rviz/src/rviz/robot/robot.h
pkg/trunk/visualization/rviz/src/rviz/robot/robot_link.cpp
pkg/trunk/visualization/rviz/src/rviz/robot/robot_link.h
Modified: pkg/trunk/calibration/auto_arm_commander/src/auto_arm_commander/settler.py
===================================================================
--- pkg/trunk/calibration/auto_arm_commander/src/auto_arm_commander/settler.py 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/calibration/auto_arm_commander/src/auto_arm_commander/settler.py 2009-06-17 23:24:43 UTC (rev 17257)
@@ -39,7 +39,8 @@
import sys
import threading
-from pr2_mechanism_controllers.srv import *
+from pr2_mechanism_controllers.srv import *\
+from mechanism_msgs.msg import MechanismState
from robot_msgs.msg import *
from roslib import rostime
from auto_arm_commander.msg_cache import MsgCache
@@ -65,7 +66,7 @@
def get_stats_interval(self, joints, start, end) :
seg = self._cache.get_segment_interval(start, end)
return self._process_stats(joints, seg)
-
+
def get_stats_latest(self, joints, N) :
seg = self._cache.get_segment_latest(N)
return self._process_stats(joints, seg)
Modified: pkg/trunk/calibration/kinematic_calibration/manifest.xml
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/manifest.xml 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/calibration/kinematic_calibration/manifest.xml 2009-06-17 23:24:43 UTC (rev 17257)
@@ -6,6 +6,7 @@
<depend package="kdl" />
<depend package="phase_space" />
<depend package="robot_msgs" />
+ <depend package="mechanism_msgs" />
<depend package="mocap_msgs" />
<depend package="std_msgs" />
<depend package="topic_synchronizer" />
Modified: pkg/trunk/calibration/kinematic_calibration/msg/CalSnapshot.msg
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/msg/CalSnapshot.msg 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/calibration/kinematic_calibration/msg/CalSnapshot.msg 2009-06-17 23:24:43 UTC (rev 17257)
@@ -1,3 +1,3 @@
Header header
-robot_msgs/MechanismState mech_state
+mechanism_msgs/MechanismState mech_state
SensorSample[] samples
\ No newline at end of file
Modified: pkg/trunk/calibration/kinematic_calibration/msg/CalibrationData.msg
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/msg/CalibrationData.msg 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/calibration/kinematic_calibration/msg/CalibrationData.msg 2009-06-17 23:24:43 UTC (rev 17257)
@@ -8,7 +8,7 @@
image_msgs/RawStereo raw_stereo
# Mechanism State - Stores the robots joint angles
-robot_msgs/MechanismState mechanism_state
+mechanism_msgs/MechanismState mechanism_state
# PointCloud generated from the tilt laser
robot_msgs/PointCloud laser_cloud
Modified: pkg/trunk/calibration/kinematic_calibration/msg/CalibrationData2.msg
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/msg/CalibrationData2.msg 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/calibration/kinematic_calibration/msg/CalibrationData2.msg 2009-06-17 23:24:43 UTC (rev 17257)
@@ -8,7 +8,7 @@
image_msgs/RawStereo raw_stereo
# Mechanism State - Stores the robots joint angles
-robot_msgs/MechanismState mechanism_state
+mechanism_msgs/MechanismState mechanism_state
# PointCloud generated from the tilt laser
robot_msgs/PointCloud laser_cloud
Modified: pkg/trunk/calibration/kinematic_calibration/msg/SensorKinematics.msg
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/msg/SensorKinematics.msg 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/calibration/kinematic_calibration/msg/SensorKinematics.msg 2009-06-17 23:24:43 UTC (rev 17257)
@@ -13,7 +13,7 @@
image_msgs/CamInfo right_info
# Mechanism State - Stores the robots joint angles
-robot_msgs/MechanismState mechanism_state
+mechanism_msgs/MechanismState mechanism_state
# PointCloud generated from the tilt laser
robot_msgs/PointCloud laser_cloud
Modified: pkg/trunk/calibration/kinematic_calibration/scripts/auto_arm_commander.py
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/scripts/auto_arm_commander.py 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/calibration/kinematic_calibration/scripts/auto_arm_commander.py 2009-06-17 23:24:43 UTC (rev 17257)
@@ -9,6 +9,7 @@
from pr2_mechanism_controllers.srv import *
from pr2_mechanism_controllers.msg import *
from robot_msgs.msg import *
+from mechanism_msgs.msg import MechanismState
from kinematic_calibration.msg import *
arm_mapping = [ ]
@@ -25,7 +26,7 @@
capture_publisher = [ ] ;
def mech_state_callback(data, interval_publisher):
-
+
#print "Callback Called"
global arm_mapping
global headers
@@ -41,7 +42,7 @@
ms_joint_names = [x.name for x in data.joint_states]
ms_joint_mapping = [ms_joint_names.index(x) for x in traj_joint_names]
- joint_state_hist.append([data.header.stamp, [data.joint_states[x].position for x in ms_joint_mapping ]] )
+ joint_state_hist.append([data.header.stamp, [data.joint_states[x].position for x in ms_joint_mapping ]] )
while (len(joint_state_hist) > N) :
joint_state_hist.pop(0)
Modified: pkg/trunk/calibration/kinematic_calibration/scripts/auto_arm_commander_original.py
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/scripts/auto_arm_commander_original.py 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/calibration/kinematic_calibration/scripts/auto_arm_commander_original.py 2009-06-17 23:24:43 UTC (rev 17257)
@@ -9,6 +9,7 @@
from pr2_mechanism_controllers.srv import *
from pr2_mechanism_controllers.msg import *
from robot_msgs.msg import *
+from mechanism_msgs.msg import MechanismState
from kinematic_calibration.msg import *
traj_actuator_names = ['r_shoulder_pan_motor',
@@ -32,7 +33,7 @@
print_count = 0
def mech_state_callback(data, interval_publisher):
-
+
#print "Callback Called"
global arm_mapping
global headers
@@ -51,7 +52,7 @@
ms_actuator_mapping = [ms_actuator_names.index(x) for x in traj_actuator_names]
ms_joint_mapping = [ms_joint_names.index(x) for x in traj_joint_names]
- joint_state_hist.append([data.header.stamp, [data.joint_states[x].position for x in ms_joint_mapping ]] )
+ joint_state_hist.append([data.header.stamp, [data.joint_states[x].position for x in ms_joint_mapping ]] )
while (len(joint_state_hist) > N) :
joint_state_hist.pop(0)
@@ -76,8 +77,8 @@
joint_state_hist[0][0], joint_state_hist[-1][0]))
ready_to_capture = False
done_capturing = True
-
+
if __name__ == '__main__':
print "Running python code"
rospy.init_node('auto_arm_commander', sys.argv, anonymous=False)
Modified: pkg/trunk/calibration/kinematic_calibration/scripts/new_cmd_node.py
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/scripts/new_cmd_node.py 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/calibration/kinematic_calibration/scripts/new_cmd_node.py 2009-06-17 23:24:43 UTC (rev 17257)
@@ -9,6 +9,7 @@
from pr2_mechanism_controllers.srv import *
from pr2_mechanism_controllers.msg import *
from robot_msgs.msg import *
+from mechanism_msgs.msg import MechanismState
from kinematic_calibration.msg import *
from kinematic_calibration.arm_commander import ArmCommander
@@ -69,7 +70,7 @@
for x in arm_stats.ranges :
arm_settled = arm_settled and (x < .0001)
time.sleep(.1)
- print " Arm is settled!"
+ print " Arm is settled!"
# Grab a stereotypical MechanismState that's close to the middle of the interval
cur_mech_state = arm_stats.seg[len(arm_stats.seg)/2]
@@ -94,7 +95,7 @@
print " [" + ', '.join( ['%.3f'%x for x in arm_stats.ranges] ) + "]"
for s in msg.samples :
print " [%3.2f, %3.2f]" % (s.m[0], s.m[1])
-
+
else :
print " Found no leds...sleeping...",
sys.stdout.flush()
Modified: pkg/trunk/calibration/kinematic_calibration/src/arm_phasespace_grabber.cpp
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/src/arm_phasespace_grabber.cpp 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/calibration/kinematic_calibration/src/arm_phasespace_grabber.cpp 2009-06-17 23:24:43 UTC (rev 17257)
@@ -36,7 +36,7 @@
#include "ros/node.h"
#include "boost/thread/mutex.hpp"
-#include "robot_msgs/MechanismState.h"
+#include "mechanism_msgs/MechanismState.h"
#include "mocap_msgs/MocapSnapshot.h"
#include "robot_kinematics/robot_kinematics.h"
@@ -271,7 +271,7 @@
{
// Grab mechanism state and put it in a local copy
mech_lock_.lock() ;
- robot_msgs::MechanismState mech_state = safe_mech_state_ ;
+ mechanism_msgs::MechanismState mech_state = safe_mech_state_ ;
mech_lock_.unlock() ;
angles.resize(names.size()) ;
@@ -305,12 +305,12 @@
private:
mocap_msgs::MocapSnapshot snapshot_ ;
- robot_msgs::MechanismState mech_state_ ;
+ mechanism_msgs::MechanismState mech_state_ ;
mocap_msgs::MocapSnapshot safe_snapshot_ ;
int marker_id_ ;
- robot_msgs::MechanismState safe_mech_state_ ;
+ mechanism_msgs::MechanismState safe_mech_state_ ;
boost::mutex mech_lock_ ;
boost::mutex snapshot_lock_ ;
@@ -327,7 +327,7 @@
ArmPhaseSpaceGrabber grabber ;
grabber.spin() ;
-
+
return 0 ;
}
Modified: pkg/trunk/calibration/kinematic_calibration/src/kinematic_calibration/settler.py
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/src/kinematic_calibration/settler.py 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/calibration/kinematic_calibration/src/kinematic_calibration/settler.py 2009-06-17 23:24:43 UTC (rev 17257)
@@ -40,6 +40,7 @@
import threading
from pr2_mechanism_controllers.srv import *
+from mechanism_msgs.msg import MechanismState
from robot_msgs.msg import *
from roslib import rostime
from kinematic_calibration.msg_cache import MsgCache
@@ -65,7 +66,7 @@
def get_stats_interval(self, joints, start, end) :
seg = self._cache.get_segment_interval(start, end)
return self._process_stats(joints, seg)
-
+
def get_stats_latest(self, joints, N) :
seg = self._cache.get_segment_latest(N)
return self._process_stats(joints, seg)
Modified: pkg/trunk/calibration/kinematic_calibration/src/laser_head_grabber.cpp
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/src/laser_head_grabber.cpp 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/calibration/kinematic_calibration/src/laser_head_grabber.cpp 2009-06-17 23:24:43 UTC (rev 17257)
@@ -36,7 +36,7 @@
#include "ros/node.h"
#include "boost/thread/mutex.hpp"
-#include "robot_msgs/MechanismState.h"
+#include "mechanism_msgs/MechanismState.h"
#include "topic_synchronizer/topic_synchronizer.h"
@@ -82,7 +82,7 @@
// Mechanism State
boost::mutex mech_state_lock_ ;
- ADD_MSG(robot_msgs::MechanismState, mech_state_) ;
+ ADD_MSG(mechanism_msgs::MechanismState, mech_state_) ;
// Stereo Messages
boost::mutex stereo_lock_ ;
Modified: pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/calibration/kinematic_calibration/src/sensor_kinematics_grabber.cpp 2009-06-17 23:24:43 UTC (rev 17257)
@@ -36,7 +36,7 @@
#include "ros/node.h"
#include "boost/thread/mutex.hpp"
-#include "robot_msgs/MechanismState.h"
+#include "mechanism_msgs/MechanismState.h"
#include "mocap_msgs/MocapSnapshot.h"
#include "topic_synchronizer/topic_synchronizer.h"
@@ -79,8 +79,8 @@
TopicSynchronizer<SensorKinematicsGrabber> sync_ ;
// Mechanism State Messages
- robot_msgs::MechanismState mech_state_ ;
- robot_msgs::MechanismState safe_mech_state_ ;
+ mechanism_msgs::MechanismState mech_state_ ;
+ mechanism_msgs::MechanismState safe_mech_state_ ;
boost::mutex mech_state_lock_ ;
// Empty message used for callbacks
Modified: pkg/trunk/calibration/laser_camera_calibration/gatherdata.py
===================================================================
--- pkg/trunk/calibration/laser_camera_calibration/gatherdata.py 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/calibration/laser_camera_calibration/gatherdata.py 2009-06-17 23:24:43 UTC (rev 17257)
@@ -38,7 +38,7 @@
PKG = 'laser_camera_calibration' # this package name
NAME = 'lasercamera_gatherer'
-import roslib; roslib.load_manifest(PKG)
+import roslib; roslib.load_manifest(PKG)
import sys
import thread
@@ -47,7 +47,7 @@
import rospy
from roslib import rostime
from laser_scan.msg import LaserScan
-from robot_msgs.msg import MechanismState
+from mechanism_msgs.msg import MechanismState
from checkerboard_detector.msg import ObjectDetection
import copy
@@ -70,7 +70,7 @@
rospy.Subscriber("tilt_scan", LaserScan, self.callback_laser, 1)
rospy.Subscriber("mechanism_state", MechanismState, self.callback_robot, 1)
rospy.init_node(NAME, anonymous=True)
-
+
def pose_dist(self, pose1, pose2):
t1 = array([pose1.position.x, pose1.position.y, pose1.position.z])
q1 = array([pose1.orientation.x, pose1.orientation.y, pose1.orientation.z, pose1.orientation.w])
@@ -129,7 +129,7 @@
if len(self.laserqueue) < 2 or len(self.robotqueue) < 2:
return
-
+
stamp = min(self.objdetqueue[-1].header.stamp, self.laserqueue[-1].header.stamp, self.robotqueue[-1].header.stamp)
# interpolate scanlines, find two scanlines that are between stamp
Modified: pkg/trunk/calibration/sandbox/laser_head_updater/manifest.xml
===================================================================
--- pkg/trunk/calibration/sandbox/laser_head_updater/manifest.xml 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/calibration/sandbox/laser_head_updater/manifest.xml 2009-06-17 23:24:43 UTC (rev 17257)
@@ -6,6 +6,7 @@
<review status="unreviewed" notes=""/>
<depend package="kdl" />
<depend package="robot_msgs" />
+ <depend package="mechanism_msgs" />
<depend package="std_msgs" />
<depend package="tf" />
<depend package="laser_scan" />
Modified: pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf.cpp
===================================================================
--- pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf.cpp 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf.cpp 2009-06-17 23:24:43 UTC (rev 17257)
@@ -38,7 +38,7 @@
#include "ros/ros.h"
#include "tf/tfMessage.h"
#include "tf/transform_datatypes.h"
-#include "robot_msgs/MechanismState.h"
+#include "mechanism_msgs/MechanismState.h"
#include "laser_scan/LaserScan.h"
#include "image_msgs/RawStereo.h"
@@ -128,7 +128,7 @@
after_tilt_T_ = buildTransform(after_tilt) ;
}
- double getJointPos(const robot_msgs::MechanismStateConstPtr& mech_state,
+ double getJointPos(const mechanism_msgs::MechanismStateConstPtr& mech_state,
const std::string& name)
{
for(unsigned int i=0; i<mech_state->joint_states.size(); i++)
@@ -142,7 +142,7 @@
}
- void updateTf(const robot_msgs::MechanismStateConstPtr& mech_state)
+ void updateTf(const mechanism_msgs::MechanismStateConstPtr& mech_state)
{
//printf("In callback\n") ;
Modified: pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf_pre.cpp
===================================================================
--- pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf_pre.cpp 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf_pre.cpp 2009-06-17 23:24:43 UTC (rev 17257)
@@ -38,7 +38,7 @@
#include "ros/ros.h"
#include "tf/tfMessage.h"
#include "tf/transform_datatypes.h"
-#include "robot_msgs/MechanismState.h"
+#include "mechanism_msgs/MechanismState.h"
#include "laser_scan/LaserScan.h"
#include "image_msgs/RawStereo.h"
@@ -132,7 +132,7 @@
after_tilt_T_ = buildTransform(after_tilt) ;
}
- double getJointPos(const robot_msgs::MechanismStateConstPtr& mech_state,
+ double getJointPos(const mechanism_msgs::MechanismStateConstPtr& mech_state,
const std::string& name)
{
for(unsigned int i=0; i<mech_state->joint_states.size(); i++)
@@ -146,7 +146,7 @@
}
- void updateTf(const robot_msgs::MechanismStateConstPtr& mech_state)
+ void updateTf(const mechanism_msgs::MechanismStateConstPtr& mech_state)
{
//printf("In callback\n") ;
Modified: pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf_prg.cpp
===================================================================
--- pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf_prg.cpp 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/calibration/sandbox/laser_head_updater/src/publish_laser_head_tf_prg.cpp 2009-06-17 23:24:43 UTC (rev 17257)
@@ -38,7 +38,7 @@
#include "ros/ros.h"
#include "tf/tfMessage.h"
#include "tf/transform_datatypes.h"
-#include "robot_msgs/MechanismState.h"
+#include "mechanism_msgs/MechanismState.h"
#include "laser_scan/LaserScan.h"
#include "image_msgs/RawStereo.h"
@@ -131,7 +131,7 @@
after_tilt_T_ = buildTransform(after_tilt) ;
}
- double getJointPos(const robot_msgs::MechanismStateConstPtr& mech_state,
+ double getJointPos(const mechanism_msgs::MechanismStateConstPtr& mech_state,
const std::string& name)
{
for(unsigned int i=0; i<mech_state->joint_states.size(); i++)
@@ -145,7 +145,7 @@
}
- void updateTf(const robot_msgs::MechanismStateConstPtr& mech_state)
+ void updateTf(const mechanism_msgs::MechanismStateConstPtr& mech_state)
{
//printf("In callback\n") ;
Modified: pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h
===================================================================
--- pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/calibration/wheel_odometry_calibration/src/odom_calib.h 2009-06-17 23:24:43 UTC (rev 17257)
@@ -1,13 +1,13 @@
/*********************************************************************
* 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:
-*
+*
* 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
@@ -16,7 +16,7 @@
* * 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
@@ -45,7 +45,7 @@
#include "deprecated_msgs/RobotBase2DOdom.h"
#include "robot_msgs/PoseDot.h"
#include "robot_msgs/PoseWithRatesStamped.h"
-#include "robot_msgs/MechanismState.h"
+#include "mechanism_msgs/MechanismState.h"
namespace calibration
{
@@ -79,12 +79,12 @@
void AngleOverflowCorrect(double& a, double ref);
// messages to receive
- deprecated_msgs::RobotBase2DOdom _odom;
- robot_msgs::PoseWithRatesStamped _imu;
- robot_msgs::MechanismState _mech;
+ deprecated_msgs::RobotBase2DOdom _odom;
+ robot_msgs::PoseWithRatesStamped _imu;
+ mechanism_msgs::MechanismState _mech;
// estimated robot pose message to send
- robot_msgs::PoseDot _vel;
+ robot_msgs::PoseDot _vel;
// service messages
pr2_mechanism_controllers::WheelRadiusMultiplier::Request _srv_snd, _srv_rsp;
@@ -101,7 +101,7 @@
std::vector<double> _mech_begin, _mech_end;
// mutex
- boost::mutex _odom_mutex, _imu_mutex, _mech_mutex;
+ boost::mutex _odom_mutex, _imu_mutex, _mech_mutex;
}; // class
Modified: pkg/trunk/controllers/robot_mechanism_controllers/scripts/wxeffect.py
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/scripts/wxeffect.py 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/controllers/robot_mechanism_controllers/scripts/wxeffect.py 2009-06-17 23:24:43 UTC (rev 17257)
@@ -5,6 +5,7 @@
import rospy
from robot_msgs.msg import *
+from mechanism_msgs.msg import MechanismState
import wx
from wx import xrc
Modified: pkg/trunk/demos/plug_in/fake_camera_frame.py
===================================================================
--- pkg/trunk/demos/plug_in/fake_camera_frame.py 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/demos/plug_in/fake_camera_frame.py 2009-06-17 23:24:43 UTC (rev 17257)
@@ -1,10 +1,10 @@
#! /usr/bin/env python
# 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
@@ -13,7 +13,7 @@
# * Neither the name of the Willow Garage, Inc. 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
@@ -61,11 +61,6 @@
def callback(self, msg):
self.msg = msg
-mechanism_state = Tracker('/mechanism_state', MechanismState)
-def last_time():
- global mechanism_state
- return mechanism_state.msg.header.stamp
-
t = TransformStamped()
t.header.frame_id = 'high_def_frame'
t.header.seq = 0
@@ -77,7 +72,7 @@
pub_tf = rospy.Publisher('/tf_message', tfMessage)
while not rospy.is_shutdown():
- t.header.stamp = last_time()
+ t.header.stamp = rospy.get_rostime()
msg = tfMessage([t])
pub_tf.publish(msg)
time.sleep(0.1)
Modified: pkg/trunk/demos/plug_in/fake_vision.py
===================================================================
--- pkg/trunk/demos/plug_in/fake_vision.py 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/demos/plug_in/fake_vision.py 2009-06-17 23:24:43 UTC (rev 17257)
@@ -57,16 +57,8 @@
def callback(self, msg):
self.msg = msg
-mechanism_state = Tracker('/mechanism_state', MechanismState)
-def last_time():
- return rospy.get_rostime()
- global mechanism_state
- if mechanism_state.msg:
- return mechanism_state.msg.header.stamp
- return 0
-
pub_outlet = rospy.Publisher('/outlet_detector/pose', PoseStamped)
pub_plug = rospy.Publisher('/plug_detector/pose', PoseStamped)
rospy.init_node('fake_vision', anonymous=True)
@@ -74,13 +66,13 @@
def send():
op = PoseStamped()
- op.header.stamp = last_time()
+ op.header.stamp = rospy.get_rostime()
op.header.frame_id = 'torso_lift_link'
op.pose.position = xyz(0.7, -0.4, -0.4)
op.pose.orientation = rpy(0, 0, 0)
pp = PoseStamped()
- pp.header.stamp = last_time()
+ pp.header.stamp = rospy.get_rostime()
pp.header.frame_id = 'r_gripper_tool_frame'
pp.pose.position = xyz(0.0, 0.0, 0.0)
pp.pose.orientation = rpy(0,-pi/6,0)
Modified: pkg/trunk/demos/plug_in/plug_in.py
===================================================================
--- pkg/trunk/demos/plug_in/plug_in.py 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/demos/plug_in/plug_in.py 2009-06-17 23:24:43 UTC (rev 17257)
@@ -46,15 +46,7 @@
def callback(self, msg):
self.msg = msg
-mechanism_state = Tracker('/mechanism_state', MechanismState)
-def last_time():
- global mechanism_state
- if mechanism_state.msg:
- return mechanism_state.msg.header.stamp
- return 0
-
-
def main():
rospy.init_node('plug_in')
@@ -100,7 +92,7 @@
cnt = 0
while not rospy.is_shutdown():
cnt += 1
- outlet_pose.header.stamp = last_time()
+ outlet_pose.header.stamp = rospy.get_rostime()
pub_command.publish(outlet_pose)
if cnt % 3 == 0:
try:
Modified: pkg/trunk/demos/tabletop_manipulation/scripts/movearm.py
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/scripts/movearm.py 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/demos/tabletop_manipulation/scripts/movearm.py 2009-06-17 23:24:43 UTC (rev 17257)
@@ -37,7 +37,8 @@
roslib.load_manifest('tabletop_manipulation')
import rospy
from pr2_msgs.msg import MoveArmGoal, MoveArmState
-from robot_msgs.msg import JointState, PoseConstraint, ControllerStatus
+from robot_msgs.msg import PoseConstraint, ControllerStatus
+from mechanism_msgs.msg import JointState
import sys
@@ -129,7 +130,7 @@
rospy.sleep(1.0)
return self.status.value == ControllerStatus.SUCCESS
-
+
USAGE = 'movearm.py {left|right} <shoulder_lift> <shoulder_pan>'
if __name__ == '__main__':
if len(sys.argv) != 4 or (sys.argv[1] != 'left' and sys.argv[1] != 'right'):
@@ -137,7 +138,7 @@
sys.exit(-1)
side = sys.argv[1]
- joints = {}
+ joints = {}
joints[side[0] + '_shoulder_lift_joint'] = float(sys.argv[2])
joints[side[0] + '_shoulder_pan_joint'] = float(sys.argv[3])
Modified: pkg/trunk/drivers/robot/pr2/dense_laser_assembler/manifest.xml
===================================================================
--- pkg/trunk/drivers/robot/pr2/dense_laser_assembler/manifest.xml 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/manifest.xml 2009-06-17 23:24:43 UTC (rev 17257)
@@ -13,6 +13,7 @@
<depend package="roscpp"/>
<depend package="std_msgs"/>
<depend package="robot_msgs"/>
+ <depend package="mechanism_msgs" />
<depend package="laser_scan"/>
<depend package="pr2_mechanism_controllers"/>
<depend package="image_msgs" />
Modified: pkg/trunk/drivers/robot/pr2/dense_laser_assembler/scripts/assembler_node.py
===================================================================
--- pkg/trunk/drivers/robot/pr2/dense_laser_assembler/scripts/assembler_node.py 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/scripts/assembler_node.py 2009-06-17 23:24:43 UTC (rev 17257)
@@ -34,13 +34,14 @@
#
-import roslib; roslib.load_manifest('dense_laser_assembler')
+import roslib; roslib.load_manifest('dense_laser_assembler')
import sys
import rospy
import threading
from time import sleep
from laser_scan.msg import *
from robot_msgs.msg import *
+from mechanism_msgs.msg import MechanismState
from std_msgs.msg import *
from dense_laser_assembler.msg import *
from pr2_mechanism_controllers.msg import *
@@ -107,7 +108,7 @@
msg_header = roslib.msg.Header()
msg_header.stamp = scans[-1].scan.header.stamp
msg_header.frame_id = scans[-1].scan.header.frame_id
-
+
layout = MultiArrayLayout([ MultiArrayDimension('rows', rows, rows*cols),
MultiArrayDimension('cols', cols, cols),
MultiArrayDimension('elem', 1, 1) ], 0 )
@@ -139,7 +140,7 @@
intensity_msg.data.data.extend(x.scan.intensities)
range_msg.data.data.extend(x.scan.ranges)
joint_msg.data.data.extend(x.pos)
-
+
intensity_pub.publish(intensity_msg)
range_pub.publish(range_msg)
info_pub.publish(info_msg)
@@ -179,7 +180,7 @@
info_pub = rospy.Publisher("dense_tilt_scan/scan_info", LaserScan)
joint_pos_pub = rospy.Publisher("dense_tilt_scan/joint_info",Float32MultiArrayStamped)
#image_pub = rospy.Publisher("test_image", Image)
-
+
scan_sub = rospy.Subscriber("tilt_scan", LaserScan, scan_callback)
mech_sub = rospy.Subscriber("mechanism_state", MechanismState, mech_callback)
Modified: pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/dense_laser_assembler/dense_laser_cache.py
===================================================================
--- pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/dense_laser_assembler/dense_laser_cache.py 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/drivers/robot/pr2/dense_laser_assembler/src/dense_laser_assembler/dense_laser_cache.py 2009-06-17 23:24:43 UTC (rev 17257)
@@ -40,6 +40,7 @@
import threading
from laser_scan.msg import *
from robot_msgs.msg import *
+from mechanism_msgs.msg import MechanismState
from roslib import rostime
# Stores a single laser scan plus some meta information
@@ -182,7 +183,7 @@
else :
latest_time = rostime.Time().from_seconds(0.0)
self._laser_done_lock.release()
- return latest_time
+ return latest_time
# Adds a request for extracting data during an interval. This request will be attempted to
# processed when process_interval_reqs is called.
Modified: pkg/trunk/highlevel/executive_trex/executive_trex_pr2/manifest.xml
===================================================================
--- pkg/trunk/highlevel/executive_trex/executive_trex_pr2/manifest.xml 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/highlevel/executive_trex/executive_trex_pr2/manifest.xml 2009-06-17 23:24:43 UTC (rev 17257)
@@ -15,6 +15,7 @@
<depend package="pr2_msgs" />
<depend package="std_msgs" />
<depend package="robot_msgs" />
+ <depend package="mechanism_msgs" />
<depend package="kdl" />
<depend package="door_msgs" />
<depend package="deprecated_msgs" />
Modified: pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/arm_state_adapter.cpp
===================================================================
--- pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/arm_state_adapter.cpp 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/highlevel/executive_trex/executive_trex_pr2/src/arm_state_adapter.cpp 2009-06-17 23:24:43 UTC (rev 17257)
@@ -1,21 +1,21 @@
#include "executive_trex_pr2/ros_state_adapter.h"
#include "Domains.hh"
-#include <robot_msgs/MechanismState.h>
+#include <mechanism_msgs/MechanismState.h>
#include <vector>
namespace TREX {
- class ArmStateAdapter: public ROSStateAdapter<robot_msgs::MechanismState> {
+ class ArmStateAdapter: public ROSStateAdapter<mechanism_msgs::MechanismState> {
public:
ArmStateAdapter(const LabelStr& agentName, const TiXmlElement& configData)
- : ROSStateAdapter<robot_msgs::MechanismState> ( agentName, configData) {
+ : ROSStateAdapter<mechanism_msgs::MechanismState> ( agentName, configData) {
}
virtual ~ArmStateAdapter(){}
private:
- bool readJointValue(const robot_msgs::MechanismState& mechanismStateMsg, const std::string& name, double& value){
+ bool readJointValue(const mechanism_msgs::MechanismState& mechanismStateMsg, const std::string& name, double& value){
for(unsigned int i = 0; i < mechanismStateMsg.get_joint_states_size(); i++){
const std::string& jointName = mechanismStateMsg.joint_states[i].name;
if(name == jointName){
Modified: pkg/trunk/mechanism/mechanism_msgs/msg/JointStates.msg
===================================================================
--- pkg/trunk/mechanism/mechanism_msgs/msg/JointStates.msg 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/mechanism/mechanism_msgs/msg/JointStates.msg 2009-06-17 23:24:43 UTC (rev 17257)
@@ -1,2 +1,2 @@
Header header
-robot_msgs/JointState[] joints
+mechanism_msgs/JointState[] joints
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h 2009-06-17 23:24:43 UTC (rev 17257)
@@ -1,13 +1,13 @@
/*********************************************************************
* 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
@@ -17,7 +17,7 @@
* * 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
@@ -40,7 +40,7 @@
#include "planning_environment/robot_models.h"
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
-#include <robot_msgs/MechanismState.h>
+#include <mechanism_msgs/MechanismState.h>
#include <boost/bind.hpp>
#include <vector>
#include <string>
@@ -52,33 +52,33 @@
If the pose is not included, the robot state is the frame of the link it attaches to the world. If the pose is included,
the frame of the robot is the one in which the pose is published.
<hr>
-
+
@section topic ROS topics
-
+
Subscribes to (name/type):
- @b "mechanism_model"/MechanismModel : position for each of the robot's joints
- @b "localized_pose"/PoseWithCovariance : localized robot pose
-
- */
+
+ */
class KinematicModelStateMonitor
{
public:
-
+
KinematicModelStateMonitor(RobotModels *rm)
{
rm_ = rm;
includePose_ = false;
setupRSM();
}
-
+
KinematicModelStateMonitor(RobotModels *rm, const std::string &frame_id)
{
rm_ = rm;
includePose_ = true;
- frame_id_ = frame_id;
+ frame_id_ = frame_id;
setupRSM();
}
-
+
virtual ~KinematicModelStateMonitor(void)
{
if (robotState_)
@@ -92,7 +92,7 @@
{
onStateUpdate_ = callback;
}
-
+
/** \brief Get the kinematic model that is being monitored */
planning_models::KinematicModel* getKinematicModel(void) const
{
@@ -104,31 +104,31 @@
{
return rm_;
}
-
+
/** \brief Return a pointer to the maintained robot state */
const planning_models::KinematicModel::StateParams* getRobotState(void) const
{
return robotState_;
}
-
+
/** \brief Return the frame id of the state */
const std::string& getFrameId(void) const
{
return frame_id_;
}
-
+
/** \brief Return the robot frame id (robot without pose) */
const std::string& getRobotFrameId(void) const
{
return robot_frame_;
}
-
+
/** \brief Return true if a full mechanism state has been received */
- bool haveState(void) const
+ bool haveState(void) const
{
return haveMechanismState_ && (!includePose_ || (includePose_ && havePose_));
}
-
+
/** \brief Return the time of the last state update */
const ros::Time& lastStateUpdate(void) const
{
@@ -140,20 +140,20 @@
/** \brief Return true if a full mechanism state has been received in the last sec seconds */
bool isStateUpdated(double sec) const;
-
+
/** \brief Return true if the pose is included in the state */
bool isPoseIncluded(void) const
{
return includePose_;
}
-
+
/** \brief Output the current state as ROS INFO */
void printRobotState(void) const;
protected:
void setupRSM(void);
- void mechanismStateCallback(const robot_msgs::MechanismStateConstPtr &mechanismState);
+ void mechanismStateCallback(const mechanism_msgs::MechanismStateConstPtr &mechanismState);
RobotModels *rm_;
@@ -161,23 +161,23 @@
planning_models::KinematicModel *kmodel_;
std::string planarJoint_;
std::string floatingJoint_;
-
+
ros::NodeHandle nh_;
- ros::Subscriber mechanismStateSubscriber_;
+ ros::Subscriber mechanismStateSubscriber_;
tf::TransformListener *tf_;
planning_models::KinematicModel::StateParams *robotState_;
tf::Pose pose_;
std::string robot_frame_;
std::string frame_id_;
-
+
boost::function<void(void)> onStateUpdate_;
-
+
bool havePose_;
bool haveMechanismState_;
ros::Time lastStateUpdate_;
};
-
+
}
#endif
Modified: pkg/trunk/motion_planning/planning_environment/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/planning_environment/manifest.xml 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/motion_planning/planning_environment/manifest.xml 2009-06-17 23:24:43 UTC (rev 17257)
@@ -15,6 +15,7 @@
<depend package="angles" />
<depend package="pr2_defs" />
<depend package="robot_msgs" />
+ <depend package="mechanism_msgs" />
<depend package="motion_planning_msgs" />
<depend package="planning_models" />
<depend package="collision_space" />
Modified: pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp 2009-06-17 23:24:43 UTC (rev 17257)
@@ -1,13 +1,13 @@
/*********************************************************************
* 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
@@ -17,7 +17,7 @@
* * 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
@@ -49,7 +49,7 @@
{
kmodel_ = rm_->getKinematicModel().get();
robotState_ = kmodel_->newStateParams();
-
+
if (kmodel_->getRobotCount() > 1)
{
ROS_WARN("Using more than one robot. A frame_id cannot be set (there multiple frames) and pose cannot be maintained");
@@ -62,10 +62,10 @@
planarJoint_ = kmodel_->getRobot(0)->chain->name;
if (dynamic_cast<planning_models::KinematicModel::FloatingJoint*>(kmodel_->getRobot(0)->chain))
floatingJoint_ = kmodel_->getRobot(0)->chain->name;
-
+
robot_frame_ = kmodel_->getRobot(0)->chain->after->name;
ROS_DEBUG("Robot frame is '%s'", robot_frame_.c_str());
-
+
if (includePose_)
{
tf_ = new tf::TransformListener();
@@ -75,18 +75,18 @@
else
frame_id_ = robot_frame_;
}
-
+
mechanismStateSubscriber_ = nh_.subscribe("mechanism_state", 1, &KinematicModelStateMonitor::mechanismStateCallback, this);
ROS_DEBUG("Listening to mechanism state");
}
}
-void planning_environment::KinematicModelStateMonitor::mechanismStateCallback(const robot_msgs::MechanismStateConstPtr &mechanismState)
+void planning_environment::KinematicModelStateMonitor::mechanismStateCallback(const mechanism_msgs::MechanismStateConstPtr &mechanismState)
{
bool change = !haveMechanismState_;
-
+
static bool first_time = true;
-
+
unsigned int n = mechanismState->get_joint_states_size();
for (unsigned int i = 0 ; i < n ; ++i)
{
@@ -111,58 +111,58 @@
ROS_ERROR("Unknown joint: %s", mechanismState->joint_states[i].name.c_str());
}
}
-
+
first_time = false;
lastStateUpdate_ = mechanismState->header.stamp;
if (!haveMechanismState_)
haveMechanismState_ = robotState_->seenAll();
-
+
if (includePose_)
{
- // use tf to figure out pose
+ // use tf to figure out pose
if (tf_->canTransform(frame_id_, robot_frame_, mechanismState->header.stamp))
- {
+ {
tf::Stamped<tf::Pose> transf;
tf_->lookupTransform(frame_id_, robot_frame_, mechanismState->header.stamp, transf);
pose_ = transf;
-
+
if (!planarJoint_.empty())
{
double planar_joint[3];
planar_joint[0] = pose_.getOrigin().x();
planar_joint[1] = pose_.getOrigin().y();
-
+
double yaw, pitch, roll;
pose_.getBasis().getEulerZYX(yaw, pitch, roll);
planar_joint[2] = yaw;
-
+
bool this_changed = robotState_->setParamsJoint(planar_joint, planarJoint_);
change = change || this_changed;
}
-
+
if (!floatingJoint_.empty())
{
double floating_joint[7];
floating_joint[0] = pose_.getOrigin().x();
floating_joint[1] = pose_.getOrigin().y();
floating_joint[2] = pose_.getOrigin().z();
- btQuaternion q = pose_.getRotation();
+ btQuaternion q = pose_.getRotation();
floating_joint[3] = q.x();
floating_joint[4] = q.y();
floating_joint[5] = q.z();
floating_joint[6] = q.w();
-
+
bool this_changed = robotState_->setParamsJoint(floating_joint, floatingJoint_);
change = change || this_changed;
}
-
+
havePose_ = true;
}
else
ROS_WARN("Unable fo find transform from link '%s' to link '%s'", robot_frame_.c_str(), frame_id_.c_str());
}
-
+
if (change && onStateUpdate_ != NULL)
onStateUpdate_();
}
@@ -171,7 +171,7 @@
{
while (nh_.ok() && !haveState())
{
- ROS_INFO("Waiting for mechanism state ...");
+ ROS_INFO("Waiting for mechanism state ...");
ros::spinOnce();
ros::Duration().fromSec(0.05).sleep();
}
Modified: pkg/trunk/openrave_planning/orrosplanning/manifest.xml
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/manifest.xml 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/openrave_planning/orrosplanning/manifest.xml 2009-06-17 23:24:43 UTC (rev 17257)
@@ -11,5 +11,6 @@
<depend package="pr2_mechanism_controllers"/>
<depend package="tf"/>
<depend package="robot_msgs"/>
+ <depend package="mechanism_msgs" />
<depend package="mocap_msgs"/>
</package>
Modified: pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h
===================================================================
--- pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/openrave_planning/orrosplanning/src/rosrobotcontroller.h 2009-06-17 23:24:43 UTC (rev 17257)
@@ -24,7 +24,7 @@
#ifndef RAVE_ROS_ROBOT_CONTROLLER
#define RAVE_ROS_ROBOT_CONTROLLER
-#include <robot_msgs/MechanismState.h>
+#include <mechanism_msgs/MechanismState.h>
#include <pr2_mechanism_controllers/TrajectoryStart.h>
#include <pr2_mechanism_controllers/TrajectoryCancel.h>
#include <pr2_mechanism_controllers/TrajectoryWait.h>
@@ -599,7 +599,7 @@
string _topic;
- robot_msgs::MechanismState _mstate_cb, _mstate;
+ mechanism_msgs::MechanismState _mstate_cb, _mstate;
vector<dReal> _vecdesired;
set< pair<string, int> > _setEnabledJoints; // set of enabled joints and their indices
mutable boost::mutex _mutexstate;
Modified: pkg/trunk/pr2/cb_characterization/scripts/cmd_node.py
===================================================================
--- pkg/trunk/pr2/cb_characterization/scripts/cmd_node.py 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/pr2/cb_characterization/scripts/cmd_node.py 2009-06-17 23:24:43 UTC (rev 17257)
@@ -9,6 +9,7 @@
from pr2_mechanism_controllers.srv import *
from pr2_mechanism_controllers.msg import *
from robot_msgs.msg import *
+from mechanism_msgs.msg import MechanismState
from auto_arm_commander.settler import *
from auto_arm_commander.arm_commander import ArmCommander
@@ -48,7 +49,7 @@
arm_settled = arm_settled and (x < .00000001)
time.sleep(.25)
print " [" + ', '.join( ['%.6f'%x for x in arm_stats.ranges] ) + "]"
- print " Arm is settled!"
+ print " Arm is settled!"
# Grab a stereotypical MechanismState that's close to the middle of the interval
cur_mech_state = arm_stats.seg[len(arm_stats.seg)/2]
Modified: pkg/trunk/pr2/life_test/caster_life_test/periodic_drive.py
===================================================================
--- pkg/trunk/pr2/life_test/caster_life_test/periodic_drive.py 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/pr2/life_test/caster_life_test/periodic_drive.py 2009-06-17 23:24:43 UTC (rev 17257)
@@ -33,7 +33,7 @@
roslib.load_manifest('caster_life_test')
import rospy
from std_msgs.msg import Float64
-from robot_msgs.msg import MechanismState
+from mechanism_msgs.msg import MechanismState
STRAIGHT = 0.82
ROTATION_JOINT = 'fl_caster_rotation_joint'
@@ -90,7 +90,7 @@
if(random.random() > 0.05):
pub_drive.publish(Float64(speed))
time.sleep(0.75)
- speed *= -1
+ speed *= -1
if angle > 3.314:
angle -= 3.314
else:
Modified: pkg/trunk/pr2/life_test/scripts/periodic_drive.py
===================================================================
--- pkg/trunk/pr2/life_test/scripts/periodic_drive.py 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/pr2/life_test/scripts/periodic_drive.py 2009-06-17 23:24:43 UTC (rev 17257)
@@ -33,7 +33,7 @@
roslib.load_manifest('life_test')
import rospy
from std_msgs.msg import Float64
-from robot_msgs.msg import MechanismState
+from mechanism_msgs.msg import MechanismState
STRAIGHT = 0.82
ROTATION_JOINT = 'fl_caster_rotation_joint'
@@ -89,7 +89,7 @@
if(random.random() > 0.05): # Rotate caster
pub_drive.publish(Float64(speed))
time.sleep(0.75)
- speed *= -1
+ speed *= -1
if angle > PI:
angle -= PI
else:
Modified: pkg/trunk/pr2/motorconf_gui/src/motorconf_gui/ui.py
===================================================================
--- pkg/trunk/pr2/motorconf_gui/src/motorconf_gui/ui.py 2009-06-17 23:22:22 UTC (rev 17256)
+++ pkg/trunk/pr2/motorconf_gui/src/motorconf_gui/ui.py 2009-06-17 23:24:43 UTC (rev 17257)
@@ -52,9 +52,9 @@
from mechanism_control import mechanism
from robot_srvs.srv import SpawnController, KillController
-from robot_msgs.msg import MechanismState
+from mechanism_msgs.msg import MechanismState
-import cStringIO
+import cStringIO
class MotorConfFrame(wx.Frame):
def __init__(self, parent):
@@ -65,63 +65,63 @@
resource = xrc.XmlResource(xrc_path)
# Robots with ethercat files
- self._robots = {
+ self._robots = {
'PRE' : 'pre.launch',
'PRF' : 'prf.launch',
'PRG' : 'prg.launch' }
# Load robot selection dialog
dialog = resource.LoadDialog(None, 'robot_select_dialog')
-
+
robot_box = xrc.XRCCTRL(dialog, 'robot_list_box')
robot_box.InsertItems(dict.keys(self._robots), 0)
-
+
xrc.XRCCTRL(dialog, 'instruct_text').Wrap(350)
dialog.Layout()
dialog.Fit()
-
+
if (dialog.ShowModal() == wx.ID_OK):
robot = robot_box.GetStringSelection()
self._ethercat_script = self._robots[robot]
-
+
# Initialize all buttons, etc
self._root_panel = resource.LoadPanel(self, 'motorconf_panel')
self._sizer = wx.BoxSizer(wx.VERTICAL)
self._sizer.Add(self._root_panel, 1, wx.EXPAND|wx.ALIGN_CENTER_VERTICAL)
- self.SetSizer(self._sizer)
-
+ self.SetSizer(self._sizer)
+
# Panels
self._control_panel = xrc.XRCCTRL(self._root_panel, 'control_panel')
self._log_panel = xrc.XRCCTRL(self._root_panel, 'log_panel')
-
+
# Buttons with events
self._conf_button = xrc.XRCCTRL(self._control_panel, 'configure_mcb_button')
self._conf_button.Bind(wx.EVT_BUTTON, self.on_conf)
-
+
self._twitch_button = xrc.XRCCTRL(self._control_panel, 'twitch_button')
self._twitch_button.Bind(wx.EVT_BUTTON, self.on_twitch)
-
+
self._actuator_num_box = xrc.XRCCTRL(self._control_panel, 'actuator_number_box')
-
+
xrc.XRCCTRL(self._control_panel, 'instruct_text').Wrap(340)
# Add actuators to name box
self.setup_actuators()
self._actuator_name_box = xrc.XRCCTRL(self._control_panel, 'actuator_list_box')
self._actuator_name_box.InsertItems(self._actuators, 0)
-
+
# Add joints to twitch box
self.setup_joints()
self._twitch_list_box = xrc.XRCCTRL(self._control_panel, 'twitch_list_box')
self._twitch_list_box.InsertItems(dict.keys(self._joints), 0)
-
+
# Windows and text boxes
self._detect_window = xrc.XRCCTRL(self._control_panel, 'detect_window')
self._log = xrc.XRCCTRL(self._log_panel, 'log_text')
-
+
self.Bind(wx.EVT_CLOSE, self.on_close)
-
+
self.Layout()
self.Fit()
@@ -130,30 +130,30 @@
rospy.init_node("motorconf_GUI", anonymous = True)
self.start_pr2_etherCAT()
rospy.Subscriber("/mechanism_state", MechanismState, self.on_mech_state_msg)
-
-
+
+
else: # close or cancel
sys.exit(0)
-
+
def on_close(self, event):
event.Skip()
if self._launcher is not None:
self._launcher.stop()
-
+
if self._core_launcher is not None:
self._core_launcher.stop()
def launch_core(self):
self.log('Launching ros core services')
-
+
# Create a roslauncher
config = roslaunch.ROSLaunchConfig()
config.master.auto = config.master.AUTO_RESTART
-
+
launcher = roslaunch.ROSLaunchRunner(config)
launcher.launch()
-
+
return launcher
def log(self, msg):
@@ -166,7 +166,7 @@
self.log('Starting pr2_etherCAT')
config = roslaunch.ROSLaunchConfig()
script = os.path.join(roslib.packages.get_pkg_dir('pr2_alpha'), self._ethercat_script)
-
+
# Call subprocess with pr2_etherCAT and script
self._launcher = None
@@ -174,17 +174,17 @@
try:
loader = roslaunch.XmlLoader()
loader.load(script, config)
-
+
launcher = roslaunch.ROSLaunchRunner(config)
# if (listener_obj != None):
# launcher.pm.add_process_listener(listener_obj)
-
+
launcher.launch()
self._launcher = launcher
except roslaunch.RLException, e:
self.log('Caught expection shutting down pr2_etherCAT.\nException:\n%s' % e)
self._launcher = None
-
+
def stop_pr2_etherCAT(self):
self.log('Stopping pr2_etherCAT')
if (self._launcher != None):
@@ -195,16 +195,16 @@
# Record selected actuator index, name
num = self._actuator_num_box.GetValue()
name = self._actuator_name_box.GetStringSelection()
-
+
self.log('Configuring MCB %s as %s.' % (num, name))
-
+
self.stop_pr2_etherCAT()
-
+
try:
# Run motorconf with given commands
path = roslib.packages.get_pkg_dir("ethercat_hardware", True)
actuator_path = path + "/actuators.conf"
-
+
cmd = path + "/motorconf" + " -i eth0 -p -n %s -d %s -a %s"%(name, num, actuator_path)
#retcode = subprocess.call(cmd, shell=True)
self.log('Attempting to program actuator number %s as %s' % (num, name))
@@ -213,7 +213,7 @@
retcode = p.returncode
details = 'Ran motorconf. Attempted to program MCB %s with actuator name %s. Return code: %s.\n\n' % (num, name, retcode)
details = details + 'STDOUT:\n' + stdout + '\n' + 'STDERR:\n' + stderr
-
+
if retcode != 0:
# Popup msg that says it failed
self.log(details)
@@ -221,7 +221,7 @@
self.log('Motorconf succeeded. MCB %s programmed as %s' % (num, name))
except Exception, e:
self.log('Caught exception attempting to program motor.\nException:\n%s' % e)
-
+
self.start_pr2_etherCAT()
def xml_for_twitch(self, joint):
@@ -229,8 +229,8 @@
<controller name=\"%s\" type=\"JointEffortControllerNode\">\
<joint name=\"%s\" />\
</controller>" % (joint + '_controller', joint)
-
+
def setup_actuators(self):
# Parse from actuators.conf file?
self._actuators = [
@@ -241,7 +241,7 @@
# Alpha 2.0 head tilt motors are different
'head_tilt_motor_alpha2a',
'head_tilt_motor_alpha2b',
- 'laser_tilt_mount_motor',
+ 'laser_tilt_mount_motor',
# Right arm
'r_shoulder_pan_motor',
'r_shoulder_lift_motor',
@@ -284,7 +284,7 @@
'br_caster_l_wheel_motor_alpha2',
'br_caster_r_wheel_motor_alpha2',
'br_caster_rotation_motor_alpha2' ]
-
+
def setup_joints(self):
self._joints = {
# Head and spine
@@ -329,7 +329,7 @@
return # Can't twitch if pr2_etherCAT isn't up
joint = self._twitch_box.GetStringSelection()
-
+
self.log('Twitching %s' % joint)
# Launch effort controller for that joint
rospy.wait_for_service('spawn_controller')
@@ -342,20 +342,20 @@
try:
effort = self._joints[joint]
pub = rospy.Publisher("/%s_controller/set_command" % joint, Float64)
-
+
# Twitch joint
pub.publish(Float64(effort))
sleep(0.3)
pub.publish(Float64(-effort))
sleep(0.3)
pub.publish(Float64(0))
-
+
kill_controller(joint + '_controller')
finally:
self.log('Twitched %s' % joint)
kill_controller(joint + '_controller')
-
+
# Comparison to sort mechanism state messages by velocity
def vel_cmp(self, a, b):
return cmp(abs(a[2]), abs(b[2]))
@@ -366,7 +366,7 @@
mec...
[truncated message content] |
