Menu
▾
▴
personalrobots-commit
|
From: <stu...@us...> - 2008-10-09 00:21:56
|
Revision: 5185
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5185&view=rev
Author: stuglaser
Date: 2008-10-09 00:21:46 +0000 (Thu, 09 Oct 2008)
Log Message:
-----------
Reprogrammed motor boards on the one-armed bandit, so the config and calibration code needed updating.
Modified Paths:
--------------
pkg/trunk/manip/teleop_robot/scripts/calibrate.py
pkg/trunk/robot_descriptions/wg_robot_description/one_armed_bandit/one_armed_bandit.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/arm_defs.xml
Modified: pkg/trunk/manip/teleop_robot/scripts/calibrate.py
===================================================================
--- pkg/trunk/manip/teleop_robot/scripts/calibrate.py 2008-10-09 00:03:12 UTC (rev 5184)
+++ pkg/trunk/manip/teleop_robot/scripts/calibrate.py 2008-10-09 00:21:46 UTC (rev 5185)
@@ -36,6 +36,7 @@
import rostools
import copy
+import threading
# Loads interface with the robot.
rostools.update_path('teleop_robot')
@@ -93,10 +94,19 @@
print "Calibrated"
+class FunThread(threading.Thread):
+ def __init__(self, fun, *args):
+ self.fun = fun
+ self.start()
+
+ def run():
+ self.fun(*args)
+
+
calibrate_optically('''
<controller name="cal_shoulder_pan" topic="cal_shoulder_pan" type="JointCalibrationControllerNode">
<calibrate joint="shoulder_pan_right_joint"
- actuator="shoulder_pan_right_act"
+ actuator="shoulder_pan_right_motor"
transmission="shoulder_pan_right_trans"
velocity="0.6" />
<pid p="7" i="0.5" d="0" iClamp="1.0" />
@@ -106,8 +116,8 @@
calibrate_optically('''
<controller name="cal_shoulder_pitch" topic="cal_shoulder_pitch" type="JointCalibrationControllerNode">
<calibrate joint="shoulder_pitch_right_joint"
- actuator="shoulder_lift_right_act"
- transmission="shoulder_lift_right_trans"
+ actuator="shoulder_pitch_right_motor"
+ transmission="shoulder_pitch_right_trans"
velocity="0.6" />
<pid p="7" i="0.5" d="0" iClamp="1.0" />
</controller>
@@ -116,7 +126,7 @@
calibrate_blindly('''
<controller name="upperarm_calibration" topic="upperarm_calibration" type="JointBlindCalibrationControllerNode">
<calibrate joint="upperarm_roll_right_joint"
- actuator="upperarm_roll_right_act"
+ actuator="upperarm_roll_right_motor"
transmission="upperarm_roll_right_trans"
velocity="0.9" />
<pid p="5" i="0.5" d="0" iClamp="1.0" />
@@ -126,9 +136,9 @@
calibrate_blindly('''
<controller name="cal_elbow" topic="cal_elbow" type="JointBlindCalibrationControllerNode">
- <calibrate joint="elbow_right_joint"
- actuator="elbow_right_act"
- transmission="elbow_right_trans"
+ <calibrate joint="elbow_flex_right_joint"
+ actuator="elbow_flex_right_motor"
+ transmission="elbow_flex_right_trans"
velocity="0.8" />
<pid p="5" i="0.5" d="0" iClamp="1.0" />
</controller>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/one_armed_bandit/one_armed_bandit.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/one_armed_bandit/one_armed_bandit.xml 2008-10-09 00:03:12 UTC (rev 5184)
+++ pkg/trunk/robot_descriptions/wg_robot_description/one_armed_bandit/one_armed_bandit.xml 2008-10-09 00:21:46 UTC (rev 5185)
@@ -17,12 +17,40 @@
<include filename="../pr2/head_defs.xml" />
<joint name="base_joint" type="planar" />
-
- <link name="torso">
+ <link name="base">
<parent name="world" />
- <origin xyz="0 0 0.05" rpy="0 0 0" />
<joint name="base_joint" />
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <inertial>
+ <mass value="1000" />
+ <com xyz=" 0 0 0 " />
+ <inertia ixx="10000" ixy="0" ixz="0" iyy="10000" iyz="0" izz="10000" />
+ </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="pr2_base_mesh_file">
+ <mesh scale="1 1 0.01" />
+ </geometry>
+ </visual>
+
+ <collision>
+ <origin xyz="0 0 0" rpy="0.0 0.0 0.0 " />
+ <geometry name="base_collision_geom">
+ <box size="1 1 0.01" />
+ </geometry>
+ </collision>
+ </link>
+
+ <joint name="torso_joint" type="fixed" />
+ <link name="torso">
+ <parent name="base" />
+ <origin xyz="0 0 0.5" rpy="0 0 0" />
+ <joint name="torso_joint" />
+
<inertial>
<mass value="1000" />
<com xyz="0 0 0" />
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/arm_defs.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/arm_defs.xml 2008-10-09 00:03:12 UTC (rev 5184)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/arm_defs.xml 2008-10-09 00:21:46 UTC (rev 5185)
@@ -242,7 +242,7 @@
</link>
<transmission type="SimpleTransmission" name="shoulder_pitch_${suffix}_trans">
- <actuator name="shoulder_pitch_${suffix}_act" />
+ <actuator name="shoulder_pitch_${suffix}_motor" />
<joint name="shoulder_pitch_${suffix}_joint" />
<mechanicalReduction>57.36</mechanicalReduction>
</transmission>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2008-10-09 00:53:16
|
Revision: 5186
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5186&view=rev
Author: stuglaser
Date: 2008-10-09 00:53:12 +0000 (Thu, 09 Oct 2008)
Log Message:
-----------
Placed the torso and laser in the proper locations for the one-armed bandit.
Modified Paths:
--------------
pkg/trunk/manip/teleop_robot/scripts/calibrate.py
pkg/trunk/robot_descriptions/wg_robot_description/one_armed_bandit/one_armed_bandit.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/arm_defs.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/head_defs.xml
Modified: pkg/trunk/manip/teleop_robot/scripts/calibrate.py
===================================================================
--- pkg/trunk/manip/teleop_robot/scripts/calibrate.py 2008-10-09 00:21:46 UTC (rev 5185)
+++ pkg/trunk/manip/teleop_robot/scripts/calibrate.py 2008-10-09 00:53:12 UTC (rev 5186)
@@ -144,3 +144,13 @@
</controller>
''')
+
+calibrate_optically('''
+<controller name="cal_laser_tilt" topic="cal_laser_tilt" type="JointCalibrationControllerNode">
+ <calibrate joint="tilt_laser_mount_joint"
+ actuator="tilt_laser_motor"
+ transmission="tilt_laser_mount_trans"
+ velocity="-0.6" />
+ <pid p=".25" i="0.1" d="0" iClamp="1.0" />
+</controller>
+''')
Modified: pkg/trunk/robot_descriptions/wg_robot_description/one_armed_bandit/one_armed_bandit.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/one_armed_bandit/one_armed_bandit.xml 2008-10-09 00:21:46 UTC (rev 5185)
+++ pkg/trunk/robot_descriptions/wg_robot_description/one_armed_bandit/one_armed_bandit.xml 2008-10-09 00:53:12 UTC (rev 5186)
@@ -48,7 +48,7 @@
<joint name="torso_joint" type="fixed" />
<link name="torso">
<parent name="base" />
- <origin xyz="0 0 0.5" rpy="0 0 0" />
+ <origin xyz="0 0 1.0414" rpy="0 0 0" />
<joint name="torso_joint" />
<inertial>
@@ -63,7 +63,7 @@
<elem key="material">Gazebo/Red</elem>
</map>
<geometry name="pr2_base_mesh_file">
- <mesh scale="1 1 0.01" />
+ <mesh filename="torso" />
</geometry>
</visual>
@@ -75,9 +75,11 @@
</collision>
</link>
- <pr2_upperarm suffix="right" reflect="-1" parent="torso" />
+ <pr2_upperarm suffix="right" reflect="-1" parent="torso">
+ <origin xyz="0 -0.188 0" rpy="0 0 0" />
+ </pr2_upperarm>
<tilting_hokuyo name="tilt_laser" parent="torso">
- <origin xyz="0.1 0.1 1.15" rpy="0 0 0" />
+ <origin xyz="0.0635 0.01905 0.2413" rpy="0 0 0" />
</tilting_hokuyo>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/arm_defs.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/arm_defs.xml 2008-10-09 00:21:46 UTC (rev 5185)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/arm_defs.xml 2008-10-09 00:53:12 UTC (rev 5186)
@@ -154,7 +154,7 @@
- <macro name="pr2_upperarm" params="suffix parent reflect">
+ <macro name="pr2_upperarm" params="suffix parent reflect *origin">
<!-- Shoulder pan -->
@@ -169,7 +169,7 @@
<link name="shoulder_pan_${suffix}">
<parent name="${parent}" />
- <origin xyz="0 ${reflect*0.188} 0" rpy="0 0 0" />
+ <insert_block name="origin" />
<joint name="shoulder_pan_${suffix}_joint" />
<inertial>
<mass value="16.29553" />
@@ -352,8 +352,10 @@
</macro>
- <macro name="pr2_arm" params="suffix parent reflect">
- <pr2_upperarm suffix="${suffix}" reflect="${reflect}" parent="${parent}" />
+ <macro name="pr2_arm" params="suffix parent reflect *origin">
+ <pr2_upperarm suffix="${suffix}" reflect="${reflect}" parent="${parent}">
+ <insert_block name="origin" />
+ </pr2_upperarm>
<pr2_forearm suffix="${suffix}" reflect="${reflect}" parent="elbow_flex_${suffix}" />
</macro>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/head_defs.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/head_defs.xml 2008-10-09 00:21:46 UTC (rev 5185)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/head_defs.xml 2008-10-09 00:53:12 UTC (rev 5186)
@@ -9,7 +9,7 @@
<axis xyz="0 1 0" />
<anchor xyz="0 0 0" />
<limit min="-0.785" max="1.48" effort="0.5292" velocity="1256.0" />
- <calibration reference_position="-0.35" values="0 0" />
+ <calibration reference_position="-0.28" values="0 0" />
</joint>
<link name="${name}_mount">
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-10-10 17:38:16
|
Revision: 5233
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5233&view=rev
Author: tfoote
Date: 2008-10-10 17:38:06 +0000 (Fri, 10 Oct 2008)
Log Message:
-----------
adding full capacity to battery message, and populating it
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/IBPSBatteryInterface/monitorBatteries
pkg/trunk/robot_msgs/msg/BatteryState.msg
Modified: pkg/trunk/drivers/robot/pr2/IBPSBatteryInterface/monitorBatteries
===================================================================
--- pkg/trunk/drivers/robot/pr2/IBPSBatteryInterface/monitorBatteries 2008-10-10 17:36:06 UTC (rev 5232)
+++ pkg/trunk/drivers/robot/pr2/IBPSBatteryInterface/monitorBatteries 2008-10-10 17:38:06 UTC (rev 5233)
@@ -282,12 +282,13 @@
full_energy = full_energy + b.design_voltage * b.full_charge_capacity * 3.6 ## assuming mAh capacity
power = power + b.design_voltage * b.current ## assuming mAh capacity
+## \todo remove now redundent
if self.full_charge_energy != full_energy:
self.full_charge_energy = full_energy
rospy.set_param("full_charge_energy", full_energy)
- out = BatteryState(None, energy, power)
- print "Sent Energy: %.0f (J) Power: %.2f (W)\n"%(energy, power)
+ out = BatteryState(None, energy, full_energy, power)
+ print "Sent Energy: %.0f (J) of %.0f (J) Power: %.2f (W)\n"%(energy, full_energy, power)
self.battery_state_pub.publish(out)
Modified: pkg/trunk/robot_msgs/msg/BatteryState.msg
===================================================================
--- pkg/trunk/robot_msgs/msg/BatteryState.msg 2008-10-10 17:36:06 UTC (rev 5232)
+++ pkg/trunk/robot_msgs/msg/BatteryState.msg 2008-10-10 17:38:06 UTC (rev 5233)
@@ -1,3 +1,4 @@
Header header
float64 energy_remaining ## Joules
-float64 power_consumption ## Watts
\ No newline at end of file
+float64 energy_capacity ## Joules
+float64 power_consumption ## Watts
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tpr...@us...> - 2008-10-10 22:49:05
|
Revision: 5263
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5263&view=rev
Author: tpratkanis
Date: 2008-10-10 22:49:01 +0000 (Fri, 10 Oct 2008)
Log Message:
-----------
Start fixing arm.1 test.
Modified Paths:
--------------
pkg/trunk/highlevel/highlevel_controllers/src/MoveArm.cpp
pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h
pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
Modified: pkg/trunk/highlevel/highlevel_controllers/src/MoveArm.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/MoveArm.cpp 2008-10-10 22:47:28 UTC (rev 5262)
+++ pkg/trunk/highlevel/highlevel_controllers/src/MoveArm.cpp 2008-10-10 22:49:01 UTC (rev 5263)
@@ -80,6 +80,7 @@
#include <robot_msgs/DisplayKinematicPath.h>
#include <robot_srvs/NamedKinematicPlanState.h>
#include <robot_srvs/PlanNames.h>
+#include <rosTF/rosTF.h>
static const unsigned int RIGHT_ARM_JOINTS_BASE_INDEX = 11;
static const unsigned int LEFT_ARM_JOINTS_BASE_INDEX = 12;
@@ -119,6 +120,7 @@
mechanism_control::MechanismState mechanismState;
robot_srvs::NamedKinematicPlanState::response plan;
unsigned int currentWaypoint; /*!< The waypoint in the plan that we are targetting */
+ rosTFClient tf_; /**< Used to do transforms */
protected:
std::vector<std::string> jointNames_; /*< The collection of joint names of interest. Instantiate in the derived class.*/
@@ -142,7 +144,7 @@
MoveArm::MoveArm(const std::string& nodeName, const std::string& stateTopic, const std::string& goalTopic,
const std::string& armPosTopic, const std::string& _armCmdTopic, const std::string& _kinematicModel)
: HighlevelController<pr2_msgs::MoveArmState, pr2_msgs::MoveArmGoal>(nodeName, stateTopic, goalTopic),
- armCmdTopic(_armCmdTopic), kinematicModel(_kinematicModel), currentWaypoint(0){
+ armCmdTopic(_armCmdTopic), kinematicModel(_kinematicModel), currentWaypoint(0), tf_(*this, true, 10000000000ULL) {
// Advertise for messages to command the arm
@@ -213,6 +215,30 @@
req.times = 1;
+
+ //Get the pose of the robot:
+ libTF::TFPose2D robotPose, globalPose;
+ robotPose.x = 0;
+ robotPose.y = 0;
+ robotPose.yaw = 0;
+ robotPose.frame = "base";
+ robotPose.time = 0;
+ try{
+ globalPose = this->tf_.transformPose2D("map", robotPose);
+ }
+ catch(libTF::TransformReference::LookupException& ex){
+ std::cout << "No Transform available Error\n";
+ }
+ catch(libTF::TransformReference::ConnectivityException& ex){
+ std::cout << "Connectivity Error\n";
+ }
+ catch(libTF::TransformReference::ExtrapolateException& ex){
+ std::cout << "Extrapolation Error\n";
+ }
+
+
+
+
//initializing full value state
req.start_state.set_names_size(names.get_names_size());
req.start_state.set_joints_size(names.get_names_size());
@@ -220,8 +246,15 @@
req.start_state.names[i] = names.names[i];
//std::cout << req.start_state.names[i] << ": " << names.num_values[i] << std::endl;
req.start_state.joints[i].set_vals_size(names.num_values[i]);
- for (int k = 0 ; k < names.num_values[i]; k++) {
- req.start_state.joints[i].vals[k] = 0; //FIXME: should be the value from mechanism state.
+ if (names.names[i] == "base_joint") {
+ std::cout << "Base: " << i << ", " << globalPose.x << ", " << globalPose.y << ", " << globalPose.yaw << std::endl;
+ req.start_state.joints[i].vals[0] = globalPose.x;
+ req.start_state.joints[i].vals[1] = globalPose.y;
+ req.start_state.joints[i].vals[2] = globalPose.yaw;
+ } else {
+ for (int k = 0 ; k < names.num_values[i]; k++) {
+ req.start_state.joints[i].vals[k] = 0; //FIXME: should be the value from mechanism state.
+ }
}
}
Modified: pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h
===================================================================
--- pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h 2008-10-10 22:47:28 UTC (rev 5262)
+++ pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h 2008-10-10 22:49:01 UTC (rev 5263)
@@ -251,7 +251,14 @@
for (unsigned int i = 0 ; i < n ; ++i)
{
double pos = m_mechanismState.joint_states[i].position;
- m_robotState->setParams(&pos, m_mechanismState.joint_states[i].name);
+ //printf("%d: %s\n", i, m_mechanismState.joint_states[i].name.c_str());
+ if (m_mechanismState.joint_states[i].name == "base_joint" && m_haveBasePos) {
+ m_robotState->setParams(m_basePos, m_mechanismState.joint_states[i].name);
+ }
+ else
+ {
+ m_robotState->setParams(&pos, m_mechanismState.joint_states[i].name);
+ }
}
}
if (m_robotStateSimple)
Modified: pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
===================================================================
--- pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-10-10 22:47:28 UTC (rev 5262)
+++ pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-10-10 22:49:01 UTC (rev 5263)
@@ -188,6 +188,7 @@
void stateUpdate(void)
{
+ //m_robotState->print();
planning_node_util::NodeRobotModel::stateUpdate();
if (m_kmodel)
m_kmodel->computeTransforms(m_robotState->getParams());
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rob...@us...> - 2008-10-11 01:19:43
|
Revision: 5283
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5283&view=rev
Author: rob_wheeler
Date: 2008-10-11 01:19:33 +0000 (Sat, 11 Oct 2008)
Log Message:
-----------
Add service to reset MCBs after a safety lockout
Modified Paths:
--------------
pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h
pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp
pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp
Modified: pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h 2008-10-11 01:01:59 UTC (rev 5282)
+++ pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h 2008-10-11 01:19:33 UTC (rev 5283)
@@ -64,7 +64,7 @@
/*!
* \brief update send most recent motor commands and retrieve updates. This command must be run at a sufficient rate or else the motors will be disabled.
*/
- void update();
+ void update(bool reset);
/*!
* \brief Initialize the EtherCAT Master Library.
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp 2008-10-11 01:01:59 UTC (rev 5282)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp 2008-10-11 01:19:33 UTC (rev 5283)
@@ -257,7 +257,7 @@
}
}
-void EthercatHardware::update()
+void EthercatHardware::update(bool reset)
{
unsigned char *current, *last;
@@ -273,7 +273,14 @@
hw_->actuators_[a]->state_.last_requested_effort_ = hw_->actuators_[a]->command_.effort_;
hw_->actuators_[a]->state_.last_requested_current_ = hw_->actuators_[a]->command_.current_;
slaves_[s]->truncateCurrent(hw_->actuators_[a]->command_);
- slaves_[s]->convertCommand(hw_->actuators_[a]->command_, current);
+ if (reset) {
+ bool tmp = hw_->actuators_[a]->command_.enable_;
+ hw_->actuators_[a]->command_.enable_ = false;
+ slaves_[s]->convertCommand(hw_->actuators_[a]->command_, current);
+ hw_->actuators_[a]->command_.enable_ = tmp;
+ } else {
+ slaves_[s]->convertCommand(hw_->actuators_[a]->command_, current);
+ }
current += slaves_[s]->command_size_ + slaves_[s]->status_size_;
++a;
}
Modified: pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp
===================================================================
--- pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp 2008-10-11 01:01:59 UTC (rev 5282)
+++ pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp 2008-10-11 01:19:33 UTC (rev 5283)
@@ -74,7 +74,8 @@
}
}
-static int quit = 0;
+static int g_quit = 0;
+static bool g_reset_motors = false;
static const int NSEC_PER_SEC = 1e+9;
static struct
@@ -136,7 +137,7 @@
static void *syncClocks(void *)
{
- while (!quit)
+ while (!g_quit)
{
struct timespec ts;
struct timeval tv;
@@ -208,10 +209,18 @@
int period = 1e+6; // 1 ms in nanoseconds
static int count = 0;
- while (!quit)
+ while (!g_quit)
{
double start = now();
- ec.update();
+ if (g_reset_motors)
+ {
+ ec.update(true);
+ g_reset_motors = false;
+ }
+ else
+ {
+ ec.update(false);
+ }
double after_ec = now();
mcn.update();
double after_mc = now();
@@ -240,7 +249,7 @@
ec.hw_->actuators_[i]->command_.enable_ = false;
ec.hw_->actuators_[i]->command_.effort_ = 0;
}
- ec.update();
+ ec.update(false);
publisher.stop();
@@ -254,7 +263,7 @@
void quitRequested(int sig)
{
- quit = 1;
+ g_quit = 1;
}
class Shutdown {
@@ -266,6 +275,15 @@
}
};
+class Reset {
+public:
+ bool resetMotorsService(std_srvs::Empty::request &req, std_srvs::Empty::response &resp)
+ {
+ g_reset_motors = true;
+ return true;
+ }
+};
+
void warnOnSecondary(int sig)
{
void *bt[32];
@@ -356,6 +374,7 @@
pthread_attr_setschedpolicy(&rtThreadAttr, SCHED_FIFO);
node->advertise_service("shutdown", &Shutdown::shutdownService);
+ node->advertise_service("reset_motors", &Reset::resetMotorsService);
//Start thread
int rv;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rob...@us...> - 2008-10-14 01:08:39
|
Revision: 5315
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5315&view=rev
Author: rob_wheeler
Date: 2008-10-14 01:08:32 +0000 (Tue, 14 Oct 2008)
Log Message:
-----------
Move mechanism_control msgs and srvs to robot_msgs and robot_srvs.
- manifest files and scripts missed in previous checkin
Modified Paths:
--------------
pkg/trunk/hardware_test/qualification/manifest.xml
pkg/trunk/hardware_test/qualification/src/qualification/tests/motor_test/motor_test.py
pkg/trunk/manip/teleop_robot/manifest.xml
pkg/trunk/manip/teleop_robot/scripts/calibrate.py
pkg/trunk/manip/teleop_robot/scripts/calibrate_tilt_laser.py
pkg/trunk/manip/teleop_robot/src/teleop_robot/robot_interface.py
pkg/trunk/mechanism/mechanism_bringup/manifest.xml
pkg/trunk/mechanism/mechanism_bringup/scripts/calibrate_base.py
pkg/trunk/mechanism/mechanism_bringup/scripts/calibrate_head.py
pkg/trunk/mechanism/mechanism_bringup/scripts/calibrate_tilt_laser.py
pkg/trunk/motion_planning/planning_node_util/manifest.xml
Modified: pkg/trunk/hardware_test/qualification/manifest.xml
===================================================================
--- pkg/trunk/hardware_test/qualification/manifest.xml 2008-10-14 00:48:39 UTC (rev 5314)
+++ pkg/trunk/hardware_test/qualification/manifest.xml 2008-10-14 01:08:32 UTC (rev 5315)
@@ -10,7 +10,6 @@
<depend package="hokuyo_node" />
<depend package="imu_node" />
<depend package="wxmpl" />
- <depend package="mechanism_control" />
<depend package="robot_mechanism_controllers" />
<url></url>
<sysdepend os="ubuntu" version="7.04-feisty" package="python-wxgtk2.8"/>
Modified: pkg/trunk/hardware_test/qualification/src/qualification/tests/motor_test/motor_test.py
===================================================================
--- pkg/trunk/hardware_test/qualification/src/qualification/tests/motor_test/motor_test.py 2008-10-14 00:48:39 UTC (rev 5314)
+++ pkg/trunk/hardware_test/qualification/src/qualification/tests/motor_test/motor_test.py 2008-10-14 01:08:32 UTC (rev 5315)
@@ -45,7 +45,6 @@
from robot_msgs.msg import *
from robot_srvs.srv import *
-from mechanism_control.msg import *
from mechanism_control import mechanism
Modified: pkg/trunk/manip/teleop_robot/manifest.xml
===================================================================
--- pkg/trunk/manip/teleop_robot/manifest.xml 2008-10-14 00:48:39 UTC (rev 5314)
+++ pkg/trunk/manip/teleop_robot/manifest.xml 2008-10-14 01:08:32 UTC (rev 5315)
@@ -4,5 +4,5 @@
<license>BSD</license>
<depend package="robot_mechanism_controllers"/>
<depend package="pr2_mechanism_controllers"/>
- <depend package="mechanism_control"/>
+ <depend package="robot_srvs"/>
</package>
Modified: pkg/trunk/manip/teleop_robot/scripts/calibrate.py
===================================================================
--- pkg/trunk/manip/teleop_robot/scripts/calibrate.py 2008-10-14 00:48:39 UTC (rev 5314)
+++ pkg/trunk/manip/teleop_robot/scripts/calibrate.py 2008-10-14 01:08:32 UTC (rev 5315)
@@ -41,7 +41,7 @@
# Loads interface with the robot.
rostools.update_path('teleop_robot')
import rospy
-from mechanism_control.srv import *
+from robot_srvs.srv import *
from robot_mechanism_controllers.srv import *
def slurp(filename):
Modified: pkg/trunk/manip/teleop_robot/scripts/calibrate_tilt_laser.py
===================================================================
--- pkg/trunk/manip/teleop_robot/scripts/calibrate_tilt_laser.py 2008-10-14 00:48:39 UTC (rev 5314)
+++ pkg/trunk/manip/teleop_robot/scripts/calibrate_tilt_laser.py 2008-10-14 01:08:32 UTC (rev 5315)
@@ -40,7 +40,7 @@
# Loads interface with the robot.
rostools.update_path('teleop_robot')
import rospy
-from mechanism_control.srv import *
+from robot_srvs.srv import *
from robot_mechanism_controllers.srv import *
def slurp(filename):
Modified: pkg/trunk/manip/teleop_robot/src/teleop_robot/robot_interface.py
===================================================================
--- pkg/trunk/manip/teleop_robot/src/teleop_robot/robot_interface.py 2008-10-14 00:48:39 UTC (rev 5314)
+++ pkg/trunk/manip/teleop_robot/src/teleop_robot/robot_interface.py 2008-10-14 01:08:32 UTC (rev 5315)
@@ -39,8 +39,8 @@
import rostools
import sys
import rospy
-rostools.update_path('mechanism_control')
-from mechanism_control.srv import *
+rostools.update_path('robot_srvs')
+from robot_srvs.srv import *
#TODO: add a controller factory mechanism
@@ -126,4 +126,4 @@
self._channel._previous_time = None
def calibrateArm(self):
- pass
\ No newline at end of file
+ pass
Modified: pkg/trunk/mechanism/mechanism_bringup/manifest.xml
===================================================================
--- pkg/trunk/mechanism/mechanism_bringup/manifest.xml 2008-10-14 00:48:39 UTC (rev 5314)
+++ pkg/trunk/mechanism/mechanism_bringup/manifest.xml 2008-10-14 01:08:32 UTC (rev 5315)
@@ -4,5 +4,5 @@
<license>BSD</license>
<depend package="robot_mechanism_controllers"/>
<depend package="pr2_mechanism_controllers"/>
- <depend package="mechanism_control"/>
+ <depend package="robot_srvs"/>
</package>
Modified: pkg/trunk/mechanism/mechanism_bringup/scripts/calibrate_base.py
===================================================================
--- pkg/trunk/mechanism/mechanism_bringup/scripts/calibrate_base.py 2008-10-14 00:48:39 UTC (rev 5314)
+++ pkg/trunk/mechanism/mechanism_bringup/scripts/calibrate_base.py 2008-10-14 01:08:32 UTC (rev 5315)
@@ -41,7 +41,7 @@
# Loads interface with the robot.
rostools.update_path('teleop_robot')
import rospy
-from mechanism_control.srv import *
+from robot_srvs.srv import *
from robot_mechanism_controllers.srv import *
def slurp(filename):
Modified: pkg/trunk/mechanism/mechanism_bringup/scripts/calibrate_head.py
===================================================================
--- pkg/trunk/mechanism/mechanism_bringup/scripts/calibrate_head.py 2008-10-14 00:48:39 UTC (rev 5314)
+++ pkg/trunk/mechanism/mechanism_bringup/scripts/calibrate_head.py 2008-10-14 01:08:32 UTC (rev 5315)
@@ -41,7 +41,7 @@
# Loads interface with the robot.
rostools.update_path('teleop_robot')
import rospy
-from mechanism_control.srv import *
+from robot_srvs.srv import *
from robot_mechanism_controllers.srv import *
def slurp(filename):
Modified: pkg/trunk/mechanism/mechanism_bringup/scripts/calibrate_tilt_laser.py
===================================================================
--- pkg/trunk/mechanism/mechanism_bringup/scripts/calibrate_tilt_laser.py 2008-10-14 00:48:39 UTC (rev 5314)
+++ pkg/trunk/mechanism/mechanism_bringup/scripts/calibrate_tilt_laser.py 2008-10-14 01:08:32 UTC (rev 5315)
@@ -41,7 +41,7 @@
# Loads interface with the robot.
rostools.update_path('teleop_robot')
import rospy
-from mechanism_control.srv import *
+from robot_srvs.srv import *
from robot_mechanism_controllers.srv import *
def slurp(filename):
Modified: pkg/trunk/motion_planning/planning_node_util/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/planning_node_util/manifest.xml 2008-10-14 00:48:39 UTC (rev 5314)
+++ pkg/trunk/motion_planning/planning_node_util/manifest.xml 2008-10-14 01:08:32 UTC (rev 5315)
@@ -14,7 +14,6 @@
<depend package="planning_models"/>
<depend package="collision_space"/>
- <depend package="mechanism_control"/>
<export>
<cpp cflags="-I${prefix}/include" lflags=""/>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rob...@us...> - 2008-10-14 01:22:10
|
Revision: 5314
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5314&view=rev
Author: rob_wheeler
Date: 2008-10-14 00:48:39 +0000 (Tue, 14 Oct 2008)
Log Message:
-----------
Move mechanism_control msgs and srvs to robot_msgs and robot_srvs.
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_dynamics_controller.cpp
pkg/trunk/highlevel/highlevel_controllers/src/ControlCli.cpp
pkg/trunk/highlevel/highlevel_controllers/src/MoveArm.cpp
pkg/trunk/highlevel/highlevel_controllers/src/MoveEndEffector.cpp
pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
pkg/trunk/mechanism/mechanism_control/manifest.xml
pkg/trunk/mechanism/mechanism_control/scripts/detect.py
pkg/trunk/mechanism/mechanism_control/scripts/listen.py
pkg/trunk/mechanism/mechanism_control/scripts/writer.py
pkg/trunk/mechanism/mechanism_control/src/mechanism_control/mechanism.py
pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
pkg/trunk/mechanism/mechanism_control/test/ms_publisher_test.cpp
pkg/trunk/mechanism/mechanism_control_rt/manifest.xml
pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h
pkg/trunk/pr2_msgs/manifest.xml
pkg/trunk/pr2_msgs/msg/MoveArmGoal.msg
pkg/trunk/pr2_msgs/msg/MoveArmState.msg
pkg/trunk/pr2_msgs/msg/MoveEndEffectorGoal.msg
pkg/trunk/pr2_msgs/msg/MoveEndEffectorState.msg
Added Paths:
-----------
pkg/trunk/robot_msgs/msg/ActuatorState.msg
pkg/trunk/robot_msgs/msg/JointState.msg
pkg/trunk/robot_msgs/msg/MechanismState.msg
pkg/trunk/robot_srvs/srv/KillController.srv
pkg/trunk/robot_srvs/srv/ListControllerTypes.srv
pkg/trunk/robot_srvs/srv/ListControllers.srv
pkg/trunk/robot_srvs/srv/SpawnController.srv
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_dynamics_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_dynamics_controller.cpp 2008-10-14 00:45:55 UTC (rev 5313)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/test/test_arm_dynamics_controller.cpp 2008-10-14 00:48:39 UTC (rev 5314)
@@ -54,7 +54,7 @@
~test_run_arm(){};
- mechanism_control::MechanismState state;
+ robot_msgs::MechanismState state;
void jointMsgReceived()
{
Modified: pkg/trunk/highlevel/highlevel_controllers/src/ControlCli.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/ControlCli.cpp 2008-10-14 00:45:55 UTC (rev 5313)
+++ pkg/trunk/highlevel/highlevel_controllers/src/ControlCli.cpp 2008-10-14 00:48:39 UTC (rev 5314)
@@ -37,7 +37,7 @@
#include <ros/node.h>
#include <pr2_msgs/MoveArmGoal.h>
-#include <mechanism_control/MechanismState.h>
+#include <robot_msgs/MechanismState.h>
#include <highlevel_controllers/PlugInGoal.h>
#include <highlevel_controllers/PlugInState.h>
#include <robot_srvs/PlanNames.h>
@@ -55,7 +55,7 @@
advertise<pr2_msgs::MoveArmGoal>("right_arm_goal", 1);
advertise<highlevel_controllers::PlugInGoal>("plugin_goal", 1);
advertise<robot_msgs::BatteryState>("battery_state", 1);
- advertise<mechanism_control::MechanismState>("mechanism_state", 1);
+ advertise<robot_msgs::MechanismState>("mechanism_state", 1);
runCLI();
}
bool alive() { return !dead; }
@@ -134,7 +134,7 @@
} else if (c == 'I') {
- mechanism_control::MechanismState mechanismState;
+ robot_msgs::MechanismState mechanismState;
std::vector<std::string> names;
fillNamesLeftArm(names);
@@ -146,7 +146,7 @@
mechanismState.joint_states[i].name = names[i];
}
printf("Publishing states.\n");
- publish<mechanism_control::MechanismState>("mechanism_state", mechanismState);
+ publish<robot_msgs::MechanismState>("mechanism_state", mechanismState);
} else if (c == 'S') {
std::vector<std::string> names;
Modified: pkg/trunk/highlevel/highlevel_controllers/src/MoveArm.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/MoveArm.cpp 2008-10-14 00:45:55 UTC (rev 5313)
+++ pkg/trunk/highlevel/highlevel_controllers/src/MoveArm.cpp 2008-10-14 00:48:39 UTC (rev 5314)
@@ -57,7 +57,7 @@
* @section topic ROS topics
*
* Subscribes to (name/type):
- * - @b "mechanism_state"/mechanism_control::MechanismState : The state of the robot joints and actuators
+ * - @b "mechanism_state"/robot_msgs::MechanismState : The state of the robot joints and actuators
* - @b "right_arm_goal"/pr2_msgs::MoveArmGoal : The new goal containing a setpoint to achieve for the joint angles
* - @b "left_arm_goal"/pr2_msgs::MoveArmGoal : The new goal containing a setpoint to achieve for the joint angles
*
@@ -74,7 +74,7 @@
#include <HighlevelController.hh>
#include <pr2_mechanism_controllers/JointPosCmd.h>
-#include <mechanism_control/MechanismState.h>
+#include <robot_msgs/MechanismState.h>
#include <pr2_msgs/MoveArmState.h>
#include <pr2_msgs/MoveArmGoal.h>
#include <robot_msgs/DisplayKinematicPath.h>
@@ -103,7 +103,7 @@
* @brief Helper Method to obtain the joint value by name
* @return true if the joint is present, otherwise false
*/
- bool readJointValue(const mechanism_control::MechanismState& mechanismStateMsg, const std::string& name, double& value);
+ bool readJointValue(const robot_msgs::MechanismState& mechanismStateMsg, const std::string& name, double& value);
void handleArmConfigurationCallback();
void updateGoalMsg();
@@ -117,7 +117,7 @@
const std::string armCmdTopic;
const std::string kinematicModel;
- mechanism_control::MechanismState mechanismState;
+ robot_msgs::MechanismState mechanismState;
robot_srvs::NamedKinematicPlanState::response plan;
unsigned int currentWaypoint; /*!< The waypoint in the plan that we are targetting */
rosTFClient tf_; /**< Used to do transforms */
@@ -127,7 +127,7 @@
};
-bool MoveArm::readJointValue(const mechanism_control::MechanismState& mechanismStateMsg, const std::string& name, double& value){
+bool MoveArm::readJointValue(const robot_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/highlevel/highlevel_controllers/src/MoveEndEffector.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/MoveEndEffector.cpp 2008-10-14 00:45:55 UTC (rev 5313)
+++ pkg/trunk/highlevel/highlevel_controllers/src/MoveEndEffector.cpp 2008-10-14 00:48:39 UTC (rev 5314)
@@ -57,7 +57,7 @@
* @section topic ROS topics
*
* Subscribes to (name/type):
- * - @b "mechanism_state"/mechanism_control::MechanismState : The state of the robot joints and actuators
+ * - @b "mechanism_state"/robot_msgs::MechanismState : The state of the robot joints and actuators
* - @b "left/right_end_effector_goal"/pr2_msgs::MoveEndEffectorGoal: Where to move end effector
*
* Publishes to (name / type):
@@ -72,7 +72,7 @@
#include <HighlevelController.hh>
#include <pr2_mechanism_controllers/JointPosCmd.h>
-#include <mechanism_control/MechanismState.h>
+#include <robot_msgs/MechanismState.h>
#include <pr2_msgs/MoveEndEffectorState.h>
#include <pr2_msgs/MoveEndEffectorGoal.h>
#include <robot_srvs/KinematicPlanState.h>
@@ -98,7 +98,7 @@
* @brief Helper Method to obtain the joint value by name
* @return true if the joint is present, otherwise false
*/
- bool readJointValue(const mechanism_control::MechanismState& mechanismStateMsg, const std::string& name, double& value);
+ bool readJointValue(const robot_msgs::MechanismState& mechanismStateMsg, const std::string& name, double& value);
void handleArmConfigurationCallback();
void updateGoalMsg();
@@ -112,7 +112,7 @@
const std::string armCmdTopic;
const std::string kinematicModel;
- mechanism_control::MechanismState mechanismState;
+ robot_msgs::MechanismState mechanismState;
robot_srvs::KinematicPlanState::response plan;
unsigned int currentWaypoint; /*!< The waypoint in the plan that we are targetting */
@@ -121,7 +121,7 @@
};
-bool MoveEndEffector::readJointValue(const mechanism_control::MechanismState& mechanismStateMsg, const std::string& name, double& value){
+bool MoveEndEffector::readJointValue(const robot_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_control/include/mechanism_control/mechanism_control.h
===================================================================
--- pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2008-10-14 00:45:55 UTC (rev 5313)
+++ pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2008-10-14 00:48:39 UTC (rev 5314)
@@ -45,11 +45,11 @@
#include <mechanism_model/controller.h>
#include <misc_utils/realtime_publisher.h>
-#include "mechanism_control/ListControllerTypes.h"
-#include "mechanism_control/ListControllers.h"
-#include "mechanism_control/SpawnController.h"
-#include "mechanism_control/KillController.h"
-#include "mechanism_control/MechanismState.h"
+#include <robot_srvs/ListControllerTypes.h>
+#include <robot_srvs/ListControllers.h>
+#include <robot_srvs/SpawnController.h>
+#include <robot_srvs/KillController.h>
+#include <robot_msgs/MechanismState.h>
#include "rosTF/TransformArray.h"
@@ -106,25 +106,25 @@
void update(); // Must be realtime safe
- bool listControllerTypes(mechanism_control::ListControllerTypes::request &req,
- mechanism_control::ListControllerTypes::response &resp);
- bool listControllers(mechanism_control::ListControllers::request &req,
- mechanism_control::ListControllers::response &resp);
- bool spawnController(mechanism_control::SpawnController::request &req,
- mechanism_control::SpawnController::response &resp);
+ bool listControllerTypes(robot_srvs::ListControllerTypes::request &req,
+ robot_srvs::ListControllerTypes::response &resp);
+ bool listControllers(robot_srvs::ListControllers::request &req,
+ robot_srvs::ListControllers::response &resp);
+ bool spawnController(robot_srvs::SpawnController::request &req,
+ robot_srvs::SpawnController::response &resp);
private:
ros::node *node_;
- bool killController(mechanism_control::KillController::request &req,
- mechanism_control::KillController::response &resp);
+ bool killController(robot_srvs::KillController::request &req,
+ robot_srvs::KillController::response &resp);
MechanismControl *mc_;
static const double STATE_PUBLISHING_PERIOD = 0.01; // this translates to about 100Hz
const char* const mechanism_state_topic_;
- misc_utils::RealtimePublisher<mechanism_control::MechanismState> publisher_;
+ misc_utils::RealtimePublisher<robot_msgs::MechanismState> publisher_;
misc_utils::RealtimePublisher<rosTF::TransformArray> transform_publisher_;
};
Modified: pkg/trunk/mechanism/mechanism_control/manifest.xml
===================================================================
--- pkg/trunk/mechanism/mechanism_control/manifest.xml 2008-10-14 00:45:55 UTC (rev 5313)
+++ pkg/trunk/mechanism/mechanism_control/manifest.xml 2008-10-14 00:48:39 UTC (rev 5314)
@@ -12,8 +12,10 @@
<depend package="misc_utils" />
<depend package="rospy" />
<depend package="tf" />
+<depend package="robot_msgs" />
+<depend package="robot_srvs" />
<export>
- <cpp cflags='-I${prefix}/include -I${prefix}/msg/cpp -I${prefix}/srv/cpp'
+ <cpp cflags='-I${prefix}/include'
lflags='-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lmechanism_control'/>
</export>
</package>
Modified: pkg/trunk/mechanism/mechanism_control/scripts/detect.py
===================================================================
--- pkg/trunk/mechanism/mechanism_control/scripts/detect.py 2008-10-14 00:45:55 UTC (rev 5313)
+++ pkg/trunk/mechanism/mechanism_control/scripts/detect.py 2008-10-14 00:48:39 UTC (rev 5314)
@@ -2,10 +2,10 @@
import curses
import rostools
-rostools.update_path('mechanism_control')
+rostools.update_path('robot_msgs')
import sys, traceback, logging, rospy
-from mechanism_control.msg import MechanismState
+from robot_msgs.msg import MechanismState
NAME = 'joint_listener'
Modified: pkg/trunk/mechanism/mechanism_control/scripts/listen.py
===================================================================
--- pkg/trunk/mechanism/mechanism_control/scripts/listen.py 2008-10-14 00:45:55 UTC (rev 5313)
+++ pkg/trunk/mechanism/mechanism_control/scripts/listen.py 2008-10-14 00:48:39 UTC (rev 5314)
@@ -1,10 +1,10 @@
#!/usr/bin/env python
import rostools
-rostools.update_path('mechanism_control')
+rostools.update_path('robot_msgs')
import sys, traceback, logging, rospy
-from mechanism_control.msg import MechanismState
+from robot_msgs.msg import MechanismState
NAME = 'joint_listener'
Modified: pkg/trunk/mechanism/mechanism_control/scripts/writer.py
===================================================================
--- pkg/trunk/mechanism/mechanism_control/scripts/writer.py 2008-10-14 00:45:55 UTC (rev 5313)
+++ pkg/trunk/mechanism/mechanism_control/scripts/writer.py 2008-10-14 00:48:39 UTC (rev 5314)
@@ -1,10 +1,10 @@
#!/usr/bin/env python
import rostools
-rostools.update_path('mechanism_control')
+rostools.update_path('robot_msgs')
import sys, traceback, logging, rospy
-from mechanism_control.msg import MechanismState
+from robot_msgs.msg import MechanismState
print "File name is: %s" %sys.argv[1]
filehandle = open(sys.argv[1],'w')
Modified: pkg/trunk/mechanism/mechanism_control/src/mechanism_control/mechanism.py
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/mechanism_control/mechanism.py 2008-10-14 00:45:55 UTC (rev 5313)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control/mechanism.py 2008-10-14 00:48:39 UTC (rev 5314)
@@ -2,11 +2,11 @@
# Wrappers around the services provided by MechanismControlNode
import rostools
-rostools.update_path('mechanism_control')
+rostools.update_path('robot_srvs')
rostools.update_path('std_srvs')
import rospy, sys
-from mechanism_control.srv import *
+from robot_srvs.srv import *
import std_srvs.srv
def list_controller_types():
Modified: pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-10-14 00:45:55 UTC (rev 5313)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-10-14 00:48:39 UTC (rev 5314)
@@ -206,7 +206,7 @@
node_->advertise_service("kill_controller", &MechanismControlNode::killController, this);
// Advertise topics
- node_->advertise<mechanism_control::MechanismState>(mechanism_state_topic_,10);
+ node_->advertise<robot_msgs::MechanismState>(mechanism_state_topic_,10);
node_->advertise<rosTF::TransformArray>("TransformArray");
node_->advertise<rostools::Time>("time");
}
@@ -246,7 +246,7 @@
assert(mc_->model_.joints_.size() == publisher_.msg_.get_joint_states_size());
for (unsigned int i = 0; i < mc_->model_.joints_.size(); ++i)
{
- mechanism_control::JointState *out = &publisher_.msg_.joint_states[i];
+ robot_msgs::JointState *out = &publisher_.msg_.joint_states[i];
mechanism::JointState *in = &mc_->state_->joint_states_[i];
out->name = mc_->model_.joints_[i]->name_;
out->position = in->position_;
@@ -257,7 +257,7 @@
for (unsigned int i = 0; i < mc_->hw_->actuators_.size(); ++i)
{
- mechanism_control::ActuatorState *out = &publisher_.msg_.actuator_states[i];
+ robot_msgs::ActuatorState *out = &publisher_.msg_.actuator_states[i];
ActuatorState *in = &mc_->hw_->actuators_[i]->state_;
out->name = mc_->hw_->actuators_[i]->name_;
out->encoder_count = in->encoder_count_;
@@ -318,8 +318,8 @@
}
bool MechanismControlNode::listControllerTypes(
- mechanism_control::ListControllerTypes::request &req,
- mechanism_control::ListControllerTypes::response &resp)
+ robot_srvs::ListControllerTypes::request &req,
+ robot_srvs::ListControllerTypes::response &resp)
{
std::vector<std::string> types;
@@ -330,8 +330,8 @@
}
bool MechanismControlNode::spawnController(
- mechanism_control::SpawnController::request &req,
- mechanism_control::SpawnController::response &resp)
+ robot_srvs::SpawnController::request &req,
+ robot_srvs::SpawnController::response &resp)
{
TiXmlDocument doc;
doc.Parse(req.xml_config.c_str());
@@ -375,8 +375,8 @@
}
bool MechanismControlNode::listControllers(
- mechanism_control::ListControllers::request &req,
- mechanism_control::ListControllers::response &resp)
+ robot_srvs::ListControllers::request &req,
+ robot_srvs::ListControllers::response &resp)
{
std::vector<std::string> controllers;
@@ -387,8 +387,8 @@
}
bool MechanismControlNode::killController(
- mechanism_control::KillController::request &req,
- mechanism_control::KillController::response &resp)
+ robot_srvs::KillController::request &req,
+ robot_srvs::KillController::response &resp)
{
resp.ok = mc_->killController(req.name);
return true;
Modified: pkg/trunk/mechanism/mechanism_control/test/ms_publisher_test.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/test/ms_publisher_test.cpp 2008-10-14 00:45:55 UTC (rev 5313)
+++ pkg/trunk/mechanism/mechanism_control/test/ms_publisher_test.cpp 2008-10-14 00:48:39 UTC (rev 5314)
@@ -26,13 +26,13 @@
//////////////////////////////////////////////////////////////////////////////
#include "ros/node.h"
-#include "mechanism_control/MechanismState.h"
-#include "mechanism_control/ActuatorState.h"
-#include "mechanism_control/JointState.h"
+#include "robot_msgs/MechanismState.h"
+#include "robot_msgs/ActuatorState.h"
+#include "robot_msgs/JointState.h"
void publish(ros::node *node)
{
- mechanism_control::MechanismState mechanism_state;
+ robot_msgs::MechanismState mechanism_state;
mechanism_state.set_joint_states_size(250);
mechanism_state.set_actuator_states_size(260);
@@ -41,7 +41,7 @@
{
for (unsigned int i = 0; i < mechanism_state.get_joint_states_size(); ++i)
{
- mechanism_control::JointState *out = mechanism_state.joint_states + i;
+ robot_msgs::JointState *out = mechanism_state.joint_states + i;
out->name = "jointstate";
out->position = 1.0;
out->velocity = 1.0;
@@ -51,7 +51,7 @@
for (unsigned int i = 0; i < mechanism_state.get_actuator_states_size(); ++i)
{
- mechanism_control::ActuatorState *out = mechanism_state.actuator_states + i;
+ robot_msgs::ActuatorState *out = mechanism_state.actuator_states + i;
out->name = "actuatorstate";
out->encoder_count = i;
out->position = 1.0;
@@ -81,7 +81,7 @@
ros::init(argc, argv);
ros::node *node = new ros::node("mechanism_control");
- node->advertise<mechanism_control::MechanismState>("mechanism_state");
+ node->advertise<robot_msgs::MechanismState>("mechanism_state");
while (1) {
std::cout << "publish" << std::endl;
publish(node);
Modified: pkg/trunk/mechanism/mechanism_control_rt/manifest.xml
===================================================================
--- pkg/trunk/mechanism/mechanism_control_rt/manifest.xml 2008-10-14 00:45:55 UTC (rev 5313)
+++ pkg/trunk/mechanism/mechanism_control_rt/manifest.xml 2008-10-14 00:48:39 UTC (rev 5314)
@@ -11,10 +11,12 @@
<depend package="mechanism_model"/>
<depend package="misc_utils" />
<depend package="rospy" />
-<depend package="rosTF" />
+<depend package="tf" />
+<depend package="robot_msgs" />
+<depend package="robot_srvs" />
<depend package="xenomai" />
<export>
- <cpp cflags='-I${prefix}/include -I${prefix}/msg/cpp -I${prefix}/srv/cpp'
+ <cpp cflags='-I${prefix}/include'
lflags='-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lmechanism_control_rt'/>
</export>
</package>
Modified: pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h
===================================================================
--- pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h 2008-10-14 00:45:55 UTC (rev 5313)
+++ pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h 2008-10-14 00:48:39 UTC (rev 5314)
@@ -51,7 +51,7 @@
#include <rosTF/rosTF.h>
#include <cmath>
-#include <mechanism_control/MechanismState.h>
+#include <robot_msgs/MechanismState.h>
/** Main namespace */
namespace planning_node_util
@@ -297,7 +297,7 @@
std_msgs::RobotBase2DOdom m_localizedPose;
bool m_haveBasePos;
- mechanism_control::MechanismState m_mechanismState; // this message should be moved to robot_msgs
+ robot_msgs::MechanismState m_mechanismState; // this message should be moved to robot_msgs
bool m_haveMechanismState;
robot_desc::URDF *m_urdf;
Modified: pkg/trunk/pr2_msgs/manifest.xml
===================================================================
--- pkg/trunk/pr2_msgs/manifest.xml 2008-10-14 00:45:55 UTC (rev 5313)
+++ pkg/trunk/pr2_msgs/manifest.xml 2008-10-14 00:48:39 UTC (rev 5314)
@@ -10,7 +10,6 @@
<logo>http://www.willowgarage.com/files/willowgarage/robot10.jpg</logo>
<depend package="robot_msgs"/>
<depend package="std_msgs"/>
- <depend package="mechanism_control"/>
<export>
<cpp cflags="-I${prefix}/msg/cpp"/>
</export>
Modified: pkg/trunk/pr2_msgs/msg/MoveArmGoal.msg
===================================================================
--- pkg/trunk/pr2_msgs/msg/MoveArmGoal.msg 2008-10-14 00:45:55 UTC (rev 5313)
+++ pkg/trunk/pr2_msgs/msg/MoveArmGoal.msg 2008-10-14 00:48:39 UTC (rev 5314)
@@ -1,6 +1,6 @@
Header header
# The desired joint configuratin
-mechanism_control/JointState[] configuration
+robot_msgs/JointState[] configuration
# Indicate if the goal is being enabled or disabled
byte enable
Modified: pkg/trunk/pr2_msgs/msg/MoveArmState.msg
===================================================================
--- pkg/trunk/pr2_msgs/msg/MoveArmState.msg 2008-10-14 00:45:55 UTC (rev 5313)
+++ pkg/trunk/pr2_msgs/msg/MoveArmState.msg 2008-10-14 00:48:39 UTC (rev 5314)
@@ -6,6 +6,6 @@
#Did we actually successfully arrive at the goal?
byte done
#Current arm configuration
-mechanism_control/JointState[] configuration
+robot_msgs/JointState[] configuration
#Goal arm configuration
-mechanism_control/JointState[] goal
+robot_msgs/JointState[] goal
Modified: pkg/trunk/pr2_msgs/msg/MoveEndEffectorGoal.msg
===================================================================
--- pkg/trunk/pr2_msgs/msg/MoveEndEffectorGoal.msg 2008-10-14 00:45:55 UTC (rev 5313)
+++ pkg/trunk/pr2_msgs/msg/MoveEndEffectorGoal.msg 2008-10-14 00:48:39 UTC (rev 5314)
@@ -1,6 +1,6 @@
Header header
# The desired joint configuratin
-mechanism_control/JointState[] configuration
+robot_msgs/JointState[] configuration
# Indicate if the goal is being enabled or disabled
byte enable
Modified: pkg/trunk/pr2_msgs/msg/MoveEndEffectorState.msg
===================================================================
--- pkg/trunk/pr2_msgs/msg/MoveEndEffectorState.msg 2008-10-14 00:45:55 UTC (rev 5313)
+++ pkg/trunk/pr2_msgs/msg/MoveEndEffectorState.msg 2008-10-14 00:48:39 UTC (rev 5314)
@@ -6,6 +6,6 @@
#Did we actually successfully arrive at the goal?
byte done
#Current arm configuration
-mechanism_control/JointState[] configuration
+robot_msgs/JointState[] configuration
#Goal arm configuration
-mechanism_control/JointState[] goal
+robot_msgs/JointState[] goal
Copied: pkg/trunk/robot_msgs/msg/ActuatorState.msg (from rev 5297, pkg/trunk/mechanism/mechanism_control/msg/ActuatorState.msg)
===================================================================
--- pkg/trunk/robot_msgs/msg/ActuatorState.msg (rev 0)
+++ pkg/trunk/robot_msgs/msg/ActuatorState.msg 2008-10-14 00:48:39 UTC (rev 5314)
@@ -0,0 +1,20 @@
+string name
+int32 encoder_count
+float64 position
+float64 timestamp
+float64 encoder_velocity
+float64 velocity
+byte calibration_reading
+float64 last_calibration_high_transition
+float64 last_calibration_low_transition
+byte is_enabled
+byte run_stop_hit
+float64 last_requested_current
+float64 last_commanded_current
+float64 last_measured_current
+float64 last_requested_effort
+float64 last_commanded_effort
+float64 last_measured_effort
+float64 motor_voltage
+int32 num_encoder_errors
+
Copied: pkg/trunk/robot_msgs/msg/JointState.msg (from rev 5297, pkg/trunk/mechanism/mechanism_control/msg/JointState.msg)
===================================================================
--- pkg/trunk/robot_msgs/msg/JointState.msg (rev 0)
+++ pkg/trunk/robot_msgs/msg/JointState.msg 2008-10-14 00:48:39 UTC (rev 5314)
@@ -0,0 +1,5 @@
+string name
+float64 position
+float64 velocity
+float64 applied_effort
+float64 commanded_effort
Copied: pkg/trunk/robot_msgs/msg/MechanismState.msg (from rev 5297, pkg/trunk/mechanism/mechanism_control/msg/MechanismState.msg)
===================================================================
--- pkg/trunk/robot_msgs/msg/MechanismState.msg (rev 0)
+++ pkg/trunk/robot_msgs/msg/MechanismState.msg 2008-10-14 00:48:39 UTC (rev 5314)
@@ -0,0 +1,4 @@
+Header header
+float64 time
+ActuatorState[] actuator_states
+JointState[] joint_states
Copied: pkg/trunk/robot_srvs/srv/KillController.srv (from rev 5297, pkg/trunk/mechanism/mechanism_control/srv/KillController.srv)
===================================================================
--- pkg/trunk/robot_srvs/srv/KillController.srv (rev 0)
+++ pkg/trunk/robot_srvs/srv/KillController.srv 2008-10-14 00:48:39 UTC (rev 5314)
@@ -0,0 +1,3 @@
+string name
+---
+byte ok
Copied: pkg/trunk/robot_srvs/srv/ListControllerTypes.srv (from rev 5297, pkg/trunk/mechanism/mechanism_control/srv/ListControllerTypes.srv)
===================================================================
--- pkg/trunk/robot_srvs/srv/ListControllerTypes.srv (rev 0)
+++ pkg/trunk/robot_srvs/srv/ListControllerTypes.srv 2008-10-14 00:48:39 UTC (rev 5314)
@@ -0,0 +1,2 @@
+---
+string[] types
\ No newline at end of file
Copied: pkg/trunk/robot_srvs/srv/ListControllers.srv (from rev 5297, pkg/trunk/mechanism/mechanism_control/srv/ListControllers.srv)
===================================================================
--- pkg/trunk/robot_srvs/srv/ListControllers.srv (rev 0)
+++ pkg/trunk/robot_srvs/srv/ListControllers.srv 2008-10-14 00:48:39 UTC (rev 5314)
@@ -0,0 +1,2 @@
+---
+string[] controllers
Copied: pkg/trunk/robot_srvs/srv/SpawnController.srv (from rev 5297, pkg/trunk/mechanism/mechanism_control/srv/SpawnController.srv)
===================================================================
--- pkg/trunk/robot_srvs/srv/SpawnController.srv (rev 0)
+++ pkg/trunk/robot_srvs/srv/SpawnController.srv 2008-10-14 00:48:39 UTC (rev 5314)
@@ -0,0 +1,4 @@
+string xml_config
+---
+byte[] ok
+string[] name
\ No newline at end of file
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rob...@us...> - 2008-10-14 17:11:40
|
Revision: 5338
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5338&view=rev
Author: rob_wheeler
Date: 2008-10-14 17:11:32 +0000 (Tue, 14 Oct 2008)
Log Message:
-----------
Remove mechanism_control dependency from ethercat_hardware
Modified Paths:
--------------
pkg/trunk/drivers/motor/ethercat_hardware/manifest.xml
pkg/trunk/mechanism/mechanism_control_rt/manifest.xml
Modified: pkg/trunk/drivers/motor/ethercat_hardware/manifest.xml
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/manifest.xml 2008-10-14 15:43:07 UTC (rev 5337)
+++ pkg/trunk/drivers/motor/ethercat_hardware/manifest.xml 2008-10-14 17:11:32 UTC (rev 5338)
@@ -9,7 +9,7 @@
<depend package='eml'/>
<depend package='tinyxml'/>
<depend package='roscpp' />
-<depend package='mechanism_control' />
+<depend package='mechanism_control_rt' />
<depend package='loki' />
<depend package='misc_utils' />
<depend package='robot_msgs' />
Modified: pkg/trunk/mechanism/mechanism_control_rt/manifest.xml
===================================================================
--- pkg/trunk/mechanism/mechanism_control_rt/manifest.xml 2008-10-14 15:43:07 UTC (rev 5337)
+++ pkg/trunk/mechanism/mechanism_control_rt/manifest.xml 2008-10-14 17:11:32 UTC (rev 5338)
@@ -16,7 +16,7 @@
<depend package="robot_srvs" />
<depend package="xenomai" />
<export>
- <cpp cflags='-I${prefix}/include'
+ <cpp cflags='-I${prefix}/../mechanism_control/include'
lflags='-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lmechanism_control_rt'/>
</export>
</package>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-10-14 21:43:30
|
Revision: 5364
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5364&view=rev
Author: gerkey
Date: 2008-10-14 21:43:21 +0000 (Tue, 14 Oct 2008)
Log Message:
-----------
Moved known failing test to the test-future target, to allow the mainline
test suite to pass. "future" tests will be migrated back to mainline when
they pass.
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt
pkg/trunk/motion_planning/sbpl/CMakeLists.txt
pkg/trunk/simulators/rosstage/CMakeLists.txt
pkg/trunk/util/tf/CMakeLists.txt
pkg/trunk/util/tf/test/tf_unittest.cpp
Added Paths:
-----------
pkg/trunk/util/tf/test/tf_unittest_future.cpp
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt 2008-10-14 21:34:00 UTC (rev 5363)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt 2008-10-14 21:43:21 UTC (rev 5364)
@@ -24,7 +24,7 @@
#rospack_add_executable(player_pr2 src/player/Pr2_Player.cc)
#target_link_libraries(player_pr2 playerc++)
-rospack_add_rostest(test/testslide.xml)
-rospack_add_rostest(test/testcameras.xml)
-rospack_add_rostest(test/testscan.xml)
+rospack_add_rostest_future(test/testslide.xml)
+rospack_add_rostest_future(test/testcameras.xml)
+rospack_add_rostest_future(test/testscan.xml)
Modified: pkg/trunk/motion_planning/sbpl/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/sbpl/CMakeLists.txt 2008-10-14 21:34:00 UTC (rev 5363)
+++ pkg/trunk/motion_planning/sbpl/CMakeLists.txt 2008-10-14 21:43:21 UTC (rev 5364)
@@ -21,7 +21,7 @@
target_link_libraries(test_sbpl sbpl)
# Test target for module tests to be included in gtest regression test harness
-rospack_add_gtest(utest src/test/module-tests.cpp)
+rospack_add_gtest_future(utest src/test/module-tests.cpp)
target_link_libraries(utest sbpl)
# Send output of tests to the test directory
Modified: pkg/trunk/simulators/rosstage/CMakeLists.txt
===================================================================
--- pkg/trunk/simulators/rosstage/CMakeLists.txt 2008-10-14 21:34:00 UTC (rev 5363)
+++ pkg/trunk/simulators/rosstage/CMakeLists.txt 2008-10-14 21:43:21 UTC (rev 5364)
@@ -3,4 +3,4 @@
rospack(rosstage)
rospack_add_executable(rosstage rosstage.cc)
-rospack_add_rostest(test/hztest.xml)
+rospack_add_rostest_future(test/hztest.xml)
Modified: pkg/trunk/util/tf/CMakeLists.txt
===================================================================
--- pkg/trunk/util/tf/CMakeLists.txt 2008-10-14 21:34:00 UTC (rev 5363)
+++ pkg/trunk/util/tf/CMakeLists.txt 2008-10-14 21:43:21 UTC (rev 5364)
@@ -20,6 +20,8 @@
#target_link_libraries(pose3d_unittest TF)
rospack_add_gtest(tf_unittest test/tf_unittest.cpp)
target_link_libraries(tf_unittest tf)
+rospack_add_gtest_future(tf_unittest_future test/tf_unittest_future.cpp)
+target_link_libraries(tf_unittest_future tf)
rospack_add_gtest(cache_unittest test/cache_unittest.cpp)
target_link_libraries(cache_unittest tf)
rospack_add_gtest(bullet_unittest test/bullet_unittest.cpp)
Modified: pkg/trunk/util/tf/test/tf_unittest.cpp
===================================================================
--- pkg/trunk/util/tf/test/tf_unittest.cpp 2008-10-14 21:34:00 UTC (rev 5363)
+++ pkg/trunk/util/tf/test/tf_unittest.cpp 2008-10-14 21:43:21 UTC (rev 5364)
@@ -29,6 +29,8 @@
using namespace tf;
+// Moved to tf_unittest_future.cpp
+/*
TEST(tf, ListOneForward)
{
unsigned int runs = 400;
@@ -117,6 +119,7 @@
}
}
+*/
Added: pkg/trunk/util/tf/test/tf_unittest_future.cpp
===================================================================
--- pkg/trunk/util/tf/test/tf_unittest_future.cpp (rev 0)
+++ pkg/trunk/util/tf/test/tf_unittest_future.cpp 2008-10-14 21:43:21 UTC (rev 5364)
@@ -0,0 +1,116 @@
+#include <gtest/gtest.h>
+#include <tf/tf.h>
+#include <math_utils/angles.h>
+#include <sys/time.h>
+
+#include "LinearMath/btVector3.h"
+
+
+void seed_rand()
+{
+ //Seed random number generator with current microseond count
+ timeval temp_time_struct;
+ gettimeofday(&temp_time_struct,NULL);
+ srand(temp_time_struct.tv_usec);
+};
+
+using namespace tf;
+
+
+TEST(tf, ListOneForward)
+{
+ unsigned int runs = 400;
+ double epsilon = 1e-6;
+ seed_rand();
+
+ tf::Transformer mTR(true);
+ std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
+ for ( unsigned int i = 0; i < runs ; i++ )
+ {
+ xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
+ yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
+ zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
+
+ Stamped<btTransform> tranStamped(btTransform(btQuaternion(0,0,0), btVector3(xvalues[i],yvalues[i],zvalues[i])), 10 + i, "child", "my_parent");
+ mTR.setTransform(tranStamped);
+ }
+
+ std::cout << mTR.allFramesAsString() << std::endl;
+ // std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl;
+
+ for ( unsigned int i = 0; i < runs ; i++ )
+
+ {
+ Stamped<btTransform> inpose (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), 10 + i, "child");
+
+ try{
+ Stamped<Pose> outpose;
+ outpose.data_.setIdentity(); //to make sure things are getting mutated
+ mTR.transformPose("my_parent",inpose, outpose);
+ EXPECT_NEAR(outpose.data_.getOrigin().x(), xvalues[i], epsilon);
+ EXPECT_NEAR(outpose.data_.getOrigin().y(), yvalues[i], epsilon);
+ EXPECT_NEAR(outpose.data_.getOrigin().z(), zvalues[i], epsilon);
+ }
+ catch (tf::TransformException & ex)
+ {
+ std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl;
+ bool exception_improperly_thrown = true;
+ EXPECT_FALSE(exception_improperly_thrown);
+ }
+ }
+
+}
+
+
+TEST(tf, ListOneInverse)
+{
+ unsigned int runs = 400;
+ double epsilon = 1e-6;
+ seed_rand();
+
+ tf::Transformer mTR(true);
+ std::vector<double> xvalues(runs), yvalues(runs), zvalues(runs);
+ for ( unsigned int i = 0; i < runs ; i++ )
+ {
+ xvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
+ yvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
+ zvalues[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
+
+ Stamped<btTransform> tranStamped(btTransform(btQuaternion(0,0,0), btVector3(xvalues[i],yvalues[i],zvalues[i])), 10 + i, "child", "my_parent");
+ mTR.setTransform(tranStamped);
+ }
+
+ std::cout << mTR.allFramesAsString() << std::endl;
+ // std::cout << mTR.chainAsString("child", 0, "my_parent2", 0, "my_parent2") << std::endl;
+
+ for ( unsigned int i = 0; i < runs ; i++ )
+
+ {
+ Stamped<btTransform> inpose (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), 10 + i, "my_parent");
+
+ try{
+ Stamped<btTransform> outpose;
+ outpose.data_.setIdentity(); //to make sure things are getting mutated
+ mTR.transformPose("child",inpose, outpose);
+ EXPECT_NEAR(outpose.data_.getOrigin().x(), -xvalues[i], epsilon);
+ EXPECT_NEAR(outpose.data_.getOrigin().y(), -yvalues[i], epsilon);
+ EXPECT_NEAR(outpose.data_.getOrigin().z(), -zvalues[i], epsilon);
+ }
+ catch (tf::TransformException & ex)
+ {
+ std::cout << "TransformExcepion got through!!!!! " << ex.what() << std::endl;
+ bool exception_improperly_thrown = true;
+ EXPECT_FALSE(exception_improperly_thrown);
+ }
+ }
+
+}
+
+
+/** @todo Make this actually Assert something */
+
+int main(int argc, char **argv){
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mor...@us...> - 2008-10-15 02:05:31
|
Revision: 5379
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5379&view=rev
Author: morgan_quigley
Date: 2008-10-15 02:05:21 +0000 (Wed, 15 Oct 2008)
Log Message:
-----------
some starter audio utilities to grab an audio stream and chop it up into segments separated by silence
Added Paths:
-----------
pkg/trunk/audio/
pkg/trunk/audio/audio_capture/
pkg/trunk/audio/audio_capture/CMakeLists.txt
pkg/trunk/audio/audio_capture/Makefile
pkg/trunk/audio/audio_capture/audio_capture.cpp
pkg/trunk/audio/audio_capture/audio_clip_writer.cpp
pkg/trunk/audio/audio_capture/manifest.xml
Added: pkg/trunk/audio/audio_capture/CMakeLists.txt
===================================================================
--- pkg/trunk/audio/audio_capture/CMakeLists.txt (rev 0)
+++ pkg/trunk/audio/audio_capture/CMakeLists.txt 2008-10-15 02:05:21 UTC (rev 5379)
@@ -0,0 +1,10 @@
+cmake_minimum_required(VERSION 2.6)
+include(rosbuild)
+rospack(audio_capture)
+
+rospack_add_executable(audio_capture audio_capture.cpp)
+target_link_libraries(audio_capture portaudio)
+
+rospack_add_executable(audio_clip_writer audio_clip_writer.cpp)
+target_link_libraries(audio_clip_writer sndfile)
+
Added: pkg/trunk/audio/audio_capture/Makefile
===================================================================
--- pkg/trunk/audio/audio_capture/Makefile (rev 0)
+++ pkg/trunk/audio/audio_capture/Makefile 2008-10-15 02:05:21 UTC (rev 5379)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Added: pkg/trunk/audio/audio_capture/audio_capture.cpp
===================================================================
--- pkg/trunk/audio/audio_capture/audio_capture.cpp (rev 0)
+++ pkg/trunk/audio/audio_capture/audio_capture.cpp 2008-10-15 02:05:21 UTC (rev 5379)
@@ -0,0 +1,117 @@
+///////////////////////////////////////////////////////////////////////////////
+// A simple audio grabber which uses PortAudio to put an audio stream into ROS
+//
+// Copyright (C) 2008, Morgan Quigley
+//
+// 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 Stanford University 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 <cstdio>
+#include <portaudio.h>
+#include "ros/node.h"
+#include "robot_msgs/AudioRawStream.h"
+
+const static int SAMPLE_RATE = 44100; // todo: make this a parameter.
+ros::node *g_audio_node = NULL;
+bool g_caught_sigint = false;
+
+#define SHOW_MAX_SAMPLE
+//#define GRATUITOUS_DEBUGING
+
+static int audio_cb(const void *input, void *output,
+ unsigned long frame_count,
+ const PaStreamCallbackTimeInfo *time_info,
+ PaStreamCallbackFlags status, void *user_data)
+{
+ static robot_msgs::AudioRawStream audio_msg;
+ audio_msg.num_channels = 1;
+ audio_msg.sample_rate = SAMPLE_RATE;
+#ifdef GRATUITOUS_DEBUGGING
+ static ros::Time prev_t;
+ ros::Time t(ros::Time::now());
+ ROS_DEBUG("got %lu samples, dt = %.3f\n", frame_count, (t - prev_t).to_double());
+ prev_t = t;
+#endif
+#ifdef SHOW_MAX_SAMPLE
+ float max = 0;
+#endif
+ audio_msg.samples.resize(frame_count);
+ for (uint32_t i = 0; i < frame_count; i++)
+ {
+ float val = ((float *)input)[i];
+ audio_msg.samples[i] = val;
+#ifdef SHOW_MAX_SAMPLE
+ if (val > max)
+ max = val;
+#endif
+ }
+#ifdef SHOW_MAX_SAMPLE
+ printf("%.3f\r", max);
+ fflush(stdout);
+#endif
+ g_audio_node->publish("audio", audio_msg);
+ return paContinue;
+}
+
+void sig_handler(int sig)
+{
+ g_caught_sigint = true;
+ ROS_DEBUG("caught sigint, init shutdown sequence...");
+}
+
+int main(int argc, char **argv)
+{
+ PaStream *stream;
+ PaError err;
+ ROS_DEBUG("initializing portaudio");
+ if (Pa_Initialize() != paNoError)
+ ROS_FATAL("unable to initialize portaudio");
+ ROS_DEBUG("opening default audio stream");
+ ros::init(argc, argv);
+ ros::node n("audio_capture", ros::node::DONT_HANDLE_SIGINT);
+ g_audio_node = &n;
+ n.advertise<robot_msgs::AudioRawStream>("audio", 5);
+ err = Pa_OpenDefaultStream(&stream, 1, 0, paFloat32, SAMPLE_RATE, 4096,
+ audio_cb, NULL);
+ if (err != paNoError)
+ ROS_FATAL("unable to open audio stream\n");
+ ROS_INFO("starting audio stream, ctrl-c to stop.");
+#ifdef SHOW_MAX_SAMPLE
+ ROS_INFO("the printout stream shows the maximum audio sample for each portaudio snippet; use this to sanity-check your audio configuration and to set the gains inside alsamixer and on your audio hardware. You want the peaks to be near 1.0 but not quite 1.0 -- digital audio doesn't saturate very nicely.");
+#endif
+ if (Pa_StartStream(stream) != paNoError)
+ ROS_FATAL("unable to start audio stream");
+ signal(SIGINT, sig_handler);
+ while (!g_caught_sigint)
+ usleep(500*1000);
+ ROS_DEBUG("stopping audio stream");
+ if (Pa_StopStream(stream) != paNoError)
+ ROS_FATAL("unable to stop audio stream");
+ ROS_DEBUG("shutting down portaudio");
+ if (Pa_Terminate() != paNoError)
+ ROS_FATAL("unable to close portaudio");
+ ros::fini();
+ return 0;
+}
+
Added: pkg/trunk/audio/audio_capture/audio_clip_writer.cpp
===================================================================
--- pkg/trunk/audio/audio_capture/audio_clip_writer.cpp (rev 0)
+++ pkg/trunk/audio/audio_capture/audio_clip_writer.cpp 2008-10-15 02:05:21 UTC (rev 5379)
@@ -0,0 +1,172 @@
+///////////////////////////////////////////////////////////////////////////////
+// This program watches a ROS audio stream and writes its clips into files
+//
+// Copyright (C) 2008, Morgan Quigley
+//
+// 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 Stanford University 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 <cstdio>
+#include <deque>
+#include <vector>
+#include "ros/node.h"
+#include "robot_msgs/AudioRawStream.h"
+#include <sndfile.h>
+
+using std::deque;
+using std::vector;
+
+static const float CLIP_START_POWER = 0.05f;
+static const float CLIP_MAINTAIN_POWER = 0.03f;
+static const float WINDOW_LENGTH = 1.0; // seconds
+static const float MIN_CLIP_LENGTH = 2.0; // seconds
+
+class AudioWriter
+{
+public:
+ AudioWriter(ros::node *n)
+ : clip_num(0), clip_start(0), clip_end(0), audio_clock(0),
+ clip_state(IDLE), window_power(0)
+ {
+ n->subscribe("audio", audio_msg, &AudioWriter::audio_cb, this, 5);
+ }
+ void audio_cb()
+ {
+ if (audio_msg.num_channels != 1)
+ ROS_FATAL("audio_clip_writer can only handle single-channel audio.");
+ for (size_t i = 0; i < audio_msg.samples.size(); i++)
+ {
+ audio_clock++;
+ // maintain a window of 0.5 seconds
+ if (window.size() >= WINDOW_LENGTH * audio_msg.sample_rate)
+ window.pop_front();
+ window.push_back(audio_msg.samples[i]);
+ if (audio_clock % 100 == 0)
+ {
+ // compute RMS power
+ float sum_squares = 0;
+ int win_idx = 0;
+ for (deque<float>::iterator j = window.begin(); j != window.end();
+ ++j, win_idx++)
+ sum_squares += (*j) * (*j);// * ((float)win_idx / window.size());
+ //window_power = sqrt(2 * sum_squares / window.size());
+ window_power = sqrt(sum_squares / window.size());
+ //printf("%f\n", window_power);
+ }
+ switch (clip_state)
+ {
+ case IDLE:
+ if (fabs(audio_msg.samples[i]) > CLIP_START_POWER)
+ {
+ printf("clip start\n");
+ clip_start = audio_clock;
+ clip_state = CLIP_START;
+ clip.clear();
+ clip.reserve(window.size());
+ for (deque<float>::iterator j = window.begin();
+ j != window.end(); ++j)
+ clip.push_back(*j);
+ }
+ break;
+ case CLIP_START:
+ clip.push_back(audio_msg.samples[i]);
+ if (audio_clock - clip_start < audio_msg.sample_rate * WINDOW_LENGTH)
+ {
+ if (window_power > CLIP_MAINTAIN_POWER)
+ clip_state = IN_CLIP;
+ }
+ else
+ {
+ clip_state = IDLE;
+ printf("audio power never ramped up; it's not a real clip.\n");
+ }
+ break;
+ case IN_CLIP:
+ clip.push_back(audio_msg.samples[i]);
+ if (window_power < CLIP_MAINTAIN_POWER)
+ {
+ clip_state = IDLE;
+ const float clip_len = (float)clip.size() / audio_msg.sample_rate;
+ if (clip_len >= MIN_CLIP_LENGTH)
+ {
+ printf("good clip, length = %.2f seconds\n", clip_len);
+ // normalize it
+ float max_amp = 0, mean = 0;
+ for (vector<float>::iterator j = clip.begin();
+ j != clip.end(); ++j)
+ mean += *j;
+ mean /= clip.size();
+ for (vector<float>::iterator j = clip.begin();
+ j != clip.end(); ++j)
+ {
+ *j -= mean;
+ if (fabs(*j) > max_amp)
+ max_amp = fabs(*j);
+ }
+ for (vector<float>::iterator j = clip.begin();
+ j != clip.end(); ++j)
+ *j /= max_amp * 1.05;
+ SF_INFO sf_info;
+ sf_info.samplerate = audio_msg.sample_rate;
+ sf_info.channels = 1;
+ sf_info.format = SF_FORMAT_AU | SF_FORMAT_FLOAT;
+ char fnamebuf[100];
+ snprintf(fnamebuf, sizeof(fnamebuf), "clip%06d.au", clip_num++);
+ SNDFILE *sf = sf_open(fnamebuf, SFM_WRITE, &sf_info);
+ ROS_ASSERT(sf);
+ sf_write_float(sf, &clip[0], clip.size());
+ sf_close(sf);
+ //char cmd[500];
+ //system(cmd);
+ printf("done\n");
+ }
+ else
+ printf("too short. length = %.2f seconds\n", clip_len);
+ }
+ break;
+ }
+ }
+ }
+private:
+ int32_t clip_num, clip_start, clip_end;
+ uint64_t audio_clock;
+ enum { IDLE, CLIP_START, IN_CLIP } clip_state;
+ float window_power;
+ robot_msgs::AudioRawStream audio_msg;
+ deque<float> window;
+ vector<float> clip;
+};
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv);
+ ros::node n("audio_clip_writer");
+ AudioWriter *aw = new AudioWriter(&n);
+ while (n.ok())
+ ros::Duration(0, 500000000).sleep();
+ ros::fini();
+ delete aw;
+ return 0;
+}
+
Added: pkg/trunk/audio/audio_capture/manifest.xml
===================================================================
--- pkg/trunk/audio/audio_capture/manifest.xml (rev 0)
+++ pkg/trunk/audio/audio_capture/manifest.xml 2008-10-15 02:05:21 UTC (rev 5379)
@@ -0,0 +1,9 @@
+<package>
+ <description>This package grabs a live audio stream from PortAudio and ships it across the ROS botnet. The program grabs the "default" audio stream. I can't speak for other distros, but on Ubuntu the default stream is settable via the GNOME "System->Preferences->Sound" box. I set the "Sound Capture" field to "ALSA" and use "alsamixer" on the command line to poke around until I get the microphone jack working. Each motherboard seems to be a little different in how the various jacks come up in ALSA.</description>
+ <author>Morgan Quigley</author>
+ <license>BSD</license>
+ <depend package="roscpp"/>
+ <depend package="robot_msgs"/>
+ <sysdepend os="ubuntu" version="8.04-hardy" package="portaudio19-dev"/>
+ <sysdepend os="ubuntu" version="8.04-hardy" package="libsndfile-dev"/>
+</package>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-10-15 07:29:54
|
Revision: 5384
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5384&view=rev
Author: gerkey
Date: 2008-10-15 07:29:45 +0000 (Wed, 15 Oct 2008)
Log Message:
-----------
deleted irrlicht and irrlicht_viewer
Removed Paths:
-------------
pkg/trunk/3rdparty/irrlicht/
pkg/trunk/visualization/irrlicht_viewer/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-10-15 18:54:11
|
Revision: 5398
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5398&view=rev
Author: gerkey
Date: 2008-10-15 18:54:04 +0000 (Wed, 15 Oct 2008)
Log Message:
-----------
Blacklisted opencv_latest and all dependent packages, awaiting a fix for opencv's cvs update machinery
Added Paths:
-----------
pkg/trunk/3rdparty/opencv_latest/ROS_BUILD_BLACKLIST
pkg/trunk/drivers/cam/dc1394_cam/ROS_BUILD_BLACKLIST
pkg/trunk/drivers/cam/dc1394_cam_server/ROS_BUILD_BLACKLIST
pkg/trunk/drivers/cam/videre_cam/ROS_BUILD_BLACKLIST
pkg/trunk/drivers/robot/pr2/bread_board/ROS_BUILD_BLACKLIST
pkg/trunk/drivers/robot/sensor_cart/ROS_BUILD_BLACKLIST
pkg/trunk/vision/calonder_descriptor/ROS_BUILD_BLACKLIST
pkg/trunk/vision/star_detector/ROS_BUILD_BLACKLIST
pkg/trunk/vision/visual_odometry/ROS_BUILD_BLACKLIST
pkg/trunk/visualization/scene_labeler/ROS_BUILD_BLACKLIST
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-10-15 19:19:08
|
Revision: 5400
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5400&view=rev
Author: gerkey
Date: 2008-10-15 19:19:04 +0000 (Wed, 15 Oct 2008)
Log Message:
-----------
added reasons for blacklisting
Modified Paths:
--------------
pkg/trunk/3rdparty/opencv_latest/ROS_BUILD_BLACKLIST
pkg/trunk/drivers/cam/dc1394_cam/ROS_BUILD_BLACKLIST
pkg/trunk/drivers/cam/dc1394_cam_server/ROS_BUILD_BLACKLIST
pkg/trunk/drivers/cam/videre_cam/ROS_BUILD_BLACKLIST
pkg/trunk/drivers/robot/pr2/bread_board/ROS_BUILD_BLACKLIST
pkg/trunk/drivers/robot/sensor_cart/ROS_BUILD_BLACKLIST
pkg/trunk/vision/calonder_descriptor/ROS_BUILD_BLACKLIST
pkg/trunk/vision/star_detector/ROS_BUILD_BLACKLIST
pkg/trunk/vision/visual_odometry/ROS_BUILD_BLACKLIST
pkg/trunk/visualization/scene_labeler/ROS_BUILD_BLACKLIST
Modified: pkg/trunk/3rdparty/opencv_latest/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/3rdparty/opencv_latest/ROS_BUILD_BLACKLIST 2008-10-15 18:56:58 UTC (rev 5399)
+++ pkg/trunk/3rdparty/opencv_latest/ROS_BUILD_BLACKLIST 2008-10-15 19:19:04 UTC (rev 5400)
@@ -0,0 +1 @@
+cvs update step is broken
Modified: pkg/trunk/drivers/cam/dc1394_cam/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/drivers/cam/dc1394_cam/ROS_BUILD_BLACKLIST 2008-10-15 18:56:58 UTC (rev 5399)
+++ pkg/trunk/drivers/cam/dc1394_cam/ROS_BUILD_BLACKLIST 2008-10-15 19:19:04 UTC (rev 5400)
@@ -0,0 +1 @@
+depends on opencv_latest, which is blacklisted
Modified: pkg/trunk/drivers/cam/dc1394_cam_server/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/drivers/cam/dc1394_cam_server/ROS_BUILD_BLACKLIST 2008-10-15 18:56:58 UTC (rev 5399)
+++ pkg/trunk/drivers/cam/dc1394_cam_server/ROS_BUILD_BLACKLIST 2008-10-15 19:19:04 UTC (rev 5400)
@@ -0,0 +1,2 @@
+depends on opencv_latest, which is blacklisted
+
Modified: pkg/trunk/drivers/cam/videre_cam/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/drivers/cam/videre_cam/ROS_BUILD_BLACKLIST 2008-10-15 18:56:58 UTC (rev 5399)
+++ pkg/trunk/drivers/cam/videre_cam/ROS_BUILD_BLACKLIST 2008-10-15 19:19:04 UTC (rev 5400)
@@ -0,0 +1,2 @@
+depends on opencv_latest, which is blacklisted
+
Modified: pkg/trunk/drivers/robot/pr2/bread_board/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/drivers/robot/pr2/bread_board/ROS_BUILD_BLACKLIST 2008-10-15 18:56:58 UTC (rev 5399)
+++ pkg/trunk/drivers/robot/pr2/bread_board/ROS_BUILD_BLACKLIST 2008-10-15 19:19:04 UTC (rev 5400)
@@ -0,0 +1,2 @@
+depends on opencv_latest, which is blacklisted
+
Modified: pkg/trunk/drivers/robot/sensor_cart/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/drivers/robot/sensor_cart/ROS_BUILD_BLACKLIST 2008-10-15 18:56:58 UTC (rev 5399)
+++ pkg/trunk/drivers/robot/sensor_cart/ROS_BUILD_BLACKLIST 2008-10-15 19:19:04 UTC (rev 5400)
@@ -0,0 +1,2 @@
+depends on opencv_latest, which is blacklisted
+
Modified: pkg/trunk/vision/calonder_descriptor/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/vision/calonder_descriptor/ROS_BUILD_BLACKLIST 2008-10-15 18:56:58 UTC (rev 5399)
+++ pkg/trunk/vision/calonder_descriptor/ROS_BUILD_BLACKLIST 2008-10-15 19:19:04 UTC (rev 5400)
@@ -0,0 +1,2 @@
+depends on opencv_latest, which is blacklisted
+
Modified: pkg/trunk/vision/star_detector/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/vision/star_detector/ROS_BUILD_BLACKLIST 2008-10-15 18:56:58 UTC (rev 5399)
+++ pkg/trunk/vision/star_detector/ROS_BUILD_BLACKLIST 2008-10-15 19:19:04 UTC (rev 5400)
@@ -0,0 +1,2 @@
+depends on opencv_latest, which is blacklisted
+
Modified: pkg/trunk/vision/visual_odometry/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/vision/visual_odometry/ROS_BUILD_BLACKLIST 2008-10-15 18:56:58 UTC (rev 5399)
+++ pkg/trunk/vision/visual_odometry/ROS_BUILD_BLACKLIST 2008-10-15 19:19:04 UTC (rev 5400)
@@ -0,0 +1,2 @@
+depends on opencv_latest, which is blacklisted
+
Modified: pkg/trunk/visualization/scene_labeler/ROS_BUILD_BLACKLIST
===================================================================
--- pkg/trunk/visualization/scene_labeler/ROS_BUILD_BLACKLIST 2008-10-15 18:56:58 UTC (rev 5399)
+++ pkg/trunk/visualization/scene_labeler/ROS_BUILD_BLACKLIST 2008-10-15 19:19:04 UTC (rev 5400)
@@ -0,0 +1,2 @@
+depends on opencv_latest, which is blacklisted
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2008-10-15 22:33:48
|
Revision: 5411
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5411&view=rev
Author: stuglaser
Date: 2008-10-15 22:33:39 +0000 (Wed, 15 Oct 2008)
Log Message:
-----------
Calibration of joints with optical sensors is now asynchronous
Modified Paths:
--------------
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_calibration_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h
Added Paths:
-----------
pkg/trunk/robot_descriptions/wg_robot_description/one_armed_bandit/calibrate.py
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_calibration_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_calibration_controller.h 2008-10-15 22:33:12 UTC (rev 5410)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_calibration_controller.h 2008-10-15 22:33:39 UTC (rev 5411)
@@ -59,7 +59,8 @@
#include "joint_manual_calibration_controller.h"
-
+#include "misc_utils/realtime_publisher.h"
+#include "std_msgs/Empty.h"
// Services
#include <robot_mechanism_controllers/CalibrateJoint.h>
@@ -70,30 +71,14 @@
class JointCalibrationController : public controller::Controller
{
public:
- /*!
- * \brief Default Constructor.
- *
- */
JointCalibrationController();
-
- /*!
- * \brief Destructor.
- */
virtual ~JointCalibrationController();
- /*!
- * \brief Functional way to initialize limits and gains.
- *
- */
virtual bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
-
- /*!
- * \brief Issues commands to the joint. Should be called at regular intervals
- */
virtual void update();
- bool calibrated() { return state_ == STOPPED; }
+ bool calibrated() { return state_ == CALIBRATED; }
void beginCalibration()
{
if (state_ == INITIALIZED)
@@ -102,7 +87,7 @@
protected:
- enum { INITIALIZED, BEGINNING, MOVING, STOPPED };
+ enum { INITIALIZED, BEGINNING, MOVING, CALIBRATED };
int state_;
double search_velocity_;
@@ -116,29 +101,19 @@
};
-/** @class controller::JointControllerCalibrationNode
- * @\brief ROS interface for a joint calibration controller
- * This class is a wrapper around the calibrateCmd service call and it should be its only use
- */
class JointCalibrationControllerNode : public Controller
{
public:
- /*!
- * \brief Default Constructor
- *
- */
JointCalibrationControllerNode();
-
- /*!
- * \brief Destructor
- */
~JointCalibrationControllerNode();
void update();
bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
- /** \brief initializes the calibration procedure (blocking service)
+ /** DEPRECATED. Listen to /topic/calibrated instead
+ *
+ * \brief initializes the calibration procedure (blocking service)
* This service starts the calibration sequence of the joint and waits to return until the calibration sequence is finished.
*
* @param req
@@ -149,8 +124,12 @@
robot_mechanism_controllers::CalibrateJoint::response &resp);
private:
- JointCalibrationController *c_;
+ mechanism::RobotState* robot_;
+ JointCalibrationController c_;
AdvertisedServiceGuard guard_calibrate_;
+
+ double last_publish_time_;
+ misc_utils::RealtimePublisher<std_msgs::Empty> *pub_calibrated_;
};
}
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp 2008-10-15 22:33:12 UTC (rev 5410)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp 2008-10-15 22:33:39 UTC (rev 5411)
@@ -38,7 +38,7 @@
using namespace std;
using namespace controller;
-ROS_REGISTER_CONTROLLER(JointCalibrationController)
+//ROS_REGISTER_CONTROLLER(JointCalibrationController)
JointCalibrationController::JointCalibrationController()
: state_(INITIALIZED)
@@ -119,6 +119,7 @@
{
case INITIALIZED:
vc_.setCommand(0.0);
+ state_ = BEGINNING;
break;
case BEGINNING:
original_switch_state_ = actuator_->state_.calibration_reading_ > 0.5;
@@ -147,42 +148,55 @@
actuator_->state_.zero_offset_ = fake_a[0]->state_.position_;
- state_ = STOPPED;
+ state_ = CALIBRATED;
+ vc_.setCommand(0.0);
}
break;
}
- case STOPPED:
- vc_.setCommand(0.0);
+ case CALIBRATED:
break;
}
- vc_.update();
+ if (state_ != CALIBRATED)
+ vc_.update();
}
ROS_REGISTER_CONTROLLER(JointCalibrationControllerNode)
JointCalibrationControllerNode::JointCalibrationControllerNode()
+: robot_(NULL), last_publish_time_(0), pub_calibrated_(NULL)
{
- c_ = new JointCalibrationController();
}
JointCalibrationControllerNode::~JointCalibrationControllerNode()
{
- delete c_;
}
void JointCalibrationControllerNode::update()
{
- c_->update();
+ c_.update();
+
+ if (c_.calibrated())
+ {
+ if (last_publish_time_ + 0.5 < robot_->hw_->current_time_)
+ {
+ assert(pub_calibrated_);
+ if (pub_calibrated_->trylock())
+ {
+ last_publish_time_ = robot_->hw_->current_time_;
+ pub_calibrated_->unlockAndPublish();
+ }
+ }
+ }
}
bool JointCalibrationControllerNode::calibrateCommand(robot_mechanism_controllers::CalibrateJoint::request &req, robot_mechanism_controllers::CalibrateJoint::response &resp)
{
- c_->beginCalibration();
+ c_.beginCalibration();
ros::Duration d=ros::Duration(0,1000000);
- while(!c_->calibrated())
+ while(!c_.calibrated())
d.sleep();
resp.offset = 0;
return true;
@@ -190,6 +204,8 @@
bool JointCalibrationControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
{
+ assert(robot);
+ robot_ = robot;
ros::node *node = ros::node::instance();
std::string topic = config->Attribute("topic") ? config->Attribute("topic") : "";
@@ -199,10 +215,12 @@
return false;
}
- if (!c_->initXml(robot, config))
+ if (!c_.initXml(robot, config))
return false;
node->advertise_service(topic + "/calibrate", &JointCalibrationControllerNode::calibrateCommand, this);
guard_calibrate_.set(topic + "/calibrate");
+ pub_calibrated_ = new misc_utils::RealtimePublisher<std_msgs::Empty>(topic + "/calibrated", 1);
+
return true;
}
Modified: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h 2008-10-15 22:33:12 UTC (rev 5410)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h 2008-10-15 22:33:39 UTC (rev 5411)
@@ -36,19 +36,10 @@
* The robot model tracks the state of the robot.
*
* State path:
- * +---------------+ +--------+
- * Actuators --> | Transmissions | --> Joints --> | Chains | --> Links
- * +---------------+ +--------+
+ * +---------------+
+ * Actuators --> | Transmissions | --> Joints --> Links
+ * +---------------+
*
- * The actuators, joints, and links, hold the state information. The
- * actuators hold the encoder info, the joints hold the joint angles
- * and velocities, and the links hold the frame transforms of the body
- * segments.
- *
- * The transmissions and chains are for propagating the state through
- * the model, and they themselves do not hold any information on the
- * robot's current state.
- *
* Author: Stuart Glaser
*/
Added: pkg/trunk/robot_descriptions/wg_robot_description/one_armed_bandit/calibrate.py
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/one_armed_bandit/calibrate.py (rev 0)
+++ pkg/trunk/robot_descriptions/wg_robot_description/one_armed_bandit/calibrate.py 2008-10-15 22:33:39 UTC (rev 5411)
@@ -0,0 +1,185 @@
+#!/usr/bin/python
+# 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: Stuart Glaser
+
+import rostools
+import copy
+import threading
+import sys
+from time import sleep
+
+# Loads interface with the robot.
+rostools.update_path('teleop_robot')
+import rospy
+from std_msgs.msg import *
+from robot_srvs.srv import *
+from robot_mechanism_controllers.srv import *
+
+def slurp(filename):
+ f = open(filename)
+ stuff = f.read()
+ f.close()
+ return stuff
+
+rospy.wait_for_service('spawn_controller')
+spawn_controller = rospy.ServiceProxy('spawn_controller', SpawnController)
+kill_controller = rospy.ServiceProxy('kill_controller', KillController)
+
+
+def calibrate_optically(config):
+ resp = spawn_controller(config)
+ if len(resp.ok) != 1 or not resp.ok[0]:
+ print "FAIL"
+ return
+ name = resp.name[0]
+ do_calibration = rospy.ServiceProxy("/%s/calibrate" % name, CalibrateJoint)
+ do_calibration()
+ kill_controller(name)
+ print "Calibrated"
+
+def calibrate_manually(config):
+ resp = spawn_controller(config)
+ if len(resp.ok) != 1 or not resp.ok[0]:
+ print "FAIL"
+ return
+ name = resp.name[0]
+ begin = rospy.ServiceProxy("/%s/begin_manual_calibration" % name, CalibrateJoint)
+ end = rospy.ServiceProxy("/%s/end_manual_calibration" % name, CalibrateJoint)
+ begin()
+ print "Move the joint to the limits, and then hit enter"
+ raw_input()
+ end()
+ kill_controller(name)
+ print "Calibrated manually"
+
+# Hits the joint stops
+def calibrate_blindly(config):
+ resp = spawn_controller(config)
+ if len(resp.ok) != 1 or not resp.ok[0]:
+ print "FAIL"
+ return
+ name = resp.name[0]
+ do_calibration = rospy.ServiceProxy("/%s/calibrate" % name, CalibrateJoint)
+ do_calibration()
+ kill_controller(name)
+ print "Calibrated"
+
+def calibrate(config):
+ # Spawns the controllers
+ resp = spawn_controller(config)
+
+ # Accumulates the list of spawned controllers
+ launched = []
+ try:
+ for i in range(len(resp.ok)):
+ if not resp.ok[i]:
+ print "Failed: %s" % resp.name[i]
+ else:
+ launched.append(resp.name[i])
+ print "Launched: %s" % ', '.join(launched)
+
+ # Sets up callbacks for calibration completion
+ waiting_for = launched[:]
+ def calibrated(msg, name): # Somewhat not thread-safe
+ if name in waiting_for:
+ waiting_for.remove(name)
+ for name in waiting_for:
+ rospy.Subscriber("/%s/calibrated" % name, Empty, calibrated, name)
+
+ # Waits until all the controllers have calibrated
+ while waiting_for:
+ print "Waiting for: %s" % ', '.join(waiting_for)
+ sleep(0.5)
+ finally:
+ [kill_controller(name) for name in launched]
+
+
+rospy.init_node('calibration', anonymous=True)
+
+
+calibrate('''
+<controllers>
+
+ <controller name="cal_shoulder_pan" topic="cal_shoulder_pan" type="JointCalibrationControllerNode">
+ <calibrate joint="shoulder_pan_right_joint"
+ actuator="shoulder_pan_right_motor"
+ transmission="shoulder_pan_right_trans"
+ velocity="0.6" />
+ <pid p="7" i="0.5" d="0" iClamp="1.0" />
+ </controller>
+
+ <controller name="cal_shoulder_pitch" topic="cal_shoulder_pitch" type="JointCalibrationControllerNode">
+ <calibrate joint="shoulder_pitch_right_joint"
+ actuator="shoulder_pitch_right_motor"
+ transmission="shoulder_pitch_right_trans"
+ velocity="0.6" />
+ <pid p="7" i="0.5" d="0" iClamp="1.0" />
+ </controller>
+
+ <controller name="cal_laser_tilt" topic="cal_laser_tilt" type="JointCalibrationControllerNode">
+ <calibrate joint="tilt_laser_mount_joint"
+ actuator="tilt_laser_motor"
+ transmission="tilt_laser_mount_trans"
+ velocity="-0.6" />
+ <pid p=".25" i="0.1" d="0" iClamp="1.0" />
+ </controller>
+
+</controllers>
+''')
+
+
+calibrate_blindly('''
+<controller name="upperarm_calibration" topic="upperarm_calibration" type="JointBlindCalibrationControllerNode">
+ <calibrate joint="upperarm_roll_right_joint"
+ actuator="upperarm_roll_right_motor"
+ transmission="upperarm_roll_right_trans"
+ velocity="1.3" />
+ <pid p="5" i="0.5" d="0" iClamp="1.0" />
+</controller>
+
+''')
+
+calibrate_blindly('''
+<controller name="cal_elbow" topic="cal_elbow" type="JointBlindCalibrationControllerNode">
+ <calibrate joint="elbow_flex_right_joint"
+ actuator="elbow_flex_right_motor"
+ transmission="elbow_flex_right_trans"
+ velocity="1.0" />
+ <pid p="5" i="0.5" d="0" iClamp="1.0" />
+</controller>
+
+''')
+
+print "Calibration complete"
Property changes on: pkg/trunk/robot_descriptions/wg_robot_description/one_armed_bandit/calibrate.py
___________________________________________________________________
Added: svn:executable
+ *
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2008-10-15 23:31:30
|
Revision: 5413
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5413&view=rev
Author: stuglaser
Date: 2008-10-15 23:31:25 +0000 (Wed, 15 Oct 2008)
Log Message:
-----------
Caster calibration is now asynchronous (they all calibrate at once).
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_calibration_controller.h
pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp
pkg/trunk/mechanism/mechanism_bringup/scripts/calibrate_base.py
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_calibration_controller.h
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_calibration_controller.h 2008-10-15 23:04:35 UTC (rev 5412)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/include/pr2_mechanism_controllers/caster_calibration_controller.h 2008-10-15 23:31:25 UTC (rev 5413)
@@ -35,6 +35,8 @@
#define CASTER_CALIBRATION_CONTROLLER_H
#include "pr2_mechanism_controllers/caster_controller.h"
+#include "misc_utils/realtime_publisher.h"
+#include "std_msgs/Empty.h"
#include <robot_mechanism_controllers/CalibrateJoint.h>
namespace controller {
@@ -53,7 +55,7 @@
*/
virtual void update();
- bool calibrated() { return state_ == STOPPED; }
+ bool calibrated() { return state_ == CALIBRATED; }
void beginCalibration()
{
if (state_ == INITIALIZED)
@@ -62,7 +64,7 @@
protected:
- enum { INITIALIZED, BEGINNING, MOVING, STOPPED };
+ enum { INITIALIZED, BEGINNING, MOVING, CALIBRATED };
int state_;
double search_velocity_;
@@ -90,8 +92,12 @@
robot_mechanism_controllers::CalibrateJoint::response &resp);
private:
+ mechanism::RobotState *robot_;
CasterCalibrationController c_;
AdvertisedServiceGuard guard_calibrate_;
+
+ double last_publish_time_;
+ misc_utils::RealtimePublisher<std_msgs::Empty> *pub_calibrated_;
};
}
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp 2008-10-15 23:04:35 UTC (rev 5412)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp 2008-10-15 23:31:25 UTC (rev 5413)
@@ -107,6 +107,7 @@
case INITIALIZED:
cc_.steer_velocity_ = 0.0;
cc_.drive_velocity_ = 0.0;
+ state_ = BEGINNING;
break;
case BEGINNING:
original_switch_state_ = actuator_->state_.calibration_reading_ > 0.5;
@@ -117,10 +118,12 @@
bool switch_state_ = actuator_->state_.calibration_reading_ > 0.5;
if (switch_state_ != original_switch_state_)
{
+ Actuator a;
+ mechanism::JointState j;
std::vector<Actuator*> fake_a;
std::vector<mechanism::JointState*> fake_j;
- fake_a.push_back(new Actuator);
- fake_j.push_back(new mechanism::JointState);
+ fake_a.push_back(&a);
+ fake_j.push_back(&j);
// Where was the joint when the optical switch triggered?
if (original_switch_state_ == true)
@@ -135,32 +138,49 @@
actuator_->state_.zero_offset_ = fake_a[0]->state_.position_;
- state_ = STOPPED;
+ state_ = CALIBRATED;
}
break;
}
- case STOPPED:
+ case CALIBRATED:
cc_.steer_velocity_ = 0.0;
break;
}
- cc_.update();
+ if (state_ != CALIBRATED)
+ cc_.update();
}
ROS_REGISTER_CONTROLLER(CasterCalibrationControllerNode)
CasterCalibrationControllerNode::CasterCalibrationControllerNode()
+: last_publish_time_(0), pub_calibrated_(NULL)
{
}
CasterCalibrationControllerNode::~CasterCalibrationControllerNode()
{
+ if (pub_calibrated_)
+ delete pub_calibrated_;
}
void CasterCalibrationControllerNode::update()
{
c_.update();
+
+ if (c_.calibrated())
+ {
+ if (last_publish_time_ + 0.5 < robot_->hw_->current_time_)
+ {
+ assert(pub_calibrated_);
+ if (pub_calibrated_->trylock())
+ {
+ last_publish_time_ = robot_->hw_->current_time_;
+ pub_calibrated_->unlockAndPublish();
+ }
+ }
+ }
}
@@ -176,6 +196,8 @@
bool CasterCalibrationControllerNode::initXml(mechanism::RobotState *robot, TiXmlElement *config)
{
+ assert(robot);
+ robot_ = robot;
ros::node *node = ros::node::instance();
std::string topic = config->Attribute("topic") ? config->Attribute("topic") : "";
@@ -190,6 +212,8 @@
node->advertise_service(topic + "/calibrate", &CasterCalibrationControllerNode::calibrateCommand, this);
guard_calibrate_.set(topic + "/calibrate");
+ pub_calibrated_ = new misc_utils::RealtimePublisher<std_msgs::Empty>(topic + "/calibrated", 1);
+
return true;
}
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp 2008-10-15 23:04:35 UTC (rev 5412)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp 2008-10-15 23:31:25 UTC (rev 5413)
@@ -130,10 +130,12 @@
bool switch_state_ = actuator_->state_.calibration_reading_ > 0.5;
if (switch_state_ != original_switch_state_)
{
+ Actuator a;
+ mechanism::JointState j;
std::vector<Actuator*> fake_a;
std::vector<mechanism::JointState*> fake_j;
- fake_a.push_back(new Actuator);
- fake_j.push_back(new mechanism::JointState);
+ fake_a.push_back(&a);
+ fake_j.push_back(&j);
// Where was the joint when the optical switch triggered?
if (original_switch_state_ == true)
@@ -171,6 +173,8 @@
JointCalibrationControllerNode::~JointCalibrationControllerNode()
{
+ if (pub_calibrated_)
+ delete pub_calibrated_;
}
void JointCalibrationControllerNode::update()
Modified: pkg/trunk/mechanism/mechanism_bringup/scripts/calibrate_base.py
===================================================================
--- pkg/trunk/mechanism/mechanism_bringup/scripts/calibrate_base.py 2008-10-15 23:04:35 UTC (rev 5412)
+++ pkg/trunk/mechanism/mechanism_bringup/scripts/calibrate_base.py 2008-10-15 23:31:25 UTC (rev 5413)
@@ -37,6 +37,7 @@
import rostools
import copy
import threading
+from time import sleep
# Loads interface with the robot.
rostools.update_path('teleop_robot')
@@ -44,11 +45,9 @@
rostools.update_path('mechanism_control')
import rospy
+from std_msgs.msg import Empty
from robot_srvs.srv import *
from robot_mechanism_controllers.srv import *
-from std_srvs.srv import *
-from robot_srvs.srv import *
-from robot_mechanism_controllers.srv import *
from mechanism_control.srv import *
def slurp(filename):
@@ -62,7 +61,8 @@
kill_controller = rospy.ServiceProxy('kill_controller', KillController)
-def calibrate_optically(config):
+# Hits the joint stops
+def calibrate_blindly(config):
resp = spawn_controller(config)
if len(resp.ok) != 1 or not resp.ok[0]:
print "FAIL"
@@ -75,47 +75,43 @@
kill_controller(name)
print "Calibrated"
-def calibrate_manually(config):
+
+
+def calibrate(config):
+ # Spawns the controllers
resp = spawn_controller(config)
- if len(resp.ok) != 1 or not resp.ok[0]:
- print "FAIL"
- return
- name = resp.name[0]
- begin = rospy.ServiceProxy("/%s/begin_manual_calibration" % name, CalibrateJoint)
- end = rospy.ServiceProxy("/%s/end_manual_calibration" % name, CalibrateJoint)
- begin()
- print "Move the joint to the limits, and then hit enter"
- raw_input()
- end()
- kill_controller(name)
- print "Calibrated manually"
-# Hits the joint stops
-def calibrate_blindly(config):
- resp = spawn_controller(config)
- if len(resp.ok) != 1 or not resp.ok[0]:
- print "FAIL"
- return
- name = resp.name[0]
+ # Accumulates the list of spawned controllers
+ launched = []
try:
- do_calibration = rospy.ServiceProxy("/%s/calibrate" % name, CalibrateJoint)
- do_calibration()
+ for i in range(len(resp.ok)):
+ if not resp.ok[i]:
+ print "Failed: %s" % resp.name[i]
+ else:
+ launched.append(resp.name[i])
+ print "Launched: %s" % ', '.join(launched)
+
+ # Sets up callbacks for calibration completion
+ waiting_for = launched[:]
+ def calibrated(msg, name): # Somewhat not thread-safe
+ if name in waiting_for:
+ waiting_for.remove(name)
+ for name in waiting_for:
+ rospy.Subscriber("/%s/calibrated" % name, Empty, calibrated, name)
+
+ # Waits until all the controllers have calibrated
+ while waiting_for:
+ print "Waiting for: %s" % ', '.join(waiting_for)
+ sleep(0.5)
finally:
- kill_controller(name)
- print "Calibrated"
+ [kill_controller(name) for name in launched]
-class FunThread(threading.Thread):
- def __init__(self, fun, *args):
- self.fun = fun
- self.start()
+rospy.init_node('calibration', anonymous=True)
- def run():
- self.fun(*args)
template = '''
-<controllers>
<controller type="CasterCalibrationControllerNode" name="cal_caster_SUFFIX" topic="cal_caster_SUFFIX">
<calibrate joint="caster_SUFFIX_joint"
actuator="caster_SUFFIX_motor"
@@ -127,10 +123,19 @@
<caster_pid p="6" i="0" d="0" iClamp="0" />
<wheel_pid p="4" i="0" d="0" iClamp="0" />
</controller>
-</controllers>
'''
-calibrate_optically(template.replace('SUFFIX', 'front_left'))
-calibrate_optically(template.replace('SUFFIX', 'front_right'))
-calibrate_optically(template.replace('SUFFIX', 'rear_left'))
-calibrate_optically(template.replace('SUFFIX', 'rear_right'))
+
+calibrate(
+'''
+<controllers>
+'''
++ template.replace('SUFFIX', 'front_left')
++ template.replace('SUFFIX', 'front_right')
++ template.replace('SUFFIX', 'rear_left')
++ template.replace('SUFFIX', 'rear_right')
++ '''
+</controllers>
+''')
+
+print "Calibrated"
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rob...@us...> - 2008-10-15 23:54:38
|
Revision: 5415
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5415&view=rev
Author: rob_wheeler
Date: 2008-10-15 23:54:32 +0000 (Wed, 15 Oct 2008)
Log Message:
-----------
last_calibration_high_transition -> last_calibration_rising_edge
last_calibration_low_transition -> last_calibration_falling_edge
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp
pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg0x.h
pkg/trunk/drivers/motor/ethercat_hardware/src/wg0x.cpp
pkg/trunk/mechanism/hardware_interface/include/hardware_interface/hardware_interface.h
pkg/trunk/mechanism/mechanism_control/CMakeLists.txt
pkg/trunk/mechanism/mechanism_control/scripts/listen.py
pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
pkg/trunk/robot_msgs/msg/ActuatorState.msg
Removed Paths:
-------------
pkg/trunk/mechanism/mechanism_control/msg/
pkg/trunk/mechanism/mechanism_control/srv/
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp 2008-10-15 23:50:11 UTC (rev 5414)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/caster_calibration_controller.cpp 2008-10-15 23:54:32 UTC (rev 5415)
@@ -127,9 +127,9 @@
// Where was the joint when the optical switch triggered?
if (original_switch_state_ == true)
- fake_a[0]->state_.position_ = actuator_->state_.last_calibration_high_transition_;
+ fake_a[0]->state_.position_ = actuator_->state_.last_calibration_rising_edge_;
else
- fake_a[0]->state_.position_ = actuator_->state_.last_calibration_low_transition_;
+ fake_a[0]->state_.position_ = actuator_->state_.last_calibration_falling_edge_;
transmission_->propagatePosition(fake_a, fake_j);
// What is the actuator position at the joint's zero?
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp 2008-10-15 23:50:11 UTC (rev 5414)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp 2008-10-15 23:54:32 UTC (rev 5415)
@@ -139,9 +139,9 @@
// Where was the joint when the optical switch triggered?
if (original_switch_state_ == true)
- fake_a[0]->state_.position_ = actuator_->state_.last_calibration_high_transition_;
+ fake_a[0]->state_.position_ = actuator_->state_.last_calibration_rising_edge_;
else
- fake_a[0]->state_.position_ = actuator_->state_.last_calibration_low_transition_;
+ fake_a[0]->state_.position_ = actuator_->state_.last_calibration_falling_edge_;
transmission_->propagatePosition(fake_a, fake_j);
// What is the actuator position at the joint's zero?
Modified: pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg0x.h
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg0x.h 2008-10-15 23:50:11 UTC (rev 5414)
+++ pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg0x.h 2008-10-15 23:54:32 UTC (rev 5415)
@@ -175,8 +175,8 @@
uint16_t num_encoder_errors_;
uint8_t encoder_status_;
uint8_t calibration_reading_;
- int32_t last_calibration_high_transition_;
- int32_t last_calibration_low_transition_;
+ int32_t last_calibration_rising_edge_;
+ int32_t last_calibration_falling_edge_;
uint16_t board_temperature_;
uint16_t bridge_temperature_;
uint16_t supply_voltage_;
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/wg0x.cpp
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/wg0x.cpp 2008-10-15 23:50:11 UTC (rev 5414)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/wg0x.cpp 2008-10-15 23:54:32 UTC (rev 5415)
@@ -341,8 +341,8 @@
/ (this_status->timestamp_ - prev_status->timestamp_) * 1e+6;
state.velocity_ = state.encoder_velocity_ / actuator_info_.pulses_per_revolution_ * 2 * M_PI;
state.calibration_reading_ = this_status->calibration_reading_ & LIMIT_SENSOR_0_STATE;
- state.last_calibration_high_transition_ = double(this_status->last_calibration_high_transition_) / actuator_info_.pulses_per_revolution_ * 2 * M_PI;
- state.last_calibration_low_transition_ = double(this_status->last_calibration_low_transition_) / actuator_info_.pulses_per_revolution_ * 2 * M_PI;
+ state.last_calibration_rising_edge_ = double(this_status->last_calibration_rising_edge_) / actuator_info_.pulses_per_revolution_ * 2 * M_PI;
+ state.last_calibration_falling_edge_ = double(this_status->last_calibration_falling_edge_) / actuator_info_.pulses_per_revolution_ * 2 * M_PI;
state.is_enabled_ = this_status->mode_ != MODE_OFF;
state.run_stop_hit_ = (this_status->mode_ & MODE_UNDERVOLTAGE) != 0;
@@ -806,8 +806,8 @@
ADD_STRING_FMT("Num encoder_errors", "%d", status->num_encoder_errors_);
ADD_STRING_FMT("Encoder status", "%d", status->encoder_status_);
ADD_STRING_FMT("Calibration reading", "%d", status->calibration_reading_);
- ADD_STRING_FMT("Last calibration high transition", "%d", status->last_calibration_high_transition_);
- ADD_STRING_FMT("Last calibration low transition", "%d", status->last_calibration_low_transition_);
+ ADD_STRING_FMT("Last calibration rising edge", "%d", status->last_calibration_rising_edge_);
+ ADD_STRING_FMT("Last calibration falling edge", "%d", status->last_calibration_falling_edge_);
ADD_STRING_FMT("Board temperature", "%f", 0.0078125 * status->board_temperature_);
ADD_STRING_FMT("Bridge temperature", "%f", 0.0078125 * status->bridge_temperature_);
ADD_STRING_FMT("Supply voltage", "%f", status->supply_voltage_ * config_info_.nominal_voltage_scale_);
Modified: pkg/trunk/mechanism/hardware_interface/include/hardware_interface/hardware_interface.h
===================================================================
--- pkg/trunk/mechanism/hardware_interface/include/hardware_interface/hardware_interface.h 2008-10-15 23:50:11 UTC (rev 5414)
+++ pkg/trunk/mechanism/hardware_interface/include/hardware_interface/hardware_interface.h 2008-10-15 23:54:32 UTC (rev 5415)
@@ -42,8 +42,8 @@
class ActuatorState{
public:
ActuatorState() :
- last_calibration_high_transition_(0),
- last_calibration_low_transition_(0),
+ last_calibration_rising_edge_(0),
+ last_calibration_falling_edge_(0),
is_enabled_(0),
run_stop_hit_(0),
last_requested_current_(0),
@@ -63,8 +63,8 @@
double velocity_;
bool calibration_reading_;
- double last_calibration_high_transition_; // Last transition from high to low.
- double last_calibration_low_transition_;
+ double last_calibration_rising_edge_;
+ double last_calibration_falling_edge_;
bool is_enabled_;
bool run_stop_hit_;
Modified: pkg/trunk/mechanism/mechanism_control/CMakeLists.txt
===================================================================
--- pkg/trunk/mechanism/mechanism_control/CMakeLists.txt 2008-10-15 23:50:11 UTC (rev 5414)
+++ pkg/trunk/mechanism/mechanism_control/CMakeLists.txt 2008-10-15 23:54:32 UTC (rev 5415)
@@ -1,8 +1,6 @@
cmake_minimum_required(VERSION 2.6)
include(rosbuild)
rospack(mechanism_control)
-genmsg()
-gensrv()
rospack_add_library(mechanism_control src/mechanism_control.cpp)
#rospack_add_executable(ms_publisher_test test/ms_publisher_test.cpp)
#rospack_add_rostest(test/test-mechanism-state-cpp.xml)
Modified: pkg/trunk/mechanism/mechanism_control/scripts/listen.py
===================================================================
--- pkg/trunk/mechanism/mechanism_control/scripts/listen.py 2008-10-15 23:50:11 UTC (rev 5414)
+++ pkg/trunk/mechanism/mechanism_control/scripts/listen.py 2008-10-15 23:54:32 UTC (rev 5415)
@@ -33,8 +33,8 @@
print " encoder_velocity: %.4f" % a.encoder_velocity
print " velocity: %.4f" % a.velocity
print " calibration_reading: %d" % a.calibration_reading
- print " last_calibration_high_transition: %d" % a.last_calibration_high_transition
- print " last_calibration_low_transition: %d" % a.last_calibration_low_transition
+ print " last_calibration_rising_edge: %d" % a.last_calibration_rising_edge
+ print " last_calibration_falling_edge: %d" % a.last_calibration_falling_edge
print " is_enabled: %d" % a.is_enabled
print " run_stop_hit: %d" % a.run_stop_hit
print " last_requested_current: %.4f" % a.last_requested_current
Modified: pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-10-15 23:50:11 UTC (rev 5414)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-10-15 23:54:32 UTC (rev 5415)
@@ -266,8 +266,8 @@
out->encoder_velocity = in->encoder_velocity_;
out->velocity = in->velocity_;
out->calibration_reading = in->calibration_reading_;
- out->last_calibration_high_transition = in->last_calibration_high_transition_;
- out->last_calibration_low_transition = in->last_calibration_low_transition_;
+ out->last_calibration_rising_edge = in->last_calibration_rising_edge_;
+ out->last_calibration_falling_edge = in->last_calibration_falling_edge_;
out->is_enabled = in->is_enabled_;
out->run_stop_hit = in->run_stop_hit_;
out->last_requested_current = in->last_requested_current_;
Modified: pkg/trunk/robot_msgs/msg/ActuatorState.msg
===================================================================
--- pkg/trunk/robot_msgs/msg/ActuatorState.msg 2008-10-15 23:50:11 UTC (rev 5414)
+++ pkg/trunk/robot_msgs/msg/ActuatorState.msg 2008-10-15 23:54:32 UTC (rev 5415)
@@ -5,8 +5,8 @@
float64 encoder_velocity
float64 velocity
byte calibration_reading
-float64 last_calibration_high_transition
-float64 last_calibration_low_transition
+float64 last_calibration_rising_edge
+float64 last_calibration_falling_edge
byte is_enabled
byte run_stop_hit
float64 last_requested_current
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mc...@us...> - 2008-10-16 20:19:22
|
Revision: 5443
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5443&view=rev
Author: mcgann
Date: 2008-10-16 20:18:27 +0000 (Thu, 16 Oct 2008)
Log Message:
-----------
Added filter for free space projection based on height of the point
Modified Paths:
--------------
pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h
pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp
pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc
Modified: pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2008-10-16 20:15:38 UTC (rev 5442)
+++ pkg/trunk/highlevel/highlevel_controllers/src/MoveBase.cpp 2008-10-16 20:18:27 UTC (rev 5443)
@@ -80,7 +80,8 @@
double windowLength(1.0);
unsigned char lethalObstacleThreshold(100);
unsigned char noInformation(CostMap2D::NO_INFORMATION);
- double maxZ(2.0);
+ double maxZ(2.0);
+ double freeSpaceProjectionHeight(0.5);
double inflationRadius(0.46);
double circumscribedRadius(0.46);
double inscribedRadius(0.325);
@@ -88,6 +89,7 @@
param("costmap_2d/lethal_obstacle_threshold", lethalObstacleThreshold, lethalObstacleThreshold);
param("costmap_2d/no_information_value", noInformation, noInformation);
param("costmap_2d/z_threshold", maxZ, maxZ);
+ param("costmap_2d/freespace_projection_height", freeSpaceProjectionHeight, freeSpaceProjectionHeight);
param("costmap_2d/inflation_radius", inflationRadius, inflationRadius);
param("costmap_2d/circumscribed_radius", circumscribedRadius, circumscribedRadius);
param("costmap_2d/inscribed_radius", inscribedRadius, inscribedRadius);
@@ -119,7 +121,7 @@
// Now allocate the cost map and its sliding window used by the controller
costMap_ = new CostMap2D((unsigned int)resp.map.width, (unsigned int)resp.map.height,
inputData , resp.map.resolution,
- windowLength, lethalObstacleThreshold, maxZ,
+ windowLength, lethalObstacleThreshold, maxZ, freeSpaceProjectionHeight,
inflationRadius, circumscribedRadius, inscribedRadius);
// Allocate Velocity Controller
Modified: pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h
===================================================================
--- pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h 2008-10-16 20:15:38 UTC (rev 5442)
+++ pkg/trunk/world_models/costmap_2d/include/costmap_2d/costmap_2d.h 2008-10-16 20:18:27 UTC (rev 5443)
@@ -88,14 +88,16 @@
* @param window_length how long to hold onto obstacle data [sec]
* @param threshold The cost threshold where a cell is considered an obstacle
* @param maxZ gives the cut-off for points in 3D space
+ * @param freeSpaceProjectionHeight gives the upper bound for evaluating points in z for projecting free space
* @param inflationRadius the radius used to bound inflation - limit of cost propagation
* @param circumscribedRadius the radius used to indicate objects in the circumscribed circle around the robot
* @param inscribedRadius the radius used to indicate objects in the inscribed circle around the robot
*/
CostMap2D(unsigned int width, unsigned int height, const std::vector<unsigned char>& data,
double resolution, double window_length,
- unsigned char threshold, double maxZ = 0, double inflationRadius = 0,
- double circumscribedRadius = 0, double inscribedRadius = 0);
+ unsigned char threshold,
+ double maxZ = 0, double freeSpaceProjectionHeight = 0,
+ double inflationRadius = 0, double circumscribedRadius = 0, double inscribedRadius = 0);
/**
* @brief Destructor.
@@ -227,6 +229,7 @@
static const TICK WATCHDOG_LIMIT = 255; /**< The value for a reset watchdog time for observing dynamic obstacles */
const double tickLength_; /**< The duration in seconds of a tick, used to manage the watchdog timeout on obstacles. Computed from window length */
const double maxZ_; /**< Points above this will be excluded from consideration */
+ const double freeSpaceProjectionHeight_; /**< Filters points for free space projection */
const unsigned int inflationRadius_; /**< The radius in meters to propagate cost and obstacle information */
const unsigned int circumscribedRadius_;
const unsigned int inscribedRadius_;
Modified: pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp 2008-10-16 20:15:38 UTC (rev 5442)
+++ pkg/trunk/world_models/costmap_2d/src/costmap_2d.cpp 2008-10-16 20:18:27 UTC (rev 5443)
@@ -69,11 +69,12 @@
CostMap2D::CostMap2D(unsigned int width, unsigned int height, const std::vector<unsigned char>& data,
double resolution, double window_length, unsigned char threshold,
- double maxZ, double inflationRadius,
- double circumscribedRadius, double inscribedRadius)
+ double maxZ, double freeSpaceProjectionHeight,
+ double inflationRadius, double circumscribedRadius, double inscribedRadius)
: ObstacleMapAccessor(0, 0, width, height, resolution),
tickLength_(window_length/WATCHDOG_LIMIT),
maxZ_(maxZ),
+ freeSpaceProjectionHeight_(freeSpaceProjectionHeight),
inflationRadius_(toCellDistance(inflationRadius, (unsigned int) ceil(width * resolution), resolution)),
circumscribedRadius_(toCellDistance(circumscribedRadius, inflationRadius_, resolution)),
inscribedRadius_(toCellDistance(inscribedRadius, circumscribedRadius_, resolution)),
@@ -153,8 +154,11 @@
unsigned int ind = WC_IND(cloud.pts[i].x, cloud.pts[i].y);
queue.push(std::make_pair(0, ind));
- // Immediately update free space, which is dominated by propagated costs so they are applied afterwards
- updateFreeSpace(ind, updates);
+ // Immediately update free space, which is dominated by propagated costs so they are applied afterwards.
+ // This only applies for points with the projection height limit
+
+ if(cloud.pts[i].z <= freeSpaceProjectionHeight_)
+ updateFreeSpace(ind, updates);
}
// Propagate costs
Modified: pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc
===================================================================
--- pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc 2008-10-16 20:15:38 UTC (rev 5442)
+++ pkg/trunk/world_models/costmap_2d/src/test/module-tests.cc 2008-10-16 20:18:27 UTC (rev 5443)
@@ -269,7 +269,7 @@
* Test inflation for both static and dynamic obstacles
*/
TEST(costmap, test7){
- CostMap2D map(GRID_WIDTH, GRID_HEIGHT, MAP_10_BY_10, RESOLUTION, WINDOW_LENGTH, THRESHOLD, MAX_Z,
+ CostMap2D map(GRID_WIDTH, GRID_HEIGHT, MAP_10_BY_10, RESOLUTION, WINDOW_LENGTH, THRESHOLD, MAX_Z, MAX_Z,
ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS);
@@ -354,7 +354,7 @@
* Test specific inflation scenario to ensure we do not set inflated obstacles to be raw obstacles.
*/
TEST(costmap, test8){
- CostMap2D map(GRID_WIDTH, GRID_HEIGHT, MAP_10_BY_10, RESOLUTION, WINDOW_LENGTH, THRESHOLD, MAX_Z,
+ CostMap2D map(GRID_WIDTH, GRID_HEIGHT, MAP_10_BY_10, RESOLUTION, WINDOW_LENGTH, THRESHOLD, MAX_Z, MAX_Z,
ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS);
std::vector<unsigned int> updates;
@@ -388,7 +388,7 @@
}
}
- CostMap2D map(GRID_WIDTH, GRID_HEIGHT, mapData, RESOLUTION, WINDOW_LENGTH, THRESHOLD, MAX_Z,
+ CostMap2D map(GRID_WIDTH, GRID_HEIGHT, mapData, RESOLUTION, WINDOW_LENGTH, THRESHOLD, MAX_Z, MAX_Z,
ROBOT_RADIUS * 3, ROBOT_RADIUS * 2, ROBOT_RADIUS);
// There should be no occupied cells
@@ -427,7 +427,7 @@
* Test for the cost map accessor
*/
TEST(costmap, test10){
- CostMap2D map(GRID_WIDTH, GRID_HEIGHT, MAP_10_BY_10, RESOLUTION, WINDOW_LENGTH, THRESHOLD, MAX_Z, ROBOT_RADIUS);
+ CostMap2D map(GRID_WIDTH, GRID_HEIGHT, MAP_10_BY_10, RESOLUTION, WINDOW_LENGTH, THRESHOLD, MAX_Z, MAX_Z, ROBOT_RADIUS);
// A window around a robot in the top left
CostMapAccessor ma(map, 5, 0, 0);
@@ -464,7 +464,7 @@
* Test for ray tracing free space
*/
TEST(costmap, test11){
- CostMap2D map(GRID_WIDTH, GRID_HEIGHT, MAP_10_BY_10, RESOLUTION, WINDOW_LENGTH, THRESHOLD, MAX_Z, ROBOT_RADIUS);
+ CostMap2D map(GRID_WIDTH, GRID_HEIGHT, MAP_10_BY_10, RESOLUTION, WINDOW_LENGTH, THRESHOLD, MAX_Z * 2, MAX_Z, ROBOT_RADIUS);
// The initial position will be <0,0> by default. So if we add an obstacle at 9,9, we would expect cells
// <0, 0> thru <7, 7> to be free. This is despite the fact that some of these cells are not zero cost in the
@@ -473,7 +473,7 @@
c0.set_pts_size(1);
c0.pts[0].x = 9.5;
c0.pts[0].y = 9.5;
- c0.pts[0].z = 0;
+ c0.pts[0].z = MAX_Z;
std::vector<unsigned int> updates;
map.updateDynamicObstacles(1, c0, updates);
@@ -504,6 +504,17 @@
// setting NO_INFORMATION to free space. Minor redundant cell update
map.updateDynamicObstacles(WINDOW_LENGTH + 2, 0.5, 9.5, c0, updates);
ASSERT_EQ(updates.size(), 6);
+
+ // Stale out all dynamic obstacles - then try again with point that is beyond free space projection
+ map.removeStaleObstacles(WINDOW_LENGTH * 3, updates);
+ std_msgs::PointCloudFloat32 c1;
+ c1.set_pts_size(1);
+ c1.pts[0].x = 9.5;
+ c1.pts[0].y = 9.5;
+ c1.pts[0].z = MAX_Z + 1;
+ map.updateDynamicObstacles(1, c1, updates);
+ ASSERT_EQ(updates.size(), 4); // Just obstacle cost propagation - no free space impact
+
}
int main(int argc, char** argv){
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2008-10-16 23:28:57
|
Revision: 5458
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5458&view=rev
Author: stuglaser
Date: 2008-10-16 23:28:55 +0000 (Thu, 16 Oct 2008)
Log Message:
-----------
Fixed velocity computations in FK
Modified Paths:
--------------
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_velocity_controller.cpp
pkg/trunk/mechanism/mechanism_model/src/link.cpp
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_velocity_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_velocity_controller.cpp 2008-10-16 23:28:38 UTC (rev 5457)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_velocity_controller.cpp 2008-10-16 23:28:55 UTC (rev 5458)
@@ -86,17 +86,15 @@
effort_.command_[1] = -pid_y_.updatePid(error.y(), time - last_time_);
effort_.command_[2] = -pid_z_.updatePid(error.z(), time - last_time_);
- //effort_.update();
+ effort_.update();
last_time_ = time;
}
void CartesianVelocityController::getTipVelocity(tf::Vector3 *v)
{
- tf::Vector3 abs_position =
- tip_->abs_position_ + quatRotate(tip_->abs_orientation_, effort_.offset_);
-
- *v = tip_->abs_velocity_ + cross(tip_->abs_rot_velocity_, abs_position);
+ *v = tip_->abs_velocity_ + cross(tip_->abs_rot_velocity_,
+ quatRotate(tip_->abs_orientation_, effort_.offset_));
}
Modified: pkg/trunk/mechanism/mechanism_model/src/link.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_model/src/link.cpp 2008-10-16 23:28:38 UTC (rev 5457)
+++ pkg/trunk/mechanism/mechanism_model/src/link.cpp 2008-10-16 23:28:55 UTC (rev 5458)
@@ -143,10 +143,11 @@
abs_velocity_ =
p->abs_velocity_
- + cross(p->abs_rot_velocity_, abs_position_)
- + quatRotate(abs_orientation_, j->getTransVelocity());
+ + cross(p->abs_rot_velocity_, quatRotate(p->abs_orientation_,
+ link_->origin_xyz_))
+ + quatRotate(p->abs_orientation_, j->getTransVelocity());
- abs_rot_velocity_ = p->abs_rot_velocity_ + quatRotate(abs_orientation_, j->getRotVelocity());
+ abs_rot_velocity_ = p->abs_rot_velocity_ + quatRotate(p->abs_orientation_, j->getRotVelocity());
// Computes the relative frame transform
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rob...@us...> - 2008-10-17 00:38:10
|
Revision: 5468
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5468&view=rev
Author: rob_wheeler
Date: 2008-10-17 00:38:00 +0000 (Fri, 17 Oct 2008)
Log Message:
-----------
Bringup motor control boards more slowly (one per ms)
Modified Paths:
--------------
pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h
pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp
pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp
Modified: pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h 2008-10-17 00:34:45 UTC (rev 5467)
+++ pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h 2008-10-17 00:38:00 UTC (rev 5468)
@@ -99,6 +99,9 @@
unsigned char *buffers_;
unsigned int buffer_size_;
+ bool halt_motors_;
+ unsigned int reset_state_;
+
misc_utils::RealtimePublisher<robot_msgs::DiagnosticMessage> publisher_;
struct {
struct {
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp 2008-10-17 00:34:45 UTC (rev 5467)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp 2008-10-17 00:38:00 UTC (rev 5468)
@@ -40,7 +40,7 @@
#include <ros/node.h>
EthercatHardware::EthercatHardware() :
- hw_(0), ni_(0), current_buffer_(0), last_buffer_(0), buffer_size_(0), publisher_("/diagnostics", 1)
+ hw_(0), ni_(0), current_buffer_(0), last_buffer_(0), buffer_size_(0), reset_state_(0), publisher_("/diagnostics", 1)
{
}
@@ -265,27 +265,40 @@
// Convert HW Interface commands to MCB-specific buffers
current = current_buffer_;
+ memset(current, 0, buffer_size_);
+
+ if (reset)
+ {
+ reset_state_ = num_slaves_;
+ }
+
for (unsigned int s = 0, a = 0; s < num_slaves_; ++s)
{
if (slaves_[s]->has_actuator_)
{
- slaves_[s]->computeCurrent(hw_->actuators_[a]->command_);
- hw_->actuators_[a]->state_.last_requested_effort_ = hw_->actuators_[a]->command_.effort_;
- hw_->actuators_[a]->state_.last_requested_current_ = hw_->actuators_[a]->command_.current_;
- slaves_[s]->truncateCurrent(hw_->actuators_[a]->command_);
- if (reset) {
- bool tmp = hw_->actuators_[a]->command_.enable_;
- hw_->actuators_[a]->command_.enable_ = false;
- slaves_[s]->convertCommand(hw_->actuators_[a]->command_, current);
- hw_->actuators_[a]->command_.enable_ = tmp;
+ Actuator *act = hw_->actuators_[a];
+ slaves_[s]->computeCurrent(act->command_);
+ act->state_.last_requested_effort_ = act->command_.effort_;
+ act->state_.last_requested_current_ = act->command_.current_;
+ slaves_[s]->truncateCurrent(act->command_);
+ if (reset_state_ && s < reset_state_)
+ {
+ bool tmp = act->command_.enable_;
+ act->command_.enable_ = false;
+ act->command_.current_ = 0;
+ slaves_[s]->convertCommand(act->command_, current);
+ act->command_.enable_ = tmp;
} else {
- slaves_[s]->convertCommand(hw_->actuators_[a]->command_, current);
+ slaves_[s]->convertCommand(act->command_, current);
}
current += slaves_[s]->command_size_ + slaves_[s]->status_size_;
++a;
}
}
+ if (reset_state_)
+ --reset_state_;
+
// Transmit process data
double start = now();
em_->txandrx_PD(buffer_size_, current_buffer_);
@@ -298,8 +311,9 @@
{
if (slaves_[s]->has_actuator_)
{
- slaves_[s]->convertState(hw_->actuators_[a]->state_, current, last);
- slaves_[s]->verifyState(hw_->actuators_[a]->state_, current, last);
+ Actuator *act = hw_->actuators_[a];
+ slaves_[s]->convertState(act->state_, current, last);
+ slaves_[s]->verifyState(act->state_, current, last);
current += slaves_[s]->command_size_ + slaves_[s]->status_size_;
last += slaves_[s]->command_size_ + slaves_[s]->status_size_;
++a;
Modified: pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp
===================================================================
--- pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp 2008-10-17 00:34:45 UTC (rev 5467)
+++ pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp 2008-10-17 00:38:00 UTC (rev 5468)
@@ -75,7 +75,7 @@
}
static int g_quit = 0;
-static bool g_reset_motors = false;
+static bool g_reset_motors = true;
static const int NSEC_PER_SEC = 1e+9;
static struct
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-10-17 18:47:44
|
Revision: 5493
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5493&view=rev
Author: tfoote
Date: 2008-10-17 18:47:35 +0000 (Fri, 17 Oct 2008)
Log Message:
-----------
adding teleop_head
Modified Paths:
--------------
pkg/trunk/nav/teleop_base/CMakeLists.txt
pkg/trunk/nav/teleop_base/manifest.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/joy_pr2_roslaunch.xml
Added Paths:
-----------
pkg/trunk/nav/teleop_base/teleop_head.cpp
Modified: pkg/trunk/nav/teleop_base/CMakeLists.txt
===================================================================
--- pkg/trunk/nav/teleop_base/CMakeLists.txt 2008-10-17 17:55:28 UTC (rev 5492)
+++ pkg/trunk/nav/teleop_base/CMakeLists.txt 2008-10-17 18:47:35 UTC (rev 5493)
@@ -2,3 +2,4 @@
include(rosbuild)
rospack(teleop_base)
rospack_add_executable(teleop_base teleop_base.cpp)
+rospack_add_executable(teleop_head teleop_head.cpp)
Modified: pkg/trunk/nav/teleop_base/manifest.xml
===================================================================
--- pkg/trunk/nav/teleop_base/manifest.xml 2008-10-17 17:55:28 UTC (rev 5492)
+++ pkg/trunk/nav/teleop_base/manifest.xml 2008-10-17 18:47:35 UTC (rev 5493)
@@ -3,4 +3,5 @@
<license>BSD</license>
<depend package="joy"/>
<depend package="std_msgs"/>
+ <depend package="pr2_mechanism_controllers"/>
</package>
Added: pkg/trunk/nav/teleop_base/teleop_head.cpp
===================================================================
--- pkg/trunk/nav/teleop_base/teleop_head.cpp (rev 0)
+++ pkg/trunk/nav/teleop_base/teleop_head.cpp 2008-10-17 18:47:35 UTC (rev 5493)
@@ -0,0 +1,109 @@
+#include <unistd.h>
+#include <math.h>
+#include "ros/node.h"
+#include "joy/Joy.h"
+
+#include "pr2_mechanism_controllers/SetJointCmd.h"
+
+using namespace ros;
+
+class TeleopHead : public node
+{
+ public:
+ // std_msgs::HeadVel cmd, cmd_passthrough;
+ joy::Joy joy;
+ double req_pan, req_tilt, max_pan, max_tilt;
+ int axis_pan, axis_tilt;
+ int deadman_button, passthrough_button;
+ double pan_step, tilt_step;
+ bool deadman_no_publish_;
+
+
+ TeleopHead(bool deadman_no_publish = false) : node("teleop_head"), max_pan(0.6), max_tilt(0.4), pan_step(0.1), tilt_step(0.1), deadman_no_publish_(deadman_no_publish)
+ {
+ // cmd.vx = cmd.vy = cmd.vw = 0;
+ if (!has_param("max_pan") || !get_param("max_pan", max_pan))
+ log(WARNING, "maximum pan not set. Assuming 0.6");
+ if (!has_param("max_tilt") || !get_param("max_tilt", max_tilt))
+ log(WARNING, "maximum tilt not set. Assuming 0.4");
+
+ param<int>("axis_pan", axis_pan, 4);
+ param<int>("axis_tilt", axis_tilt, 5);
+ param<int>("deadman_button", deadman_button, 0);
+ param<int>("passthrough_button", passthrough_button, 1);
+
+ /* printf("max_vx: %.3f m/s\n", max_vx);
+ printf("max_vy: %.3f m/s\n", max_vy);
+ printf("max_vw: %.3f deg/s\n", max_vw*180.0/M_PI);
+ printf("axis_vx: %d\n", axis_vx);
+ printf("axis_vy: %d\n", axis_vy);
+ printf("axis_vw: %d\n", axis_vw);
+ printf("deadman_button: %d\n", deadman_button);
+ printf("passthrough_button: %d\n", passthrough_button);
+ */
+ // advertise<std_msgs::HeadVel>("cmd_vel", 1);
+ subscribe("joy", joy, &TeleopHead::joy_cb, 1);
+ //subscribe("cmd_passthrough", cmd_passthrough, &TeleopHead::passthrough_cb, 1);
+ printf("done with ctor\n");
+ }
+ void joy_cb()
+ {
+ /*
+ printf("axes: ");
+ for(int i=0;i<joy.get_axes_size();i++)
+ printf("%.3f ", joy.axes[i]);
+ puts("");
+ printf("buttons: ");
+ for(int i=0;i<joy.get_buttons_size();i++)
+ printf("%d ", joy.buttons[i]);
+ puts("");
+ */
+
+ if((axis_pan >= 0) && (((unsigned int)axis_pan) < joy.get_axes_size()))
+ {
+ req_pan += joy.axes[axis_pan] * pan_step;
+ req_pan = std::max(std::min(req_pan, max_pan), -max_pan);
+ }
+
+ if ((axis_tilt >= 0) && (((unsigned int)axis_tilt) < joy.get_axes_size()))
+ {
+ req_tilt += joy.axes[axis_tilt] * tilt_step;
+ req_tilt = std::max(std::min(req_tilt, max_tilt), -max_tilt);
+ }
+
+ pr2_mechanism_controllers::SetJointCmd::request req;
+ pr2_mechanism_controllers::SetJointCmd::response res;
+ req.positions.push_back(req_pan);
+ req.positions.push_back(req_tilt);
+ req.velocity.push_back(0.0);
+ req.velocity.push_back(0.0);
+ req.acc.push_back(0.0);
+ req.acc.push_back(0.0);
+ req.names.push_back("head_pan_joint");
+ req.names.push_back("head_tilt_joint");
+ ros::service::call("head_controller/set_command_array", req, res);
+
+ }
+};
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv);
+ const char* opt_no_publish = "--deadman_no_publish";
+
+ bool no_publish = false;
+ for(int i=1;i<argc;i++)
+ {
+ if(!strncmp(argv[i], opt_no_publish, strlen(opt_no_publish)))
+ no_publish = true;
+ }
+ TeleopHead teleop_base(no_publish);
+ while (teleop_base.ok())
+ {
+ usleep(50000);
+ }
+ ros::fini();
+ exit(0);
+ return 0;
+}
+
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/joy_pr2_roslaunch.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/joy_pr2_roslaunch.xml 2008-10-17 17:55:28 UTC (rev 5492)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/joy_pr2_roslaunch.xml 2008-10-17 18:47:35 UTC (rev 5493)
@@ -1,8 +1,10 @@
<launch>
<group name="wg">
<param name="axis_vx" value="3" type="int"/>
-<param name="axis_vy" value="0" type="int"/>
-<param name="axis_vw" value="2" type="int"/>
+<param name="axis_vy" value="2" type="int"/>
+<param name="axis_vw" value="0" type="int"/>
+<param name="pan" value="4" type="int"/>
+<param name="tilt" value="5" type="int"/>
<param name="max_vw" value="1.0" />
<param name="max_vx" value="0.6" />
<param name="max_vy" value="0.6" />
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2008-10-17 22:49:29
|
Revision: 5519
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5519&view=rev
Author: stuglaser
Date: 2008-10-17 22:49:22 +0000 (Fri, 17 Oct 2008)
Log Message:
-----------
Cleaner calibration for the prototype
Modified Paths:
--------------
pkg/trunk/mechanism/mechanism_bringup/scripts/calibrate.py
pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/cal_arm.xml
Added Paths:
-----------
pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/calibrate.launch
pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/cal_base.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/cal_head.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/cal_torso.xml
Modified: pkg/trunk/mechanism/mechanism_bringup/scripts/calibrate.py
===================================================================
--- pkg/trunk/mechanism/mechanism_bringup/scripts/calibrate.py 2008-10-17 22:40:25 UTC (rev 5518)
+++ pkg/trunk/mechanism/mechanism_bringup/scripts/calibrate.py 2008-10-17 22:49:22 UTC (rev 5519)
@@ -37,7 +37,7 @@
import rostools
import copy
import threading
-import sys
+import sys, os
from time import sleep
# Loads interface with the robot.
@@ -95,7 +95,8 @@
xml = ''
if len(sys.argv) > 1:
- xmls = [slurp(filename) for filename in sys.argv[1:]]
+ #xmls = [slurp(filename) for filename in sys.argv[1:]]
+ xmls = [os.popen2("rosrun xacro xacro.py %s" % f)[1].read() for f in sys.argv[1:]]
# Poor man's xml splicer
for i in range(len(xmls) - 1):
Added: pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/calibrate.launch
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/calibrate.launch (rev 0)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_prototype1/calibrate.launch 2008-10-17 22:49:22 UTC (rev 5519)
@@ -0,0 +1,7 @@
+<launch>
+ <node pkg="mechanism_bringup" type="calibrate.py"
+ args="$(find wg_robot_description)/pr2_robot_defs/cal_tilt_laser.xml
+ $(find wg_robot_description)/pr2_robot_defs/cal_base.xml
+ $(find wg_robot_description)/pr2_robot_defs/cal_head.xml
+ $(find wg_robot_description)/pr2_robot_defs/cal_torso.xml" />
+</launch>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/cal_arm.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/cal_arm.xml 2008-10-17 22:40:25 UTC (rev 5518)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/cal_arm.xml 2008-10-17 22:49:22 UTC (rev 5519)
@@ -1,17 +1,28 @@
<controllers>
- <controller name="cal_shoulder_pan" topic="cal_shoulder_pan" type="JointCalibrationControllerNode">
- <calibrate joint="shoulder_pan_right_joint"
- actuator="shoulder_pan_right_motor"
- transmission="shoulder_pan_right_trans"
- velocity="0.6" />
- <pid p="7" i="0.5" d="0" iClamp="1.0" />
- </controller>
- <controller name="cal_shoulder_pitch" topic="cal_shoulder_pitch" type="JointCalibrationControllerNode">
- <calibrate joint="shoulder_pitch_right_joint"
- actuator="shoulder_pitch_right_motor"
- transmission="shoulder_pitch_right_trans"
- velocity="0.6" />
- <pid p="7" i="0.5" d="0" iClamp="1.0" />
- </controller>
+ <macro name="shoulder_pan_calibrator" params="suffix">
+ <controller name="cal_shoulder_pan_${suffix}" topic="cal_shoulder_pan_${suffix}"
+ type="JointCalibrationControllerNode">
+ <calibrate joint="shoulder_pan_${suffix}_joint"
+ actuator="shoulder_pan_${suffix}_motor"
+ transmission="shoulder_pan_${suffix}_trans"
+ velocity="0.6" />
+ <pid p="7" i="0.5" d="0" iClamp="1.0" />
+ </controller>
+ </macro>
+
+ <macro name="shoulder_pitch_calibrator" params="suffix">
+ <controller name="cal_shoulder_pitch_${suffix}" topic="cal_shoulder_pitch_${suffix}"
+ type="JointCalibrationControllerNode">
+ <calibrate joint="shoulder_pitch_${suffix}_joint"
+ actuator="shoulder_pitch_${suffix}_motor"
+ transmission="shoulder_pitch_${suffix}_trans"
+ velocity="0.6" />
+ <pid p="7" i="0.5" d="0" iClamp="1.0" />
+ </controller>
+ </macro>
+
+ <shoulder_pan_calibrator suffix="right" />
+ <shoulder_pitch_calibrator suffix="right" />
+
</controllers>
Added: pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/cal_base.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/cal_base.xml (rev 0)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/cal_base.xml 2008-10-17 22:49:22 UTC (rev 5519)
@@ -0,0 +1,23 @@
+<controllers>
+ <macro name="caster_calibrator" params="suffix">
+ <controller type="CasterCalibrationControllerNode"
+ name="cal_caster_${suffix}" topic="cal_caster_${suffix}">
+ <calibrate joint="caster_${suffix}_joint"
+ actuator="caster_${suffix}_motor"
+ transmission="caster_${suffix}_trans"
+ velocity="1.0" />
+ <joints caster="caster_${suffix}_joint"
+ wheel_l="wheel_${suffix}_l_joint"
+ wheel_r="wheel_${suffix}_r_joint" />
+ <caster_pid p="6" i="0" d="0" iClamp="0" />
+ <wheel_pid p="4" i="0" d="0" iClamp="0" />
+ </controller>
+ </macro>
+
+
+ <caster_calibrator suffix="front_left" />
+ <caster_calibrator suffix="front_right" />
+ <caster_calibrator suffix="rear_left" />
+ <caster_calibrator suffix="rear_right" />
+
+</controllers>
Added: pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/cal_head.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/cal_head.xml (rev 0)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/cal_head.xml 2008-10-17 22:49:22 UTC (rev 5519)
@@ -0,0 +1,21 @@
+<controllers>
+
+ <controller name="cal_head_pan" topic="cal_head_pan"
+ type="JointCalibrationControllerNode">
+ <calibrate joint="head_pan_joint"
+ actuator="head_pan_motor"
+ transmission="head_pan_trans"
+ velocity="1.5" />
+ <pid p="0.4" i="0.0" d="0" iClamp="1.0" />
+ </controller>
+
+ <controller name="cal_head_tilt" topic="cal_head_tilt"
+ type="JointCalibrationControllerNode">
+ <calibrate joint="head_tilt_joint"
+ actuator="head_tilt_motor"
+ transmission="head_tilt_trans"
+ velocity="0.7" />
+ <pid p="0.3" i="0.0" d="0" iClamp="5.0" />
+ </controller>
+
+</controllers>
Added: pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/cal_torso.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/cal_torso.xml (rev 0)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_robot_defs/cal_torso.xml 2008-10-17 22:49:22 UTC (rev 5519)
@@ -0,0 +1,10 @@
+<controllers>
+ <controller type="JointCalibrationControllerNode"
+ name="cal_torso" topic="cal_torso">
+ <calibrate joint="torso_joint"
+ actuator="torso_motor"
+ transmission="torso_trans"
+ velocity="9.0" />
+ <pid p="1.0" i="0.4" d="0" iClamp="2" />
+ </controller>
+</controllers>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <vij...@us...> - 2008-10-18 02:18:17
|
Revision: 5531
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5531&view=rev
Author: vijaypradeep
Date: 2008-10-18 02:18:11 +0000 (Sat, 18 Oct 2008)
Log Message:
-----------
Added timestamp to laser_scanner_signal. Added cloud service request to point_cloud_assembler
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/msg/LaserScannerSignal.msg
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/CMakeLists.txt
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/manifest.xml
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler.cpp
Added Paths:
-----------
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/grab_cloud_data.cpp
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_snapshotter.cpp
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/srv/
pkg/trunk/drivers/robot/pr2/point_cloud_assembler/srv/BuildCloud.srv
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/msg/LaserScannerSignal.msg
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/msg/LaserScannerSignal.msg 2008-10-18 02:06:27 UTC (rev 5530)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/msg/LaserScannerSignal.msg 2008-10-18 02:18:11 UTC (rev 5531)
@@ -1,3 +1,7 @@
+
+
+Header header
+
# signal == 0 => Half profile complete
# signal == 1 => Full Profile Complete
byte signal
Modified: pkg/trunk/drivers/robot/pr2/point_cloud_assembler/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/robot/pr2/point_cloud_assembler/CMakeLists.txt 2008-10-18 02:06:27 UTC (rev 5530)
+++ pkg/trunk/drivers/robot/pr2/point_cloud_assembler/CMakeLists.txt 2008-10-18 02:18:11 UTC (rev 5531)
@@ -1,8 +1,9 @@
cmake_minimum_required(VERSION 2.6)
include(rosbuild)
rospack(point_cloud_assembler)
-#genmsg()
-#find_library(curl REQUIRED)
+gensrv()
rospack_add_executable(point_cloud_assembler src/point_cloud_assembler.cpp)
+rospack_add_executable(grab_cloud_data src/grab_cloud_data.cpp)
+rospack_add_executable(point_cloud_snapshotter src/point_cloud_snapshotter.cpp)
Modified: pkg/trunk/drivers/robot/pr2/point_cloud_assembler/manifest.xml
===================================================================
--- pkg/trunk/drivers/robot/pr2/point_cloud_assembler/manifest.xml 2008-10-18 02:06:27 UTC (rev 5530)
+++ pkg/trunk/drivers/robot/pr2/point_cloud_assembler/manifest.xml 2008-10-18 02:18:11 UTC (rev 5531)
@@ -1,17 +1,15 @@
<package>
-<description brief="A node for assembling point clouds out of laser scanlines.">
-
-This node accumulates laser range finder scan lines into a point cloud.
-
-</description>
-<author>Caroline Pantofaru & Jeremy Leibs</author>
-<license>BSD</license>
-<url>http://pr.willowgarage.com/wiki/Point_clouds_from_scanning_URG</url>
-<depend package="roscpp"/>
-<depend package="std_msgs"/>
-<depend package="laser_scan_utils"/>
-<depend package="rosTF"/>
-<depend package="libTF"/>
-<depend package="pr2_mechanism_controllers"/>
+ <description brief="A node for assembling point clouds out of laser scanlines.">
+ This node accumulates laser range finder scan lines into a point cloud.
+ </description>
+ <author>Caroline Pantofaru, Jeremy Leibs, Vijay Pradeep</author>
+ <license>BSD</license>
+ <url>http://pr.willowgarage.com/wiki/Point_clouds_from_scanning_URG</url>
+ <depend package="roscpp"/>
+ <depend package="std_msgs"/>
+ <depend package="laser_scan_utils"/>
+ <depend package="rosTF"/>
+ <depend package="libTF"/>
+ <depend package="pr2_mechanism_controllers"/>
</package>
Added: pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/grab_cloud_data.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/grab_cloud_data.cpp (rev 0)
+++ pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/grab_cloud_data.cpp 2008-10-18 02:18:11 UTC (rev 5531)
@@ -0,0 +1,113 @@
+/*********************************************************************
+* 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"
+
+
+
+// Services
+#include "point_cloud_assembler/BuildCloud.h"
+
+// Messages
+#include "std_msgs/PointCloudFloat32.h"
+
+
+using namespace std_msgs ;
+
+/***
+ * This a simple test app that requests a point cloud from the point_cloud_assembler, and then publishes the resulting data
+ */
+
+namespace point_cloud_assembler
+{
+
+class GrabCloudData : public ros::node
+{
+
+public:
+
+ GrabCloudData() : ros::node("grab_cloud_data")
+ {
+ advertise<PointCloudFloat32> ("full_cloud", 1) ;
+ }
+
+ ~GrabCloudData()
+ {
+ unadvertise("full_cloud") ;
+ }
+
+ bool spin()
+ {
+ ros::Duration period(4,0) ; // Repeat Every 4 seconds
+
+ ros::Time next_time = ros::Time::now() ;
+
+ while ( ok() )
+ {
+ usleep(100) ;
+ if (ros::Time::now() >= next_time)
+ {
+ next_time.from_double(next_time.to_double() + period.to_double()) ;
+
+ BuildCloud::request req ;
+ BuildCloud::response resp ;
+
+ req.begin.from_double(next_time.to_double() - period.to_double() ) ;
+ req.end = next_time ;
+ req.target_frame_id = "base" ;
+
+ printf("Making Service Call...\n") ;
+ ros::service::call("build_cloud", req, resp) ;
+ printf("Done with service call\n") ;
+
+ publish("full_cloud", resp.cloud) ;
+ printf("Published Cloud size=%u\n", resp.cloud.get_pts_size()) ;
+ }
+ }
+ return true ;
+ }
+} ;
+
+}
+
+using namespace point_cloud_assembler ;
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv);
+ GrabCloudData grabber ;
+ grabber.spin();
+ ros::fini();
+ return 0;
+}
Modified: pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler.cpp 2008-10-18 02:06:27 UTC (rev 5530)
+++ pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_assembler.cpp 2008-10-18 02:18:11 UTC (rev 5531)
@@ -56,18 +56,10 @@
Subscribes to (name/type):
- @b "scan"/<a href="../../std_msgs/html/classstd__msgs_1_1LaserScan.html">LaserScan</a> : laser scan data
-Publishes to (name / type):
-- @b "cloud"/<a href="../../std_msgs/html/classstd__msgs_1_1PointCloudFloat32.html">PointCloudFloat32</a> : Incremental cloud data. Each scan from the laser is converted into a PointCloud.
-- @b "full_cloud"/<a href="../../std_msgs/html/classstd__msgs_1_1PointCloudFloat32.html">PointCloudFloat32</a> : A full point cloud containing all of the points between the last two shutters
-
<hr>
@section parameters ROS parameters
-Reads the following parameters from the parameter server
-
-- @b "tilting_laser/num_scans" : @b [int] the number of scans to take between the min and max angles (Default: 400)
-
**/
@@ -75,15 +67,25 @@
#include "rosTF/rosTF.h"
#include "std_msgs/LaserScan.h"
#include "std_msgs/PointCloudFloat32.h"
-#include "pr2_mechanism_controllers/LaserScannerSignal.h"
+
+#include <deque>
-//Laser projection
+
+// Service
+#include "point_cloud_assembler/BuildCloud.h"
+
+// Laser projection
#include "laser_scan_utils/laser_scan.h"
#include "math.h"
using namespace std_msgs;
+using namespace std ;
+
+namespace point_cloud_assembler
+{
+
class PointCloudAssembler : public ros::node
{
public:
@@ -91,148 +93,106 @@
rosTFClient tf_;
laser_scan::LaserProjection projector_;
- PointCloudFloat32 cur_scan_cloud_;
+ LaserScan scan_;
- LaserScan scans_;
- pr2_mechanism_controllers::LaserScannerSignal scanner_signal_;
+ unsigned int max_scans_;
- PointCloudFloat32 full_cloud_;
- int full_cloud_cnt_;
-
- int num_scans_;
-
- /* Reinstate to let user control the angles.
- double min_ang;
- double max_ang; */
-
- //ros::Time last_motor_time;
- int count_; // This just counts the number of scans per second for output. It's unnecessary.
- ros::Time next_time_;
-
- bool start_scan_;
- bool stop_scan_;
- bool scanning_;
-
- PointCloudAssembler() : ros::node("point_cloud_assembler"), tf_(*this), full_cloud_cnt_(0), count_(0), start_scan_(false), stop_scan_(false), scanning_(false)
+ deque<LaserScan> scan_hist_ ; //!< Stores history of scans. We want them in time-ordered, which is (in most situations) the same as time-of-receipt-ordered
+ unsigned int total_pts_ ; //!< Stores the total number of range points in the entire stored history of scans. Useful for estimating points/scan
+
+
+ PointCloudAssembler() : ros::node("point_cloud_assembler"), tf_(*this)
{
-
- advertise<PointCloudFloat32>("cloud", 1);
- advertise<PointCloudFloat32>("full_cloud", 1);
+ advertise_service("build_cloud", &PointCloudAssembler::buildCloud, this, 0) ; // Don't spawn threads so that we can avoid dealing with mutexing [for now]
+ subscribe("scan", scan_, &PointCloudAssembler::scans_callback, 40) ;
- subscribe("scan", scans_, &PointCloudAssembler::scans_callback, 40);
- subscribe("laser_scanner_signal", scanner_signal_, &PointCloudAssembler::scanner_signal_callback, 40);
+ param("point_cloud_assembler/max_scans", max_scans_, (unsigned int) 400) ;
- param("point_cloud_assembler/num_scans", num_scans_, 400);
-
- //last_motor_time = ros::Time::now();
- next_time_ = ros::Time::now();
-
+ total_pts_ = 0 ; // We're always going to start with no points in our history
}
virtual ~PointCloudAssembler()
{
- unadvertise("cloud") ;
- unadvertise("full_cloud") ;
- //unsubscribe("scan") ;
- //unsubscribe("laser_scanner_signal") ;
+ unadvertise_service("build_cloud") ;
+ unsubscribe("scan") ;
}
void scans_callback()
{
- //printf("start %d stop %d scan %d\n", start_scan_, stop_scan_, scanning_);
-
- count_++;
- ros::Time now_time = ros::Time::now();
- if (now_time > next_time_)
+ if (scan_hist_.size() == max_scans_) // Is our deque full?
{
- //std::cout << count_ << " scans/sec at " << now_time << std::endl;
- count_ = 0;
- next_time_ = next_time_ + ros::Duration(1,0);
+ total_pts_ -= scan_hist_.front().get_ranges_size() ; // We're removing an elem, so this reduces our total point count
+ scan_hist_.pop_front() ; // The front of the deque has the oldest elem, so we can get rid of it
}
+
+ scan_hist_.push_back(scan_) ; // Add the newest scan to the back of the deque
+ total_pts_ += scan_.get_ranges_size() ; // Add the new scan to the running total of points
+
+ printf("Got Scan: TotalPoints=%u\n", total_pts_) ;
+ }
- // If told to stop scanning and you have some points in your full cloud, publish the cloud.
- if (stop_scan_)
- {
- if (scanning_)
- {
- // What happens if the point_cloud_cnt is 0? Should I still publish?
- full_cloud_.set_pts_size(full_cloud_cnt_);
- full_cloud_.chan[0].set_vals_size(full_cloud_cnt_);
- publish("full_cloud",full_cloud_);
- printf("PointCloudAssembler::Publishing scan of size %d.\n", full_cloud_cnt_);
- }
- full_cloud_cnt_ = 0;
- scanning_ = false;
- stop_scan_ = false;
- }
+ bool buildCloud(BuildCloud::request& req, BuildCloud::response& resp)
+ {
+ // Allocate space for the cloud
+ resp.cloud.set_pts_size( total_pts_ ) ; // There's no way to have a point cloud bigger than our entire history of scans.
+ resp.cloud.set_chan_size(1) ;
+ resp.cloud.chan[0].name = "intensities" ;
+ resp.cloud.chan[0].set_vals_size( total_pts_ ) ;
+
+ unsigned int cloud_count = 0 ; // Store the number of points in the current cloud
-
- // If you need to start scanning, allocate the cloud size.
- if (start_scan_)
+ PointCloudFloat32 projector_cloud ; // Stores the current scan after being projected into the laser frame
+ PointCloudFloat32 target_frame_cloud ; // Stores the current scan in the target frame
+
+ unsigned int i = 0 ;
+
+ // Find the beginning of the request. Probably should be a search
+ while ( i < scan_hist_.size() && // Don't go past end of deque
+ scan_hist_[i].header.stamp < req.begin ) // Keep stepping until we've exceeded the start time
{
- full_cloud_.set_pts_size(scans_.get_ranges_size()*num_scans_);
- full_cloud_.set_chan_size(1);
- full_cloud_.chan[0].name = "intensities";
- full_cloud_.chan[0].set_vals_size(scans_.get_ranges_size()*num_scans_);
- full_cloud_cnt_ = 0;
- scanning_ = true;
- start_scan_=false;
+ i++ ;
}
+ printf(" Start i=%u\n", i) ;
- // Mid-scan
- if (scanning_)
+ // Populate the cloud
+ while ( i < scan_hist_.size() && // Don't go past end of deque
+ scan_hist_[i].header.stamp < req.end ) // Don't go past the end-time of the request
{
- // CRP: Clouds are now stl vectors.
- // This will be the cloud storing points from the current single scan.
- cur_scan_cloud_.set_pts_size(scans_.get_ranges_size());
- cur_scan_cloud_.set_chan_size(1);
- cur_scan_cloud_.chan[0].name = "intensities";
- cur_scan_cloud_.chan[0].set_vals_size(scans_.get_ranges_size());
-
- //printf("size(cur_scan_cloud_) = %d\n", cur_scan_cloud_.pts.size()) ;
-
+ const LaserScan& cur_scan = scan_hist_[i] ;
- // Define cloud to store the current single scan after being projected into the laser frame by the projector
- std_msgs::PointCloudFloat32 projector_cloud ;
- // CRP: Will be a transform in TF that takes scan time into account. High-precision, recalc for every point, from scan to point cloud. More expensive. Wait for new library to do this.
- projector_.projectLaser(scans_, projector_cloud); // CRP: This takes care of converting the scan msg to the right format, so you don't really need to know what the format is.
-
- //printf("size(projector_cloud_) = %d\n", projector_cloud.pts.size()) ;
-
- cur_scan_cloud_ = tf_.transformPointCloud("base", projector_cloud); // CRP: We'll make people do this themselves for one scan line, we'll just tansform and publish aggregate clouds.
-
- //printf("size(cur_scan_cloud_ %d\n", cur_scan_cloud_.pts.size());
-
- publish("cloud", cur_scan_cloud_);
-
- full_cloud_.header = cur_scan_cloud_.header; //find a better place to do this/way to do this
- full_cloud_.header.stamp = ros::Time::now(); //HACK
-
- // CRP: Might make this more efficient by using stl vector functions instead.
- // CRP: Try this with full_cloud.pts.push_back(cloud.pts[i]);, and use an iterator to go through the elements. Actually, that's inefficient, you should index them. But perhaps I can just copy them wholesale?
- //Populate full_cloud from the cloud
- for(unsigned int i = 0; i < cur_scan_cloud_.get_pts_size(); i ++)
+ projector_.projectLaser(cur_scan, projector_cloud) ; // CRP: This takes care of converting the scan msg to the right format, so you don't really need to know what the format is.
+ // CRP: Will be a transform in TF that takes scan time into account. High-precision, recalc for every point, from scan to point cloud. More expensive. Wait for new library to do this.
+ target_frame_cloud=tf_.transformPointCloud(req.target_frame_id, projector_cloud) ;// CRP: We'll make people do this themselves for one scan line, we'll just tansform and publish aggregate clouds.
+
+ resp.cloud.header = target_frame_cloud.header ; // Find a better place to do this/way to do this
+ // full_cloud_.header.stamp = ros::Time::now(); //HACK
+
+ for(unsigned int j = 0; j < target_frame_cloud.get_pts_size(); j++) // Populate full_cloud from the cloud
{
- full_cloud_.pts[full_cloud_cnt_].x = cur_scan_cloud_.pts[i].x;
- full_cloud_.pts[full_cloud_cnt_].y = cur_scan_cloud_.pts[i].y;
- full_cloud_.pts[full_cloud_cnt_].z = cur_scan_cloud_.pts[i].z;
- full_cloud_.chan[0].vals[full_cloud_cnt_] = cur_scan_cloud_.chan[0].vals[i];
- full_cloud_cnt_++;
+ resp.cloud.pts[cloud_count].x = target_frame_cloud.pts[j].x;
+ resp.cloud.pts[cloud_count].y = target_frame_cloud.pts[j].y;
+ resp.cloud.pts[cloud_count].z = target_frame_cloud.pts[j].z;
+ resp.cloud.chan[0].vals[cloud_count] = target_frame_cloud.chan[0].vals[j];
+ cloud_count++ ;
}
+
+ i++ ; // Check the next scan in the scan history
}
- }
+
+ printf(" End i=%u\n", i) ;
+
+ resp.cloud.set_pts_size( cloud_count ) ; // Resize the output accordingly
+ resp.cloud.chan[0].set_vals_size( cloud_count ) ;
+
- // Msg callback for top/bottom msg.
- void scanner_signal_callback()
- {
- // If the scanner signal is 0, the scanner is at the bottom of it's sine wave, and at the top for 1.
- // I'm going to listen to both and produce one scan on the way down and one on the way up, so I don't actually care what the signal's value is.
- stop_scan_ = true;
- start_scan_ = true;
+ return true ;
}
-
};
+}
+
+using namespace point_cloud_assembler ;
+
int main(int argc, char **argv)
{
ros::init(argc, argv);
@@ -241,4 +201,3 @@
ros::fini();
return 0;
}
-
Added: pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_snapshotter.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_snapshotter.cpp (rev 0)
+++ pkg/trunk/drivers/robot/pr2/point_cloud_assembler/src/point_cloud_snapshotter.cpp 2008-10-18 02:18:11 UTC (rev 5531)
@@ -0,0 +1,105 @@
+/*********************************************************************
+* 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"
+
+// Services
+#include "point_cloud_assembler/BuildCloud.h"
+
+// Messages
+#include "std_msgs/PointCloudFloat32.h"
+#include "pr2_mechanism_controllers/LaserScannerSignal.h"
+
+using namespace std_msgs ;
+
+/***
+ * This uses the point_cloud_assembler's build_cloud service call to grab all the laser scans between two tilt-laser shutters
+ */
+
+namespace point_cloud_assembler
+{
+
+class PointCloudSnapshotter : public ros::node
+{
+
+public:
+
+ pr2_mechanism_controllers::LaserScannerSignal prev_signal_;
+ pr2_mechanism_controllers::LaserScannerSignal cur_signal_;
+
+ PointCloudSnapshotter() : ros::node("point_cloud_snapshotter")
+ {
+ prev_signal_.header.stamp.fromNSec(0) ;
+
+ advertise<PointCloudFloat32> ("full_cloud", 1) ;
+ subscribe("laser_scanner_signal", cur_signal_, &PointCloudSnapshotter::scannerSignalCallback, 40) ;
+ }
+
+ ~PointCloudSnapshotter()
+ {
+ unadvertise("full_cloud") ;
+ }
+
+ void scannerSignalCallback()
+ {
+ BuildCloud::request req ;
+ BuildCloud::response resp ;
+
+ req.begin = prev_signal_.header.stamp ;
+ req.end = cur_signal_.header.stamp ;
+ req.target_frame_id = "base" ;
+
+ printf("Making Service Call...\n") ;
+ ros::service::call("build_cloud", req, resp) ;
+ printf("Done with service call\n") ;
+
+ publish("full_cloud", resp.cloud) ;
+ printf("Published Cloud size=%u\n", resp.cloud.get_pts_size()) ;
+
+ prev_signal_ = cur_signal_ ;
+ }
+} ;
+
+}
+
+using namespace point_cloud_assembler ;
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv);
+ PointCloudSnapshotter snapshotter ;
+ snapshotter.spin();
+ ros::fini();
+ return 0;
+}
Added: pkg/trunk/drivers/robot/pr2/point_cloud_assembler/srv/BuildCloud.srv
===================================================================
--- pkg/trunk/drivers/robot/pr2/point_cloud_assembler/srv/BuildCloud.srv (rev 0)
+++ pkg/trunk/drivers/robot/pr2/point_cloud_assembler/srv/BuildCloud.srv 2008-10-18 02:18:11 UTC (rev 5531)
@@ -0,0 +1,5 @@
+time begin
+time end
+string target_frame_id
+---
+std_msgs/PointCloudFloat32 cloud
\ No newline at end of file
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-10-19 17:58:06
|
Revision: 5542
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5542&view=rev
Author: gerkey
Date: 2008-10-19 17:57:59 +0000 (Sun, 19 Oct 2008)
Log Message:
-----------
Changed publication of 'scan' to 'base_scan' in rosstage, to match
rosgazebo and PR2. Update roslaunch files to match, remapping scan to
base_scan for amcl_player and wavefront_player.
Modified Paths:
--------------
pkg/trunk/demos/2dnav_stage/2dnav_stage.xml
pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization.xml
pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_5cm.xml
pkg/trunk/demos/2dnav_stage/manifest.xml
pkg/trunk/simulators/rosstage/rosstage.cc
Modified: pkg/trunk/demos/2dnav_stage/2dnav_stage.xml
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage.xml 2008-10-19 17:53:26 UTC (rev 5541)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage.xml 2008-10-19 17:57:59 UTC (rev 5542)
@@ -1,8 +1,8 @@
<launch>
<group name="wg">
- <node pkg="rosstage" type="rosstage" args="$(find 2dnav-stage)/willow-erratic.world" respawn="false" output="screen"/>
- <node pkg="map_server" type="map_server" args="$(find 2dnav-stage)/willow-full.pgm 0.1" respawn="false" output="screen"/>
+ <node pkg="rosstage" type="rosstage" args="$(find 2dnav_stage)/willow-erratic.world" respawn="false" output="screen"/>
+ <node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/willow-full.pgm 0.1" respawn="false" output="screen"/>
<param name="pf_laser_max_beams" value="20"/>
<param name="pf_min_samples" value="500"/>
<param name="pf_max_samples" value="10000"/>
@@ -12,9 +12,14 @@
<param name="pf_odom_drift_xa" value="0.2"/>
<param name="pf_min_d" value="0.25"/>
<param name="pf_min_a" value="0.524"/>
- <node pkg="amcl_player" type="amcl_player" respawn="false" output="screen"/>
- <node pkg="wavefront_player" type="wavefront_player" respawn="false" output="screen" />
- <node pkg="nav_view" type="nav_view" respawn="false" output="screen"/>
+ <node pkg="amcl_player" type="amcl_player" respawn="false" output="screen">
+ <remap from="scan" to="base_scan" />
+ </node>
+ <node pkg="wavefront_player" type="wavefront_player" respawn="false" output="screen">
+ <remap from="scan" to="base_scan" />
+ </node>
+ <!-- <node pkg="nav_view" type="nav_view" respawn="false" output="screen"/> -->
+ <node pkg="nav_view_sdl" type="nav_view" respawn="false" output="screen"/> -->
</group>
</launch>
Modified: pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization.xml
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization.xml 2008-10-19 17:53:26 UTC (rev 5541)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization.xml 2008-10-19 17:57:59 UTC (rev 5542)
@@ -3,7 +3,9 @@
<group name="wg">
<node pkg="rosstage" type="rosstage" args="$(find 2dnav-stage)/willow-pr2.world" respawn="false" />
<node pkg="map_server" type="map_server" args="$(find 2dnav-stage)/willow-full.pgm 0.1" respawn="false" />
- <node pkg="wavefront_player" type="wavefront_player" respawn="false" />
+ <node pkg="wavefront_player" type="wavefront_player" respawn="false" >
+ <remap from="scan" to="base_scan" />
+ </node>
<node pkg="nav_view" type="nav_view" respawn="false"/>
<!-- TODO: remove this remap after fake_localization is fixed to listen
Modified: pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_5cm.xml
===================================================================
--- pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_5cm.xml 2008-10-19 17:53:26 UTC (rev 5541)
+++ pkg/trunk/demos/2dnav_stage/2dnav_stage_fake_localization_5cm.xml 2008-10-19 17:57:59 UTC (rev 5542)
@@ -3,7 +3,9 @@
<group name="wg">
<node pkg="rosstage" type="rosstage" args="$(find 2dnav-stage)/willow-pr2-5cm.world" respawn="false" />
<node pkg="map_server" type="map_server" args="$(find 2dnav-stage)/willow-full-0.05.pgm 0.05" respawn="false" />
- <node pkg="wavefront_player" type="wavefront_player" respawn="false" />
+ <node pkg="wavefront_player" type="wavefront_player" respawn="false" >
+ <remap from="scan" to="base_scan" />
+ </node>
<node pkg="nav_view" type="nav_view" respawn="false"/>
<!-- TODO: remove this remap after fake_localization is fixed to listen
Modified: pkg/trunk/demos/2dnav_stage/manifest.xml
===================================================================
--- pkg/trunk/demos/2dnav_stage/manifest.xml 2008-10-19 17:53:26 UTC (rev 5541)
+++ pkg/trunk/demos/2dnav_stage/manifest.xml 2008-10-19 17:57:59 UTC (rev 5542)
@@ -3,7 +3,7 @@
<author>Brian Gerkey</author>
<license>BSD</license>
<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"/>
Modified: pkg/trunk/simulators/rosstage/rosstage.cc
===================================================================
--- pkg/trunk/simulators/rosstage/rosstage.cc 2008-10-19 17:53:26 UTC (rev 5541)
+++ pkg/trunk/simulators/rosstage/rosstage.cc 2008-10-19 17:57:59 UTC (rev 5542)
@@ -62,7 +62,7 @@
Publishes to (name / type):
- @b "odom"/RobotBase2DOdom : odometry data from the position model.
-- @b "scan"/LaserScan : scans from the laser model.
+- @b "base_scan"/LaserScan : scans from the laser model.
<hr>
@@ -205,7 +205,7 @@
return(-1);
}
- advertise<std_msgs::LaserScan>("scan",10);
+ advertise<std_msgs::LaserScan>("base_scan",10);
advertise<std_msgs::RobotBase2DOdom>("odom",10);
advertise<std_msgs::TransformWithRateStamped>("base_pose_ground_truth",10);
subscribe("cmd_vel", velMsg, &StageNode::cmdvelReceived, 10);
@@ -226,6 +226,9 @@
// Let the simulator update (it will sleep if there's time)
this->world->Update();
+ ros::Time sim_time;
+ sim_time.fromSec(world->SimTimeNow() / 1e6);
+
// Get latest laser data
Stg::stg_laser_sample_t* samples = this->lasermodel->GetSamples();
if(samples)
@@ -235,25 +238,33 @@
this->laserMsg.angle_min = -cfg.fov/2.0;
this->laserMsg.angle_max = +cfg.fov/2.0;
this->laserMsg.angle_increment = cfg.fov / (double)(cfg.sample_count-1);
+ this->laserMsg.range_min = 0.0;
this->laserMsg.range_max = cfg.range_bounds.max;
- this->laserMsg.set_ranges_size(cfg.sample_count);
- this->laserMsg.set_intensities_size(cfg.sample_count);
+ this->laserMsg.ranges.resize(cfg.sample_count);
+ this->laserMsg.intensities.resize(cfg.sample_count);
for(unsigned int i=0;i<cfg.sample_count;i++)
{
this->laserMsg.ranges[i] = samples[i].range;
this->laserMsg.intensities[i] = (uint8_t)samples[i].reflectance;
}
- // TODO: get the frame ID from somewhere
this->laserMsg.header.frame_id = "base_laser";
- this->laserMsg.header.stamp.from_double(world->SimTimeNow() / 1e6);
- //this->laserMsg.header.stamp.sec =
- //(unsigned long)floor(world->SimTimeNow() / 1e6);
- //this->laserMsg.header.stamp.nsec =
- //(unsigned long)rint(1e3 * (world->SimTimeNow() -
- //this->laserMsg.header.stamp.sec * 1e6));
- publish("scan",this->laserMsg);
+ this->laserMsg.header.stamp = sim_time;
+ publish("base_scan",this->laserMsg);
}
+
+ // Also publish the base->base_laser Tx. This could eventually move
+ // into being retrieved from the param server as a static Tx.
+ Stg::stg_pose_t lp = this->lasermodel->GetPose();
+ tf.sendEuler("base_laser",
+ "base",
+ lp.x,
+ lp.y,
+ 0.0,
+ lp.a,
+ 0.0,
+ 0.0,
+ sim_time);
// Get latest odometry data
// Translate into ROS message format and publish
@@ -265,18 +276,9 @@
this->odomMsg.vel.y = v.y;
this->odomMsg.vel.th = v.a;
this->odomMsg.stall = this->positionmodel->Stall();
- // TODO: get the frame ID from somewhere
this->odomMsg.header.frame_id = "odom";
-
- this->odomMsg.header.stamp.from_double(world->SimTimeNow() / 1e6);
- //this->odomMsg.header.stamp.sec =
- //(unsigned long)floor(world->SimTimeNow() / 1e6);
- //this->odomMsg.header.stamp.nsec =
- //(unsigned long)rint(1e3 * (world->SimTimeNow() -
- //this->odomMsg.header.stamp.sec * 1e6));
- // printf("%u \n",world->SimTimeNow());
- //printf("time: %u, %u \n",odomMsg.header.stamp.sec, odomMsg.header.stamp.nsec);
-
+ this->odomMsg.header.stamp = sim_time;
+ publish("odom",this->odomMsg);
tf.sendInverseEuler("odom",
"base",
odomMsg.pos.x,
@@ -285,10 +287,8 @@
odomMsg.pos.th,
0.0,
0.0,
- odomMsg.header.stamp);
+ sim_time);
- publish("odom",this->odomMsg);
-
// Also publish the ground truth pose
Stg::stg_pose_t gpose = this->positionmodel->GetGlobalPose();
// Use libTF to construct our outgoing message
@@ -308,7 +308,7 @@
this->groundTruthMsg.transform.rotation.w = tmpPose3D.orientation.w;
this->groundTruthMsg.header.frame_id = "odom";
- this->groundTruthMsg.header.stamp.from_double(world->SimTimeNow() / 1e6);
+ this->groundTruthMsg.header.stamp = sim_time;
publish("base_pose_ground_truth", this->groundTruthMsg);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-10-20 05:46:10
|
Revision: 5554
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5554&view=rev
Author: tfoote
Date: 2008-10-20 05:45:56 +0000 (Mon, 20 Oct 2008)
Log Message:
-----------
converting tf::Stamped<T> to inherit instead of have a member data_
Modified Paths:
--------------
pkg/trunk/nav/slam_gmapping/src/slam_gmapping.cpp
pkg/trunk/nav/wavefront_player/wavefront_player.cc
pkg/trunk/util/tf/include/tf/transform_broadcaster.h
pkg/trunk/util/tf/include/tf/transform_datatypes.h
pkg/trunk/util/tf/src/cache.cpp
pkg/trunk/util/tf/src/tf.cpp
pkg/trunk/util/tf/test/cache_unittest.cpp
pkg/trunk/util/tf/test/tf_unittest.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/helpers/robot.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/marker_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/planning_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/point_cloud_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/robot_base2d_pose_visualizer.cpp
Modified: pkg/trunk/nav/slam_gmapping/src/slam_gmapping.cpp
===================================================================
--- pkg/trunk/nav/slam_gmapping/src/slam_gmapping.cpp 2008-10-20 05:32:58 UTC (rev 5553)
+++ pkg/trunk/nav/slam_gmapping/src/slam_gmapping.cpp 2008-10-20 05:45:56 UTC (rev 5554)
@@ -84,7 +84,7 @@
// Get the robot's pose
tf::Stamped<tf::Pose> ident;
tf::Stamped<btTransform> odom_pose;
- ident.data_.setIdentity();
+ ident.setIdentity();
ident.frame_id_ = "base";
ident.stamp_ = t;
try
@@ -97,11 +97,11 @@
return false;
}
double yaw,pitch,roll;
- btMatrix3x3 mat = odom_pose.data_.getBasis();
+ btMatrix3x3 mat = odom_pose.getBasis();
mat.getEulerZYX(yaw, pitch, roll);
- gmap_pose = GMapping::OrientedPoint(odom_pose.data_.getOrigin().x(),
- odom_pose.data_.getOrigin().y(),
+ gmap_pose = GMapping::OrientedPoint(odom_pose.getOrigin().x(),
+ odom_pose.getOrigin().y(),
yaw);
return true;
}
@@ -112,7 +112,7 @@
// @todo Get the laser's pose, relative to base.
tf::Stamped<tf::Pose> ident;
tf::Stamped<btTransform> laser_pose;
- ident.data_.setIdentity();
+ ident.setIdentity();
ident.frame_id_ = "base_laser";
ident.stamp_ = scan.header.stamp;
try
@@ -126,15 +126,15 @@
return false;
}
double yaw,pitch,roll;
- btMatrix3x3 mat = laser_pose.data_.getBasis();
+ btMatrix3x3 mat = laser_pose.getBasis();
mat.getEulerZYX(yaw, pitch, roll);
- GMapping::OrientedPoint gmap_pose(laser_pose.data_.getOrigin().x(),
- laser_pose.data_.getOrigin().y(),
+ GMapping::OrientedPoint gmap_pose(laser_pose.getOrigin().x(),
+ laser_pose.getOrigin().y(),
yaw);
ROS_DEBUG("laser's pose wrt base: %.3f %.3f %.3f",
- laser_pose.data_.getOrigin().x(),
- laser_pose.data_.getOrigin().y(),
+ laser_pose.getOrigin().x(),
+ laser_pose.getOrigin().y(),
yaw);
// The laser must be called "FLASER"
@@ -248,8 +248,8 @@
/*
ROS_DEBUG("scan pose (%.3f): %.3f %.3f %.3f",
scan.header.stamp.toSec(),
- odom_pose.data_.getOrigin().x(),
- odom_pose.data_.getOrigin().y(),
+ odom_pose.getOrigin().x(),
+ odom_pose.getOrigin().y(),
yaw);
*/
Modified: pkg/trunk/nav/wavefront_player/wavefront_player.cc
===================================================================
--- pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-10-20 05:32:58 UTC (rev 5553)
+++ pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-10-20 05:45:56 UTC (rev 5554)
@@ -426,12 +426,12 @@
}
double yaw,pitch,roll;
- btMatrix3x3 mat = global_pose.data_.getBasis();
+ btMatrix3x3 mat = global_pose.getBasis();
mat.getEulerZYX(yaw, pitch, roll);
// Fill out and publish response
- this->pstate.pos.x = global_pose.data_.getOrigin().x();
- this->pstate.pos.y = global_pose.data_.getOrigin().y();
+ this->pstate.pos.x = global_pose.getOrigin().x();
+ this->pstate.pos.y = global_pose.getOrigin().y();
this->pstate.pos.th = yaw;
this->pstate.goal.x = this->goal[0];
this->pstate.goal.y = this->goal[1];
@@ -609,7 +609,7 @@
//convert!
tf::Stamped<tf::Pose> robotPose;
- robotPose.data_.setIdentity();
+ robotPose.setIdentity();
robotPose.frame_id_ = "base";
robotPose.stamp_ = 0; // request most recent pose
//robotPose.time = laserMsg.header.stamp.sec * 1000000000ULL +
@@ -663,12 +663,12 @@
{
double yaw,pitch,roll; //fixme make cleaner namespace
- btMatrix3x3 mat = global_pose.data_.getBasis();
+ btMatrix3x3 mat = global_pose.getBasis();
mat.getEulerZYX(yaw, pitch, roll);
// Are we done?
if(plan_check_done(this->plan,
- global_pose.data_.getOrigin().x(), global_pose.data_.getOrigin().y(), yaw,
+ global_pose.getOrigin().x(), global_pose.getOrigin().y(), yaw,
this->goal[0], this->goal[1], this->goal[2],
this->dist_eps, this->ang_eps))
{
@@ -688,11 +688,11 @@
// Try a local plan
if((this->planner_state == NEW_GOAL) ||
- (plan_do_local(this->plan, global_pose.data_.getOrigin().x(), global_pose.data_.getOrigin().y(),
+ (plan_do_local(this->plan, global_pose.getOrigin().x(), global_pose.getOrigin().y(),
this->plan_halfwidth) < 0))
{
// Fallback on global plan
- if(plan_do_global(this->plan, global_pose.data_.getOrigin().x(), global_pose.data_.getOrigin().y(),
+ if(plan_do_global(this->plan, global_pose.getOrigin().x(), global_pose.getOrigin().y(),
this->goal[0], this->goal[1]) < 0)
{
// no global plan
@@ -722,7 +722,7 @@
{
// global plan succeeded; now try the local plan again
this->printed_warning = false;
- if(plan_do_local(this->plan, global_pose.data_.getOrigin().x(), global_pose.data_.getOrigin().y(),
+ if(plan_do_local(this->plan, global_pose.getOrigin().x(), global_pose.getOrigin().y(),
this->plan_halfwidth) < 0)
{
// no local plan; better luck next time through
@@ -743,12 +743,12 @@
// double yaw,pitch,roll; //used temporarily earlier fixme make cleaner
//btMatrix3x3
- mat = global_pose.data_.getBasis();
+ mat = global_pose.getBasis();
mat.getEulerZYX(yaw, pitch, roll);
if(plan_compute_diffdrive_cmds(this->plan, &vx, &va,
&this->rotate_dir,
- global_pose.data_.getOrigin().x(), global_pose.data_.getOrigin().y(),
+ global_pose.getOrigin().x(), global_pose.getOrigin().y(),
yaw,
this->goal[0], this->goal[1],
this->goal[2],
@@ -789,15 +789,15 @@
assert(0);
}
double yaw,pitch,roll;
- btMatrix3x3 mat = global_pose.data_.getBasis();
+ btMatrix3x3 mat = global_pose.getBasis();
mat.getEuler(yaw, pitch, roll);
this->pstate.active = (this->enable &&
(this->planner_state == PURSUING_GOAL)) ? 1 : 0;
this->pstate.valid = (this->plan->path_count > 0) ? 1 : 0;
this->pstate.done = (this->planner_state == REACHED_GOAL) ? 1 : 0;
- this->pstate.pos.x = global_pose.data_.getOrigin().x();
- this->pstate.pos.y = global_pose.data_.getOrigin().y();
+ this->pstate.pos.x = global_pose.getOrigin().x();
+ this->pstate.pos.y = global_pose.getOrigin().y();
this->pstate.pos.th = yaw;
this->pstate.goal.x = this->goal[0];
this->pstate.goal.y = this->goal[1];
Modified: pkg/trunk/util/tf/include/tf/transform_broadcaster.h
===================================================================
--- pkg/trunk/util/tf/include/tf/transform_broadcaster.h 2008-10-20 05:32:58 UTC (rev 5553)
+++ pkg/trunk/util/tf/include/tf/transform_broadcaster.h 2008-10-20 05:45:56 UTC (rev 5554)
@@ -72,10 +72,10 @@
tfArray.quaternions[0].header.frame_id = transform.frame_id_;
tfArray.quaternions[0].parent = transform.parent_id_;
- Quaternion q = transform.data_.getRotation();
- tfArray.quaternions[0].xt = transform.data_.getOrigin().x();
- tfArray.quaternions[0].yt = transform.data_.getOrigin().y();
- tfArray.quaternions[0].zt = transform.data_.getOrigin().z();
+ Quaternion q = transform.getRotation();
+ tfArray.quaternions[0].xt = transform.getOrigin().x();
+ tfArray.quaternions[0].yt = transform.getOrigin().y();
+ tfArray.quaternions[0].zt = transform.getOrigin().z();
tfArray.quaternions[0].xr = q.x();
tfArray.quaternions[0].yr = q.y();
tfArray.quaternions[0].zr = q.z();
Modified: pkg/trunk/util/tf/include/tf/transform_datatypes.h
===================================================================
--- pkg/trunk/util/tf/include/tf/transform_datatypes.h 2008-10-20 05:32:58 UTC (rev 5553)
+++ pkg/trunk/util/tf/include/tf/transform_datatypes.h 2008-10-20 05:45:56 UTC (rev 5554)
@@ -60,9 +60,8 @@
/** \brief The data type which will be cross compatable with std_msgs
* this will require the associated rosTF package to convert */
template <typename T>
-class Stamped{
+class Stamped : public T{
public:
- T data_;
ros::Time stamp_;
std::string frame_id_;
std::string parent_id_; ///only used for transform
@@ -70,14 +69,15 @@
Stamped() :stamp_ (0),frame_id_ ("NO_ID"), parent_id_("NOT A TRANSFORM"){}; //Default constructor used only for preallocation
Stamped(const T& input, const ros::Time& timestamp, const std::string & frame_id, const std::string & parent_id = "NOT A TRANSFORM"):
- data_ (input), stamp_ ( timestamp ), frame_id_ (frame_id), parent_id_(parent_id){ };
+ T (input), stamp_ ( timestamp ), frame_id_ (frame_id), parent_id_(parent_id){ };
//Stamped(const Stamped<T>& input):data_(input.data_), stamp_(input.stamp_), frame_id_(input.frame_id_), parent_id_(input.parent_id_){};
//Stamped& operator=(const Stamped<T>& input){data_ = input.data_; stamp_ = input.stamp_; frame_id_ = input.frame_id_;
// parent_id_ = input.parent_id_; return *this;};
- void stripStamp(T & output) { output = data_;};
+ void setData(const T& input){*static_cast<T*>(this) = input;};
+ // void stripStamp(T & output) { output = data_;}; //just down cast it
};
@@ -88,10 +88,10 @@
/** \brief convert QuaternionStamped msg to Stamped<Quaternion> */
static inline void QuaternionStampedMsgToTF(const std_msgs::QuaternionStamped & msg, Stamped<Quaternion>& bt)
-{QuaternionMsgToTF(msg.quaternion, bt.data_); bt.stamp_ = msg.header.stamp.to_ull(); bt.frame_id_ = msg.header.frame_id;};
+{QuaternionMsgToTF(msg.quaternion, bt); bt.stamp_ = msg.header.stamp.to_ull(); bt.frame_id_ = msg.header.frame_id;};
/** \brief convert Stamped<Quaternion> to QuaternionStamped msg*/
static inline void QuaternionStampedTFToMsg(const Stamped<Quaternion>& bt, std_msgs::QuaternionStamped & msg)
-{QuaternionTFToMsg(bt.data_, msg.quaternion); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_;};
+{QuaternionTFToMsg(bt, msg.quaternion); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_;};
/** \brief convert Vector3 msg to Vector3 */
static inline void Vector3MsgToTF(const std_msgs::Vector3& msg_v, Vector3& bt_v) {bt_v = Vector3(msg_v.x, msg_v.y, msg_v.z);};
@@ -100,10 +100,10 @@
/** \brief convert Vector3Stamped msg to Stamped<Vector3> */
static inline void Vector3StampedMsgToTF(const std_msgs::Vector3Stamped & msg, Stamped<Vector3>& bt)
-{Vector3MsgToTF(msg.vector, bt.data_); bt.stamp_ = msg.header.stamp.to_ull(); bt.frame_id_ = msg.header.frame_id;};
+{Vector3MsgToTF(msg.vector, bt); bt.stamp_ = msg.header.stamp.to_ull(); bt.frame_id_ = msg.header.frame_id;};
/** \brief convert Stamped<Vector3> to Vector3Stamped msg*/
static inline void Vector3StampedTFToMsg(const Stamped<Vector3>& bt, std_msgs::Vector3Stamped & msg)
-{Vector3TFToMsg(bt.data_, msg.vector); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_;};
+{Vector3TFToMsg(bt, msg.vector); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_;};
/** \brief convert Point msg to Point */
@@ -113,10 +113,10 @@
/** \brief convert PointStamped msg to Stamped<Point> */
static inline void PointStampedMsgToTF(const std_msgs::PointStamped & msg, Stamped<Point>& bt)
-{PointMsgToTF(msg.point, bt.data_); bt.stamp_ = msg.header.stamp.to_ull(); bt.frame_id_ = msg.header.frame_id;};
+{PointMsgToTF(msg.point, bt); bt.stamp_ = msg.header.stamp.to_ull(); bt.frame_id_ = msg.header.frame_id;};
/** \brief convert Stamped<Point> to PointStamped msg*/
static inline void PointStampedTFToMsg(const Stamped<Point>& bt, std_msgs::PointStamped & msg)
-{PointTFToMsg(bt.data_, msg.point); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_;};
+{PointTFToMsg(bt, msg.point); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_;};
/** \brief convert Transform msg to Transform */
@@ -128,10 +128,10 @@
/** \brief convert TransformStamped msg to Stamped<Transform> */
static inline void TransformStampedMsgToTF(const std_msgs::TransformStamped & msg, Stamped<Transform>& bt)
-{TransformMsgToTF(msg.transform, bt.data_); bt.stamp_ = msg.header.stamp.to_ull(); bt.frame_id_ = msg.header.frame_id; bt.parent_id_ = msg.parent_id;};
+{TransformMsgToTF(msg.transform, bt); bt.stamp_ = msg.header.stamp.to_ull(); bt.frame_id_ = msg.header.frame_id; bt.parent_id_ = msg.parent_id;};
/** \brief convert Stamped<Transform> to TransformStamped msg*/
static inline void TransformStampedTFToMsg(const Stamped<Transform>& bt, std_msgs::TransformStamped & msg)
-{TransformTFToMsg(bt.data_, msg.transform); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_; msg.parent_id = bt.parent_id_;};
+{TransformTFToMsg(bt, msg.transform); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_; msg.parent_id = bt.parent_id_;};
/** \brief convert Pose msg to Pose */
static inline void PoseMsgToTF(const std_msgs::Pose& msg, Pose& bt)
@@ -142,10 +142,10 @@
/** \brief convert PoseStamped msg to Stamped<Pose> */
static inline void PoseStampedMsgToTF(const std_msgs::PoseStamped & msg, Stamped<Pose>& bt)
-{PoseMsgToTF(msg.pose, bt.data_); bt.stamp_ = msg.header.stamp.to_ull(); bt.frame_id_ = msg.header.frame_id;};
+{PoseMsgToTF(msg.pose, bt); bt.stamp_ = msg.header.stamp.to_ull(); bt.frame_id_ = msg.header.frame_id;};
/** \brief convert Stamped<Pose> to PoseStamped msg*/
static inline void PoseStampedTFToMsg(const Stamped<Pose>& bt, std_msgs::PoseStamped & msg)
-{PoseTFToMsg(bt.data_, msg.pose); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_;};
+{PoseTFToMsg(bt, msg.pose); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_;};
/** \brief Convert the transform to a Homogeneous matrix for large operations */
Modified: pkg/trunk/util/tf/src/cache.cpp
===================================================================
--- pkg/trunk/util/tf/src/cache.cpp 2008-10-20 05:32:58 UTC (rev 5553)
+++ pkg/trunk/util/tf/src/cache.cpp 2008-10-20 05:45:56 UTC (rev 5554)
@@ -160,14 +160,14 @@
//Interpolate translation
btVector3 v;
- v.setInterpolate3(one.data_.getOrigin(), two.data_.getOrigin(), ratio);
- output.data_.setOrigin(v);
+ v.setInterpolate3(one.getOrigin(), two.getOrigin(), ratio);
+ output.setOrigin(v);
//Interpolate rotation
btQuaternion q1,q2;
- one.data_.getBasis().getRotation(q1);
- two.data_.getBasis().getRotation(q2);
- output.data_.setRotation(slerp( q1, q2 , ratio));
+ one.getBasis().getRotation(q1);
+ two.getBasis().getRotation(q2);
+ output.setRotation(slerp( q1, q2 , ratio));
output.frame_id_ = one.frame_id_;
output.parent_id_ = one.parent_id_;
output.parent_frame_id = one.parent_frame_id;
Modified: pkg/trunk/util/tf/src/tf.cpp
===================================================================
--- pkg/trunk/util/tf/src/tf.cpp 2008-10-20 05:32:58 UTC (rev 5553)
+++ pkg/trunk/util/tf/src/tf.cpp 2008-10-20 05:45:56 UTC (rev 5554)
@@ -83,7 +83,7 @@
{
TransformLists t_list = lookupLists(lookupFrameNumber( target_frame), time, lookupFrameNumber( source_frame));
- transform.data_ = computeTransformFromList(t_list);
+ transform.setData( computeTransformFromList(t_list));
transform.stamp_ = time;
transform.frame_id_ = target_frame;
@@ -94,14 +94,14 @@
{
//calculate first leg
TransformLists t_list = lookupLists(lookupFrameNumber( fixed_frame), source_time, lookupFrameNumber( source_frame));
- transform.data_ = computeTransformFromList(t_list);
+ btTransform temp1 = computeTransformFromList(t_list);
TransformLists t_list2 = lookupLists(lookupFrameNumber( target_frame), target_time, lookupFrameNumber( fixed_frame));
btTransform temp = computeTransformFromList(t_list2);
- transform.data_*= temp; ///\todo check order here
+ transform.setData( temp1 * temp); ///\todo check order here
transform.stamp_ = target_time;
transform.frame_id_ = target_frame;
@@ -278,7 +278,7 @@
for (unsigned int i = 0; i < lists.inverseTransforms.size(); i++)
{
try {
- retTrans *= (lists.inverseTransforms[lists.inverseTransforms.size() -1 - i]).data_; //Reverse to get left multiply
+ retTrans *= (lists.inverseTransforms[lists.inverseTransforms.size() -1 - i]); //Reverse to get left multiply
}
catch (tf::ExtrapolationException &ex)
{
@@ -290,7 +290,7 @@
for (unsigned int i = 0; i < lists.forwardTransforms.size(); i++)
{
try {
- retTrans = (lists.forwardTransforms[lists.forwardTransforms.size() -1 - i]).data_.inverse() * retTrans; //Do this list backwards(from backwards) for it was generated traveling the wrong way
+ retTrans = (lists.forwardTransforms[lists.forwardTransforms.size() -1 - i]).inverse() * retTrans; //Do this list backwards(from backwards) for it was generated traveling the wrong way
}
catch (tf::ExtrapolationException &ex)
{
@@ -390,7 +390,7 @@
btTransform transform = computeTransformFromList(t_list);
- stamped_out.data_ = transform * stamped_in.data_;
+ stamped_out.setData( transform * stamped_in);
stamped_out.stamp_ = stamped_in.stamp_;
stamped_out.frame_id_ = target_frame;
};
@@ -404,10 +404,10 @@
btTransform transform = computeTransformFromList(t_list);
/** \todo may not be most efficient */
- btVector3 end = stamped_in.data_;
+ btVector3 end = stamped_in;
btVector3 origin = btVector3(0,0,0);
btVector3 output = (transform * end) - (transform * origin);
- stamped_out.data_ = output;
+ stamped_out.setData( output);
stamped_out.stamp_ = stamped_in.stamp_;
stamped_out.frame_id_ = target_frame;
@@ -420,7 +420,7 @@
btTransform transform = computeTransformFromList(t_list);
- stamped_out.data_ = transform * stamped_in.data_;
+ stamped_out.setData(transform * stamped_in);
stamped_out.stamp_ = stamped_in.stamp_;
stamped_out.frame_id_ = target_frame;
stamped_out.parent_id_ = stamped_in.parent_id_;//only useful for transforms
@@ -432,7 +432,7 @@
btTransform transform = computeTransformFromList(t_list);
- stamped_out.data_ = transform * stamped_in.data_;
+ stamped_out.setData(transform * stamped_in);
stamped_out.stamp_ = stamped_in.stamp_;
stamped_out.frame_id_ = target_frame;
// stamped_out.parent_id_ = stamped_in.parent_id_;//only useful for transforms
Modified: pkg/trunk/util/tf/test/cache_unittest.cpp
===================================================================
--- pkg/trunk/util/tf/test/cache_unittest.cpp 2008-10-20 05:32:58 UTC (rev 5553)
+++ pkg/trunk/util/tf/test/cache_unittest.cpp 2008-10-20 05:45:56 UTC (rev 5554)
@@ -55,7 +55,7 @@
std::vector<double> values(runs);
TransformStorage stor;
- stor.data_.setIdentity();
+ stor.setIdentity();
for ( unsigned int i = 1; i < runs ; i++ )
{
@@ -92,7 +92,7 @@
std::vector<double> values(runs);
TransformStorage stor;
- stor.data_.setIdentity();
+ stor.setIdentity();
for ( int i = runs -1; i >= 0 ; i-- )
{
@@ -129,7 +129,7 @@
unsigned int runs = values.size();
TransformStorage stor;
- stor.data_.setIdentity();
+ stor.setIdentity();
for ( unsigned int i = 0; i <runs ; i++ )
{
values[i] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
@@ -164,7 +164,7 @@
std::vector<double> values(runs);
TransformStorage stor;
- stor.data_.setIdentity();
+ stor.setIdentity();
for ( unsigned int i = 1; i < runs ; i++ )
{
@@ -229,7 +229,7 @@
unsigned int offset = 200;
TransformStorage stor;
- stor.data_.setIdentity();
+ stor.setIdentity();
for ( unsigned int i = 1; i < runs ; i++ )
{
@@ -240,7 +240,7 @@
yvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
zvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
- stor.data_.setOrigin(btVector3(xvalues[step], yvalues[step], zvalues[step]));
+ stor.setOrigin(btVector3(xvalues[step], yvalues[step], zvalues[step]));
stor.frame_id_ = "NO_NEED";
stor.parent_frame_id = 2;
stor.stamp_ = step * 100 + offset;
@@ -250,9 +250,9 @@
for (int pos = 0; pos < 100 ; pos ++)
{
cache.getData(offset + pos, stor);
- double x_out = stor.data_.getOrigin().x();
- double y_out = stor.data_.getOrigin().y();
- double z_out = stor.data_.getOrigin().z();
+ double x_out = stor.getOrigin().x();
+ double y_out = stor.getOrigin().y();
+ double z_out = stor.getOrigin().z();
EXPECT_NEAR(xvalues[0] + (xvalues[1] - xvalues[0]) * (double)pos/100.0, x_out, epsilon);
EXPECT_NEAR(yvalues[0] + (yvalues[1] - yvalues[0]) * (double)pos/100.0, y_out, epsilon);
EXPECT_NEAR(zvalues[0] + (zvalues[1] - zvalues[0]) * (double)pos/100.0, z_out, epsilon);
@@ -277,7 +277,7 @@
std::vector<double> zvalues(2);
TransformStorage stor;
- stor.data_.setIdentity();
+ stor.setIdentity();
for (unsigned int step = 0; step < 2 ; step++)
{
@@ -285,7 +285,7 @@
yvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
zvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
- stor.data_.setOrigin(btVector3(xvalues[step], yvalues[step], zvalues[step]));
+ stor.setOrigin(btVector3(xvalues[step], yvalues[step], zvalues[step]));
stor.frame_id_ = "NO_NEED";
stor.parent_frame_id = step + 4;
stor.stamp_ = step * 100 + offset;
@@ -295,9 +295,9 @@
for (int pos = 0; pos < 100 ; pos ++)
{
cache.getData(offset + pos, stor);
- double x_out = stor.data_.getOrigin().x();
- double y_out = stor.data_.getOrigin().y();
- double z_out = stor.data_.getOrigin().z();
+ double x_out = stor.getOrigin().x();
+ double y_out = stor.getOrigin().y();
+ double z_out = stor.getOrigin().z();
EXPECT_NEAR(xvalues[0], x_out, epsilon);
EXPECT_NEAR(yvalues[0], y_out, epsilon);
EXPECT_NEAR(zvalues[0], z_out, epsilon);
@@ -306,9 +306,9 @@
for (int pos = 100; pos < 120 ; pos ++)
{
cache.getData(offset + pos, stor);
- double x_out = stor.data_.getOrigin().x();
- double y_out = stor.data_.getOrigin().y();
- double z_out = stor.data_.getOrigin().z();
+ double x_out = stor.getOrigin().x();
+ double y_out = stor.getOrigin().y();
+ double z_out = stor.getOrigin().z();
EXPECT_NEAR(xvalues[1], x_out, epsilon);
EXPECT_NEAR(yvalues[1], y_out, epsilon);
EXPECT_NEAR(zvalues[1], z_out, epsilon);
@@ -331,7 +331,7 @@
unsigned int offset = 555;
TransformStorage stor;
- stor.data_.setIdentity();
+ stor.setIdentity();
for ( unsigned int i = 1; i < runs ; i++ )
{
@@ -342,7 +342,7 @@
yvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
zvalues[step] = 10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
- stor.data_.setOrigin(btVector3(xvalues[step], yvalues[step], zvalues[step]));
+ stor.setOrigin(btVector3(xvalues[step], yvalues[step], zvalues[step]));
stor.frame_id_ = "NO_NEED";
stor.parent_frame_id = 2;
stor.stamp_ = step * 100 + offset;
@@ -352,9 +352,9 @@
for (int pos = -200; pos < 300 ; pos ++)
{
cache.getData(offset + pos, stor);
- double x_out = stor.data_.getOrigin().x();
- double y_out = stor.data_.getOrigin().y();
- double z_out = stor.data_.getOrigin().z();
+ double x_out = stor.getOrigin().x();
+ double y_out = stor.getOrigin().y();
+ double z_out = stor.getOrigin().z();
EXPECT_NEAR(xvalues[0] + (xvalues[1] - xvalues[0]) * (double)pos/100.0, x_out, epsilon);
EXPECT_NEAR(yvalues[0] + (yvalues[1] - yvalues[0]) * (double)pos/100.0, y_out, epsilon);
EXPECT_NEAR(zvalues[0] + (zvalues[1] - zvalues[0]) * (double)pos/100.0, z_out, epsilon);
@@ -406,7 +406,7 @@
std::vector<btQuaternion> quats(2);
TransformStorage stor;
- stor.data_.setIdentity();
+ stor.setIdentity();
for ( unsigned int i = 1; i < runs ; i++ )
{
@@ -417,7 +417,7 @@
pitchvalues[step] = 0;//10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
rollvalues[step] = 0;//10.0 * ((double) rand() - (double)RAND_MAX /2.0) /(double)RAND_MAX;
quats[step] = btQuaternion(yawvalues[step], pitchvalues[step], rollvalues[step]);
- stor.data_.setRotation(quats[step]);
+ stor.setRotation(quats[step]);
stor.frame_id_ = "NO_NEED";
stor.parent_frame_id = 3;
stor.stamp_ = offset + (step * 100); //step = 0 or 1
@@ -427,7 +427,7 @@
for (int pos = -100; pos < 200 ; pos ++)
{
cache.getData(offset + pos, stor); //get the transform for the position
- btQuaternion quat = stor.data_.getRotation(); //get the quaternion out of the transform
+ btQuaternion quat = stor.getRotation(); //get the quaternion out of the transform
//Generate a ground truth quaternion directly calling slerp
btQuaternion ground_truth = quats[0].slerp(quats[1], pos/100.0);
Modified: pkg/trunk/util/tf/test/tf_unittest.cpp
===================================================================
--- pkg/trunk/util/tf/test/tf_unittest.cpp 2008-10-20 05:32:58 UTC (rev 5553)
+++ pkg/trunk/util/tf/test/tf_unittest.cpp 2008-10-20 05:45:56 UTC (rev 5554)
@@ -61,13 +61,12 @@
Stamped<btTransform> inpose (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), 10 + i, "child");
try{
- Stamped<Pose
-> outpose;
- outpose.data_.setIdentity(); //to make sure things are getting mutated
+ Stamped<Pose> outpose;
+ outpose.setIdentity(); //to make sure things are getting mutated
mTR.transformPose("my_parent",inpose, outpose);
- EXPECT_NEAR(outpose.data_.getOrigin().x(), xvalues[i], epsilon);
- EXPECT_NEAR(outpose.data_.getOrigin().y(), yvalues[i], epsilon);
- EXPECT_NEAR(outpose.data_.getOrigin().z(), zvalues[i], epsilon);
+ EXPECT_NEAR(outpose.getOrigin().x(), xvalues[i], epsilon);
+ EXPECT_NEAR(outpose.getOrigin().y(), yvalues[i], epsilon);
+ EXPECT_NEAR(outpose.getOrigin().z(), zvalues[i], epsilon);
}
catch (tf::TransformException & ex)
{
@@ -79,11 +78,11 @@
Stamped<Pose> inpose (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), runs, "child");
Stamped<Pose> outpose;
- outpose.data_.setIdentity(); //to make sure things are getting mutated
+ outpose.setIdentity(); //to make sure things are getting mutated
mTR.transformPose("child",inpose, outpose);
- EXPECT_NEAR(outpose.data_.getOrigin().x(), 0, epsilon);
- EXPECT_NEAR(outpose.data_.getOrigin().y(), 0, epsilon);
- EXPECT_NEAR(outpose.data_.getOrigin().z(), 0, epsilon);
+ EXPECT_NEAR(outpose.getOrigin().x(), 0, epsilon);
+ EXPECT_NEAR(outpose.getOrigin().y(), 0, epsilon);
+ EXPECT_NEAR(outpose.getOrigin().z(), 0, epsilon);
}
@@ -123,12 +122,12 @@
try{
Stamped<Pose> outpose;
- outpose.data_.setIdentity(); //to make sure things are getting mutated
+ outpose.setIdentity(); //to make sure things are getting mutated
mTR.transformPose("child",inpose, outpose);
- EXPECT_NEAR(outpose.data_.getOrigin().x(), 0, epsilon);
- EXPECT_NEAR(outpose.data_.getOrigin().y(), 0, epsilon);
- EXPECT_NEAR(outpose.data_.getOrigin().z(), 0, epsilon);
- EXPECT_NEAR(outpose.data_.getRotation().w(), 1, epsilon); //Identity is 0,0,0,1
+ EXPECT_NEAR(outpose.getOrigin().x(), 0, epsilon);
+ EXPECT_NEAR(outpose.getOrigin().y(), 0, epsilon);
+ EXPECT_NEAR(outpose.getOrigin().z(), 0, epsilon);
+ EXPECT_NEAR(outpose.getRotation().w(), 1, epsilon); //Identity is 0,0,0,1
}
catch (tf::TransformException & ex)
{
@@ -140,11 +139,11 @@
Stamped<Pose> inpose (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), runs, "child");
Stamped<Pose> outpose;
- outpose.data_.setIdentity(); //to make sure things are getting mutated
+ outpose.setIdentity(); //to make sure things are getting mutated
mTR.transformPose("child",inpose, outpose);
- EXPECT_NEAR(outpose.data_.getOrigin().x(), 0, epsilon);
- EXPECT_NEAR(outpose.data_.getOrigin().y(), 0, epsilon);
- EXPECT_NEAR(outpose.data_.getOrigin().z(), 0, epsilon);
+ EXPECT_NEAR(outpose.getOrigin().x(), 0, epsilon);
+ EXPECT_NEAR(outpose.getOrigin().y(), 0, epsilon);
+ EXPECT_NEAR(outpose.getOrigin().z(), 0, epsilon);
}
@@ -184,11 +183,11 @@
try{
Stamped<Point> outvec(btVector3(0,0,0), 10 + i, "child");
- // outpose.data_.setIdentity(); //to make sure things are getting mutated
+ // outpose.setIdentity(); //to make sure things are getting mutated
mT...
[truncated message content] |
|
From: <stu...@us...> - 2008-10-20 17:08:22
|
Revision: 5556
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5556&view=rev
Author: stuglaser
Date: 2008-10-20 17:08:12 +0000 (Mon, 20 Oct 2008)
Log Message:
-----------
Mechanism control now refuses to create a controller when one with the same name already exists.
Modified Paths:
--------------
pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
Added Paths:
-----------
pkg/trunk/util/misc_utils/include/misc_utils/mutex_guard.h
Modified: pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-10-20 17:07:54 UTC (rev 5555)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-10-20 17:08:12 UTC (rev 5556)
@@ -30,6 +30,7 @@
#include "mechanism_control/mechanism_control.h"
#include "rosthread/member_thread.h"
+#include "misc_utils/mutex_guard.h"
using namespace mechanism;
@@ -112,28 +113,22 @@
bool MechanismControl::addController(controller::Controller *c, const std::string &name)
{
- //Add controller to list of controllers in realtime-safe manner;
- controllers_lock_.lock(); //This lock is only to prevent us from other non-realtime threads. The realtime thread may be spinning through the list of controllers while we are in here, so we need to keep that list always in a valid state. This is why we fully allocate and set up the controller before adding it into the list of active controllers.
+ misc_utils::MutexGuard guard(&controllers_lock_);
- bool spot_found = false;
+ if (getControllerByName(name))
+ return false;
+
for (int i = 0; i < MAX_NUM_CONTROLLERS; i++)
{
if (controllers_[i] == NULL)
{
- spot_found = true;
controllers_[i] = c;
controller_names_[i] = name;
- break;
+ return true;
}
}
- controllers_lock_.unlock();
- if (!spot_found)
- {
- return false;
- }
-
- return true;
+ return false;
}
Added: pkg/trunk/util/misc_utils/include/misc_utils/mutex_guard.h
===================================================================
--- pkg/trunk/util/misc_utils/include/misc_utils/mutex_guard.h (rev 0)
+++ pkg/trunk/util/misc_utils/include/misc_utils/mutex_guard.h 2008-10-20 17:08:12 UTC (rev 5556)
@@ -0,0 +1,61 @@
+/*
+ * 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, 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
+ * 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: Stuart Glaser
+ */
+#ifndef MUTEX_GUARD_H
+#define MUTEX_GUARD_H
+
+#include "rosthread/mutex.h"
+
+namespace misc_utils {
+
+class MutexGuard
+{
+public:
+ MutexGuard(ros::thread::mutex *mutex)
+ {
+ assert(mutex);
+ m = mutex;
+ m->lock();
+ }
+
+ ~MutexGuard()
+ {
+ m->unlock();
+ }
+
+private:
+ ros::thread::mutex *m;
+};
+
+}
+
+#endif
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